Trajectory planning and tracking control of unmanned ground vehicle leading by motion virtual leader on expressway

Formation driving can effectively solve the problem of driving efﬁciency and safety of unmanned ground vehicles (UGVs) on expressways. This study introduces a trajectory planning and tracking control method from initial position to its motion virtual leader in formation driving. In order to reduce the disturbance to the surrounding UGVs, trajectory planning is divided into two stages: steady speed tracking stage and parameter value matching stage. A tracking controller combined with a trajectory planner is designed to track the reference trajectory accurately. To ensure calculation speed and accuracy, a nonlinear model-predictive control model is used in the trajectory planner, and the tracking controller is based on the linear time-varying model-predictive control (LTV MPC) model. Simu-lations are carried out with Carsim and MATLAB/Simulink, and the simulation results and comparative analysis show that the planned trajectory is feasible and can effectively avoid static and moving obstacles. Compared with the Stanley controller, the LTV MPC controller has the advantages of faster convergence speed, better stability and accurately tracking.


Problem and motivation
In order to meet the needs of intelligent travel, the driving efficiency and safety of unmanned ground vehicles (UGVs) have increasingly become the focus and difficulty of academic research, and formation driving provides an effective solution for this problem [1]. Formation driving is that several UGVs form and maintain a certain formation, among which trajectory planning and tracking control are extremely important, which will affect formation driving efficiency and safety [2]. Therefore, it is of great significance to study the trajectory planning and tracking control from its initial position to the motion virtual leader.

Related works
The concept of formation driving is inspired by the platoon motion behaviour of social animals such as birds, which results in a motion mechanism that helps them effectively avoid preda-This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited. © 2020 The Authors. IET Intelligent Transport Systems published by John Wiley & Sons Ltd on behalf of The Institution of Engineering and Technology tors, quicken their search for food and improve their overall migration efficiency [3]. The behaviour of animal individual movement is simple, including acceleration, deceleration, change direction and so on. Individual behaviour choice is to determine their own behaviour rules by obtaining the distance of the surrounding individuals and tends to be consistent with their own neighbour behaviour. C. Reynolds first proposed the cluster motion control model and discussed how to imitate the behaviour of birds and fish groups in computer animation [4]. The idea of applying the platoon movement pattern of animals to vehicle formation control began in the early 1980s. At present, the research hotspot is focused on the swarm formation system, which is suitable for modelling and control of multi-robot group. Formation control methods for robot systems include virtual leader method [5], virtual structure method [6], artificial potential energy field method [7] and behaviourbased method [8].
In terms of trajectory planning, the driving process of UGV must meet the requirements of smoothness, comfort and collision avoidance [9]. The trajectory planning algorithms can be grouped into map-based construction methods, traditional planning methods and intelligent planning methods. The map-based construction method mainly describes the state of the object through the shape and position of the object in the environment. The map-based construction methods can be roughly divided into visibility graph [10], Voronoi diagram [11], cell decomposition [12] and grid method [13]. The map-based construction method can display the driving environment visually, easy to carry on the follow-up planning, and can complete the obstacle avoidance, but the terminal point is usually static. The traditional planning methods include but not limited to forward graph search algorithm [14], artificial potential field method and rapidly exploring random trees [15]. These kinds of methods can be suitable for trajectory planning with high accuracy; however, they cannot be applied to systems with high realtime requirements. Intelligent planning methods include genetic algorithm [16], neural network algorithm and ant colony algorithm [17]. These kinds of methods can obtain the best trajectory in a specific environment or under specific conditions, but it has some problems, such as complex algorithm, lack of previous knowledge and easily falling into local optimization.
Tracking control requires UGVs to reach a given or planned trajectory point at a specified time, which is realised by automatic steering system or throttle, pedal and other control units. The tracking control of UGVs is mainly divided into geometrybased methods and model-based methods: pure pursuit controller, Stanley controller and feedforward-feedback tracking controller are commonly used in geometry-based methods, while the linear-quadratic regulator (LQR) and modelpredictive control (MPC) are commonly used in model-based methods. The pure pursuit controller [18] is essentially a proportional controller that converts the lateral offset between its position and the desired position at the preview direction into a lateral control quantity. The method has a good robustness and tracks effectively when the lateral offset distance is large and the curvature of the reference path is not continuous; however, the preview distance is easily affected by the parameters such as reference path curvature, lateral offset distance and speed. Stanley is the name of the vehicle of Stanford University that took part in the DARPA Robotics Challenge in 2005 and won the championship, and it is an excellent lateral tracking method under off-road conditions. Its tracking controller is called Stanley controller [19]. The Stanley controller is a nonlinear feedback controller, which takes into account the front wheel angle and lateral offset distance. In most cases, the performance of the Stanley controller is better than that of the pure pursuit controller, but the tracking effect is poor in large lateral error and discrete curvature path. The feedforward-feedback tracking controller has the advantages of integrated feedback control and feedforward control, which makes up for their shortcomings, and the compound control system has been widely used. The feedforward controller mainly compensates the disturbance of the curvature change of the reference path, and the characteristic of the system remains unchanged. The feedback control minimises the influence of disturbance and model error on tracking by adjusting the control input according to the vehicle state feedback. The complete compensation condition in the feedforward control does not change, which makes the vehicle closer to the path and remains stable [20]. However, when the disturbance vari-ables change sharply, the control effect will deteriorate. Both LQR and MPC methods are based on the optimization principle to establish models. LQR is a linear system in the form of state space in modern control theory. The task of LQR is to keep the component of the system state close to the equilibrium state without consuming too much energy when the system state deviates from the balance state for any reason, which is more suitable for expressway track trajectory [21]. This method cannot deal with the constraints, and the nonlinear system is linearised and approximated. It also does not consider the influence of the reference trajectory shape on the control system, sensitive to the curvature mutation region, and requires high accuracy of the control model.
The MPC algorithm is a kind of computer optimal control algorithm, which first appeared in the industrial fields of the United States and France in the late 1970s. Its basic characteristics are based on the control principle of predictive model, rolling optimization and feedback correction. The most remarkable advantage is that it can add a variety of constraints in the control process. The MPC method can explicitly take into account the kinematic and dynamic constraints of the vehicle by optimizing the objective function, which can effectively reduce or even eliminate the influence of the time delay of the closedloop system, and can optimise the motion control and improve the control performance combined with the trajectory information given by the planner [22][23][24].

