https://orcid.org/0000-0003-0996-6103
Figures
Abstract
The control method is the central point of the unmanned vehicles. As the core system to guarantee the properties of self-decision and trajectory tracking of the unmanned vehicles, a new kind of trajectory tracking method based on the circulation of feasible path planning for the unmanned vehicles are proposed in this article which considered the dynamics and kinematics characteristics of vehicles. The multi-trace-points cooperative trajectory tracking control strategy on the basis of the circulation of feasible path generation method is proposed and the lateral controller is designed for trajectory tracking. The process of feasible path generation is conducted once the tracking error exceeded. A simulation platform of the trajectory tracking simulation of unmanned vehicles is built considering the mechanical properties of system elements and the mechanical characteristics. Finally, the proposed trajectory tracking method is verified. The tracking error would be reduced to make sure the vehicles move along the pre-set virtual track.
Citation: Yu Y, Han P (2021) Trajectory tracking method based on the circulation of feasible path planning. PLoS ONE 16(6): e0252542. https://doi.org/10.1371/journal.pone.0252542
Editor: Feng Chen, Tongii University, CHINA
Received: January 7, 2021; Accepted: May 18, 2021; Published: June 7, 2021
Copyright: © 2021 Yu, Han. This is an open access article distributed under the terms of the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original author and source are credited.
Data Availability: All relevant data are within the paper and its Supporting Information files.
Funding: This research was funded by the National Key Research and Development Program of China (2017YFB0503401); Scientific Research Project of Tianjin Education Commission (2019KJ128); Zhe Jiang Key laboratory of General Aviation Operation technology, (JDGA2020-8); Fundamental Research Funds for the Central Universities (3122018C035), and the Opening Fund of Civil Aviation Key Laboratory of Wide-area Surveillance and Security Control Technology (KG001).
Competing interests: The authors have declared that no competing interests exist.
1. Introduction
With the application of unmanned vehicles, the researches on trajectory tracking are very rich at this field. The unmanned vehicles are typically nonholonomic constraint system which have the characteristics of highly non-linear and complexity. Thus, it is difficult to propose the control strategy based on precise modeling. The commonly used tracking control strategy decouples the lateral and longitudinal motion to control the path following and velocity seperately.
The path following could be divided into preview control and compensation control. The preview control methods [1–3] calculated the target quantities to control the motion state of the unmanned vehicles based on the dynamics model and the kinematics model. The real time condition monitoring system should give a feedback of the actual lateral acceleration, yaw velocity, and sideslip angle, and so on. The controller made the difference between the calculated target and actual quantities decrease to approach the target path. Xia [4] proposed a novel approach combining the sliding mode control and extended state observer (ESO) for attitude control of a missile model. Tang [5] developed trajectory tracking and configuration stabilization for the vertical takeoff and landing (VTOL) aircraft which addressed global configuration stabilization with a strong input coupling using a smooth static state feedback. The compensation control method monitored the target trajectory and actual location of the vehicle. Then the actuators were controlled directly to move along the trajectory. The steering model, kinematics model and dynamics model were often used in the control system. The control method could also be classified based on the control strategy, such as PID method, optimum control, sliding-mode control, model predictive control, fuzzy control, and neural network control. Oreh [6] proposed a new desired articulation angle for directional control of the articulated vehicles. The proposed reference value tracking ensured that the rear end of the trailer unit closely followed the trajectory of the fifth wheel. Overcoming the methods mentioned above, several research efforts [2, 7–13] had been reported on dynamic analysis and active control systems development in articulated vehicles. The scopes of these researches were the maneuverability improvement and stability limits enhancement of the articulated vehicles. These methods applied in unmanned vehicles had obtained good results.
The development of these advanced technologies provided proper environments for the development of unmanned vehicles. On this background, a new trajectory tracking method based on the circulation of feasible path planning was proposed in this paper.
2. Kinematics model of unmanned vehicles
2.1. The expected position determination method
The location of the vehicle could be divided into two steps. The first step is to determine the position and attitude of the first element. Then, the location of the other elements could be decided according to the constraint conditions of the virtual track and articulate points. To ensure the vehicle moving along the track, the concepts of trace points are raised to guarantee to move along the track. The numbers of trace points should be subject to ensure the uniqueness property of position and attitude of each vehicle element. There are two basic factors for the trace points.
- It is fixed in the carbody and moved with each element;
- The trace points should always move along the track.
According to the two assumptions and the principle of rigid body kinematics, the third implicated assumption inferred that the velocity in trace points is along the tangential direction of the curvature.
As is shown in Fig 1, the position and attitude of the first vehicle element should be confirmed uniquely by two trace points, and the following vehicles could be confirmed by only one trace point since the articulated point between two vehicles isalso used to locate. Thus, the expected position of each vehicle element along the virtual track is confirmed uniquely and definitely.
The trace points are labeled as Of1, Or1, Or2 and Or3. The absolute coordinates are described as and
. The attitude of each vehicle element and the position of each point in the vehicle could be described as follow.
(1)
(2)
As well, θc2, θc3 and
could be determined in the same method.
2.2. Kinematic model of the vehicle
As is shown in Fig 2, the kinematics constraints are acted between two vehicle elements. The moving coordinate system are denoted as OiXiYiZi and OjXjYjZj. Taking the revolute joint as an example, three degrees of freedom including parallel horizontal and longitudinal motion are limited. Only the vertical rotational motion is retained. The revolution of two vehicle elements are recorded as and
. Two orthogonal vectors are selected in the second element and recorded as
and
separately. The equations of kinematics constraints in the hinge point are expressed as Eq (3).
3. Feasible path planning
The aim of feasible path planning is to generate a feasible path of the vehicle which satisfy the kinematics constraints, boundary conditions and the characteristic of actuators. As Fig 4 shows, the problem of path planning could be represented as the planning of a series of movement attitudes and gestures between the original state and the terminal state. The vehicle could operate according to the planning gestures to achieve the goal of path following.
The physical quantities which describe the motion of vehicle are labelled as set C. The set contains the location coordinates of each trace point , the instantaneous turn center
, and the articulated mechanism
. The attitudes, yaw velocity of each vehicle element, the steering angle and velocity of each wheel which recorded separately as φi, ωi, δijk and vijk are all included. The mapping function which is labelled as Γ from the planned path to these physical quantities which control the motion features of the vehicle could be written as following.
(8)
In the equation, g(x) is the planned path, and
is the velocity of each trace point. Parameter m is the number of trace points. Parameter i is the number of vehicle elements. Parameter j and k represent the location of each wheel.
3.1. Boundary constraint conditions
The essence of path planning of the vehicle is the problem of curve generation from given original state and the terminal state. The generated curve should meet the two requirements listed below. The cycle of path planning begins from the original state and ends at the terminal state. The start and end time of the cycle are labelled as tb and te. Cb and Ce represent the physical quantities of the original and terminal motion state which could be assigned as Eqs (9) and (10).
(9)
(10)
The constraints of the original and terminal boundary are expressed as the control of the location and shape of the generated curve. The trace points should be always on the curve and the tangent slope of each trace point on the curve is given by the boundary constraint. The constraint equation is shown in Eq (11).
3.2. Curve generation method
Cubic spline and B-Spline interpolation are appropriately considered the kinematic characteristics of the vehicle. The two curves could fit the function values in trace points Which are both second order derivatives to control the radius of curvature. These advantages are good to control the continuity of the path. However, the first-order derivative is uncontrollable which means the direction of the curve is uncertain. The higher-degree polynomial is all right to control both the value and the first-order derivative in trace points, but the curvature of planned curve is hard to guarantee. Thus, a method of segmental interpolation based on quartic polynomial is proposed here.
The polynomial equation in each segment is expressed as Eq (12).
(12)
Thus, the first and second order derivatives are expressed as Eqs (13) and (14).
(13)
(14)
The function value and the first derivative of each trace point are given by the boundary constraint Eq (11). Bring them into (13) and (14), we can get the following equation.
(15)
(16)
(17)
(18)
The characteristics of the segmental interpolation based on quartic polynomial should guarantee the continuity of function values and the second order derivatives in the boundary points. Thus, we can get,
(19)
(20)
(21)
If n represents the number of interpolating points, the number of the equations listed above is 5n-6 and the number of undetermined coefficients of the piecewise curve based on quartic polynomial is 5n-5. The curvilinear equation could be uniquely determined when any equation is given. In general, the radius of curvature in the last trace point of the terminal state is used here, which is expressed as Eq (22).
(22)
The expression of the planned feasible path of the vehicle in the cycle is determined uniformly according to these simultaneous equations. The physical quantities which describe the motion could also be calculated to guarantee the vehicle to move along the planned path.
4. The circulation of feasible path planning
According to the kinematics model of the vehicle, the values of the variables which control the motion of vehicle are related to the actual operating path when the location and quantities of the trace points are confirmed. Thus, the coordinate trajectory tracking strategy of multi-units of the whole vehicle is converted to the planning of feasible operating path. When the detected tracking error is larger than the preset virtual track, a new preview point is selected from the virtual track as the terminal boundary of circulation. The feasible operating path is preplanned and the values of control variables are recalculated. The decision condition of the circulation and the selection method of the preview point are described in detail.
4.1. Circulation condition
The tracking error of the whole vehicle should be defined first. There are two essential means to represent the tracking error. The first is the distance from a certain point on the vehicle to a certain point on the virtual track, and the second is the difference between the actual attitude of the vehicle and the ideal one calculated by the virtual track. Thus, the tracking error of each vehicle unit should be considered. As is shown in Fig 5, the actual position of trace points is expressed as Eq (23) and the actual attitude of each vehicle element is labeled as φi.
4.2. Terminal boundary
At present, the preview distance is calculated by the product of vehicle speed and preview time.
(28)
As Eq (28) shows, the preview time here is an empirical value from 0.8 to 1.2. The quality of planned path could be evaluated by the curvature and its change rate. As Eq (29) shows, the index kt is defined to evaluate the quality.
(29)
The bigger the index kt is, the better quality the planned path possesses. In the equation, rmin and
are the limit radius and change rate restrained by the characteristic of the vehicle.
5. Dynamics model of the unmanned vehicle
The degrees of freedom in dynamics model are selected according to the research purpose. The contents here are focused on path-following control of the whole guided bus. Thus, the response speed needs to be fast. The requirement of calculation speed and accuracy should be considered comprehensively. The most relative eleven degrees of freedom are built based on the following assumptions.
- Only the planar motion paralleled of the pavement is considered;
- The axle load transfer caused by the horizontal and longitudinal acceleration is considered;
- The calculation difference of unspring mass is ignored.
The dynamic model of each vehicle element with the characteristics of all-wheel drived are built based on function modularization method. The main system modules include carbody module, tyre module, wheel hub motor module and a few correspondent modules shown in Fig 6. The required intermediate quantities such as slip angle, slip rate and wheel vertical forces are calculated according to the demand of each function modularization. The parameters used in the simulation such as the mass, rotational inertia and length of each body, tyre radius, friction coefficient, camber angle and other designation parameters are provided in designation.
As the steering angle of the left and right wheel is functioned by the same steering trapezium mechanism, there are nine independent degrees of freedom in total.
5.1. Articulated mechanism
As is shown in Fig 7, the articulated mechanism is constituted of kinematic pairs and force elements. The articulated mechanism should satisfy the needs of connection force and the freedom of motion between each vehicle element. Taking the spring-damper system as an example, the mechanical properties of the articulated mechanism are analyzed.
The function of spring-damper system here is used to decrease the impact of longitudinal impulse between each vehicle. The acting points of the force element system in the front and rear vehicle element are recorded as Si and Sj. As is shown in Eq (30), the distance vector between Si and Sj could be calculated by the position vector.
(30)
(31)
In the equation,
is the velocity vector and
is the first-order derivative of the attitude in the generalized coordinate. Thus, the acting force FS could be expressed as Eq (32).
(32)
K and C are the stiffness matrix and the damping matrix separately.
is the initial distance vector. The form of generalized force vector could be described as Eq (33).
5.2. Dynamics model of the unmanned articulated vehicle
The vehicle should consider the influence of the connection between each conjoint element. In the modeling, each vehicle element is regarded as a basic unit which is the same with the dynamic model above. Then the connection forces are considered. The modeling method of long-large train based on the loop variable developed by CHI [14] is used here. The basic idea of this method is to regard each vehicle element as central integral unit. The kinematic equation could be expressed as
(34)
In which, M, C and K represent the mass matrix, damping matrix and stiffness matrix of the vehicle element.
,
and X are the generalized acceleration vector, velocity vector and displacement vector. P is the generalized load vector.
The Eq (35) changes to (36) while considering the acting forces F between each vehicle element.
(35)
The Eq (35) is expanded for each basic integral unit, which express as follow.
(36)
In this way, the integral of the whole vehicle is divided into small integral units as Fig 8 shows. While giving the initial value and acting force of each vehicle element, the dynamic models of the whole vehicle could be built.
6. Verification of the trajectory tracking strategy
The coordinate trajectory tracking strategy of the vehicle based on the circulation of feasible path planning was verified by the constructed simulation platform. The simulation platform was built based on the dynamics model and trajectory calculation model. The values of control variables were calculated according to the re-planned path and then transferred to the dynamics model. The actual motion trajectory and the kinetic parameters were calculated. The operating velocity was controlled by the first trace point which is preset at a medium constant value.
The trajectory of lemniscate was preset as the virtual track. This kind of track was always used in vehicle handling and stability testing. It was suitable to verify the trajectory tracking ability of the vehicle considering the executability and stability. The equation of the preset track was shown in Eq (37).
(37)
The tracking effect of the control method proposed in this paper was compared with the traditional PID control method. As was shown in Fig 9, the solid black line in the figure was the preset lemniscate trajectory. The red solid line was the actual trajectory of the tracking method proposed in this paper. The red dotted line was the actual trajectory of PID method. Results showed that, the overall tracking error of PID control was slightly better than the tracking method proposed in this paper. But the overshoot phenomenon appears a few times, which directly reflected in the decrease of vehicle stability, especially for multiple articulated vehicles. The tracking error of the algorithm proposed in this paper was larger than that of PID control as a result of considering the kinematic coordination of multiple carriages. The tracking accuracy was reduced slightly as the cost of improving the overall stability of the vehicle.
The controller had detected sixteen times of the condition that the position or attitude error exceeded the preset threshold value. Under the circumstances, the controller conducted the command of feasible path re-planning to adjust the position and attitude to follow the guidance in front of the virtual track. The speed of the first trace point was constant at 6 m/s. The actual speed of each wheel controlled by hub motors was exported as Fig 9. The fluctuation of the velocity curves was agreed with the time of path re-planning which mean that the hub motors execute as control instructions. The vehicle moved at the right side of the lemniscate first. The processes of path re-planning for trajectory tracking were increased obviously. As a result, the velocity curves and the actual trajectory of the vehicle were unsmooth than that of the left side. The whole variant trend of the velocity curves of each vehicle was nearly the same. However, there was a difference of time phase to guarantee each vehicle element to path through the planned feasible curve in turns.
The dynamics performances in the tracking process were plotted as Fig 10. The yaw rate, side slip angle and lateral acceleration were selected as quantified indicators. The three dynamics indicators were all fluctuated as the process of path re-planning. It was preferable that the dynamics indicators were in the normal range in the trajectory tracking process. Simulated results indicated that the proposed method was of good trajectory tracking control ability.
7. Conclusions
A new kind of trajectory tracking method based on the circulation of feasible path planning for the unmanned vehicles were proposed in this article. As the core technology of the unmanned vehicle, the feasible path planning method based on the kinematics model were analyzed. The expected position determination method of the unmanned vehicles was proposed first to locate the vehicle. Then, the boundary constraint conditions were analyzed, and curve generation method was proposed to generate feasible path of the whole vehicle. Finally, the trajectory tracking based on the circulation of feasible path planning was proposed. The Circulation condition and terminal boundary of the circulation were analyzed. The dynamics model of the unmanned vehicle was built to reflect its real service status and verify the trajectory tracking strategy. The results showed that the coordinate traction control strategy of the vehicle based on the circulation of feasible path planning was fairly good in the preset lemniscate track.
Acknowledgments
We thank Prof. Weihua Zhang, Bingyu Zhang, Yifei Zhao for their help in field measurements and chemical analyses.
References
- 1.
Aguilar W G, Sandoval S, Limaico A, et al. Path Planning Based Navigation Using LIDAR for an Ackerman Unmanned Ground Vehicle[J]. Springer, Cham, 2019. DOI: https://doi.org/10.1007/978-3-030-27541-9_33.
- 2. Dong Y, Zhang Y, Ai J. Experimental Test of Unmanned Ground Vehicle Delivering Goods Using RRT Path Planning Algorithm[J]. Unmanned Systems, 2017, 05(1):45–57.
- 3.
Gholami F, Monfared P, Mousavi A. Experimental Path Planning of a Three-Wheeled Mobile Robot using Particle Swarm Optimization[C]// 2017 5th RSI International Conference on Robotics and Mechatronics (ICRoM). 2017.
- 4. Xia Y, ZHU Z Q, Fu M. Back-stepping sliding mode control for missile systems based on an extended state observer[J]. Control Theory & Applications, IET, 2011, 5(1):93–102. DOI: https://doi.org/10.1049/iet-cta.2009.0341.
- 5. Tang S H, Yeong C F, Su E, et al. Range and bearing-based simultaneous localization and mapping of unmanned ground vehicle using unscented Kalman filter. 2016.
- 6. Oreh S H T, Kazemi R, Azadi S. A new desired articulation angle for directional control of articulated vehicles[J]. Proceedings of the Institution of Mechanical Engineers, Part K: Journal of multi-body dynamics, 2012.
- 7.
Zoto J, Musci M A, Khaliq A, et al. Automatic Path Planning for Unmanned Ground Vehicle Using UAV Imagery[M]. 2020.
- 8. Lee SH, Park TW, Moon KH, et al. The articulated vehicle dynamic analysis using the AWS (All Wheel Steering) ECU (Electronic Control Unit) test. J MechSci Technol 2009; 23(4): 923–926.
- 9. Li B, Shao Z. Precise trajectory optimization for articulated wheeled vehicles in cluttered environments[J]. Advances in Engineering Software, 2016, 92: 40–47. Doi: https://doi.org/10.1016/j.advengsoft.2015.10.008.
- 10. Ghilardelli F, Lini G, Piazzi A. Path Generation Using-Splines for a Truck and Trailer Vehicle[J]. IEEE Transactions on Automation Science and Engineering, 2014, 11(1): 187–203.
- 11. Hu Y, Li D, He Y, et al. Path Planning of UGV Based on Bézier Curves[J]. Robotica, 2019, 37(6):1–29.
- 12. Gianni M, Ferri F, Menna M, et al. Adaptive Robust Three-dimensional Trajectory Tracking for Actively Articulated Tracked Vehicles[J]. Journal of Field Robotics, 2015.
- 13. Wang T, Huang P, Dong G. Modeling and Path Planning for Persistent Surveillance by Unmanned Ground Vehicle[J]. IEEE Transactions on Automation Science and Engineering, 2020, PP (99):1–11.
- 14. Mao-ru C H I, Yi-ping J, Wei-hua Z, et al. System dynamics of long and heavy haul train[J]. Journal of Traffic and Transportation Engineering, 2011, 11(3): 35–40.