Research on Path Planning and Tracking Control of Automatic Parking System

Abstract

In this paper, we focus on the parking path planning and path tracking control under parallel parking conditions with automatic parking system as the research object. In order to solve the problem of discontinuity of curvature in the path planning of traditional arc-straight combined curve, a quintic polynomial is used to smooth the path. we design a path tracking controller based on the incremental model predictive control (MPC). The preview control based on pure tracking algorithm is used as the comparison algorithm for path tracking. The feasibility of the controller is verified by building a Simulink/CarSim co-simulation platform. In addition, the practicality of the parking controller is further verified by using the ROS intelligent car in the laboratory environment.

1. Introduction

With the improvement of living standards, the role of cars has gradually changed from a luxury to a necessity of people’s daily lives. Parking spaces are getting smaller and smaller, the parking environment is complex and changeable, the driver level is uneven, and other factors all affect the success rate of parking; a little carelessness may lead to the occurrence of parking accidents.

At present, the domestic and foreign researchers mainly adopt the following methods for automatic parking system research: (1) parking method based on intelligent control. Hanafy et al. used artificial fish swarm algorithm and bee swarm algorithm to optimize the parameters of the fuzzy controller, obtain the time required for parking, and optimize the performance of the parking controller [1]. Chengrui Zhang designed the control strategy of the automatic parking system based on the fuzzy control theory and verified the feasibility of the designed strategy through the Simulink/CarSim co-simulation and the vehicle controller built on dSPACE [2]. Demirli designed an Adaptive Neuro-Fuzzy Inference System. The vehicle does not need to know the desired polynomial path to track, but it will approximate such a way by knowing only the start configuration [3]. Huang proposed a model-free intelligent, self-organizing fuzzy controller for parking path tracking. This intelligent controller has a system-learning mechanism without expert knowledge or a trial-and-error process [4]. (2) parking method based on path planning and path tracking. Kim et al. proposed a Reeds-Shepp curve path planning that takes the minimum turning radius of the vehicle and the straight line from the starting point to the target point as the shortest path. Aiming at the problem that the path formula of the Reeds-Shepp curve is limited in the actual parking lot, the curve algorithm is modified, so as to realize the effective forward and backward automatic parking system [5]. Hao Ye designed a linear time-varying path tracking prediction through a series of transformations such as discretization and Taylor’s formula expansion [6].

MPC was proposed in the 1970s. It is an advanced control method that is usually used to control a process with constraints. It is a heuristic control algorithm applied in the industrial process and has rich theoretical and practical contents [7,8,9]. Model predictive controllers depend on models of the process, which are often linear models. There should be desirable properties of approximation accuracy, physical interpretation, suitability for control, and ease of development for a good model [10]. MPC is a typical approach in industrial advanced control. It has been widely used in various domains, such as supply chain management in semiconductor manufacturing [11], application to autoclave composite processing [12], energy efficiency control in buildings [13], integrated wastewater treatment systems [14], flight control [15], and magnetic spacecraft attitude control [16].

Compared with the parking system designed by intelligent algorithm, the parking system designed by path planning and path tracking has clear and simple design logic and can easily achieve global optimization. It is easy to integrate the pre-designed control strategy into the parking controller and facilitate the use of real cars. Therefore, it is the most studied automatic parking scheme at present. Hence, in this article, the parking path is planned based on the vehicle kinematics model and an automatic parking path tracking based on the incremental model predictive control (MPC). In order to improve the safety and reliability of the parking process, the article will analyze and study the path decision planning and path tracking control algorithms of the automatic parallel parking system.

2. Parking Path Planning

Parking path planning is essentially based on on-board sensors to obtain environmental information, combined with the kinematic constraints, vehicle parking environmental constraints, initial position of motor vehicles, and parking regulations. It is generated by a parking starting point into the parking space target preset position as the end point of the path curve, which is to meet the conditions of curvature, obstacle avoidance, and ease of tracking. Figure 1 is the basic structure of path planning in this paper.

2.1. Kinematic Model of Vehicle

