Automatic parking trajectory tracking control strategy on LQR and PID

. This study selects two degree of freedom dynamic model as the research object, and the automatic parking process is analyzed. The longitudinal cascade controller of speed PID and position PID is used to control the longitudinal motion of the vehicle. The lateral tracking controller based on LQR optimal theory is adopted, and the feedforward control is added to form the optimal feedforward LQR controller as the lateral tracking controller. Based on the co-simulation platform of MATLAB/Simulink and CarSim, the function of the designed automatic parking trajectory tracking control strategy is verified. The car test was completed. The ultrasonic sensor, raspberry pie and Arduino were used as the main parts to build an intelligent car test platform to simulate the performance of the designed tracking control strategy in the real vehicle. Software simulation results and vehicle test results showed that the designed control strategy can ensure the path tracking performance of vehicle under normal parking speed, and has good tracking accuracy, real-time performance and vehicle driving stability.


Introduction
In recent years, scholars at home and abroad have carried out a lot of fruitful research work in the field of automatic parking motion control, which constantly promotes the automatic parking system into practical application [1]- [7] .Tracking control consists of lateral motion and longitudinal motion control [8] [9] .Lateral tracking control focuses on the control of vehicle steering system to make the vehicle travel along the planned expected trajectory and minimize the lateral deviation between the trajectory and the expected trajectory.Longitudinal tracking control focuses on controlling the vehicle's driving speed to keep the vehicle's speed or drive according to the expected speed, so as to reduce the impact of speed mutation.
Reference [10] takes the relative position of parking space and vehicle as reference, and timely adjusts according to the current relative position, which effectively solves the problem of parking failure caused by narrow parking space.
Reference [11] also uses the tracking geometry algorithm to meet the requirements of parking in a narrow space.Reference [12] combines pure tracking model with fuzzy control method.It makes the vehicle follow the path more quickly with less error, and can adapt to various complex paths.Reference [13] and reference [14] have studied on the basis of pure tracking algorithm and inertial navigation method, which shortens the feasible parking distance and improves the parking speed.In reference [15], the heading error is determined according to the kinematic relationship between vehicle and road, and PID controller and feedback system are used as the path tracking control method.And the vehicle can track the desired path when the speed is fast.Both reference [16] and reference [17] use PID controller to track the path, and optimize the signal processing, such as filtering, inverse processing, so as to make the whole control process more stable.
Reference [18]by analyzing the behavior of real drivers in the parking process, LQR feedback controller is used to reduce the deviation between the actual parking trajectory and the expected trajectory.
In reference [19], LQR optimal control is used to realize the closed-loop control of the linear system, which ensures the stability and fast convergence of the system.
Reference [20] [21] have designed the model predictive controller for automatic park path tracking, which improves the control accuracy, reduces the parking time, and greatly improves the ride comfort and the anti-interference ability of the system.
In reference [22], the path tracking controller of predictive control model is improved to enhance the control robustness of the controller and reduce the error caused by external disturbance in the process of path tracking.
Reference [23] uses model predictive control to track the parking path, which improves the parking control accuracy to a certain extent.In reference [24], a MPC based model predictive controller was designed and applied to parking trajectory tracking, but it can only be applied to simple vertical parking conditions.
In this paper, the longitudinal speed control of the automatic parking track tracking control adopts PID control, and the LQR optimal control is used for the lateral control of automatic parking track tracking.The combination of LQR and PID can ensure the control accuracy and reduce the complexity of calculation caused by advanced control methods.

Vehicle trajectory tracking control model
A two degree of freedom bicycle model is established to analyze the dynamic characteristics of the vehicle, as shown in Figure 1.The dynamic equations of vehicle lateral motion and yaw motion are as follows: Where, yf F , yr F is lateral force of front and rear wheels(N); xf F , xr F is longitudinal force of front and rear wheels(N); f l , r l is distance from the center of mass to the front and rear axles(m); m is Vehicle mass(kg); y v is lateral velocity (m/s); r is Yaw rate(deg/s); Z I is moment of inertia(kg.m2).
Assuming that there is no Sideslip in the process of vehicle movement, the front and rear wheel sideslip angle The relationship between the lateral force and the lateral deflection angle can be obtained: Where, f  , r  are side slip angle of front and rear wheels(rad); f c , r c are lateral stiffness of front and rear wheels (N/rad); Take the vertical distance of the expected trajectory of the vehicle as the lateral tracking error, as shown in Figure 2.
Yaw angular velocity and lateral acceleration can be expressed by desired trajectory curvature and lateral velocity: The lateral tracking error and heading error at the centroid are: By combining formula (4) (5) (6), we can get: State space expression of vehicle trajectory tracking dynamics model: 2 Longitudinal tracking control strategy

