Integrated Avoid Collision Control of Autonomous Vehicle Based on Trajectory Re-Planning and V2V Information Interaction

An integrated longitudinal-lateral control method is proposed for autonomous vehicle trajectory tracking and dynamic collision avoidance. A method of obstacle trajectory prediction is proposed, in which the trajectory of the obstacle is predicted and the dynamic solution of the reference trajectory is realized. Aiming at the lane changing scene of autonomous vehicles driving in the same direction and adjacent lanes, a trajectory re-planning motion controller with the penalty function is designed. The reference trajectory parameterized output of local reprogramming is realized by using the method of curve fitting. In the framework of integrated control, Fuzzy adaptive (proportional-integral) PI controller is proposed for longitudinal velocity tracking. The selection and control of controller and velocity are realized by logical threshold method; A model predictive control (MPC) with vehicle-to-vehicle (V2V) information interaction modular and the driver characteristics is proposed for direction control. According to the control target, the objective function and constraints of the controller are designed. The proposed method’s performance in different scenarios is verified by simulation. The results show that the autonomous vehicles can avoid collision and have good stability.


Introduction
The autonomous vehicle is an emerging technology that provides a safe and efficient transportation experience. As part of the intelligent transportation system, it has a very wide application prospect [1,2]. The autonomous vehicle refers to a class of vehicles that can carry out sensing and decision-making, track planning, and tracking. Trajectory tracking control is the basic problem and necessary condition of autonomous vehicle research [3]. Due to the application of advanced sensing technology and the development of vehicle state estimation algorithm, the state information of the vehicle becomes more observable and more accurate, such as tire force conditions, sideslip angle, yaw rate, which are closely related to vehicle handling stability [4][5][6], even the coefficient of friction of tires [7].
There has been a great deal of research on path planning and tracking control algorithms, such as inverse kinetic compensation feedback control [8], the optimization algorithm based on the sampling fusion and quadratic programming model [9], the linear combination method using weighted cost function [10], integrated local path planning and tracking control [11], using non-linear programming model [12] and so on. As the vehicle has its own mechanical structure, stability, driver handling capacity and other restrictions, it is not easy to apply these methods directly to the collision scene of the vehicle. Therefore, for the path planning problems, the movement states of other vehicles need to be considered to solve the collision problem on the road. In order to improve the active safety performance

System Modeling
In this section, the vehicle model, the driver model, and the moving obstacle trajectory prediction model are described. The vehicle model and the driver model are integrated to establish a nonlinear dynamic model considering the drivers' handling characteristics. In the following model, the superscript i = p, q is used to represent the two vehicles with different states, respectively. The multi-vehicle interactive state information is processed by the vehicle p, and the interactive state information is supplied by the vehicle q.

