Trajectory tracking method based on the circulation of feasible path planning

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.


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][2][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]  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, slidingmode 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][8][9][10][11][12][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.

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.
1. It is fixed in the carbody and moved with each element; 2. 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 O f1 , O r1 , O r2 and O r3 . The absolute coordinates are described as ðx 0 f 1 ; y 0 f 1 Þ and ðx 0 r1 ; y 0 r1 Þ. The attitude of each vehicle element and the position of each point in the vehicle could be described as follow.

Kinematic model of the vehicle
As is shown in Fig 2,    In which, r i J and r j J represent the position vector in the hinge point of the first and second vehicle element. Substituted the transformation matrix of the ground fixed coordinate and carbody following coordinate into Eq (3), we can get Eq (4).
In the formula, R i and R j represent the position vector of the original point of carbody following coordinates. The Jacob matrix of the constraint equation is expressed as follow.
As the pre-designation of the vehicle, all the vehicle elements are all-wheel drived.
Assumed that the velocity of the trace point is the control objective, the velocity of each wheel could be calculated according to the kinematics model of one element of the vehicle. The velocity of the hinge joint could be calculated based on the first vehicle element. The velocity in point J 1 is expressed as follow.
ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffiffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi In this way, the velocity of the wheels in the follow-up elements could be calculated.

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 ðx O bm ; y O bm Þ, the instantaneous turn center ðx O i ; y O i Þ, and the articulated mechanism ðx J i ; y J i Þ. The attitudes, yaw velocity of each vehicle element, the steering angle and velocity of each wheel which recorded separately as φ i , ω i , δ ijk and v ijk 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.
In the equation, g(x) is the planned path, and v ! O bm 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.

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 t b and t e . C b and C e represent the physical quantities of the original and terminal motion state which could be assigned as Eqs (9) and (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).

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).
Thus, the first and second order derivatives are expressed as Eqs (13) and (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.
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, 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).
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.

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.

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 .
The point with the minimum distance from the first trace point in the first vehicle element to the virtual track in the virtual track is regarded as the expected position of the first trace point. The expected positions and attitudes of other vehicle elements could also be calculated based on the method described above. The expected position of trace points is expressed as Eq (24). and the expected attitude of each vehicle element is labeled as φ 0 i .
The total tracing distance error and attitude error of the vehicle could be expressed as the sum of the distance error of all trace points as Eqs (25) and (26).
The upper limits of the distance error and attitude error are given as the threshold value for the circulation, which is labeled as d max and Δφ max . The decision condition of the circulation could be expressed as Eq (27).

Terminal boundary
At present, the preview distance is calculated by the product of vehicle speed and preview time.
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 k t is defined to evaluate the quality.
The bigger the index k t is, the better quality the planned path possesses. In the equation, r min and r 0 max are the limit radius and change rate restrained by the characteristic of the vehicle.

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.
1. Only the planar motion paralleled of the pavement is considered; 2. The axle load transfer caused by the horizontal and longitudinal acceleration is considered; 3. 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.

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 S i and S j . As is shown in Eq (30), the distance vector r S i S j between S i and S j could be calculated by the position vector.
In the equation, r _ S i S j is the velocity vector and θ _ j is the first-order derivative of the attitude in the generalized coordinate. Thus, the acting force F S could be expressed as Eq (32).
K and C are the stiffness matrix and the damping matrix separately. r 0 S i S j is the initial distance vector. The form of generalized force vector could be described as Eq (33).

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 In which, M, C and K represent the mass matrix, damping matrix and stiffness matrix of the vehicle element. X __ , X _ 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.

M X
The Eq (35) is expanded for each basic integral unit, which express as follow.
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.

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).
l À 60 � ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi ffi cosð2cÞ p ð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.

PLOS ONE
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.

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.