Longitudinal speed control
Automatic parking belongs to high-precision low-speed movement.Generally, the speed of the whole parking process does not exceed 5km / h, but the change of speed will seriously affect the parking accuracy.Therefore, in order to make the vehicle track the desired path well, it is necessary to accurately control the vehicle speed and control the error of the speed within a reasonable range.The longitudinal speed control is that the controller calculates the error of the expected speed and the actual feedback speed to get the speed and acceleration to be compensated, and then adjusts the throttle and braking control quantity to keep the vehicle speed within the error range of the expected speed.
The longitudinal controller of automatic parking can track the desired speed by controlling acceleration and deceleration, that is, controlling the vehicle's brake and throttle to control the speed.According to the difference between the expected speed and the actual speed, the acceleration and deceleration control quantity is calculated, and the acceleration and deceleration control command is output to the corresponding actuator.
As shown in Figure 3, the longitudinal control system diagram is mainly composed of two PID controllers and a calibration table, i.e. position PID controller and speed PID controller.The calibration table calibrates the acceleration and deceleration commands corresponding to different speeds and accelerations, and then looks up the table according to the control needs.The process of the longitudinal control system is: when the path planning is completed, the longitudinal control system obtains the current position of the vehicle, the information of the vehicle itself and the information of the planned path from the perception system and the planning system.The control system finds the reference point closest to the current position of the vehicle on the planned expected path, which is called the matching point.By calculating the parameters related to the longitudinal error, the position error is judged to be within the allowable range, and the speed compensation is calculated according to the current position error, and the speed input of the speed acceleration controller is calculated.Judge whether the speed error is in the range, and calculate the acceleration compensation.According to the calculated speed acceleration table, the current throttle / brake control value is obtained, and the vehicle longitudinal tracking is completed.
The calculation of longitudinal error is the key to the accuracy of longitudinal control.The longitudinal error includes velocity error and position error, as shown in Figure 4.
The position error is: The speed error is: Where, p V is plan vehicle speed, V is actual vehicle speed,   is the vehicle heading error, k Is the coefficient.
According to the calculated position error and speed error, the PID controller is used to compensate the error to get the required speed and acceleration.The interpolation calculation is carried out in the calibrated brake throttle calibration table to get the operation to be performed.

Simulation experiment of longitudinal control
The longitudinal control model is built by using Matlab/Simulink, as shown in Fig. 5.The longitudinal control model is simulated under four working conditions to verify the longitudinal control effect.
It can be seen from Figure 5 that the actual displacement and the expected displacement curve fit basically during the whole motion process, and the tracking error is very small.It can be seen from Figure 6 that the velocity will oscillate slightly in 0-5s, and then it will follow the desired velocity quickly after 5s and continue to track well.From the simulation results of condition one, the longitudinal controller can control the vehicle according to the planned position, acceleration and speed, and the control effect is good, and the error is within the acceptable range.Mode 2: S=-30m,T=30s, as shown in Fig. 7 to Fig. 8.According to figure 7, the actual displacement and the expected displacement curve fit basically during the whole motion process, and the tracking error is very small.It can be seen from Figure 8 that the velocity will oscillate slightly in 0-4s, and then follow the expected velocity rapidly after 4s, with the maximum velocity of 1.9m/s.From the simulation results of condition 2, the longitudinal controller can control the vehicle according to the planned position, acceleration and speed, and the control effect is good, and the error is within the acceptable range.Mode 3: S=-45m,T=30s, as shown in Fig. 9 to Fig. 10.As can be seen from Figure 9, the actual displacement and the expected displacement curve basically fit during the whole motion process, and the tracking error is very small.It can be seen from Fig. 10 that there will be a small oscillation in 0-3s, and the desired speed will be followed quickly after 3s, with the maximum speed of 2.7m/s and good tracking.From the simulation results of condition 3, the longitudinal controller can control the vehicle according to the planned position, acceleration and speed, and the control effect is good, and the error is within the acceptable range.The error is within the acceptable range.
It can be seen from Figure 11 that the actual displacement and the expected displacement curve fit basically during the whole motion process, and the tracking error is very small.It can be seen from Figure 12 that there will be a small oscillation in 0-3s, and after 3s, it will quickly follow the expected speed, with the maximum speed of 3.5m/s and continue to track well.From the simulation results of condition 3, the control effect of longitudinal controller is good, and the error is acceptable.Automatic parking is generally a low-speed movement within 5km/h.In this paper, the longitudinal tracking controller of automatic parking is verified under four working conditions, which basically covers the speed range in the process of parking.The longitudinal tracking error is shown in Table 1.The simulation results of four working conditions show that the control performance of the designed longitudinal tracking controller meets the requirements, and it can track the desired displacement and speed well.
3 Lateral tracking control strategy