Vehicle Model
It is of great significance to establish a suitable vehicle dynamics model for the MPC controller [30].
Simplifying the vehicle dynamics model not only reflects the basic dynamic characteristics of the vehicle, but also ensures the real-time performance of the control algorithm. A bicycle model, as shown in Figure 1, is used to describe the dynamics of the two vehicles, including longitudinal, yaw, and lateral motions [31].
Assuming that the front tire steering angle is small and the dynamics and parameters of the two vehicles are the same. In the global coordinates oxy, F x f and F xr are the longitudinal forces acting on the front and rear tires, respectively. From Figure 1, dynamics of the vehicle can be described as follows: In the process of vehicle lane change, the direction angle of the vehicle is very small, that is to meet the following approximate conditions, cos( ) 1 1 1 where cf C and cr C are the front and rear tire cornering stiffness and 1 α are 2 α the front and rear tire slip angle.
where i f δ is the front wheel steering angle and the vehicle side slip angle, The front and rear longitudinal tire forces can be written as the functions of the tire slip rate described as follows:  From Figure 1, dynamics of the vehicle can be described as follows: In the process of vehicle lane change, the direction angle of the vehicle is very small, that is to meet the following approximate conditions, cos( ϕ i , convert as follows: . Y i = .
x i . The front and rear lateral tire forces can be written as the functions of the tire slip angles described as follows: where C c f and C cr are the front and rear tire cornering stiffness and α 1 are α 2 the front and rear tire slip angle.
where δ i f is the front wheel steering angle and the vehicle side slip angle, β i = The front and rear longitudinal tire forces can be written as the functions of the tire slip rate described as follows: where C l f and C lr are the front and rear tire longitudinal stiffness and s i 1 are s i 2 the front and rear tire slip rate.

Human Driver Model
The driver model is essentially a physical equation that simulates the driver's behavior. In the study of the driver-vehicle system closed-loop system, the driver plays a "controller" role and the driver adjusts the direction according the characteristics. In this paper, the driver model and the vehicle model are integrated to establish the driver-vehicle closed-loop system. The young and aged driver's Sensors 2020, 20, 1079 5 of 26 handling characteristics, including advance time, delay time, and steering wheel angle ratio, are evaluated, and the driver handling characteristics can be characterized by these parameters [32]. The basic driver model is considered to be a proportional and differential controller with a delay element and attempts to minimize the difference between the vehicle trajectory and the desired trajectory. The applicable steering wheel angle for representing the driver's steering characteristics is described as follows [32]: where Y i des and Y i are the target and current lateral positions of the vehicle's center of gravity, G i h is the steering proportional gain, τ i h and T i h are derivative time constant and response time delay, respectively, and s is the Laplace operator. By assuming that the gear ratio of the steering system is R g , δ i f = R g δ i h , and the driver model in Equation (9) can be rewritten in the form of differential equation: .
The systems described in Equations (1)- (8) and (10) can be assembled as a driver-vehicle system, as shown in Figure 2. By combining the two systems and the V2V information interactive model, the information interaction system is formed, and expressed as follows: .
where the state variables of this system consist of the states of the two vehicles, defined as follows: The control variable of the system is the front wheel steering angle of two vehicles: Sensors 2020, 18, x FOR PEER REVIEW 5 of 26

Human Driver Model
The driver model is essentially a physical equation that simulates the driver's behavior. In the study of the driver-vehicle system closed-loop system, the driver plays a "controller" role and the driver adjusts the direction according the characteristics. In this paper, the driver model and the vehicle model are integrated to establish the driver-vehicle closed-loop system. The young and aged driver's handling characteristics, including advance time, delay time, and steering wheel angle ratio, are evaluated, and the driver handling characteristics can be characterized by these parameters [32]. The basic driver model is considered to be a proportional and differential controller with a delay element and attempts to minimize the difference between the vehicle trajectory and the desired trajectory. The applicable steering wheel angle for representing the driver's steering characteristics is described as follows [32]: delay, respectively, and s is the Laplace operator. By assuming that the gear ratio of the steering , and the driver model in Equation (9) can be rewritten in the form of differential equation: The systems described in Equations (1)- (8) and (10) can be assembled as a driver-vehicle system, as shown in Figure 2. By combining the two systems and the V2V information interactive model, the information interaction system is formed, and expressed as follows: where the state variables of this system consist of the states of the two vehicles, defined as follows: , , , , , , , , , , , , , The control variable of the system is the front wheel steering angle of two vehicles:

Predictive Model of Moving Obstacle Trajectory
In the actual environment, since the external environment is dynamic, the trajectory tracking control under the given trajectory does not guarantee the autonomous vehicle to deal with any problem accurately. The obstacle avoidance function which obstacles are fixed or trajectories are known has been unable to meet the requirements of dynamic obstacle avoidance. In other words, the vehicle state information at the moment k is shared by two vehicles and the vehicle p performs the obstacle avoidance function according to the information at the moment k . Since the state information of the vehicle q at the moment +1 k cannot be obtained in advance, there is still a collision risk between vehicles at the moment 1 k+ . It is necessary to predict the trajectory by using

Predictive Model of Moving Obstacle Trajectory
In the actual environment, since the external environment is dynamic, the trajectory tracking control under the given trajectory does not guarantee the autonomous vehicle to deal with any problem accurately. The obstacle avoidance function which obstacles are fixed or trajectories are known has been unable to meet the requirements of dynamic obstacle avoidance. In other words, the vehicle state information at the moment k is shared by two vehicles and the vehicle p performs the obstacle avoidance function according to the information at the moment k. Since the state information of the vehicle q at the moment k+1 cannot be obtained in advance, there is still a collision risk between vehicles at the moment k + 1. It is necessary to predict the trajectory by using the state information at the moment k provided by other vehicles to realize the obstacle avoidance function.
Based on simplified driver model, derived from the equation [33]: Based on the front wheel steering angle δ q of the interactive information, the vehicle sideslip angle β q pre and the yaw angle ϕ q pre can be estimated and compared with the vehicle sideslip angle β q and the yaw angle ϕ q in the real-time interactive information.
The above equation is integrated to obtain the vehicle center of mass position: The first derivative of the trajectory curve indicates the value of the front wheel steering angle δ The reference trajectory and the predicted trajectory are compared in Figure 3. The error between the predicted trajectory and the reference trajectory is shown in Figure 4. Based on simplified driver model, derived from the equation [33]: Based on the front wheel steering angle q δ of the interactive information, the vehicle sideslip angle q pre β and the yaw angle q pre ϕ can be estimated and compared with the vehicle sideslip angle q β and the yaw angle q ϕ in the real-time interactive information.
The above equation is integrated to obtain the vehicle center of mass position: The first derivative of the trajectory curve indicates the value of the front wheel steering angle The reference trajectory and the predicted trajectory are compared in Figure 3. The error between the predicted trajectory and the reference trajectory is shown in Figure 4.     As shown in Figures 3 and 4, in two conditions, the prediction trajectory is generated by the obstacle trajectory prediction model, and the error between predicted trajectory and reference trajectory is small. As shown in Figure 4, before the two lane change process, the error between predicted trajectory and reference trajectory is negative, making the predicted trajectory closer to the autonomous vehicle, enabling the autonomous vehicle to perceive obstacles ahead and achieve collision avoidance control. After the crossing, the error value is positive which helps the autonomous vehicle make a more rapid decision on whether there is a threat or not.

Controller Design
In this paper, a typical vehicle-to-vehicle (V2V) encountering scenario of lane exchanging with two vehicles maneuvered to exchange lanes is built. That is, two vehicles traveling on contiguous lanes in the same direction are maneuvered to exchange lanes, as shown in Figure 5. The controller is designed for this conflicting scenario. Vehicle tracking control requirements are mainly divided into longitudinal velocity control and lateral position control. The control framework is shown as Figure 6.  The integrated information interaction model constructs two vehicle systems into a V2V information exchange system. This section first introduces the overall control framework, and then gives the longitudinal control of the fuzzy adaptive PI controller design and lateral control design of the LTV MPC controller design.

Control Framework
The trajectory re-planning control framework for lane exchanging of two vehicles is described in Figure 7, where intentions of the drivers can be given by the front tire steering angles and As shown in Figures 3 and 4, in two conditions, the prediction trajectory is generated by the obstacle trajectory prediction model, and the error between predicted trajectory and reference trajectory is small. As shown in Figure 4, before the two lane change process, the error between predicted trajectory and reference trajectory is negative, making the predicted trajectory closer to the autonomous vehicle, enabling the autonomous vehicle to perceive obstacles ahead and achieve collision avoidance control. After the crossing, the error value is positive which helps the autonomous vehicle make a more rapid decision on whether there is a threat or not.

Controller Design
In this paper, a typical vehicle-to-vehicle (V2V) encountering scenario of lane exchanging with two vehicles maneuvered to exchange lanes is built. That is, two vehicles traveling on contiguous lanes in the same direction are maneuvered to exchange lanes, as shown in Figure 5. The controller is designed for this conflicting scenario. As shown in Figures 3 and 4, in two conditions, the prediction trajectory is generated by the obstacle trajectory prediction model, and the error between predicted trajectory and reference trajectory is small. As shown in Figure 4, before the two lane change process, the error between predicted trajectory and reference trajectory is negative, making the predicted trajectory closer to the autonomous vehicle, enabling the autonomous vehicle to perceive obstacles ahead and achieve collision avoidance control. After the crossing, the error value is positive which helps the autonomous vehicle make a more rapid decision on whether there is a threat or not.

Controller Design
In this paper, a typical vehicle-to-vehicle (V2V) encountering scenario of lane exchanging with two vehicles maneuvered to exchange lanes is built. That is, two vehicles traveling on contiguous lanes in the same direction are maneuvered to exchange lanes, as shown in Figure 5. The controller is designed for this conflicting scenario. Vehicle tracking control requirements are mainly divided into longitudinal velocity control and lateral position control. The control framework is shown as Figure 6.  The integrated information interaction model constructs two vehicle systems into a V2V information exchange system. This section first introduces the overall control framework, and then gives the longitudinal control of the fuzzy adaptive PI controller design and lateral control design of the LTV MPC controller design.

Control Framework
The trajectory re-planning control framework for lane exchanging of two vehicles is described in Figure 7, where intentions of the drivers can be given by the front tire steering angles and Vehicle tracking control requirements are mainly divided into longitudinal velocity control and lateral position control. The control framework is shown as Figure 6. As shown in Figures 3 and 4, in two conditions, the prediction trajectory is generated by the obstacle trajectory prediction model, and the error between predicted trajectory and reference trajectory is small. As shown in Figure 4, before the two lane change process, the error between predicted trajectory and reference trajectory is negative, making the predicted trajectory closer to the autonomous vehicle, enabling the autonomous vehicle to perceive obstacles ahead and achieve collision avoidance control. After the crossing, the error value is positive which helps the autonomous vehicle make a more rapid decision on whether there is a threat or not.

Controller Design
In this paper, a typical vehicle-to-vehicle (V2V) encountering scenario of lane exchanging with two vehicles maneuvered to exchange lanes is built. That is, two vehicles traveling on contiguous lanes in the same direction are maneuvered to exchange lanes, as shown in Figure 5. The controller is designed for this conflicting scenario. Vehicle tracking control requirements are mainly divided into longitudinal velocity control and lateral position control. The control framework is shown as Figure 6.  The integrated information interaction model constructs two vehicle systems into a V2V information exchange system. This section first introduces the overall control framework, and then gives the longitudinal control of the fuzzy adaptive PI controller design and lateral control design of the LTV MPC controller design.

Control Framework
The trajectory re-planning control framework for lane exchanging of two vehicles is described in Figure 7, where intentions of the drivers can be given by the front tire steering angles and The integrated information interaction model constructs two vehicle systems into a V2V information exchange system. This section first introduces the overall control framework, and then gives the longitudinal control of the fuzzy adaptive PI controller design and lateral control design of the LTV MPC controller design.

Control Framework
The trajectory re-planning control framework for lane exchanging of two vehicles is described in Figure 7, where intentions of the drivers can be given by the front tire steering angles and throttle/brake inputs and feedback to the trajectory re-planning controller. Different drivers have different handling characteristics for the vehicle. The driver's character, age, and other factors have a great impact on the handling of the vehicle. The data of the driver's handling characteristics are available in the onboard controllers. Communicate through the information interactive model aiming to make the state information between the two vehicles can be interactive. The desired velocity and desired trajectory determined by the controller are tracked by the longitudinal and lateral controllers, respectively.  In the lane changing process, there are conflicting threats to the desired trajectory of the vehicles. If two vehicles do not take proper measures, collisions are inevitable. Before lane exchanging, changing the vehicle velocity or changing the desired trajectory of the vehicle is an effective way of avoiding collisions. In this paper, the MPC controller is proposed to re-plan the optimal desired trajectory to avoid obstacles. The focus of this paper is trajectory re-planning function in vehicle lane exchanging process. The control framework in Figure 7 adopts fuzzy adaptive PI control and model predictive control to realize the tracking and control of the vehicle's longitudinal velocity and the desired value of the front wheel steering angle. The information of vehicles is shared by the information interaction model.

Longitudinal Control
Longitudinal control is an important part of autonomous vehicles, and it is one of the most basic and key technologies for autonomous vehicles. The longitudinal controller is designed and analyzed in this section to enable the accurate velocity tracking. The longitudinal control of autonomous vehicles is influenced by external factors. The controlled object has strong non-linearity, and it is difficult to meet the requirements of rapid and accurate tracking reference velocity with the traditional control method. The adaptive control can adjust the structure (parameters) of the controller in real time to ensure that the system always running in the ideal state and the fuzzy adaptive PI control method is adopted in this paper. The fuzzy control rule developed in the fuzzy PI control method is a reflection of the driver's knowledge and experience and the driver's handling behavior is taken into account during the design of the controller [34].

PI Controller Design
The continuous proportional-integral-derivative (PID) control algorithm cannot be directly used as a controller and needs to be discretized. The positional PID control algorithm is expressed as follows [35]: In the lane changing process, there are conflicting threats to the desired trajectory of the vehicles. If two vehicles do not take proper measures, collisions are inevitable. Before lane exchanging, changing the vehicle velocity or changing the desired trajectory of the vehicle is an effective way of avoiding collisions. In this paper, the MPC controller is proposed to re-plan the optimal desired trajectory to avoid obstacles. The focus of this paper is trajectory re-planning function in vehicle lane exchanging process. The control framework in Figure 7 adopts fuzzy adaptive PI control and model predictive control to realize the tracking and control of the vehicle's longitudinal velocity and the desired value of the front wheel steering angle. The information of vehicles is shared by the information interaction model.

Longitudinal Control
Longitudinal control is an important part of autonomous vehicles, and it is one of the most basic and key technologies for autonomous vehicles. The longitudinal controller is designed and analyzed in this section to enable the accurate velocity tracking. The longitudinal control of autonomous vehicles is influenced by external factors. The controlled object has strong non-linearity, and it is difficult to meet the requirements of rapid and accurate tracking reference velocity with the traditional control method. The adaptive control can adjust the structure (parameters) of the controller in real time to ensure that the system always running in the ideal state and the fuzzy adaptive PI control method is adopted in this paper. The fuzzy control rule developed in the fuzzy PI control method is a reflection of the driver's knowledge and experience and the driver's handling behavior is taken into account during the design of the controller [34].

PI Controller Design
The continuous proportional-integral-derivative (PID) control algorithm cannot be directly used as a controller and needs to be discretized. The positional PID control algorithm is expressed as follows [35]: where k i = k p * T/T I , k d = k p T D /T. T, T I , and T D are the sampling time, integration time, derivative time, respectively. k = 1, 2, · · · is the sampling number, error(k − 1) and error(k − 1) are the deviation signals obtained at the time of k − 1 and k. u(k) in Equation (20) is the output of the positional PID control algorithm. Set k d = 0. According to the recursive principle: Equation (22) is subtracted by Equations (20) and (21): ∆u(k) is the output of the incremental PI control algorithm. The control variable u(k) is obtained by Equation (23):

Fuzzy Adaptive PI Controller Design
Aiming at the velocity tracking problem of autonomous vehicles in this paper, the deviation between the reference velocity and the actual velocity of the vehicle e and the rate of deviation change ec are taken as input. The outputs of the fuzzy controller ∆k P , ∆k I are the increment of k P , k I for adjusting the two important parameters of the PI controller. The structure of the fuzzy adaptive PI controller is shown in Figure 8.
Equation (22) is subtracted by Equations (20) and (21): is the output of the incremental PI control algorithm. The control variable ( ) u k is obtained by Equation (23):

Fuzzy Adaptive PI Controller Design
Aiming at the velocity tracking problem of autonomous vehicles in this paper, the deviation between the reference velocity and the actual velocity of the vehicle e and the rate of deviation change ec are taken as input. The outputs of the fuzzy controller , P I k k Δ Δ are the increment of , P I k k for adjusting the two important parameters of the PI controller. The structure of the fuzzy adaptive PI controller is shown in Figure 8.  Tables 1 and 2. (1) The setting principle of P k : When the response is in the ascending process ( e is P), P k Δ takes a positive value to increase P k . When overshoot ( e is N), P k Δ takes a negative value to decrease P k . When the error is near zero ( e is Z), there are three cases: when ec is N, the overshoot is getting bigger and bigger, then P k Δ takes negative; when ec is Z, in order to reduce the error, P k Δ is positive; when ec is P, the positive error is getting bigger, then P k Δ is positive. The fuzzy control rules of P k are shown in Table 1.  Tables 1 and 2. (1) The setting principle of k P : When the response is in the ascending process (e is P), ∆k P takes a positive value to increase k P . When overshoot (e is N), ∆k P takes a negative value to decrease k P . When the error is near zero (e is Z), there are three cases: when ec is N, the overshoot is getting bigger and bigger, then ∆k P takes negative; when ec is Z, in order to reduce the error, ∆k P is positive; when ec is P, the positive error is getting bigger, then ∆k P is positive. The fuzzy control rules of k P are shown in Table 1. (2) The setting principle of k I : using the integral separation strategy, when the error value is near zero, ∆k I is positive, otherwise zero. The fuzzy control rules of k I is shown in Table 2.

Lateral Control
This paper focuses on the study of the lateral obstacle avoidance control of autonomous vehicles. The longitudinal velocity changes when necessary. The lateral controller of the autonomous vehicle is based on the planned path. The non-linear MPC have been developed for vehicle lateral control. The nonlinear MPC requires solving the optimal control problem on the finite prediction step. Although these problems are convex in the linear MPC, they are no longer convex in the nonlinear MPC. This poses a challenge to nonlinear MPC stability theory and numerical solutions, and does not satisfy real-time constraints [36]. In this paper, a linear model predictive control method is proposed to design the trajectory tracking controller.

Discretization and Linearization
At the moment of t, the nonlinear vehicle dynamics model (x 0 (t), u 0 (t − 1)) was developed for Taylor [37]: where: are calculated by the following equation [37]: Then Equation (24) can be expressed as: .
To be discretized [37]: Equation (26) is organized into a linear discretization of vehicle dynamics model of the equation of state: x(k + 1) = Ax(k) + Bu(k) + d(k) (27) where d(k) =x(k + 1) − Ax(k) − Bû(k). Since the two vehicle state information needs to be processed simultaneously by the MPC controller, , T is sampling time, , T is sampling time. Change the input of Equation (27) from control variable u(k) to control increment ∆u(k) to obtain the state equation of vehicle dynamics model as shown in Equation (28): x(k + 1) =Ãx(k) +B∆u(k) +d(k) (28) wherex , m and n are the dimension of the control variables and state variables, respectively.
In this paper, the yaw angle ϕ i and the longitudinal position of the vehicle Y i are selected as the output of the state space. In order to integrate the state information of two vehicles into the MPC controller and maintain the dimension uniformity, the output is:

Obstacle Avoidance Function
In the actual environment, since the external environment is dynamic, the trajectory tracking control under the given trajectory does not guarantee the autonomous vehicle to deal with any problem accurately. When there is an obstacle in a given desired trajectory, the autonomous vehicle re-plans the trajectory according to the information of the obstacle to avoid the obstacle and then continues to track the desired trajectory.
In this paper, based on the trajectory tracking control, the MPC-based local planning layer is proposed to form a new control system. The local path planning controller re-plans the local desired trajectory according to the reference path information and the obstacle information obtained by the sensor. At the same time, the local desired trajectory information is input to the tracking control layer, thus the global reference path is tracked while the obstacle avoidance is realized.
The basic idea of the penalty function is to adjust the size of the function value according to the distance deviation between the obstacle and the target, and the closer the distance is, the larger the function value is. The penalty function is as follows: where S obs is the weight coefficient, (x i , y i ) is the position of the obstacle in the body coordinate system, (x 0 , y 0 ) is the position of the center of mass of the vehicle, ∆ is a small positive number, preventing the denominator from being zero. The target of local trajectory planning is to achieve the obstacle avoidance function while minimizing the deviation from the global reference path. The local trajectory re-planning model predictive controller is as follows: whereỹ(k + i|k) and y re f (k + i|k) are the output value and desired value at the moment k + i. U i is the control variable sequence.
In order to make the result of trajectory planning feasible, the expansion of the obstacle is carried out according to the vehicle body size. In this paper, circles with the same radius are used instead of the vehicle body rectangle, as shown in Figure 11. In order to ensure safe driving, the distance between the vehicle body and the center of the obstacle must be greater than the radius of any circle. In order to make the result of trajectory planning feasible, the expansion of the obstacle is carried out according to the vehicle body size. In this paper, circles with the same radius are used instead of the vehicle body rectangle, as shown in Figure 11. In order to ensure safe driving, the distance between the vehicle body and the center of the obstacle must be greater than the radius of any circle. saf R Figure 11. Circular expansion of the vehicle shape.
In [38], a trajectory planning algorithm based on polynomial fitting is proposed, which uses a simplified spherical extension polyhedron to express the obstacle and add dynamic constraints and it is easier to generate smooth trajectories by selecting the coefficients to select the permissible trajectories. The fifth order polynomial is used as the fitting curve:  a a a a a a a b b b b b are the parameters to be solved.

MPC Controller Design
The structure of the model predictive controller is shown in Figure 12. The MPC controller's task is to solve the optimal solution of the trajectory re-planning under the premise of satisfying the various constraints such as driver's operation, obstacle avoidance, and the mechanical structure of the vehicle. The vehicle information interaction system includes the states of two connected vehicles. The states of the model are: , , , , , , , , , , , , , The control variable is the front wheel steering angle p f δ = u . Figure 11. Circular expansion of the vehicle shape.
In [38], a trajectory planning algorithm based on polynomial fitting is proposed, which uses a simplified spherical extension polyhedron to express the obstacle and add dynamic constraints and it is easier to generate smooth trajectories by selecting the coefficients to select the permissible trajectories. The fifth order polynomial is used as the fitting curve: where a p = [a 0 , a 1 , a 2 , a 3 , a 4 , are the parameters to be solved.

MPC Controller Design
The structure of the model predictive controller is shown in Figure 12. In order to make the result of trajectory planning feasible, the expansion of the obstacle is carried out according to the vehicle body size. In this paper, circles with the same radius are used instead of the vehicle body rectangle, as shown in Figure 11. In order to ensure safe driving, the distance between the vehicle body and the center of the obstacle must be greater than the radius of any circle. saf R Figure 11. Circular expansion of the vehicle shape.
In [38], a trajectory planning algorithm based on polynomial fitting is proposed, which uses a simplified spherical extension polyhedron to express the obstacle and add dynamic constraints and it is easier to generate smooth trajectories by selecting the coefficients to select the permissible trajectories. The fifth order polynomial is used as the fitting curve:  a a a a a a a b b b b b are the parameters to be solved.

MPC Controller Design
The structure of the model predictive controller is shown in Figure 12. The MPC controller's task is to solve the optimal solution of the trajectory re-planning under the premise of satisfying the various constraints such as driver's operation, obstacle avoidance, and the mechanical structure of the vehicle. The vehicle information interaction system includes the states of two connected vehicles. The states of the model are: , , , , , , , , , , , , , The control variable is the front wheel steering angle The MPC controller's task is to solve the optimal solution of the trajectory re-planning under the premise of satisfying the various constraints such as driver's operation, obstacle avoidance, and the mechanical structure of the vehicle. The vehicle information interaction system includes the states of two connected vehicles. The states of the model are: The control variable is the front wheel steering angle u = δ The prediction horizon of the system is defined as N p and control horizon is defined as N c . At time k, the output of the vehicle system at time k + 1, · · · , k + N p is predicted: . . .
The optimization problem to be solved in the receding horizon is defined as follows: y hc,min ≤ y hc ≤ y hc,max (34d) y sc,min − ε ≤ y sc ≤ y hc,max + ε (34e) In Equation (34a),ỹ(k + i|k) and y re f ,local (k + i|k) are the output value and desired value of the output at the future moment k + i. y re f ,local = [Y re f ,local , ϕ re f ,local ] is the local reference trajectory. ∆u(k + i|k) is the control variable sequence of future times. ρ is the weighting factor. ε is the relaxation factor. y sc and y hc are soft constraints and hard constraints, respectively.
Since the control increment in the control time domain is unknown. The objective function is designed using the control increment of the vehicle system as a variable. The optimal control increment sequence in the control horizon is obtained by minimizing the optimization of the objective function. In order to ensure the tracking accuracy of the vehicle, the vehicle lateral position deviation and yaw rate deviation are taken into account in the objective function. At the same time, in order to keep the change of the control variables stable and avoid the abrupt change of the control variables, constraints are added to the control increment in the objective function. The relaxation factor is added to ensure that the objective function has a feasible solution.
The state information of the two vehicles is used as the state variable by the controller. In order to maintain dimensions of the matrix unified, the output weight matrix is as follow: The control weighting matrix is Equations (34b)-(34f) are the design of the constraint condition. There is a certain limit to the steering range of the front wheel steering angle of the vehicle, therefore, the front wheel steering angle is restrained so as to avoid the fact that the front wheel steering angle is beyond the actual range. The constraint for the control variable is set to: In order to ensure that the vehicle trajectory tracking process is more stable, the control increment is constrained: Set relaxation factor: 0 ≤ ε ≤ 10 (37) By Equations (36) and (37): (38) is the upper and lower bounds of the objective function. In order to solve the objective function, Equation (34) is transformed into the inequality of ∆U(k). The relationship between the control variables of the vehicle system and the control increment at the moment k is as follows: . . .
written in matrix form: The Equation (35) can be converted to: Constrain the output of the vehicle system: The Equation (42) is expressed as: According to the dimension of the state information, Equation (44) is sorted by Equations (41) and (43): The problem is transformed into a single objective optimization problem with upper and lower bound constraints and inequality constraints. By optimizing Equation (34), the optimal control increment sequence in the control horizon N c can be obtained as: The MPC algorithm does not apply the optimal sequence to the vehicle system one by one, but only the first control increment ∆u * (k|k) is applied to the vehicle system. Equation (46) is used to obtain the optimal control at the current moment acting on the vehicle system.
The driver model is embedded in the MPC controller using Equation (24). This section chooses young and aged, respectively, as a comparison of driver characteristics. Set G h = 0.8, T h = 0.13, τ h = 1.1 as a young driver characteristic parameter and G h = 0.5, T h = 0.18, τ h = 1.1 as an aged driver characteristic parameter.
The parameters of MPC controller are shown in Table 3.

Simulation Results
In this section, the nonlinear vehicle dynamics model in Carsim software is used and co-simulation is carried out by Simulink. Select the D-Class Sedan model for simulation. The main parameters of the model are shown in Table 4. We mainly deal with low-traffic scenarios on the roads; thus, make the following conditions and assumptions:

1.
Initially, the vehicle q moves in the left lane of the road, then changes lanes on the right lane, ignoring other vehicles and collision threats.

2.
When the vehicle is changing lanes, no other vehicle is used as an obstacle to ensure that there are no other conflicts. 3.
The vehicle can obtain interactive information accurately including the state information of its own and obstacle vehicle.
There are three simulation scenarios in total for trajectory re-planning controller verification. These include different initial vehicle distances, vehicle constant velocity, and variable velocity. In this paper, the logic threshold switch is designed, and the selection of vehicle velocity and the controller are selected according to the logical relationship between the difference of the longitudinal position and the difference of the lateral position of the vehicles.
A. Scenario on larger initial distance. First, we consider a simple two vehicle lane exchanging scenario, as shown in Figure 4. Vehicle p is used as an active vehicle containing a trajectory re-planning controller, and the vehicle q is used as an obstacle vehicle. The initial distance between two vehicles is 12 m. The vehicle q moves at a constant velocity of 54 km/h. The vehicle p has an initial velocity of 54 km/h. The position and trajectories are shown in Figures 13 and 14. We mainly deal with low-traffic scenarios on the roads; thus, make the following conditions and assumptions: 1. Initially, the vehicle q moves in the left lane of the road, then changes lanes on the right lane, ignoring other vehicles and collision threats. 2. When the vehicle is changing lanes, no other vehicle is used as an obstacle to ensure that there are no other conflicts. 3. The vehicle can obtain interactive information accurately including the state information of its own and obstacle vehicle.
There are three simulation scenarios in total for trajectory re-planning controller verification. These include different initial vehicle distances, vehicle constant velocity, and variable velocity. In this paper, the logic threshold switch is designed, and the selection of vehicle velocity and the controller are selected according to the logical relationship between the difference of the longitudinal position and the difference of the lateral position of the vehicles.
A. Scenario on larger initial distance. First, we consider a simple two vehicle lane exchanging scenario, as shown in Figure 4. Vehicle p is used as an active vehicle containing a trajectory replanning controller, and the vehicle q is used as an obstacle vehicle. The initial distance between two vehicles is 12 m.            The discrete points of the curve in Figure 13 represent the positions of the vehicle at different times. As shown, there is no collision throughout the time course. The curves in Figure 14 are the dynamic reference trajectories of the controller which are determined by the real-time trajectory and prediction trajectory of obstacle, respectively. The two kinds of dynamic reference trajectories almost The discrete points of the curve in Figure 13 represent the positions of the vehicle at different times. As shown, there is no collision throughout the time course. The curves in Figure 14 are the dynamic reference trajectories of the controller which are determined by the real-time trajectory and prediction trajectory of obstacle, respectively. The two kinds of dynamic reference trajectories almost coincide, which proves the validity and correctness of the method proposed in this paper. As a result of the large initial distance, the logic threshold controller determines the no conflict between vehicles. Therefore, there is no change in the longitudinal velocity of the vehicle. There is no need to re-plan a new reference trajectory by local trajectory re-planning controller. As shown in Figure 15, although the longitudinal velocity of the vehicle has a very small change, it can be considered that the longitudinal velocity of the vehicle is constant. As shown in Figures 16-18, since the local trajectory is not re-planned, the MPC controller is in good condition, and the vehicle is controlled smoothly with good stability.
B. Scenario on smaller initial distance. The initial distance is changed to 6.7 m, and the other initial conditions are the same as those of scenario A. The vehicle p is an active vehicle with trajectory re-planning controller. Vehicle q is used as an obstacle vehicle. Figures 19 and 20   The discrete points of the curve in Figure 13 represent the positions of the vehicle at different times. As shown, there is no collision throughout the time course. The curves in Figure 14 are the dynamic reference trajectories of the controller which are determined by the real-time trajectory and prediction trajectory of obstacle, respectively. The two kinds of dynamic reference trajectories almost coincide, which proves the validity and correctness of the method proposed in this paper. As a result of the large initial distance, the logic threshold controller determines the no conflict between vehicles. Therefore, there is no change in the longitudinal velocity of the vehicle. There is no need to re-plan a new reference trajectory by local trajectory re-planning controller. As shown in Figure 15, although the longitudinal velocity of the vehicle has a very small change, it can be considered that the longitudinal velocity of the vehicle is constant. As shown in Figures 16-18, since the local trajectory is not re-planned, the MPC controller is in good condition, and the vehicle is controlled smoothly with good stability.
B. Scenario on smaller initial distance. The initial distance is changed to 6.         The discrete points of curves in Figure 19 indicate the position of vehicles at different times.
During the entire period of time, vehicle p under different handling characteristics have no collision, and the trajectory is smooth. In this section, vehicle p with young handling characteristics is analyzed for example. In order to give a clearer description of the position of the vehicle when the The discrete points of curves in Figure 19 indicate the position of vehicles at different times.
During the entire period of time, vehicle p under different handling characteristics have no collision, and the trajectory is smooth. In this section, vehicle p with young handling characteristics is analyzed for example. In order to give a clearer description of the position of the vehicle when the two vehicles meet, the local position of vehicles is shown in Figure 20. The large rectangles are used The discrete points of curves in Figure 19 indicate the position of vehicles at different times. During the entire period of time, vehicle p under different handling characteristics have no collision, and the trajectory is smooth. In this section, vehicle p with young handling characteristics is analyzed for example. In order to give a clearer description of the position of the vehicle when the two vehicles meet, the local position of vehicles is shown in Figure 20. The large rectangles are used to express the contour of the vehicle. The discrete points at the same time are connected by the dotted line and the arrow. There is no collision between vehicles in the process of lane exchanging. As shown in Figure 21, the logic threshold controller selects the speed of vehicle due to the existence of a collision threat under the smaller initial distance. The designed fuzzy PI controller can effectively track the velocity. As shown in Figures 22-25, due to the threat of collision, the trajectory re-planning controller needs to re-plan the trajectory to eliminate the collision threat. The peak appears at the moment where the lateral position of the center of mass of the two vehicle approaches. After the peak, with the increase of the lateral distance between the two vehicles, the collision threat is gradually reduced. Due to the coefficient of expansion of the vehicle, the collision threat will take a period of time to completely eliminate. When the collision threat is eliminated, the global controller will track the vehicle trajectory more smoothly, stably and accurately. Due to the different driver characteristics, the vehicle control response is different. Young drivers still maintain the aggressive and sensitive operating characteristics of the steering wheel, making the peak value of the vehicle parameters higher. The lateral acceleration of the vehicle is always less than 0.4g, which ensures the stability of the vehicle.
C. Scenario on smaller initial vehicle distance, and variable velocities of vehicle q. In general, vehicle q will increase the velocity to increase the distance between the rear vehicles as much as possible to reduce the collision threat. Based on the scenario B, in this example, vehicle q is set to use the acceleration of 1 m/s 2 to change lanes, and the controller is simulated and verified. Vehicle p with young handling characteristics is analyzed for example which is more radical and the collision threat is more prominent.   x         is analyzed for example. The large rectangles are used to express the contour of the vehicle. The discrete points at the same time are connected by the dotted line and the arrow. There is no collision between vehicles in the process of lane exchanging. As shown in Figure 28, the logic threshold controller selects the speed of vehicle due to the existence of a collision threat under the small initial distance. The designed fuzzy PI controller can effectively track the velocity. As shown in Figures 29-32, because of the collision threat, the trajectory re-planning controller needs to re-plan the trajectory to eliminate the collision threat. In order to track the reference trajectory produced in real time, the  is analyzed for example. The large rectangles are used to express the contour of the vehicle. The discrete points at the same time are connected by the dotted line and the arrow. There is no collision between vehicles in the process of lane exchanging. As shown in Figure 28, the logic threshold controller selects the speed of vehicle due to the existence of a collision threat under the small initial distance. The designed fuzzy PI controller can effectively track the velocity. As shown in Figures 29-32, because of the collision threat, the trajectory re-planning controller needs to re-plan the trajectory to eliminate the collision threat. In order to track the reference trajectory produced in real time, the The discrete points of curves in Figures 26 and 27 indicate the position of vehicles at different times. During the entire period of time, vehicle p under different handling characteristics has no collision, and the trajectory is smooth. In this section, vehicle p with young handling characteristics is analyzed for example. The large rectangles are used to express the contour of the vehicle. The discrete points at the same time are connected by the dotted line and the arrow. There is no collision between vehicles in the process of lane exchanging. As shown in Figure 28, the logic threshold controller selects the speed of vehicle due to the existence of a collision threat under the small initial distance. The designed fuzzy PI controller can effectively track the velocity. As shown in Figures 29-32, because of the collision threat, the trajectory re-planning controller needs to re-plan the trajectory to eliminate the collision threat. In order to track the reference trajectory produced in real time, the MPC controller has frequent fluctuations in the front wheel steering angle, resulting in frequent fluctuations in vehicle yaw angle and yaw rate. Compared with the constant velocity of vehicle q, the acceleration of vehicle makes the longitudinal distance between vehicles increase, and the collision threat between the two vehicles is relatively reduced. Meanwhile, the collision threat will be eliminated faster. The peak value of the front wheel steering angle of vehicle p is relatively reduced. It can reach a steady state faster. The peak value of the yaw angle, the yaw rate, and the lateral acceleration of vehicle p decreases relatively and reach a steady state faster. The lateral acceleration of the vehicle is always less than 0.4 g, which ensures the stability of the vehicle.
The moving obstacle trajectory prediction method proposed in this paper has little effect on the desired trajectory generated by the controller between the vehicles with real-time information interaction. The prediction information produced by the method proposed is more suitable for trajectory information as obstacles. The logic threshold control method can make the appropriate choice for the vehicle velocity and the controller. The local trajectory re-planning controller can adjust the vehicle state parameters and trajectory desired values when the collision threat occurs between vehicles, so that the vehicle can avoid obstacle under the condition of smooth operation.

Conclusions
In this paper, an integrated avoid collision control framework is proposed for autonomous vehicle trajectory tracking and dynamic collision avoidance. For longitudinal control, a fuzzy adaptive PI controller is proposed for longitudinal velocity tracking. The selection and control of controller and velocity are realized by logical threshold method. For lateral control, a local trajectory re-planning controller based on MPC controller is proposed by collision avoidance control penalty function, which includes many functions such as driver's handling characteristics, information interaction, real-time obstacle trajectory prediction, local trajectory re-planning.
Simulation results show that the designed controllers can re-plan the real-time trajectory when there is a collision threatening, so as to eliminate the collision conflict. During trajectory re-planning, although the fluctuation of the front wheel steering angle and the states increased, the vehicle parameters are always within the constraints, and the vehicle always maintains stability. The trajectory re-planning motion controller can effectively avoid collision and has good stability.
The time-delay is a key issue in control system [39], especially in autonomous vehicle real-time control. In the future research, the delay problems will be considered in the control framework in order to compensate the delays drawbacks.