Contribution
Based on above consideration, the main contribution of this paper is a proposal for a new method of trajectory planning and tracking control, which can improve driving efficiency and safety. We first propose the trajectory planning method between the UGV and its virtual leader. The trajectory planning is then divided into two stages: the steady speed tracking stage and the parameter value matching stage. Combining obstacles information, real-time trajectory planning can be achieved to avoid collisions. Then, in order to improve the stability in the tracking process of UGV, a tracking controller with a trajectory planner is designed. To ensure calculation speed and accuracy, the controller model and the UGV dynamic model are simplified. Finally, simulation experiments are carried out under different conditions to demonstrate validity and superiority of the proposed trajectory planning and tracking control method.

Structure
The rest of this paper is organised as follows. The UGV dynamic modelling and simplification is introduced in Section 2. The reference trajectory from the UGV to its motion target virtual leader is presented in Section 3. Section 4 introduces a tracking controller with a trajectory planner, and both of their algorithms used the MPC method, followed by simulation in Section 5 and conclusion in Section 6.

UGV DYNAMIC MODELLING AND SIMPLIFICATION
Vehicle dynamics play a significant role on the formation of a vehicle platoon, especially at high-speed condition [25]. In order to describe the UGV motion as accurately as possible, it is necessary to establish complex differential equations and use multiple state variables to describe its motion. According to [26], the lower precision model is used in offline trajectory planning, and the higher precision model is used in the control process, taking the control performance and calculation speed into account, so the point mass model that ignores the body size information is used in trajectory planning, and the simplified dynamic model is used in tracking control.

