We rationally simplified the vehicle model (Wu G. 2021) to obtain a three-degree-of-freedom(3-DOF) vehicle dynamics model, as shown in Figure 2, which mainly includes vehicle longitudinal, lateral and yaw motions, where δ f is the front wheel angle, and F X i is the wheel braking force, F Y i is the wheel lateral force, v x is the longitudinal speed, v y is the lateral speed, v is the speed of the vehicle, β is the sideslip angle, l f and l r are the distance of the center of mass and the front and rear axles, l s is the wheelbase, β f is the sideslip angle of the front wheel, T d is the sum of the yaw moment of the vehicle, r is the yaw rate of the vehicle.

The differential equations of motion of the vehicle: ∑ F x i = m v ˙ x − r × v y ∑ F y i = m v ˙ y + r × v x l f F y 1 + F y 2 − l r F y 3 + F y 4 + l s F x 1 + F x 3 − l s F x 2 + F x 4 = T d (1)

So far, the establishment of the 3-DOF model of the vehicle considering the lateral, longitudinal and yaw motions has been completed, and this model will be used to describe the basic characteristics of the vehicle during motion.

In the case of a small vehicle front wheel angle, the relationship between the wheel lateral force and the sideslip angle of this wheel can be approximately regarded as a linear relationship (Wu G. 2021), thus: F Y i = k f β f   i = 1,2 F Y i = k r β r   i = 3,4 (2) Where k f and k r are the cornering stiffnesses of the front and rear axles, respectively. Front and rear wheel sideslip angles, vehicle sideslip angle and their derivatives are: β f = β + l f r v x − δ f β r = β − l r r v x (3) β = v y v x β ˙ = v y ˙ v x − v x ˙ v y v x 2 (4)

Using numerical methods, the path planning problem in discrete space can be abstracted as a quadratic programming problem, and the construction of this problem mainly includes two parts: the design cost function and the determination of the constraints of the problem. In designing the cost function, we need to consider the requirements of smoothness, not deviating from the road centerline and being away from obstacles. At the same time, in order to accelerate the solution, we first use dynamic programming to open up feasible space and therefore determine the constraints of the planning problem. After the problem is constructed, we use the iterative method to solve the quadratic programming problem.

The cost function of path planning can be divided into smoothing cost, reference line cost and obstacle cost. The road is discretized along the centerline and its perpendicular direction, x is the coordinate of the road centerline, and y is the coordinate of the point which is perpendicular to the road.

The smoothing cost C p s m o o t h is divided into three parts, W p s m o o t h 1 , W p s m o o t h 3 , and W p s m o o t h 3 , and their meanings correspond to the cost weights generated by the first, second, and third derivatives of the path: C p s m o o t h = W p s m o o t h 1 y ′ T y , + W p s m o o t h 2 y ″ T y ″ + W p s m o o t h 3 y ″ ′ T y ‴ (5)

The reference line cost is C p r e f , and W p r e f represents the corresponding weight: C p r e f = W p r e f y T y (6)

The obstacle cost is C p c o l l i s i o n , W p c o l l i s i o n represents the corresponding weight, and d represents the distance between the obstacle and the vehicle, where W p c o l l i s i o n is a rather large value. C p c o l l i s i o n = 0   , i f   d ≥ 4 1000 d     , i f   3 < d < 4 W p c o l l i s i o n , i f   d ≤ 3 (7)

Combining the above three formulas, the total planning cost C p n o d e of each discrete point can be obtained, and its value is equal to the sum of the above three costs: C p n o d e = C p s m o o t h + C p r e f + C p c o l l i s i o n (8)

The planned trajectory needs that the curvature is continuous. Δ x is the sampling interval in the direction of the road centerline, y i ′ is the first derivative of y to x at the i-th sampling point, y i ″ is the second derivative of y to x at the i-th sampling point: y i + 1 ′ = y i ′ + y i ″ Δ x + 1 2 y i ″ Δ x 2 + 1 2 y i + 1 ″ − y i ″ Δ x Δ x 2 (9)

