Get 20M+ Full-Text Papers For Less Than $1.50/day. Start a 14-Day Trial for You or Your Team.

Learn More →

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

Trajectory planning and tracking control of unmanned ground vehicle leading by motion virtual... INTRODUCTIONProblem and motivationIn 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 worksThe 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 predators, 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 behaviour‐based 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 real‐time 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 geometry‐based 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 model‐predictive 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 variables 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 closed‐loop system, and can optimise the motion control and improve the control performance combined with the trajectory information given by the planner [22–24].ContributionBased 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.StructureThe 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 SIMPLIFICATIONVehicle 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 modelThe 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].1FIGUREVehicle monorail modelIn Figure 1, FlfandFlr are the longitudinal forces on the front and rear tyres, respectively. FcfandFcrarethe lateral forces on the front and rear tyres, respectively. FxfandFxr are the x‐direction forces on the front and rear tyres, respectively. FyfandFyr are the y‐direction forces on the front and rear tyres, respectively1mÿ=−mẋϕ̇+2Ccfδf−ẏ+aϕ̇ẋ+Ccrbϕ̇−ẏẋmẍ=mẏϕ̇+2C1fsf+Ccfδf−ẏ+aϕ̇ẋδf+C1rsrIzϕ̈=2aCcfδf−ẏ+aϕ̇ẋ−bCcrbϕ̇−ẏẋẎ=ẋsinϕ+ẏcosϕẊ=ẋcosϕ−ẏsinϕwhere aandb are the distances from the centre of mass to the front and rear axes, respectively, mis the mass of UGV, CcfandCcr are the side stiffness of front and rear wheels, respectively, ClfandClr are the longitudinal stiffness of front and rear wheels, respectively, sfandsr are the front and rear wheel slip rates, respectively,Iz is the moment of inertia of the UGV around the Z‐axis, δf is the front wheel angle, φ is the heading angle, and superscripts fandr 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 modelIn 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 information2ÿ=ayẍ=0ϕ̇=ayẋẎ=ẋsinϕ+ẏcosϕẊ=ẋcosϕ−ẏsinϕ.The constraint is |αy|<μg.REFERENCE TRAJECTORY FROM THE UGV TO ITS MOTION VIRTUAL LEADER IN FORMATION DRIVINGFormation 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 drivingIn 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.2FIGUREProcess of formation drivingReference trajectory in the steady speed tracking stageThe 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.3FIGURETracking schematicsAfter 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:3dxdT1=vX−x(X−x)2+(Y−y)2dydT1=vY−y(X−x)2+(Y−y)2where T1is time of the steady speed tracking stage, v is the speed of the UGV, (X, Y) are the coordinates of the motion virtual leader and (X, Y) are the coordinates of point on reference trajectory.Take the following working conditions as an example:The starting point of the UGV is (20 m, 1.5 m) and tracks the motion virtual leader at the speed of 30 m/s. The speed of the motion virtual leader is 20 m/s straight line, and the coordinates of the starting point of the virtual leader is (100 m,4.5 m). The UGV is traced in the direction of connection to the motion virtual leader. The process is shown in Figure 4. Figure 4(a) and (b) shows the curves of X and Y with time in the process of tracking. As shown in Figure 4(c), start point (1) is the initial point at which the path change begins for the UGV. Start point (2) is the starting point of the motion virtual leader at the corresponding time. Red “*” is the trajectory point. Blue “o” is the trajectory of the motion virtual leader. Considering the smoothness of the trajectory, the cubic spline interpolation of the trajectory points is carried out with blue “.” [as shown in Figure 4(d)].4FIGUREReference trajectoryAvoidance of obstacles in the environmentIn 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 Sa, the obstacle avoidance function is established4Jobs,j=∑j=1Nobskobsvi−vobs,jS2i,j,Si,j≺Sa0,Si,j≥Sawhere Nobs is the number of obstacles obtained by sensors or communications in real time, S(i,j) is the distance between the UGVi and the surrounding obstacle j, Sa is the absolute safe distance for UGVs and kobs is normal numbers.Threshold value SaThe 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 sa is as follows:5sa=sbrake,kwhere sa is the absolute safe distance andsbrake,k is the braking distance of UGVk.Analysis of the braking process and the theoretical formula of vehicle braking distanceUGV 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 tr1 and brake reaction stage tr2, the UGV runs at a steady speed, and in the braking growth stage ti, braking deceleration increases linear, and the following time tc, the UGV brakes at a fixed deceleration, as shown in Figure 5.5FIGUREUGV braking deceleration changeConsidering the heading angle, the theoretical formula of the UGV braking distance is6sbrake,k=vktr+0.5ti3.6sinφkcosφk+vk225.9ugsinφkcosφkwhere v is the speed of UGVk, tr is the reaction time, ti is the braking duration, φ is the heading angle and u is the friction coefficient.Reaching the target point smoothlyTo 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 multi‐obstacle 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 target7I=κ∑i=1NS2i,Twhere κ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 leaderTracking 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:8ξUGVpUGV,νUGV,φUGV→ξVLpVL,νVL,φVLwhere ξ 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)9∫T1T1+T2vUGVdt−∫T1T1+T2vVLdt≤lwhere T1 is the time of steady speed tracking stage time,T2 is the time of parameter value matching stage, vUGV and vVL 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 PLANNERThe 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 plannerThe 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 formulation of the trajectory planner is as follows:10minUt∑i=1HP∥ηt+it−ηreft+it∥Q2+∥μ∥R2+Jobs,jε1+Iε2where HP is the predictive time domain, ηref(t+i|t),i=1,…,HP, is the reference output variable, QandR are weight matrices and ε1andε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 controllerThe 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 NMPCTo accurately track the reference trajectory, the controller with NMPC is designed11fξt,μt−1,Δμt,ε=∑i=1HPηt+it−ηreft+itQ2+∑i=0HC−1Δμt+itR2+ρε2where HP and HC are predictive time domain and control time domain, respectively. ηref(t+i|t),i=1,…,HP are reference variables, Q,Randρ 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 problemThe 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 real‐time online solution is accomplished.Step 1. Consider the following nonlinear dynamic systems:12ξ̇t=fξt,μtηt=hξt,μtwheref(·,·) is the state transition function of the system, ξ(t)∈Rn is an n‐dimensional state variable, μ(t)∈Rm is an m‐dimensional control variable, and η(t)∈Rp is a p‐dimensional output variables.Step 2. Transform the nonlinear system into following discrete linear time‐varying system:13δξk+1=Ak,tξk+Bk,tμtδηt=Ck,tξk+Dk,tμtwhere Ak,t=∂f∂ξ|ξ0(k),μ0(k),Bk,t=∂f∂μ|ξ0(k),μ0(k),Ck,t=∂η∂ξ|ξ0(k),μ0(k)andDk,t=∂η∂μ|ξ0(k),μ0(k) are the Taylor expansion at the point (ξ0(k),μ0(k)).Step 3. Transformation solution of the QP problemControl increment Δμ(t) in the system control time domain HC 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 HC. 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 controllerThe 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.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.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.6FIGURECalculation flow of MPC controllerSIMULATION EXPERIMENTA joint simulation platform based on vehicle dynamics simulation software Carsim and control simulation software MATLAB/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 declarationThe 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 the front and rear suspensions are selected as independent suspensions. The predictive time domain is 15 s, the control time domain is 2 s and the size of the simulation step is 0.03 s.1TABLEVehicle control parametersParametervaluev‐vd/m/s[−2,2]Δv/m/s[−0.5,0.5]δ/°[−10,10]Δδ/°[−1,1]Test simulationThe 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.7FIGUREThe trajectory in double‐lane change condition8FIGUREThe heading angle in double‐lane change conditionFigure 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.9FIGURETrack trajectory at different speeds10FIGUREComparison of offset distance at different speedsFigure 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.11FIGUREHeading angle at different speeds12FIGUREComparison of angle difference at different speedsTrack trajectory comparison between the LTV MPC controller and the Stanley controllerThe LTV MPC controller and the Stanley controller are used to track the double‐lane change trajectory.Comparisons among the LTV MPC controller, the Stanley controller and the reference trajectory are shown in Figures 13 and 14. The trajectory determined by LTV MPC coincides well with the reference trajectory. The fitting degree between the Stanley method and the reference trajectory is slightly poor, and the maximum offset distance is 0.37 m. On one hand, the UGV centre mass deviates from the driving centre line during curve driving; on the other hand, it is related to the cross‐track error of the UGV centre mass from the reference trajectory.13FIGUREComparison of trajectory tracking control14FIGUREComparison of offset distance between the LTV MPC controller and the Stanley controllerThe 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.15FIGUREComparison of trajectory tracking controlFurthermore, 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 offset distance in the stable stage is less than 0.05 m, which is only 1/ 41 of the tyre width.16FIGUREComparison of offset distance between the LTV MPC controller and the Stanley controllerAnalysis of static and moving obstacle avoidanceThe process of reaching the motion virtual leader after UGV avoiding static and moving obstacles is shown in Figures 17 and 18, respectively. The speed of the UGV is 30 m/s. Static obstacle collision avoidance condition: the static obstacle is square L = 0.5 m, and the initial position of the lower left corner of the obstacle is (70 m, 3.0 m). Moving obstacle avoidance condition: square L = 0.5 m obstacle, the initial position of the lower left corner of the obstacle is (53.25 m, 3.0 m), at the speed of 10 m/s straight line.17FIGUREUGV collision avoidance of static obstacle18FIGUREUGV collision avoidance of moving obstacleThe 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 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 peak angle reaches 1.4°. The minimum distance between the UGV and the moving obstacle is about 20 cm.19FIGUREChange of velocity along the y‐axis in static obstacle situation and moving obstacle situation20FIGUREChange of velocity along the x‐axis in static obstacle situation and moving obstacle situation21FIGUREChange of the heading angular velocity in static obstacle situation and moving obstacle situation22FIGUREChange of the heading angle in static obstacle situation and moving obstacle situation23FIGURECollision avoidance trajectory of the static obstacle24FIGURECollision avoidance trajectory of the moving obstacle25FIGUREChange of the front wheel angle in static obstacle situation and moving obstacle situationThe collision avoidance trajectory at different speeds in the static and moving obstacle situations is shown in Figures 26 and 27. The solid line is the trajectory of upper UGV profile at different speeds, and the dotted line is the trajectory of the UGV mass centre at different speeds. At different speeds, the trajectory of the mass centre can reach the point M from the point S both in static and moving obstacle situations, so the UGV can complete the collision avoidance and reach the destination. 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.26FIGURECollision avoidance trajectory at different speeds in static obstacle situation27FIGURECollision avoidance trajectory at different speeds in the moving obstacle situationParameter value matching between the UGV and its motion virtual leaderIn 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 carried out. The initial point is (248 m,4.5 m) and the speed of the UGV is 30 m/s. It is assumed that the virtual leader runs in a straight line at the speed of 20 m/s, passing time T2, ξUGV(pUGV,νUGV,φUGV)→ξVL(pVL,νVL,φVL).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.28FIGURETrajectory planning of X‐axis29FIGURETrajectory planning of Y‐axisThe 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.30FIGUREVelocity change in parameter value matching stageCONCLUSIONSThis paper studies the trajectory planning and tracking control of UGVs leading by a virtual leader. The conclusions are as follows.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.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.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.ACKNOWLEDGEMENTThis work was supported in part by the National Natural Science Foundation of China under Grant 61573171 and Grant 51675235 and in part by Colleges and Universities in Jiangsu Province plans to graduate research and innovation under Grant CXLX13_675.REFERENCESGhommam, J., et al.: Formation path following control of unicycle‐type mobile robots. Robot. Auton. Syst. 58(5), 727–736 (2010)Guo, J.H., et al.: An adaptive cascade trajectory tracking control for over‐actuated autonomous electric vehicles with input saturation. Sci. China Technol. Sci. 62, 2153–2160 (2019)Morale, D.: Modeling and simulating animal grouping individual‐based models. Future Gener. Comput. Syst. 17, 883–891 (2001)Dierks, T., Jagannathan, S.: Control of nonholonomic mobile robot formations: Backstepping kinematics into dynamics. In Proc. IEEE Int. Conf. Control Appl., Singapore, pp. 94–99 Oct. (2007)Cao, J., et al.: Distributed event‐triggered consensus tracking of second‐order multi‐agent systems with a virtual leader. Chin. Phys. B 25(5), 491–496 (2016)Lalish, E., et al.: Formation tracking control using virtual structures and deconfliction. In Proc. IEEE int. Con. Decis. Control, San Diego, CA, USA, pp. 5699–5705 May (2006)Yamaguchi, H.: Adaptive formation control for distributed autonomous mobile robot group. In Proc. IEEE Int. Conf. Rob. Autom., Albuquerque, NM, USA, pp. 2300–2305 Apr. (1997)Lawton, J.R., Beard, R.W.: Synchronized multiple spacecraft rotations. Automatica 38(8), 1359–1364 (2002)Jiang, H.B., et al.: Trajectory planning and optimisation method for intelligent vehicle lane changing emergently. IET Intell. Transp. Syst. 12(10), 1336–1344 (2018)Oommen, B., et al.: Robot navigation in unknown terrains using learned visibility graphs. Part I: The disjoint convex obstacle case. IEEE J. Rob. Autom. 3(6), 672–681 (1987)Shao, M.L., et al.: Sensor‐based path planning: The two‐identical‐link hierarchical generalized Voronoi graph. Int. J. Precis. Eng. Manuf. 16(8), 1883–1887 (2015)Chen, D.Z., et al.: A framed‐quadtree approach for determining Euclidean shortest paths in a 2‐D environment. IEEE Trans. Rob. 13(5), 668–681 (1997)Park, J.Y., et al.: Task‐oriented design of robot kinematics using the grid method. Adv. Rob. 17(9), 879–907 (2003)Wang, Z., et al.: Path planning for first responders in the presence of moving obstacles with uncertain boundaries. IEEE Trans. Intell. Transp. 18(8), 2163–2173 (2017)Kothari, M., Postlethwaite, I.: A probabilistically robust path planning algorithm for UAVs using rapidly‐exploring random trees. J. Intell. Rob. Syst. 71(2), 231–253 (2013)Tuncer, A., Yildirim, M.: Dynamic path planning of mobile robots with improved genetic algorithm. Comput. Electr. Eng. 38(6), 1564–1572 (2012)Ersson, T., Hu, X.: Path planning and navigation of mobile robots in unknown environments. In Proc. IEEE Int. Con. Intell. Rob. Syst., Maui, HI, USA, pp. 858–864 Nov. (2001)Elbanhawi, M., et al.: Receding horizon lateral vehicle control for pure pursuit path tracking. J. Vib. Control 24(3), 619–642 (2018)Hoffmann, G.M., et al.: Autonomous automobile trajectory tracking for off‐road driving: Controller design, experimental validation and racing. In Proc. Int. Conf. Am. Control Conf., New York, USA, pp. 2296–2301 July (2007)Ziegler, J., et al.: Making bertha drive—An autonomous journey on a historic route. IEEE Intell. Transp. Syst. 6(2), 8–20 (2014)Levinson, J., et al.: Towards fully autonomous driving: Systems and algorithms. In Proc. IEEE Int. Conf. Intell. Veh. Symp. Baden‐Baden, Germany, pp. 163–168 Jun. (2011)Li, S., et al.: Model predictive multi‐objective vehicular adaptive cruise control. IEEE Trans. Control Syst. Technol. 19(3), 556–566 (2011)Falcone, P., et al.: Linear time‐varying model predictive control and its application to active steering systems: Stability analysis and experimental validation. Int. J. Robust Nonlinear 18(8), 862–875 (2010)Cai, J., et al.: Implementation and development of a trajectory tracking control system for intelligent vehicle. J. Intell. Rob. Syst. 94(1), 251–264 (2019)Xu, L.W., et al.: Distributed formation control of homogeneous vehicle platoon considering vehicle dynamics. Int. J. Autom. Technol. 20(6), 1103–1112 (2019)Falcone, P.: Nonlinear model predictive control for autonomous vehicles. Ph.D. dessertation, Sannio Univ., Benevento, Italy, (2007)Gong, J.W., et al.: Model Predictive Control for Self‐Driving Vehicles. Beijing Institute of Technology Press, Beijing, China (2014)Xu, C.C., et al.: Calibration of crash risk models on freeways with limited real‐time traffic data using Bayesian meta‐analysis and Bayesian inference approach. Accid. Anal. Prev. 85, 207–218 (2015)Kunhe, F., et al.: Model predictive control of a mobile robot using linearization. IEEE Mechatron. Rob. (4), 525–530 (2004)Falcone, P., et al.: MPC‐based yaw and lateral stabilisation via active front steering and braking. Veh. Syst. Dyn. 46, 611–628 (2008) http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png IET Intelligent Transport Systems Wiley

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