UGV dynamic model
The UGV dynamic model is mainly used in the prediction model; therefore, it can reduce the amount of computation. The front wheel angle of the UGV on expressway is small, and the nonlinear model of UGV dynamics combined with a linear tyre model is shown in Figure 1 [27]. In Figure 1, F l f and F lr are the longitudinal forces on the front and rear tyres, respectively. F c f and F cr are the lateral forces on the front and rear tyres, respectively. F x f and F xr are the x-direction forces on the front and rear tyres, respectively. F y f and F yr are the y-direction forces on the front and rear tyres, respectively where a and b are the distances from the centre of mass to the front and rear axes, respectively, m is the mass of UGV, C cf and C cr are the side stiffness of front and rear wheels, respectively, C lf and C lr are the longitudinal stiffness of front and rear wheels, respectively, s f and s r are the front and rear wheel slip rates, respectively, I z is the moment of inertia of the UGV around the Z-axis, f is the front wheel angle, is the heading angle, and superscripts f and r represent the leader UGV and the ego UGV, respectively. (X, Y )is the coordinates of UGVs fixed on the ground and (x, y) are the coordinates of UGVs fixed on its body.

The point mass model
In the process of trajectory planning, to ensure the calculation speed and reduce the calculation workload, the simplified dynamic model is used. The point mass model ignores the body size information The constraint is | y | < g.

REFERENCE TRAJECTORY FROM THE UGV TO ITS MOTION VIRTUAL LEADER IN FORMATION DRIVING
Formation driving is the process of UGVs' driving from disorderly to orderly. In the flocking process, UGVs track their respective virtual leaders and realise formation driving. In order to ensure that the mutual interference within the group is minimised, the trajectory planning is divided into two stages: the steady speed tracking stage and the parameter value matching stage.

Formation driving
In the flocking process of formation driving, the total potential energy in the team gradually reaches the minimum, and the formation of the team tends to be stable. The real-time position of the virtual leader, described as a dotted line shown in Figure 2, is determined by the position of the UGV leader. The virtual leader state parameter ξ is composed of position coordinates (x,y), velocity v and heading angle θ. In other words, formation driving is the process of matching the state parameter values of the UGV with that of its virtual leader. The UGV changes its motion parameter values by adjusting own components (such as throttle, brake, steering mechanism etc.), so the time of a component intervention and the number of components involved will produce a variety of state parameter value matching results.

Reference trajectory in the steady speed tracking stage
The process of UGV tracking is shown in Figure 3. First, the UGV is located at its staring point, and the dotted rectangle represents the location of the virtual leader at the same time. Then, the UGV moves to the meeting point (represented as solid rectangle) along the tracking trajectory, which is shown as a red curve in Figure 3.
After the UGV selects its motion virtual leader, it will begin to plan the trajectory from the current position to its motion virtual leader. In the steady speed tracking stage, the UGV keeps the speed unchanged and tracks to the virtual leader by controlling the front wheel angle.
In the process of tracking, the UGV follows the straight line with the virtual leader in real time. The differential equation model of reference trajectory is established as follows: where T

Avoidance of obstacles in the environment
In an expressway environment, obstacles include static obstacles (such as scattered objects and static vehicles) and moving obstacles (such as moving objects and running vehicles). The position, size and motion parameters of obstacles (such as speed and heading angle) are obtained by the UGV sensors (such as radar and camera) and C-V2X communication.
When the minimum distance between obstacles (including static and moving obstacles) and the UGV is less than a given threshold S a , the obstacle avoidance function is established where N obs is the number of obstacles obtained by sensors or communications in real time, S (i, j ) is the distance between the UGV i and the surrounding obstacle j, S a is the absolute safe distance for UGVs and k obs is normal numbers.

Threshold value S a
The absolute safe distance is expressed as the distance between the rear UGV and the front UGV when the front UGV stops suddenly, i.e. the instantaneous speed is zero, and the rear UGV is braking at a fixed deceleration to ensure that the rear UGV does not collide with it. When the speed of the front UGV is larger than the speed of the rear UGV, there is no collision between the two UGVs, and there is no absolute safe distance. Therefore, when the front UGV's speed is less than the rear UGV's speed, the model of s a is as follows: where s a is the absolute safe distance and s brake,k is the braking distance of UGV k .