The solution of the planning is first to obtain the initial solution by dynamic programming, and then to obtain the final result by secondary programming, as shown in Figure 4. In order to speed up the calculation of the quadratic programming problem, the result of the dynamic programming is regarded as a rough solution and a feasible space is opened up. y max j , y min j is the maximum and minimum value of the feasible space, y min j ′ , y max j ′ is the value range of the path restricted by the road, y _ o b s is the position corresponding to the obstacle car, w i d t h / 2 is the width of the obstacle car: y max j = min y max j ′ , y _ o b s − w i d t h / 2 , i f   y d p _ p a t h > y _ o b s y min j = max y min j ′ , y _ o b s + w i d t h / 2 , o t h e r s (10)

After a lot of tuning, the parameters of the planning algorithm are finally selected W p s m o o t h 1 = 15 ; W p s m o o t h 2 = 20000 ; W p s m o o t h 3 = 5000 ; W p r e f = 15 .

The advantage of Dynamic Programming (DP) is to decompose each column of the discrete space into a sub-problem and solve the optimal path from the last column through the inverse method. On the basis of discretization of the solution space, the initial path can be calculated by using dynamic programming. According to this path, a preliminary decision can be made on the path planning problem to reduce the search range of the quadratic feasible space, as shown in Figure 4. The advantage of the iterative method for solving quadratic programming problems is that it can balance the solution time and effect. This article does not focus on this aspect, so directly call the quadratic planner solution function in MATLAB.