Download PDF
13 pages

Loading next page...
 
/lp/wiley/trajectory-planning-and-tracking-control-of-unmanned-ground-vehicle-UqorHQNx03

References (31)

Publisher
Wiley
Copyright
© 2021 The Institution of Engineering and Technology
eISSN
1751-9578
DOI
10.1049/itr2.12013
Publisher site
See Article on Publisher Site

Abstract

INTRODUCTIONProblem and motivationIn 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 worksThe 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 predators, 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 behaviour‐based 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 real‐time 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 geometry‐based 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 model‐predictive 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 variables 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 closed‐loop system, and can optimise the motion control and improve the control performance combined with the trajectory information given by the planner [22–24].ContributionBased 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.StructureThe 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 SIMPLIFICATIONVehicle 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 modelThe 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].1FIGUREVehicle monorail modelIn Figure 1, FlfandFlr are the longitudinal forces on the front and rear tyres, respectively. FcfandFcrarethe lateral forces on the front and rear tyres, respectively. FxfandFxr are the x‐direction forces on the front and rear tyres, respectively. FyfandFyr are the y‐direction forces on the front and rear tyres, respectively1mÿ=−mẋϕ̇+2Ccfδf−ẏ+aϕ̇ẋ+Ccrbϕ̇−ẏẋmẍ=mẏϕ̇+2C1fsf+Ccfδf−ẏ+aϕ̇ẋδf+C1rsrIzϕ̈=2aCcfδf−ẏ+aϕ̇ẋ−bCcrbϕ̇−ẏẋẎ=ẋsinϕ+ẏcosϕẊ=ẋcosϕ−ẏsinϕwhere aandb are the distances from the centre of mass to the front and rear axes, respectively, mis the mass of UGV, CcfandCcr are the side stiffness of front and rear wheels, respectively, ClfandClr are the longitudinal stiffness of front and rear wheels, respectively, sfandsr are the front and rear wheel slip rates, respectively,Iz is the moment of inertia of the UGV around the Z‐axis, δf is the front wheel angle, φ is the heading angle, and superscripts fandr 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 modelIn 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 information2ÿ=ayẍ=0ϕ̇=ayẋẎ=ẋsinϕ+ẏcosϕẊ=ẋcosϕ−ẏsinϕ.The constraint is |αy|<μg.REFERENCE TRAJECTORY FROM THE UGV TO ITS MOTION VIRTUAL LEADER IN FORMATION DRIVINGFormation 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 drivingIn 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.2FIGUREProcess of formation drivingReference trajectory in the steady speed tracking stageThe 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.3FIGURETracking schematicsAfter 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:3dxdT1=vX−x(X−x)2+(Y−y)2dydT1=vY−y(X−x)2+(Y−y)2where T1is time of the steady speed tracking stage, v is the speed of the UGV, (X, Y) are the coordinates of the motion virtual leader and (X, Y) are the coordinates of point on reference trajectory.Take the following working conditions as an example:The starting point of the UGV is (20 m, 1.5 m) and tracks the motion virtual leader at the speed of 30 m/s. The speed of the motion virtual leader is 20 m/s straight line, and the coordinates of the starting point of the virtual leader is (100 m,4.5 m). The UGV is traced in the direction of connection to the motion virtual leader. The process is shown in Figure 4. Figure 4(a) and (b) shows the curves of X and Y with time in the process of tracking. As shown in Figure 4(c), start point (1) is the initial point at which the path change begins for the UGV. Start point (2) is the starting point of the motion virtual leader at the corresponding time. Red “*” is the trajectory point. Blue “o” is the trajectory of the motion virtual leader. Considering the smoothness of the trajectory, the cubic spline interpolation of the trajectory points is carried out with blue “.” [as shown in Figure 4(d)].4FIGUREReference trajectoryAvoidance of obstacles in the environmentIn 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 Sa, the obstacle avoidance function is established4Jobs,j=∑j=1Nobskobsvi−vobs,jS2i,j,Si,j≺Sa0,Si,j≥Sawhere Nobs is the number of obstacles obtained by sensors or communications in real time, S(i,j) is the distance between the UGVi and the surrounding obstacle j, Sa is the absolute safe distance for UGVs and kobs is normal numbers.Threshold value SaThe 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 sa is as follows:5sa=sbrake,kwhere sa is the absolute safe distance andsbrake,k is the braking distance of UGVk.Analysis of the braking process and the theoretical formula of vehicle braking distanceUGV 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 tr1 and brake reaction stage tr2, the UGV runs at a steady speed, and in the braking growth stage ti, braking deceleration increases linear, and the following time tc, the UGV brakes at a fixed deceleration, as shown in Figure 5.5FIGUREUGV braking deceleration changeConsidering the heading angle, the theoretical formula of the UGV braking distance is6sbrake,k=vktr+0.5ti3.6sinφkcosφk+vk225.9ugsinφkcosφkwhere v is the speed of UGVk, tr is the reaction time, ti is the braking duration, φ is the heading angle and u is the friction coefficient.Reaching the target point smoothlyTo 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 multi‐obstacle 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 target7I=κ∑i=1NS2i,Twhere κ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 leaderTracking 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:8ξUGVpUGV,νUGV,φUGV→ξVLpVL,νVL,φVLwhere ξ 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)9∫T1T1+T2vUGVdt−∫T1T1+T2vVLdt≤lwhere T1 is the time of steady speed tracking stage time,T2 is the time of parameter value matching stage, vUGV and vVL 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 PLANNERThe 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 plannerThe 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 formulation of the trajectory planner is as follows:10minUt∑i=1HP∥ηt+it−ηreft+it∥Q2+∥μ∥R2+Jobs,jε1+Iε2where HP is the predictive time domain, ηref(t+i|t),i=1,…,HP, is the reference output variable, QandR are weight matrices and ε1andε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 controllerThe 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 NMPCTo accurately track the reference trajectory, the controller with NMPC is designed11fξt,μt−1,Δμt,ε=∑i=1HPηt+it−ηreft+itQ2+∑i=0HC−1Δμt+itR2+ρε2where HP and HC are predictive time domain and control time domain, respectively. ηref(t+i|t),i=1,…,HP are reference variables, Q,Randρ 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 problemThe 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 real‐time online solution is accomplished.Step 1. Consider the following nonlinear dynamic systems:12ξ̇t=fξt,μtηt=hξt,μtwheref(·,·) is the state transition function of the system, ξ(t)∈Rn is an n‐dimensional state variable, μ(t)∈Rm is an m‐dimensional control variable, and η(t)∈Rp is a p‐dimensional output variables.Step 2. Transform the nonlinear system into following discrete linear time‐varying system:13δξk+1=Ak,tξk+Bk,tμtδηt=Ck,tξk+Dk,tμtwhere Ak,t=∂f∂ξ|ξ0(k),μ0(k),Bk,t=∂f∂μ|ξ0(k),μ0(k),Ck,t=∂η∂ξ|ξ0(k),μ0(k)andDk,t=∂η∂μ|ξ0(k),μ0(k) are the Taylor expansion at the point (ξ0(k),μ0(k)).Step 3. Transformation solution of the QP problemControl increment Δμ(t) in the system control time domain HC 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 HC. 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 controllerThe 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.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.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.6FIGURECalculation flow of MPC controllerSIMULATION EXPERIMENTA joint simulation platform based on vehicle dynamics simulation software Carsim and control simulation software MATLAB/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 declarationThe 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 the front and rear suspensions are selected as independent suspensions. The predictive time domain is 15 s, the control time domain is 2 s and the size of the simulation step is 0.03 s.1TABLEVehicle control parametersParametervaluev‐vd/m/s[−2,2]Δv/m/s[−0.5,0.5]δ/°[−10,10]Δδ/°[−1,1]Test simulationThe 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.7FIGUREThe trajectory in double‐lane change condition8FIGUREThe heading angle in double‐lane change conditionFigure 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.9FIGURETrack trajectory at different speeds10FIGUREComparison of offset distance at different speedsFigure 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.11FIGUREHeading angle at different speeds12FIGUREComparison of angle difference at different speedsTrack trajectory comparison between the LTV MPC controller and the Stanley controllerThe LTV MPC controller and the Stanley controller are used to track the double‐lane change trajectory.Comparisons among the LTV MPC controller, the Stanley controller and the reference trajectory are shown in Figures 13 and 14. The trajectory determined by LTV MPC coincides well with the reference trajectory. The fitting degree between the Stanley method and the reference trajectory is slightly poor, and the maximum offset distance is 0.37 m. On one hand, the UGV centre mass deviates from the driving centre line during curve driving; on the other hand, it is related to the cross‐track error of the UGV centre mass from the reference trajectory.13FIGUREComparison of trajectory tracking control14FIGUREComparison of offset distance between the LTV MPC controller and the Stanley controllerThe 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.15FIGUREComparison of trajectory tracking controlFurthermore, 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 offset distance in the stable stage is less than 0.05 m, which is only 1/ 41 of the tyre width.16FIGUREComparison of offset distance between the LTV MPC controller and the Stanley controllerAnalysis of static and moving obstacle avoidanceThe process of reaching the motion virtual leader after UGV avoiding static and moving obstacles is shown in Figures 17 and 18, respectively. The speed of the UGV is 30 m/s. Static obstacle collision avoidance condition: the static obstacle is square L = 0.5 m, and the initial position of the lower left corner of the obstacle is (70 m, 3.0 m). Moving obstacle avoidance condition: square L = 0.5 m obstacle, the initial position of the lower left corner of the obstacle is (53.25 m, 3.0 m), at the speed of 10 m/s straight line.17FIGUREUGV collision avoidance of static obstacle18FIGUREUGV collision avoidance of moving obstacleThe 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 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 peak angle reaches 1.4°. The minimum distance between the UGV and the moving obstacle is about 20 cm.19FIGUREChange of velocity along the y‐axis in static obstacle situation and moving obstacle situation20FIGUREChange of velocity along the x‐axis in static obstacle situation and moving obstacle situation21FIGUREChange of the heading angular velocity in static obstacle situation and moving obstacle situation22FIGUREChange of the heading angle in static obstacle situation and moving obstacle situation23FIGURECollision avoidance trajectory of the static obstacle24FIGURECollision avoidance trajectory of the moving obstacle25FIGUREChange of the front wheel angle in static obstacle situation and moving obstacle situationThe collision avoidance trajectory at different speeds in the static and moving obstacle situations is shown in Figures 26 and 27. The solid line is the trajectory of upper UGV profile at different speeds, and the dotted line is the trajectory of the UGV mass centre at different speeds. At different speeds, the trajectory of the mass centre can reach the point M from the point S both in static and moving obstacle situations, so the UGV can complete the collision avoidance and reach the destination. 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.26FIGURECollision avoidance trajectory at different speeds in static obstacle situation27FIGURECollision avoidance trajectory at different speeds in the moving obstacle situationParameter value matching between the UGV and its motion virtual leaderIn 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 carried out. The initial point is (248 m,4.5 m) and the speed of the UGV is 30 m/s. It is assumed that the virtual leader runs in a straight line at the speed of 20 m/s, passing time T2, ξUGV(pUGV,νUGV,φUGV)→ξVL(pVL,νVL,φVL).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.28FIGURETrajectory planning of X‐axis29FIGURETrajectory planning of Y‐axisThe 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.30FIGUREVelocity change in parameter value matching stageCONCLUSIONSThis paper studies the trajectory planning and tracking control of UGVs leading by a virtual leader. The conclusions are as follows.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.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.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.ACKNOWLEDGEMENTThis work was supported in part by the National Natural Science Foundation of China under Grant 61573171 and Grant 51675235 and in part by Colleges and Universities in Jiangsu Province plans to graduate research and innovation under Grant CXLX13_675.REFERENCESGhommam, J., et al.: Formation path following control of unicycle‐type mobile robots. Robot. Auton. Syst. 58(5), 727–736 (2010)Guo, J.H., et al.: An adaptive cascade trajectory tracking control for over‐actuated autonomous electric vehicles with input saturation. Sci. China Technol. Sci. 62, 2153–2160 (2019)Morale, D.: Modeling and simulating animal grouping individual‐based models. Future Gener. Comput. Syst. 17, 883–891 (2001)Dierks, T., Jagannathan, S.: Control of nonholonomic mobile robot formations: Backstepping kinematics into dynamics. In Proc. IEEE Int. Conf. Control Appl., Singapore, pp. 94–99 Oct. (2007)Cao, J., et al.: Distributed event‐triggered consensus tracking of second‐order multi‐agent systems with a virtual leader. Chin. Phys. B 25(5), 491–496 (2016)Lalish, E., et al.: Formation tracking control using virtual structures and deconfliction. In Proc. IEEE int. Con. Decis. Control, San Diego, CA, USA, pp. 5699–5705 May (2006)Yamaguchi, H.: Adaptive formation control for distributed autonomous mobile robot group. In Proc. IEEE Int. Conf. Rob. Autom., Albuquerque, NM, USA, pp. 2300–2305 Apr. (1997)Lawton, J.R., Beard, R.W.: Synchronized multiple spacecraft rotations. Automatica 38(8), 1359–1364 (2002)Jiang, H.B., et al.: Trajectory planning and optimisation method for intelligent vehicle lane changing emergently. IET Intell. Transp. Syst. 12(10), 1336–1344 (2018)Oommen, B., et al.: Robot navigation in unknown terrains using learned visibility graphs. Part I: The disjoint convex obstacle case. IEEE J. Rob. Autom. 3(6), 672–681 (1987)Shao, M.L., et al.: Sensor‐based path planning: The two‐identical‐link hierarchical generalized Voronoi graph. Int. J. Precis. Eng. Manuf. 16(8), 1883–1887 (2015)Chen, D.Z., et al.: A framed‐quadtree approach for determining Euclidean shortest paths in a 2‐D environment. IEEE Trans. Rob. 13(5), 668–681 (1997)Park, J.Y., et al.: Task‐oriented design of robot kinematics using the grid method. Adv. Rob. 17(9), 879–907 (2003)Wang, Z., et al.: Path planning for first responders in the presence of moving obstacles with uncertain boundaries. IEEE Trans. Intell. Transp. 18(8), 2163–2173 (2017)Kothari, M., Postlethwaite, I.: A probabilistically robust path planning algorithm for UAVs using rapidly‐exploring random trees. J. Intell. Rob. Syst. 71(2), 231–253 (2013)Tuncer, A., Yildirim, M.: Dynamic path planning of mobile robots with improved genetic algorithm. Comput. Electr. Eng. 38(6), 1564–1572 (2012)Ersson, T., Hu, X.: Path planning and navigation of mobile robots in unknown environments. In Proc. IEEE Int. Con. Intell. Rob. Syst., Maui, HI, USA, pp. 858–864 Nov. (2001)Elbanhawi, M., et al.: Receding horizon lateral vehicle control for pure pursuit path tracking. J. Vib. Control 24(3), 619–642 (2018)Hoffmann, G.M., et al.: Autonomous automobile trajectory tracking for off‐road driving: Controller design, experimental validation and racing. In Proc. Int. Conf. Am. Control Conf., New York, USA, pp. 2296–2301 July (2007)Ziegler, J., et al.: Making bertha drive—An autonomous journey on a historic route. IEEE Intell. Transp. Syst. 6(2), 8–20 (2014)Levinson, J., et al.: Towards fully autonomous driving: Systems and algorithms. In Proc. IEEE Int. Conf. Intell. Veh. Symp. Baden‐Baden, Germany, pp. 163–168 Jun. (2011)Li, S., et al.: Model predictive multi‐objective vehicular adaptive cruise control. IEEE Trans. Control Syst. Technol. 19(3), 556–566 (2011)Falcone, P., et al.: Linear time‐varying model predictive control and its application to active steering systems: Stability analysis and experimental validation. Int. J. Robust Nonlinear 18(8), 862–875 (2010)Cai, J., et al.: Implementation and development of a trajectory tracking control system for intelligent vehicle. J. Intell. Rob. Syst. 94(1), 251–264 (2019)Xu, L.W., et al.: Distributed formation control of homogeneous vehicle platoon considering vehicle dynamics. Int. J. Autom. Technol. 20(6), 1103–1112 (2019)Falcone, P.: Nonlinear model predictive control for autonomous vehicles. Ph.D. dessertation, Sannio Univ., Benevento, Italy, (2007)Gong, J.W., et al.: Model Predictive Control for Self‐Driving Vehicles. Beijing Institute of Technology Press, Beijing, China (2014)Xu, C.C., et al.: Calibration of crash risk models on freeways with limited real‐time traffic data using Bayesian meta‐analysis and Bayesian inference approach. Accid. Anal. Prev. 85, 207–218 (2015)Kunhe, F., et al.: Model predictive control of a mobile robot using linearization. IEEE Mechatron. Rob. (4), 525–530 (2004)Falcone, P., et al.: MPC‐based yaw and lateral stabilisation via active front steering and braking. Veh. Syst. Dyn. 46, 611–628 (2008)

Journal

IET Intelligent Transport Systems – Wiley

Published: Feb 1, 2021

Keywords: Spatial variables control; Road‐traffic system control; Mobile robots; Stability in control theory; Optimal control; Time‐varying control systems; Nonlinear control systems

There are no references for this article.