Lateral control principle
Based on the optimal LQR (linear quadratic regulator) control theory, a lateral controller for automatic parking tracking control is designed.
The optimal feedback control rate is determined to minimize the system performance index: Where, matrix element and the numerical value, The index of controlled system is chosen, ) (t Q The value of matrix elements determines the response characteristics of the system.The larger the value is, the faster the response is.
The value of matrix element determines the amplitude of control quantity.The value larger is, the more stable the control is and the less energy consumption is.It can be seen from this, It is a group of mutually constrained parameters.In order to have a better control effect of the control system, we need to get the best parameter settings according to the system.
Constructing Hamiltonian function: Derivation of the formula above: Let the above equation be equal to 0, the optimal control signal is obtained: where, is obtained by the following formula: ) (t P It is the solution of Riccati differential equation:

T T T T P t Q A P t A A P t B R B P t B B P t A
When t approaches  , ( ) 0 P t  we get: This equation is called Riccati equation.The matrix P is obtained by solving the equation, and then the optimal control rate is obtained by iterative operation: where, Gain for LQR controller.
The state space expression of vehicle path following dynamics model is simplified as the following formula: where, m is vehicle quality.f c , r c are the front and rear wheel cornering stiffness.f l , r l are distance from the center of mass to the front and rear axles.I is the moment of inertia of the vehicle.x v is longitudinal speed.
The block diagram of LQR lateral control system is shown in Figure 13.The working process is as follows: after receiving the vehicle position information, body parameter information and path planning information from the planning and sensing system, the state space equation is obtained based on the vehicle dynamics model, and its parameter matrix is calculated; Then the K matrix is solved to calculate the optimal control quantity, which is sent to the actuator for operation.According to the designed lateral tracking control controller, it is controlled by . The steady-state error state equation of the system can be obtained by introducing equation ( 18): It can be seen from equation (20) (21) that no matter what the value of K is, because r C  Is nonzero, rr e  and rr e It is impossible to be 0 at the same time, Otherwise, there will be steady lateral error and yaw error.Therefore, in order to eliminate the steady-state yaw angle error of the system, a feedforward control is added to the optimal LQR control algorithm, which makes the lateral control more accurate and the lateral control quantity is: After the introduction of feedforward control, the vehicle tracking dynamic model becomes the following equation: After the system is stable, , rr e Can be expressed as: The purpose of the track following lateral control system is to select an appropriate front wheel angle so that equation is zero as far as possible: Current feed control angle f  When a specific value is taken, the lateral error can converge to zero, and the feed-forward angle is zero: It can be seen that no matter what the feed-forward angle is, the heading angle error is not zero and the heading angle error is zero: The course angle error is known  e It is not affected by f  ,k.As shown in fig.14, it is the structure diagram of the feedforward LQR transverse control system, which is mainly composed of a feed-forward and a feedback control.The feed-forward control part mainly calculates the feedforward control amount according to the curvature of the desired trajectory, and inputs the calculation results to the steering control; In the feedback control part, the state space equation is constructed by analyzing the vehicle dynamics model, and the optimal control quantity is calculated by LQR optimal control theory, and the calculated results are fed back.

Simulation experiment of lateral control
According to the design of the optimal LQR controller as the lateral controller of the automatic parking system path tracking, the controller is optimized, and the feedforward control is added to form the feedforward optimal LQR controller.The lateral controller is built by using Simulink, and three kinds of paths are designed to simulate and analyze the lateral tracking controller model, so as to verify whether the lateral tracking controller can track the desired path and keep a small lateral error.
Path 1: as shown in Figure 15 and Figure 16, it is the track diagram of test path 1 designed to verify the lateral control algorithm.The blue path is the designed expected path, and the red path is the vehicle tracking path.It can be seen that the vehicle motion path basically coincides with the designed path.
As shown in Figure 16, the result of path tracking lateral error shows that the error reaches 0.06m in 0-5s, and then the error remains at 0.06m without any change.The error of the whole tracking process does not exceed 0.06m, which meets the requirements of tracking accuracy.Path 2: as shown in Figure 17 and Figure 18, it is the track diagram of test path 2 designed to verify the lateral control algorithm.The blue path is the designed expected path, and the red path is the vehicle tracking path.It can be seen that the vehicle motion path basically coincides with the designed path.Figure 18 shows the lateral tracking error of path 2. The lateral error will increase rapidly in the places with great curvature change.However, the error of the whole process is controlled within 0.1M, most of which is near 0.05m, which basically meets the requirements of transverse tracking accuracy.
Path 3: as shown in Figure 19 and Figure 20, it is the test path designed to verify the lateral control algorithm.
According to the track diagram of path 3 shown in Figure 19, the blue path is the designed expected path, and the red path is the vehicle tracking path.It can be seen that the error of the vehicle will increase in some positions with large curvature changes, but the expected path is followed in general.Figure 20 shows the lateral tracking error of path 3. The lateral error will increase rapidly where the curvature changes greatly, but the whole process error is controlled within 0.08m, which basically meets the requirements of lateral tracking accuracy.Under the condition of constant speed, the designed feedforward LQR transverse controller has good tracking effect on three paths, and the lateral error is basically controlled at a small value.As shown in Table 2, although curvature optimization is not considered for the designed path, the lateral tracking controller can still track in case of curvature mutation, It is proved that the designed lateral controller has good performance.

Co simulation and real vehicle test
Combined with the longitudinal tracking controller based on double PID and the transverse tracking controller based on optimal feedforward LQR, the transverse and longitudinal controller is controlled comprehensively, and the function of the control strategy determined in this paper is verified.Matlab/Simulink and CarSim are used as the co simulation platform for simulation.Whether the simulation results can meet the function of trajectory tracking control is analyzed, and the intelligent car verification platform is built to verify the control strategy.

Parallel parking
As shown in Figure 21, the longitudinal tracking error is shown.It can be seen from the figure that the maximum longitudinal error during parking is -0.02m to-0.025mdue to the large change of speed when the vehicle starts.The longitudinal error after that is kept between -0.05-0.01,and converges near 0 finally, it can be seen that the longitudinal tracking performance of the vehicle is good.Figure 22 shows the transverse tracking error diagram.It is shown that the maximum lateral error occurs in the direction of vehicle driving for parking in the whole parallel parking process, and the error is about 0.08m.In other processes, it is found that the lateral tracking error performance of the vehicle is good.

Single step vertical parking
The longitudinal tracking error is shown in Figure 23.It can be seen from the figure that the maximum longitudinal error is between -0.02m and -0.05m in the parking process due to the large speed change when the vehicle starts.The later longitudinal error converges to around 0.02 and finally converges to around 0. It can be seen that the longitudinal tracking performance of the vehicle is good.Figure 24 shows the diagram of lateral tracking error.It can be seen from the diagram that the maximum lateral error in the whole parallel parking process occurs when the vehicle is parking in the direction of driving, and the error is about 0.08m.In other processes, the error is controlled within 0.03m.It can be seen that the performance of vehicle lateral tracking error is better.

Multi step vertical parking
The longitudinal tracking error is shown in Figure 25.It can be seen from the figure that the maximum longitudinal error is 0.06m in the parking process due to the large change of steering angle when the vehicle is making steering adjustment.The subsequent longitudinal error converges to around 0.02 and finally converges to around 0. It can be seen that the longitudinal tracking performance of the vehicle is good.Figure 26 shows the diagram of lateral tracking error.It can be seen from the diagram that the maximum lateral error in the whole parallel parking process occurs when the vehicle is parking in the direction of driving, and the error is about 0.03m.In other processes, the error is controlled within 0.03m.It can be seen that the performance of vehicle lateral tracking error is better.The parallel parking, single step vertical parking and multi-step vertical parking are simulated and analyzed respectively.The simulation tracking error is shown in Table 3.

Car test verification
The test verification shows as Figure 27 and Figure 28.
Figure 27 shows the comparison between the expected path of parallel parking and the actual path.It can be seen from the figure that the planned path of parallel parking basically coincides with the actual path, and the error is controlled within 0.02m.
Figure 28 shows the comparison between the expected path and the actual path of vertical parking.It can be seen from the figure that the planned path of parking basically coincides with the actual path, and the error is controlled within 0.05m.

Conclusion
The automatic parking track tracking controller is designed to study the tracking control strategy of the automatic parking track.(1) In the low speed and large angle, the vehicle motion is controlled by the two degree of freedom dynamic model, which can improve the control accuracy of the automatic parking vehicle.(2) The general automatic parking track tracking controller is designed.The cascade controller composed of position PID and speed PID is used as the longitudinal control part of track tracking controller, and the optimal feedforward LQR controller is used as the lateral control part of track tracking controller.The effectiveness of the controller is simulated and verified by four working conditions and three paths.

e
Is the lateral deviation of the expected path from the vehicle centroid,  e Is the heading angle deviation of the vehicle, Input for front wheel angle,  Is the desired yaw rate.The matrix A, matrix B and matrix C of the state space expression are:

( 3 )
The simulation results of parallel parking and vertical parking are carried out by using Matlab/Simulink and CarSim.The simulation results show that the tracking control strategy can effectively track the planning trajectory.The experiment shows that the trajectory tracking control strategy designed in this paper is feasible and can be used in different parking modes.

Table 1 .
Longitudinal speed tracking error table.

Table 2 .
Path lateral tracking error table.

Table 3 .
Track tracking error table.