Analysis of the braking process and the theoretical formula of vehicle braking distance
UGV emergency braking in the dangerous braking process can be divided into system reaction stage, brake reaction stage, braking force growth stage and braking transmission maintenance stage [28]. For the convenience of calculation, it can be considered that in system reaction stage t r1 and brake reaction stage t r2 , the UGV runs at a steady speed, and in the braking growth stage t i , braking deceleration increases linear, and the following time t c , the UGV brakes at a fixed deceleration, as shown in Figure 5.
Considering the heading angle, the theoretical formula of the UGV braking distance is where v is the speed of UGV k , t r is the reaction time, t i is the braking duration, is the heading angle and u is the friction coefficient.

Reaching the target point smoothly
To reach the meeting point, the UGV trajectory planning process should be flexible enough. The trajectory planner can plan a new trajectory when encountering obstacle especially in a multiobstacle situation. To ensure to reach the meeting point and avoid the phenomenon of rotation after encountering multiple obstacles, an additional penalty term is added to the objective function to guide the UGV to the terminal of the target where is a normal number, N is the predictive time and S (i, d ) is the distance between the UGV and the terminal point T.

Parameter value matching between the UGV and its motion virtual leader
Tracking the motion virtual leader is required to be relatively stable. The parameter value matching is accomplished by the UGV controlling the throttle and pedal and gradually reaches the same as the motion parameter values of the virtual leader. The matching process is represented as follows: where is the state parameter, P is the position, is the velocity, is the heading angle and the superscript VL represents the virtual leader.
In the process of adjusting the motion parameter values of the UGV, a variety of matching methods are produced according to the conditions of velocity (deceleration-acceleration or acceleration-deceleration) and acceleration (uniform or variable) where T 1 is the time of steady speed tracking stage time, T 2 is the time of parameter value matching stage, v UGV and v VL are speeds of the UGV and virtual leader, respectively, and l is the precision constant.

LTV MPC TRACKING CONTROLLER WITH NONLINEAR MODEL-PREDICTIVE CONTROL (NMPC) TRAJECTORY PLANNER
The tracking controller with a trajectory planner can achieve real-time collision avoidance during driving. For the trajectory planner, the UGV uses the point mass model, and the control algorithm uses the NMPC. For the tracking controller, the UGV uses the dynamic model, and the control algorithm uses a linear time-varying model-predictive control (LTV MPC).

NMPC trajectory planner
The goal of trajectory planning is to reduce the offset distance from the reference trajectory as much as possible, avoid obstacles and reach the terminal point smoothly. The specific formu-lation of the trajectory planner is as follows: where H P is the predictive time domain, ref (t + i|t ), i = 1, … , H P, is the reference output variable, Q and R are weight matrices and 1 and 2 are weight factors.
Since the real-time requirement of trajectory planning is lower than that of tracking control and the point mass model is simplified to a great extent compared with the nonlinear dynamic model, the NMPC algorithm with higher solving accuracy can fully meet the requirements of trajectory planning. Therefore, the trajectory planner formulation is not commonly solved linearization but is directly based on the nonlinear model.
The inputs of MPC trajectory planning are (Ẏ ,Ẋ ,̇, , Y, X ), which represent the velocity of the UGV in the direction of Y and X coordinates, the heading angular velocity and the heading angle, and the Y and X coordinates, respectively. The output is the parameter of the expected trajectory equation.

LTV MPC tracking controller
The controller with LTV MPC [29] is used in the tracking control as it has the advantage of less calculation and easier solution compared to the NMPC.

Controller with NMPC
To accurately track the reference trajectory, the controller with NMPC is designed where H P and H C are predictive time domain and control time domain, respectively. ref (t + i|t ), i = 1, … , H P are reference variables, Q, R and are weight matrices and is the relaxation factor. The first item is used to correct the offset distance between the output and the reference output in the predictive time domain, and it reflects the fast-tracking ability of the system to the reference trajectory. The second item is used to penalise the control increment of the system in the control time domain, and it reflects the requirements of the system for the steady change of the control quantity. The relaxation factor is added to ensure that the suboptimal solution is obtained instead of the optimal solution in the case of no optimal solution in the control period.
In the steady speed tracking stage, the state variables are (Ẏ ,Ẋ ,̇, , Y, X ), and the control input variable is the UGV front wheel angle . In the parameter value matching stage, the state variables are (Ẏ ,Ẋ ,̇, , Y, X ), and the control input variable is the UGV front wheel angle and speed v.