The structural parameters of the vehicle, such as vehicle length, vehicle width, wheelbase, and front and rear suspension, have an important influence on the design of parking path planning and trajectory tracking controllers. In addition, the maximum steering wheel angle (the maximum steering wheel angle) and the maximum rotating speed of steering wheel Angle (the maximum rotating speed of steering wheel angle) also have a great relationship with the design of automatic parking system. The smaller the minimum turning radius of a vehicle, the smaller the required parking space size. This paper takes a certain model as the research object and simplifies the vehicle shape.

Table 1. Vehicle structure parameters.

Parameter	Symbol	Numerical Value	Unit
Vehicle length	L	4.7	m
Vehicle width	W	1.86	m
Wheelbase	l	2.71	m
Front overhang	lf	0.89	m
Rear overhang	lr	1.1	m
Maximum front wheel angle	${\delta }_{f_{max}}$	0.482	rad
Front wheel angle maximum speed	${\omega }_{f_{max}}$	0.482	rad/m

shows the vehicle structure parameters.

Because the parking speed is very low, there is no need to consider the vehicle’s handling, stability, and other dynamics issues. The tire side sliding is ignored, and the wheels only do rolling and steering [17]. As shown in Figure 2, the motion trajectory of the midpoint of the rear axle is used to represent the motion state of the vehicle. ${\left[X,Y,\phi \right]}^T$ is used to represent the pose state of the vehicle at each moment. ${\left[v,{\delta }_f\right]}^T$ is used to represent the control quantity of the target vehicle to establish the kinematics model, and its kinematics equation is as follows [18]:

$$\left[\dot{\begin{array}{c}X\\ \dot{Y}\\ \dot{\phi }\end{array}}\right]=\left[\begin{array}{c}\begin{array}{c}\cos \phi \\ \sin \phi \end{array}\\ \tan {\delta }_f/l\end{array}\right]v$$

(1)

where, X and Y are the horizontal and vertical coordinates of the midpoint of the rear axle of the target vehicle (m); φ is the heading Angle of vehicle (°/s); δ_f is equivalent front wheel Angle $\left(°/\mathrm{s}\right)$; l is wheelbase (m); v is rear axle midpoint velocity (km/h).

2.2. Parking System Analysis

The speed of the vehicle is very low when parking. According to the above hypothesis, there is no wheel slip, so the speed at the center point of the rear axle in the direction of the rear axle is zero. Through analysis and calculation, the trajectory equation of the center point of the rear axle can be obtained:

$$\{\begin{array}{l}X\left(t\right)=l\cot {\delta }_f\sin \left(v_f\frac{\sin {\delta }_f}{l}t\right)\\ Y\left(t\right)=-l\cot {\delta }_f\cos \left(v_f\frac{\sin {\delta }_f}{l}t\right)+l\cot {\delta }_f\end{array}$$

(2)

Further observation of Equation (2) shows that:

$$X^2\left(t\right)+{\left(Y\left(t\right)-l\cot {\delta }_f\right)}^2={\left(l\cot {\delta }_f\right)}^2$$

(3)

According to Equation (3), when the equivalent front wheel rotation angle ${\delta }_f$ is fixed, the trajectory of the center point of the rear axle is only related to the wheelbase of the vehicle and has nothing to do with the reversing speed $v_f$. The size of the reversing speed will not change the shape of the reversing trajectory. Given the front wheel rotation angle and speed, the reversing trajectory is strictly a circle [19]. According to the geometrical relation shown in Figure 2, the mathematical expression of the four vertex coordinates and the midpoint coordinates of the rear axle of the car body can be calculated.

Point A coordinates:

$$\{\begin{array}{l}{X}_A=X+\frac{W}{2}\sin \phi -l_r\cos \phi \\ Y_A=Y-\frac{W}{2}\cos \phi -l_r\sin \phi \end{array}$$

(4)

Point B coordinates:

$$\{\begin{array}{l}{X}_B=X+\left(L-l_r\right)\cos \phi +\frac{W}{2}\sin \phi \\ Y_B=Y+\left(L-l_r\right)\sin \phi -\frac{W}{2}\cos \phi \end{array}$$

(5)

Point C coordinates:

$$\{\begin{array}{c}{X}_C=X+\left(L-l_r\right)\cos \phi -\frac{W}{2}\sin \phi \\ Y_C=Y+\left(L-l_r\right)\sin \phi +\frac{W}{2}\cos \phi \end{array}$$

(6)

Point D coordinates:

$$\{\begin{array}{c}{X}_D=X-\frac{W}{2}\sin \phi -l_r\cos \phi \\ Y_D=Y+\frac{W}{2}\cos \phi -l_r\sin \phi \end{array}$$

(7)

As shown in Figure 3, we take the traditional arc-straight-arc path curve as an example to calculate the initial parking area that satisfies a series of constraints [20]. In order to reduce the length of parking space as much as possible and facilitate calculation, this paper sets the radius of two arcs equal to the minimum turning radius of the center of the rear axle of the vehicle, namely $R_1=R_2=R_{min}$. Then, the driving track of P3P4 segment of the circular arc is the reverse process of P1P2 segment of the circular arc. After the positions of P3 and P4 are determined according to the constraint conditions, the coordinate of P1 position can be obtained only by adjusting the length of the straight line P2P3, and then the range of the starting point of parking can be calculated.

Through analysis and calculation, the lower limit value of P1 coordinate is P_1min($x_{P_{1min}}$,$y_{P_{1min}}$) = (8.076, 3.335). The upper limit value is P_1max($x_{P_{1max}}$,$y_{P_{1max}}$) = (10.981,4.588). The upper limit value of P2 coordinate is P_2max($x_{P_{2max}}$,$y_{P_{2max}}$) = (9.246,$4.23$), and parking starting point area is as follows:

$$\{\begin{array}{l}y=\tan \theta \left(x-x_{P_{1max}}\right)+y_{P_{1max}}\\ y=y_{P_{1min}}& ,x_{P_{1min}}\le x\le x_{P_{1max}}\\ y=y_{P_{1max}}\end{array}$$

(8)

$$\{\begin{array}{c}y=y_{P_{1min}}\\ y=y_{P_{2max}}\end{array} ,x_{P_{1max}}\le x\le 12.5$$

(9)

The starting point area of parking enclosed by Equations (8) and (9) is shown in the shaded part in Figure 4.

2.3. Parallel Parking Path Planning Based on Quintic Polynomial

For parallel parking path planning, the category of the fitting path curve needs to be determined first. In this paper, we hope that the curvature of the planned path is continuous, which requires the path to be second-order continuous. The output of the parking system is the steering angle of the front wheels, and the steering angle is controlled by the power-assisted motor. Due to the motor steering performance constraints and the inertial characteristics of the steering system, the steering angle of the wheels cannot change suddenly, which requires the planned path to be three-order continuous. Therefore, if a polynomial is used to fit the parking path, the curve is at least a fourth-degree polynomial.

The following requirements must be met when parking: the curvature of the path curve is continuous to prevent the vehicle from turning in place; the curvature of the starting and ending points of the path curve is zero to ensure that the vehicle body attitude is parallel to the parking space at these two points; the steering wheel at the end of parking is in the state of returning to normal. It can be seen from

Table 2. Comparison of advantages and disadvantages of different path curves.

Path Curve Type	Advantage	Disadvantages
Double arc combination Arc-straight arc combination	The path structure is simple, easy to implement, and the size of the required parking space is appropriate	The curvature is discontinuous and it is difficult to track; there are higher requirements for the parking starting point
B-spline curve Cloth curve	Continuous curvature, easy to track	The path structure and expression are complex, and the amount of calculation is large;
Arctangent function curve	The curve expression is easy to calculate, the curvature is continuous and the curvature change rate is continuous	The curvature of the starting point and ending point of the path is not zero, which does not meet the pose requirements
Quintic polynomial curve	The curve expression is simple, the curvature is continuous and the curvature change rate is continuous, and the calculation amount is small	Properly increase the length of the parking space to ensure that the end point of the path has zero curvature

that the curve that can meet these requirements and has a simple structure of the path curve and a small amount of calculation is a fifth-order polynomial curve.

In this paper, combining the path planning constraints analyzed in the previous section and comparing the advantages and disadvantages of various path curves, a quintic polynomial is selected for path smoothing processing, and its expression is shown in Equation (10) [21].

$$y\left(x\right)=a_5{x}^5+a_4{x}^4+a_3{x}^3+a_2{x}^2+a_{1}x+a_0$$

(10)

where a₀∼a₅ are the coefficients of a quintic polynomial, and changing the coefficients can change the shape of the polynomial.

Substituting vehicle position constraints at the starting and ending points, heading angle constraints, steering stability constraints, and obstacle avoidance constraints into Equation (10), the unique path equation can be determined, which is the desired parallel parking path curve. The simulation program was written in MATLAB, and the simulation results are shown in Figure 5.

As can be seen from Figure 5, the planned path curve is a continuous curve connecting the starting and ending points of parking. The four vertices of the target vehicle body contour, A, B, C, and D, all meet the requirements of obstacle avoidance and reach the termination point without collision from their respective starting points. In other words, the parallel parking path curve using quintic polynomial programming can meet the constraint requirements. The curvature of the path curve meets the requirement that the curvature is less than the maximum curvature determined by the vehicle structure parameters, and the curvature is continuous without abrupt change during the whole parking process. Derivatives at the starting point and the ending point are approximately equal to zero, which meets the requirement that the vehicle body attitude is parallel to the horizontal direction of the parking space. In the whole parking process, the equivalent front wheel keeps rotating. The angle and speed are constantly changing to meet the constraints. Therefore, the parking path curve programmed by quintic polynomial has good tracking performance, and can satisfy the vehicle environment constraints, pose constraints, and vehicle kinematics constraints.

3. Motion Control of Path Tracking Based on MPC

In this section, the model predictive control algorithm is selected to track the parking path designed in the previous section [22].

3.1. Design of Parking Path Following Controller

In order to realize path tracking motion control effectively, a path tracking controller based on incremental model predictive control (MPC) is designed in this paper, and the preview control based on pure tracking algorithm is used as the comparison algorithm for path tracking. The kinematics model of Equation (1) is discretized, and $\chi ={\left[x,y,\phi \right]}^T$ and $u={\left[v,{\delta }_f\right]}^T$, then the system can be regarded as a control system with an input of $u\left(v,{\delta }_f\right)$ and state quantity of $\chi \left(x,y,\phi \right)$. For a given reference trajectory, Taylor’s formula is used to expand, and higher-order terms are ignored, and the following can be obtained:

$$\dot{\chi }=f\left({\chi }_r,u_r\right)+\frac{\partial f\left(\chi ,u\right)}{\partial \chi }{|}_{\begin{array}{c}\chi ={\chi }_r\\ u=u_r\end{array}}\left(\chi -{\chi }_r\right)+\frac{\partial f\left(\chi ,u\right)}{\partial u}{|}_{\begin{array}{c}\chi ={\chi }_r\\ u=u_r\end{array}}\left(u-u_r\right)$$

(11)

By making a difference between Equation (11) and the general form, a linearized parking vehicle error model can be obtained, which is further discretized and used as the input of MPC. The formula after discretization is as follows:

$$\tilde{\chi }\left(k+1\right)=A_{k,t}\tilde{\chi }\left(k\right)+B_{k,t}\tilde{u}\left(k\right)$$

(12)

where, $A_{k,t}=\left[\begin{array}{ccc}1& 0& -v_r\sin {\phi }_{r}T\\ 0& 1& v_r\cos {\phi }_{r}T\\ 0& 0& 1\end{array}\right]$; $B_{k,t};=\left[\begin{array}{cc}\cos {\phi }_{r}T& 0\\ \sin {\phi }_{r}T& 0\\ \frac{\tan {\delta }_{fr}T}{l}& \frac{v_{r}T}{l{\cos }^2{\delta }_{fr}}\end{array}\right]$; $T$ is the sampling time.

Considering the rapidity and stability of path tracking when parking, the deviation of system state quantity and control quantity need to be reflected in the objective function, so the objective function is designed as follows:

$$J\left(k\right)={\displaystyle \sum }_{i=1}^{N_p}\Vert \eta \left(k+i|t\right)-{\eta }_{ref}\left(k+i|t\right){\Vert }_Q^2+{\displaystyle \sum }_{i=1}^{N_c-1}\Vert ∆U\left(k+i|t\right){\Vert }_R^2+\rho {\epsilon }^2$$

(13)

where $\rho$ represents the weight coefficient; $\epsilon$ denotes relaxation factor; $N_p$ represents the prediction time domain; $N_c$ represents the control time domain; $Q$ and $R$ are weight matrices. The first item on the right side of the equal sign reflects the tracking ability of the controlled system to the reference path, and the last item reflects the change of the constraint control quantity of the controlled system.

In the control process of path tracking of parking system, some constraints need to be considered, which are mainly the limit control quantity constraint and control increment constraint, such as the constraint of front wheel rotation angle. The constraint expression of parking system control quantity is:

$$u_{min}\left(t+k\right)\le u\left(t+k\right)\le u_{max}\left(t+k\right),k=0,1,\cdots ,N_c-1$$

(14)

The corresponding system control increment constraint expression is as follows:

$$∆u_{min}\left(t+k\right)\le ∆u\left(t+k\right)\le ∆u_{max}\left(t+k\right),k=0,1,\cdots ,N_c-1$$

(15)

3.2. Mulation and Result Analysis of Parallel Parking

The quintic polynomial path curve planned in the path planning part was taken as the reference target curve, and the model predictive control algorithm was used to track the curve, and the comparison with the preview control algorithm was made. Figure 6 shows the parking scene in the CarSim software in the joint simulation test of parallel parking conditions.

As can be seen from the above figure, the blue vehicle successfully finished parking in the co-simulation experiment. In MPC controller, parameters such as prediction time domain, control time domain, and relaxation factor are important parameters in controller design. We comprehensively consider the calculation time and calculation ability of the controller. Through literature review and actual testing, the basic parameters of the MPC controller designed in this paper are shown in

Table 3. MPC controller basic parameters.

Prediction Horizon $\left({\mathit{N}}_{\mathit{P}}\right)$	Control Horizon $\left({\mathit{N}}_{\mathit{C}}\right)$	Sampling Period	Simulation Time	Relaxation Factor (Row)	Weight Matrix (Q)	Weight Matrix (R)
60 s	30 s	0.1 s	40 s	10	100	5

:

The simulation results of parallel parking are shown in Figure 7:

As can be seen from Figure 7a, the target vehicle can track the target path well under the control of both the MPC algorithm and the preview control algorithm, and the MPC algorithm has a better tracking effect than the preview control algorithm. In the process of vehicle tracking the desired path controlled by the two algorithms, there is no collision between the vehicle and the vehicle with obstacles before and after the target parking space and the obstacle avoidance requirements are met simultaneously. As can be seen from Figure 7b, the maximum ordinate error of path tracking based on MPC algorithm is 0.09638 m, while that based on preview control algorithm is 0.08602 m. Although the maximum ordinate error based on MPC is greater than that based on preview control, the overall error curve shows that the change of ordinate error based on MPC is smoother than that based on preview control. As can be seen from Figure 7c, at the end point of parking, the yaw angle of the vehicle tracked by the two control algorithms approaches 0, which meets the vehicle attitude requirements at the end point of parking. From the whole curve, the degree of fit between vehicle yaw angle and yaw angle of the desired path based on MPC tracking is higher than that based on preview control. As can be seen from Figure 7d, the maximum yaw error based on MPC is 0.02527 rad, and the yaw error at the parking termination point is −0.0013 rad, so the yaw tracking accuracy is high. The maximum yaw error based on the preview control is 0.026 rad, and the yaw error at the end point of parking is 0.0113 rad. The yaw tracking accuracy is less than that of the model predictive control. As can be seen from

Table 4. Parking position error.

Control Algorithm	$\mathbf{∆}{\mathit{y}}_{\mathit{e}}\left(\mathbf{m}\right)$	$\mathbf{∆}\mathit{y}\mathit{a}{\mathit{w}}_{\mathit{e}}\left(\mathbf{r}\mathbf{a}\mathbf{d}\right)$	${\mathit{y}}_{\mathit{m}\mathit{a}\mathit{x}}$	$\mathit{y}\mathit{a}{\mathit{w}}_{\mathit{m}\mathit{a}\mathit{x}}$
MPC controller	−0.018	0.0053	0.09638	0.02527
Preview controller	0.039	0.0072	0.08602	0.026

∆y_e: the average of ordinate error; ∆yaw_e: the average of yaw error; y_max: the maximum ordinate error; yaw_max: the maximum yaw error.

, the absolute value of ordinate error based on MPC is 0.018 m, while that based on preview controller is 0.039 m. And the average of yaw error based on MPC is less than that based on preview controller. To sum up, the path tracking algorithm designed by model predictive control is better than the path tracking algorithm designed by pure tracking algorithm.

4. Parking Experiment of Intelligent Car

In order to verify the actual control effect of the control model and directly observe the test results, this paper carried out parallel parking experiments based on the industrial computer (Mini Intel i3) and the ROS intelligent car. Since the MPC algorithm is based on Simulink and cannot be used directly for the car, the MPC algorithm was rewritten in C++ and implemented on an industrial computer to support the following experiments.

Table 5. Basic parameters of intelligent car.

Parameters	Numerical Value (mm)
Vehicle length	495
Vehicle width	290
Front overhang	115
Rear overhang	40
Wheelbase	340

shows the basic parameters of the smart car [23].

Firstly, the path tracking control algorithm based on MPC built by Simulink is duplicated in IPC by C++ language. Then, the serial port function of the ROS system is used to transfer the calculated expected angle and speed from the IPC to the control module with STM32F103RCT6 as the central chip. Finally, the control module transmits the control signal to the driving motor and steering gear, so as to control the transverse and longitudinal motion of the car. The basic structure of the intelligent car parking test platform built in this paper is shown in Figure 8.

Combined with the structural parameters of the intelligent car and according to the method of calculating the minimum parking size in the previous section, the minimum parallel parking size is determined to be 84 cm × 35 cm and the lane width to be 45 cm. After several experiments, the successful experimental results were recorded, as shown in Figure 9. It can be seen from the figure that the designed path tracking controller based on MPC can successfully control the car model parking and warehousing, and the car does not collide with the lane line and parking space line in the parking process, which meets the requirements of obstacle avoidance.

5. Conclusions

This paper focuses on parallel parking path planning and tracking control. Firstly, a vehicle kinematic model is built to solve for the parking start point region, and path planning is performed by using the quintic polynomial. Then, we designed a path tracking controller based on incremental model predictive control (MPC). And the preview control based on pure tracking algorithm is used as the comparison algorithm for path tracking. The simulation results show that the designed path tracking controller can better control the vehicle tracking the desired path. Finally, we use the ROS smart car in a laboratory environment to verify the actual control effect of the control model and directly observe the test results.

6. Future Research Directions

The above experimental results verify the feasibility of the parking system, and the future research directions are as follows.

• Regarding parking conditions, this article only studies parallel parking conditions. For vertical parking conditions and parking with irregular parking spaces, further research is needed.
• Regarding the experiment, due to the lack of conditions, this article only uses the smart car to carry out the parking experiment. In the future, the designed controller needs to be verified on the actual vehicle.
• Regarding the development of automatic parking technology, automatic valet parking technology is an inevitable trend in the development of automatic parking technology in the future.