The main purpose of lateral control is to control the lateral error within a certain range. As a result we can get a better track of the desired path and the heading angle of the vehicle. The content of this section is mainly based on the LQR to design the controller to track path. The state variables of the control system are four parameters, including: lateral error e y , rate of change of lateral error e y ˙ , and heading angle error e h , the rate of change of heading angle error e h ˙ . The following formula is the state space equation of the system: e y ˙ ë y e h ˙ ë h = 0 1 0 0 0 1 m u ( k f + k r − 1 m k f + k r 1 m u l f k f − l r k r 0 0 0 1 0 1 I z v x l f k f − l r k r − 1 I z l f k f − l r k r 1 I z v x l f 2 k f + l r 2 k r e y e y ˙ e h e h ˙ + 0 − k f m 0 − a k f I z δ f + 0 1 m v x a k f − b k r − v x 0 1 I z v x a 2 k f + b 2 k r δ f ˙ (11) Where: A = 0 1 0 0 0 1 m v x ( k f + k r − 1 m k f + k r 1 m v x l f k f − l r k r 0 0 0 1 0 1 I z v x l f k f − l r k r − 1 I z l f k f − l r k r 1 I z v x l f 2 k f + l r 2 k r , B = 0 − k f m 0 − a k f I z , C = 0 a k f − b k r m v x − δ f 0 a 2 k f + b 2 k r I z v x (12)

Eq.11 can be expressed as: e r r ˙ = A e r r + B u + C θ r ˙ (13)

Error: e r r = e y e y ˙ e h e h ˙ T (14)

The control quantity is δ f .

From the above state equation, we can get the objective function and corresponding constraints of the LQR controller: Min   J = 1 2 ∫ t 0 t f ( e r r t T Q   e r r ( t ) + u t T R   u ( t ) d t s . t .   e r r ˙ = A e r r t + B u (15) Where t 0 , t f is the time domain, Q and R represent the weighted matrix of state and control quantity.

The optimal solution of this problem satisfies the following: J * = e r r t T P   e r r t (16)

The expression of P is: P ˙ = P t   A t + A T t   P t − P t B t R − 1 t B T t P t + Q t (17)

The LQR controller is: u k = − K t   X t (18) Where K = − R − 1 t B T t   P t represents the controller gain.

In order to trade off the calculation efficiency and calculation accuracy, we assume that the longitudinal velocity v x in the Formula. 1 of the vehicle dynamics model remains unchanged. At this time, the three-degree-of-freedom model of the vehicle is simplified to two-degree-of-freedom. At the same time, it is brought into the Formula. 4, where the longitudinal velocity v x is a time-varying model parameter, and the sideslip angle and the yaw rate are taken as the state quantities, which can be finally simplified to obtain the following state space equation: r ˙ β ˙ = l f 2 k f + l r 2 k r v x I z l f k f − l r k r I z k f l f − k r l r m v x 2 − 1 k f + k r m v x − v x ˙ v x r β + 1 I z 0 T d + − l f k f I z − k f m u δ f (19) Where select x = r β T as the state quantity, and u = T d as the control quantity.

Eq. 9 will calculate the reference values of the vehicle’s yaw rate and sideslip angle. The deviation value is used as the index of vehicle lateral stability, and the larger the value, the greater the risk of lateral instability of the vehicle. r r e f = v x / l f + l r 1 + K v x 2 δ f β r e f = l r + m l f v x 2 / k r l f + k f l r l f + l r 1 + K v x 2 K = m l f + l r 2 l f k r − l r k f (20)

r r e f , β r e f are the reference yaw rate and sideslip angle, respectively, and K is the stability factor.

MPC is a feedback control strategy that discretizes the vehicle dynamics equation, and sets the sampling time as T s = 5 × 1 0 − 4 s . For time k, there is the following discrete equation: x k + 1 = A x k + B u u k + B d d k y k = C x k + D u k (21) Where, A = T s ∗ A c + I , B u = B c u T s , B d = B c d T s , C = C c , D = D c

x k , u k , y k represent the state, control input and output of the system at time k, respectively. Assuming that the prediction time domain is p s , and the control time domain is m s , the control quantity will not change when the time exceeds the control time domain. Then the input and output predicted by time k are: y p k + 1 | k , y p k + 2 | k , . . . , y p k + p s | k (22) u k | k , u k + 1 | k , . . . , u k + p s − 1 | k (23)

The control goal is to track the target and reduce the tracking error, that is, r k + 1 , r k + 2 , . . . , r k + p s (24)

At the same time, the control constraints and output constraints of the system are set. Finally the optimization goal function that can characterize the control performance of the system is proposed. It needs to consider the cost of the expected tracking error and some other performances, such as the control action as small as possible. The objective function is: J y k , U k = ∑ i = 1 p s Γ y r k + i − y p k + i | k 2 + ∑ i = 1 m s Γ u u k + i | k − u k + i − 1 | k 2 (25) Γ y is the weight of the output quantity, and Γ y u is the weight of the control quantity increment. For time k, the open-loop optimization problem is transformed into solving min J x k , U k , m s , p s for the control variable U.

In order to improve the calculation efficiency, the longitudinal velocity v x is assumed to be constant when calculating the state space equation, but in the process of emergency collision avoidance, its longitudinal velocity v x is a time variable. At this time, the internal model of MPC will also change with time, so an adaptive MPC solution method is proposed. The longitudinal velocity and longitudinal acceleration output by the system are fed back to the MPC controller to update the internal model of the controller, which is beneficial to improve controller performance.

After a large number of parameter tuning and system identification, the system can maintain the best performance as much as possible, and the corresponding parameters of the adaptive MPC controller are selected: prediction time domain p s = 10, control time domain m s = 5, and add hard constraints to the control input, Take u min = 4000 N m , u max = 4000 N m , Δ u max = ± 1000 N m , Γ u = 0.02 , Γ y = 1 2 .

So far, the parameter setting of the adaptive MPC controller is completed, and the optimal additional yaw moment Δ M can be calculated to avoid vehicle instability.

The additional yaw moment T d required by the vehicle has been obtained from the MPC controller as shown in Figure 6: T d = ∑ i = 1 4 − 1 i F x i L i (26)

F r i , F f i are the longitudinal forces of the front and rear tires, d r i , d f i are the lateral distances from the front and rear tires to the center of gravity of the vehicle. I represents the left or right side of the vehicle.

At present, the braking force distribution schemes for active braking of vehicles can be roughly divided into two types, single-wheel braking and multi-wheel braking. The braking force provided by the multi-wheel braking scheme is greater than that of the single-wheel braking scheme, but at the same time, the impact generated by the double-wheel braking scheme in the active braking process is also relatively large. In addition, the additional yaw moment generated by the wheel braking scheme is also larger. Therefore, in the face of emergencies, a multi-wheel braking scheme with faster control speed and larger upper limit of additional yaw moment is often adopted.

During the turning, the effects of braking different wheels on the steering dynamic performance are different, and the single-wheel control strategy will select different wheels for control under different vehicle states. If it is a two-wheel braking scheme, when the car is about to flick or understeer, the system will adopt the method of active braking the two wheels on the outer side of the vehicle’s rotation direction at the same time to adjust the body state. The two wheels on the inner side of the steering wheel perform active braking to correct the body condition.

Then, it is necessary to select the most effective wheel to generate T d according to the actual situation. If the front wheels cannot provide enough additional yaw moment, the remaining yaw moment can be generated by the training wheels. In order to design the wheel selection strategy, define the following formula: ƛ = ƛ M ⋅ ƛ γ , ƛ M = s g n T d , ƛ γ = s g n r (27)

As shown in Figure 7, the wheel selection strategy is proposed based on ƛ M and ƛ γ . In Figure 7 [left front, right front wheel, rear left and right rear wheel], when the value in the vector is set to 1, corresponding tire brakes, when the value in the vector is set to 0, tire don’t brake, 0|1 said whether need the auxiliary brake wheel brake, for example, if the right front wheel braking does not produce enough yawing moment, we need to brake the right rear wheel to generate enough yaw torque. If the vehicle is oversteering (ƛ<0), the priority braking wheels are the front outer wheels of the vehicle. When the vehicle understeers (ƛ>0), the priority braking wheel is the rear inner wheel. Once the yaw moment provided by the front wheels is not enough, the remaining yaw moment can be generated by the selected auxiliary wheels.

Finally, according to the selected braking wheels, the distributed four-wheel braking force can be obtained from Formula. 26.

Without the stability control, the planning algorithm and the lateral control algorithm as shown in Figures 9, 10 perceive that there is a slow-moving car at a speed of 10 km/h 40 m ahead, and then start to perform lane change to avoid obstacles. After about 2 s, the ego vehicle avoids obstacles, and in about 6 s, the heading angle gradually stabilizes near 0, but the yaw angle speed stabilizes slowly. At this time, the longitudinal velocity v x of the ego vehicle remains basically unchanged.

The tracking performance can be represented by the change of lateral error (The distance between the vehicle and its projection to the planned path) with time, as shown in the following Figure 11.

The stability control intervenes within 2–4 s after the vehicle avoids the obstacle as shown in Figures 12, 13. At this time, the change of the yaw rate is quickly suppressed. At the same time, it can be seen that the heading angle stabilizes rapidly to 1° in 2.5 s. But there is a certain heading angle error. The reason why 2–4 s is chosen is due to the consideration of stability control on path tracking accuracy. Firstly, before avoiding obstacles (that is, before 2 s, this time is calculated by TTC), the stability control has a certain impact on obstacle avoidance, and may even lead to accidents, which should be avoided as much as possible. Secondly, if the stability control intervention time is too long or the stability control intervenes when the yaw angle is large, the lateral control algorithm cannot control the heading angle error to zero. This will cause the planning control algorithm to be unable to track the path stably according to the road centerline after the lane change. The change of tyre braking force with time as shown in Figure 14.

On the premise of keeping other planning parameters consistent, we changed W p s m o o t h 2 from 1000 to 20,000 for a total of 20 tests (indicated by different colored trajectories). It can be seen that our algorithm successfully avoided the obstacle car ahead under the condition of considering the smooth trajectory as shown in Figure 15, which proves the effectiveness of our algorithm.