Transformation and solution of the LTV MPC problem
The nonlinear dynamics model and nonlinear constraints (vehicle physical constraint and passenger feeling etc.) will greatly increase the difficulty of solving the MPC problem. By transforming the nonlinear system into a linear time-varying system, the calculation workload is significantly reduced, and the realtime online solution is accomplished.
Step 1. Consider the following nonlinear dynamic systems: where f (⋅, ⋅) is the state transition function of the system, (t ) ∈ R n is an n-dimensional state variable, (t ) ∈ R m is an mdimensional control variable, and (t ) ∈ R p is a p-dimensional output variables.
Step 3. Transformation solution of the QP problem Control increment Δ (t ) in the system control time domain H C is unknown. By optimizing the objective function reasonably, the optimal control sequence can be obtained, which satisfies the constraint condition in the control time domain H C . In order to ensure that there is no optimal solution in the control period, the system replaces the optimal solution with the suboptimal solution and prevents the occurrence of no feasible solution, so the relaxation factor is added to the objective function.

Calculation flow of the LTV MPC controller
The calculation flow of LTV MPC consists of two parts: trajectory planning with collision avoidance and tracking control, as shown in Figure 6. The obtained reference trajectory parameter values and UGV state parameter values are used to solve objective function optimization and output the control parameter values. Implementation steps are as follows.
(i) By receiving the obstacle information from the sensor/C-V2X and the trajectory planning parameter values determined by the differential equation model of reference trajectory, combined with the motion parameters and constraint of the UGV itself, the trajectory planning with collision avoidance function is realised. (ii) Combined with the state variables of UGV and reference trajectory parameters, through prediction model, rolling optimization and feedback correction, the tracking controller is implemented, and the control variables are dynamically outputted to realise UGV driving along the optimal trajectory.

SIMULATION EXPERIMENT
A joint simulation platform based on vehicle dynamics simulation software Carsim and control simulation software MAT-LAB/Simulink is established. The platform is helpful to verify the performance of the trajectory planning and control algorithm through the output and performance of the system.

Parameter declaration
The parameters of the UGV are mainly set in Carsim, and the vehicle type is "C-Class, Hatchback". The control parameters are shown in Table 1. The tyre of the vehicle is 205/55R16, and

Test simulation
The MPC controller running on the expressway should consider not only the tracking accuracy, but also the stability in the tracking process. The test simulation of the trajectory tracking ability of the UGV is carried out by using the double-lane change trajectory in [30]. The trajectory of double-lane change is shown as follows, which is composed of reference horizontal position, as shown in Figure 7, and reference heading angle, as shown in Figure 8. Figure 9 shows track trajectory at different speeds and Figure 10 shows the comparison of the offset distance between the simulation trajectory and the reference trajectory. It can be seen  that under the same control parameters and expressway conditions, UGVs with different speeds have good trajectory tracking ability, reflecting strong robustness to speed. The designed controller can track the reference trajectory well. Figure 11 shows the heading angle at different speeds and Figure 12 shows the comparison of angle difference between the simulation heading angle and the reference angle. It can be seen from Figures 11 and 12 that under the same control parameters, UGVs with different speeds have good heading angle tracking ability, reflecting strong robustness to speed.

FIGURE 11
Heading angle at different speeds  The two controllers are also used to track the reference trajectory of the example in Section 3.2.
Comparisons among the LTV MPC controller, the Stanley controller and the reference trajectory are shown in Figure 15. Since the initial front wheel angle of the UGV is 0 and the angle is limited, the initial offset distance of the two, compared with reference trajectory, is large, and the trajectory with the controller gradually converge to the reference trajectory in the later.
Furthermore, the comparison of the offset distance between the LTV MPC controller, the Stanley controller, and the reference trajectory in Figure 16 shows that the LTV MPC controller is superior to the Stanley controller on expressway. In the early stage of the simulation, due to the initial value and limitation of UGV front wheel angle, there is a large offset distance with the reference trajectory. In the middle and later stages of the simulation, the offset distance gets smaller. The convergence rate of LTV MPC controller is better than that of the Stanley controller. The offset distance determined by the Stanley controller decreases gradually with the decrease of reference trajectory curvature. This is an indication that the Stanley controller is greatly affected by the curvature of the reference trajectory, i.e. the Stanley controller has a larger offset distance on the curve trajectory than the LTV MPC controller. The LTV MPC controller can converge quickly on the curve trajectory, and the

Analysis of static and moving obstacle avoidance
The process of reaching the motion virtual leader after UGV avoiding static and moving obstacles is shown in Figures 17  and 18 The state variables(Ẏ ,Ẋ ,̇, , Y, X ) and the control input variable change as follows.
Change of velocity along the y-axis and x-axis in static obstacle situation and moving obstacle situation is shown in Figures 19 and 20. Change of the heading angular velocity in static obstacle situation and moving obstacle situation is shown in Figure 21. Change of the heading angle in static obstacle situation and moving obstacle situation is shown in Figure 22. The moving obstacle reaches (70 m, 3.0 m) at 1.625 s, when the UGV avoids the moving obstacle. Collision avoidance trajectory of the static and moving obstacle is shown in Figures 23 and 24, and change of front wheel angle in static obstacle and motion situation is shown in Figure 25. The collision of the obstacles should meet the requirements of not being too close to the obstacles and causing collisions, but also not too far away from     Collision avoidance trajectory of the moving obstacle the obstacle, leading too longer trajectory and even affecting the surrounding traffic. From the simulation results, it can be concluded that no matter the static obstacle or the moving obstacle, the UGV has achieved better collision avoidance. The obstacle avoidance function takes effect at x = 52 m in static obstacle situation. During collision avoidance, the UGV front wheel angle has a large deflection, and the peak angle reaches 3 • . The minimum distance between the UGV and the static obstacle is about 30 cm. The influence area of the moving obstacle is larger, and the obstacle avoidance function works at x = 30 m. During collision avoidance, the front wheel angle deflects slightly, and the  In reality, the greater the speed, the greater the severity of collision with obstacles, which then provides a greater safety distance to ensure no collision. During collision avoidance, as the speed increases, the minimum distance increases to ensure safety.

Parameter value matching between the UGV and its motion virtual leader
In the process of adjusting the motion state of the UGV, different velocity and acceleration parameter values will produce a variety of matching methods. The shortest time is taken as the objective function; in order to ensure the comfort of the passengers, in the acceleration |a| ≤ 4, under the constraint condition, the simulation of parameter value matching stage is Trajectory planning for the parameter value matching stage is shown in Figures 28 and 29. Figure 28 shows the trajectory simulation on the X-axis when the minimum time is used as the objective function, and the UGV completes the parameter matching with the maximum acceleration/deceleration speed. Figure 29 shows the trajectory simulation on the Y-axis when the minimum time is used as the objective function. Because the virtual leader is assumed to be a steady speed straight line, the UGV is driven in straight line.
The velocity change of the UGV and its motion virtual leader in the parameter value matching stage is shown in Figure 30. When the minimum time is used as the objective function, the UGV slows down through 4.2 S and accelerates 1.8 S to reach the same speed as the virtual leader, which result in the same coordinate position for the UGV and its motion virtual leader.

CONCLUSIONS
This paper studies the trajectory planning and tracking control of UGVs leading by a virtual leader. The conclusions are as follows.

FIGURE 30
Velocity change in parameter value matching stage (i) A method of trajectory planning of UGV from its starting point to its motion virtual leader is proposed, and the trajectory planning process is divided into two stages: the steady speed tracking stage and parameter value matching stage. The UGVs in the group can quickly reach the position of their respective virtual leaders by tracking the reference trajectory, thereby achieving the goal of formation driving. (ii) The planner can provide trajectory re-planning for the controller in real time, which perfectly avoids the collisions between UGV and static and moving obstacles. The trajectory planner simplifies UGV as a point mass model and NMPC is used as the control algorithm. And for the trajectory controller, LTV MPC is used to realise trajectory tracking based on a dynamic vehicle model. A joint simulation platform based on Carsim and MATLAB/Simulink is established, which can realise online real-time simulation of the LTV MPC controller with the NMPC planner and meet the requirements of real-time and accuracy calculation. (iii) The simulation results show that the LTV MPC controller can effectively avoid the obstacles and meet the security requirements. The UGV tracks the reference trajectory well, with small lateral offset distance and fast convergence, meeting the safety requirements.