Time-optimal global trajectory planning for autonomous valet parking: An improved hybrid A-star algorithm-based optimization control approach

Abstract

Autonomous valet parking is an L4-level self-driving technology that solves the “last-mile freedom” of automobile users. Aiming at the motion planning problem involved in the decision-making layer of autonomous valet parking, this article proposes a trajectory planning method that obtains global time optimization by solving the optimization problem. The method is based on the path points obtained by the improved hybrid A-star algorithm as the reference waypoints. The global trajectory planning problem is segmented. In each section, the physical system constraints of the vehicle, the boundary condition constraints, and obstacle avoidance constraints in the parking process are comprehensively considered, and the parking trajectory planning task is described as an optimal control problem. The optimal problem is transformed into a nonlinear programming problem by the numerical optimization method. Finally, the global optimal trajectory is obtained on the basis of ensuring the continuity of state variables and control variables. Simulation experiments verify the effectiveness of the algorithm for horizontal and vertical parking. The real vehicle test shows that the vehicle can be safely, quickly, and accurately parked in the parking space.

Introduction

With the rapid development of automotive intelligence, networking technology, and the application of advanced sensors, the vehicle parking system has evolved from assisted parking in the early stage and fully automatic parking in the middle stage to L4 autonomous valet parking (AVP). AVP is an autonomous driving in a specific area scenario, and the driver can send instructions through the mobile terminal device to complete the process of parking or picking up the car. AVP can greatly save parking time, improve parking efficiency, solve the parking safety problem caused by lack of driving experience, and realize the “last mile” of unmanned driving. Therefore, AVP is an indispensable part of autonomous driving technology for engineering application scenarios.

Motion planning is one of the key technologies to realize AVP. The main task of motion planning is to generate a geometric path or trajectory connecting the starting point and destination of the vehicle, which is divided into path planning and trajectory planning. The latter is solved in the state space based on path planning, and the results include time-related parameter information of the vehicle traveling along the geometric path. Therefore, the chassis actuator can directly track this trajectory, so it can be considered that the results of trajectory planning are more comprehensive than those of path planning.

Many scholars and research institutions at home and abroad have carried out a lot of research on the motion planning problem of autonomous vehicles. The research results can be roughly divided into five categories. Geometric line construction methods, such as Dubins curve, ¹ Reeds-Shepp curve, ² the curve composed of multiple arcs and straight lines,^3,4 polynomial fitting curve, ⁵ and cyclotron curve. ⁶ The planning results using geometric line construction methods are mostly paths, and further smoothing is required. Graph search methods need to be solved by graph theory on the basis of binary processing of the map. For example, Dijkstra algorithm, ⁷ improved A * algorithm, ⁸ Jump Point Search method, ⁹ and Hybrid A * algorithm ¹⁰ proposed in 2010 (DARPA) city challenge. Random sampling methods mainly include the probabilistic road map (PRM) method and the rapid-exploration random tree method. The PRM algorithm cannot deal with complex constraints such as differential equations existing in vehicle motion, resulting in poor feasibility of PRM generation path. ¹¹ The rapid-exploration random tree algorithm is suitable for solving motion planning problems with differential equation constraints. ¹² The solution quality of this method is unstable and cannot directly deal with motion planning tasks with complex constraints. With the development of artificial intelligence technology, intelligent algorithms are widely used in vehicle motion planning, such as genetic algorithm, ¹³ particle swarm optimization algorithm, ¹⁴ Monte Carlo ¹⁵ method, and motion planning based on you only look once deep learning model.^16,17 The computational cost of intelligent optimization algorithms is high, and most of them are used in offline optimization. The fifth category of methods is numerical optimization methods. The general idea of the numerical optimization method is to transform the vehicle motion planning problem into a nonlinear optimal control problem with an objective function. By using numerical optimization algorithms to solve the optimization control problem and obtain planning results. Martinsen et al. ¹⁸ combined the graph search method with the convex optimization method to obtain the vehicle trajectory. Qin et al. ¹⁹ divided vehicle trajectory planning into two stages: linear programming and nonlinear programming (NLP) to obtain accurate solutions. Liu et al. ²⁰ established the dynamic optimization problem of vertical parking and used the two-stage Gaussian discrete method to solve the partial differential equation to obtain the final trajectory. The biggest advantage of this method is that the planning result is a trajectory rather than a path. Due to the complexity of the algorithm's description of constraint conditions, it is more suitable for local planning rather than global planning.

In the AVP process, the vehicle travels far away, and the significant feature of global trajectory planning is that the scene is large, so that there are many nontarget obstacle parking spaces to be considered. If numerical optimization is directly used to solve the trajectory, the complex constraint conditions lead to an extremely low convergence success rate. If the geometric line method, or the graph search method, or the probability graph method is used, although the planning can be solved in a short time, the planning result is the path. The geometric path information needs to be further converted into time parameters such as vehicle speed and steering wheel angle for the trajectory tracking of the chassis actuator. Therefore, it is of great significance for the development of AVP system to directly solve the optimal trajectory of vehicle motion in AVP process.

In this article, a method to obtain the global time optimal trajectory of AVP by directly solving the optimal control problem was proposed. First, an improved hybrid A-star algorithm was adopted to obtain the global path of the AVP process. The improved hybrid A-star algorithm could ensure that the global path solving process is fast and accurate. Second, the AVP process was segmented according to the global path. In each segment, the shortest parking time was taken as the optimization objective, and the numerical optimization method was used to solve the optimal trajectory. Of course, the continuity of vehicle state variables and control variables between different segments should be guaranteed. The global path can be used not only as a basis for segmentation but also as an initial reference for numerical optimization to address the defect that numerical optimization methods are sensitive to initial values. Finally, the segmented trajectories are combined into a global optimal trajectory.

The advantage of the segmentation processing is that it reduces the constraint cost that needs to be considered when solving the segmented trajectory. At the same time, the global path based on the improved hybrid A-star solution is used as the initial reference for the numerical optimization solution, which greatly improves the convergence rate of the numerical optimization process. It overcomes the defect that the numerical optimization method is usually applied to local planning. Through the method proposed in this article, the numerical optimization can be applied to the one-time solution of the global optimal trajectory of the AVP process.

AVP scene and global trajectory planning process description

In this article, a medium-sized parking lot is chosen as the scene for verifying the algorithm, with vertical parking spaces evenly distributed around the edges and in the middle of the parking lot, as shown in Figure 1(a). The entrance of the parking lot is chosen as the starting position when the driver opens the AVP system, assuming that the parking lot has been transformed by intelligent facilities and the vehicles can communicate with the intelligent parking system (V2I), and that the free parking spaces can be detected by the parking space sensing system and assigned to the vehicles entering the parking lot. The process of global trajectory planning for AVP can be divided into two phases after specifying the target parking space. In order to identify the two phases more intuitively, the three-dimensional view of the parking lot is converted to top view, as shown in Figure 1(b). The first stage is for the vehicle to automatically move from the entrance of the parking lot to the optimal starting point for vertical parking, namely the A–B section, which is also called the navigation planning stage, and the motion planning in this stage includes the two steps of the navigation path planning and the navigation trajectory planning as mentioned above, and the second stage is for the vehicle to park in the parking space from the optimal starting point for vertical parking, namely the B–C section, which is also called the vertical parking planning stage, and the motion planning in this stage includes the parking trajectory planning as mentioned above.

Figure 1.

Medium-sized parking lot. (a) Aerial view of parking lot and (b) diagram of valet parking process.

After receiving the target parking space number sent by the intelligent parking system of the parking lot, the AVP system first needs to clarify the position of the A–B and B–C segment correlation points, that is, the optimal starting point of the vertical parking stage. After determining the position of the associated points, an improved hybrid A-star algorithm, a graph search algorithm with kinematic constraints, is applied in the navigation planning stage to generate the globally optimal path from the entrance of the parking lot to the endpoint of the navigation stage. The global path is segmented and used as a reference route point. The local optimal trajectory planning problem is transformed into a nonlinear optimal control problem in each segment, and it is solved iteratively by the numerical optimization method. In the vertical parking planning stage, the time optimal trajectory is obtained directly by solving the optimal control problem. Throughout the global trajectory planning of AVP, the termination time of each trajectory planning in the A–B segment and the starting time of the next planning, as well as the termination time of the A–B segment and the starting time of the B-C segment, must meet the following endpoint constraints: the vehicle's X and Y direction coordinates, body heading angle, vehicle speed, and steering wheel rotation angle must be consistent, so as to ensure that the optimal trajectories of the two stages are seamlessly connected to form the final global time optimal trajectory.

Kinematics model

The vehicle kinematics model is the basis of the search tree expansion in the hybrid A-star algorithm and the trajectory calculation in the numerical optimization algorithm. Therefore, a correct vehicle kinematic model is a powerful guarantee for the accuracy of global path planning. Considering that the search tree in the hybrid A-star algorithm can be expanded into two scenarios: circular arc and straight line, the corresponding vehicle kinematic model can also be divided into two scenarios. As shown in Figure 2(a), when expanding the circular arc based on the center point of the rear axle $M(x,y)$ , the change in the vehicle's heading angle $\mathrm{\Delta }\theta$ and the instantaneous turning radius R can be expressed as follows:

$$\{\begin{array}{c}\mathrm{\Delta }\theta =s\cdot \tan (\phi )/L\\ R={\displaystyle \frac{s}{\mathrm{\Delta }\theta }}\end{array}$$

(1)

where s is the search tree expansion step size in the hybrid A-star algorithm, L is the vehicle wheelbase, and $\phi$ is the front wheel angle at the current moment. The center position corresponding to the turning radius can be obtained from the current moment position $M(x,y)$ of the vehicle and the instantaneous turning radius R. The equation is as follows:

$$\{\begin{array}{c}{O}_x=M_x-R\cdot sin(\theta )\\ O_y=M_y+R\cdot cos(\theta )\end{array}$$

(2)

where $O_x$ and $O_y$ represent the positions of the instantaneous turning center in the $x$ and y directions, respectively. The vehicle position $M^{\mathrm{\prime }}(x,y)$ and heading angle ${\theta }^{\mathrm{\prime }}$ for the next search step can be obtained from equation (2), and the solution equation is as follows:

$$\{\begin{array}{c}{M}_{\mathrm{x}}^{{}^{\mathrm{\prime }}}=O_x+R\cdot sin(\theta +\delta \cdot \mathrm{\Delta }\theta )\\ M_{\mathrm{y}}^{{}^{\mathrm{\prime }}}=O_{\mathrm{y}}-R\cdot cos(\theta +\delta \cdot \mathrm{\Delta }\theta )\\ {\theta }^{\mathrm{\prime }}=\theta +\delta \cdot \mathrm{\Delta }\theta \end{array}$$

(3)

where $\delta$ represents the direction of vehicle travel, with forward travel being 1 and backward travel being −1. As shown in Figure 2(b), when the straight line is extended, the next search step vehicle position $M^{\mathrm{\prime }}(x,y)$ and heading angle ${\theta }^{\mathrm{\prime }}$ can be directly obtained by solving the current time position $M(x,y)$ and search step size. The equations are as follows:

$$\{\begin{array}{c}{M}_x^{{}^{\mathrm{\prime }}}=M_x+\delta \cdot s\cdot \cos (\theta )\\ M_y^{{}^{\mathrm{\prime }}}=M_y+\delta \cdot s\cdot \sin (\theta )\\ {\theta }^{\mathrm{\prime }}=\theta \end{array}$$

(4)

Figure 2.

Vehicle kinematic model. (a) Arc expansion and (b) straight line expansion.

Based on the above kinematic model, the optimal position and heading angle for the next search step can be planned using the hybrid A-star algorithm. This model only contains the geometric relationships between position, heading angle, step size, and steering wheel and is not suitable for trajectory planning process to solve the optimal objective function. Therefore, the above kinematic model will be detailed. Vehicle AVP process is low-speed driving, the tires are always pure rolling state, and there is no side slip, ignoring the tire lateral force and other dynamics. Only the vehicle's kinematic characteristics are considered. According to the Ackermann steering principle, the detailed kinematic model of the vehicle can be refined into the following kinematic differential equations:

$${\displaystyle \frac{d}{dt}\left[\begin{array}{c}x(t)\\ y(t)\\ v(t)\\ a(t)\\ \theta (t)\\ \phi (t)\end{array}\right]=\left[\begin{array}{c}\begin{array}{c}v(t)\cdot cos\theta (t)\end{array}\\ v(t)\cdot sin\theta (t)\\ a(t)\\ j(t)\\ v(t)\cdot tan\phi (t)/L\\ \omega (t)\end{array}\right],t\in [t_0,t_f]}$$

(5)

where $v,a,j$ , respectively, represent the vehicle speed, acceleration, and acceleration change rate. $\omega$ is the front wheel angular velocity. $t_0$ and $t_f$ are the time intervals for trajectory solving. In Figure 2, f, $r$ , d represent the front overhang, rear overhang, and width of the vehicle, respectively. These size parameters can be used to calculate the position of the contour vertices of the vehicle in order to better coordinate obstacle collision avoidance strategies.

Navigation phase path planning based on improved hybrid A-star algorithm

The hybrid A-star algorithm is an effective fusion of search tree method and A-star algorithm. It retains the classic heuristic idea of A-star and considers the limitations of the vehicle kinematic model when expanding the search tree nodes, that is, satisfying the maximum curvature constraint of the vehicle. Expanding two-dimensional search to three-dimensional space can plan continuous vehicle position and attitude changes in a discrete grid.

The traditional hybrid A-star algorithm adopts a fixed-step node expansion method in each expansion direction, and its search effect is greatly affected by the number of expansion directions and the search step size. When the expansion direction is more or the search step is smaller, the search time is longer and the efficiency is lower. When the expansion direction is less or the search step is larger, the path is rough, the smoothness is not enough, and the possibility of collision with obstacles increases. In order to better balance search efficiency and path quality, this article proposes a new subnode expansion method while optimizing the motion cost function and collision detection method. It adopts a variable step size node expansion method, which can ensure the smoothness and security of the searched path and greatly improves the efficiency of path search. Meanwhile, the optimized motion cost function and collision detection method prevent nodes from expanding along unnecessary regions or directions, further improving the real time and security of path search.

Grid occupation map and resolution description

The node extension of the hybrid A-star algorithm is based on a grid map, using 0 and 1 to distinguish whether the smallest grid unit in the map is occupied by obstacles, where 1 indicates the presence of obstacles in the pointed grid unit, and 0 indicates that the grid unit is idle. Therefore, the higher the resolution of the map, the smaller the basic grid units, and the more accurately the obstacle occupancy information in the scene can be expressed. The map resolution can be determined using the following equations:

$$\{\begin{array}{c}{\xi }_x={\displaystyle \frac{L_x}{N_x}}\\ {\xi }_y={\displaystyle \frac{L_y}{N_y}}\end{array}$$

(6)

where $L_x$ and $L_y$ are the lengths of the parking lot map in the x and y directions, $N_x$ and $N_y$ are the number of grids in the x and y directions, and ${\xi }_x$ , ${\xi }_y$ are the corresponding resolutions in the x and $y$ directions. In the hybrid A-star algorithm, the minimum grid cell requirement is a square, therefore, ${\xi }_x={\xi }_y=\xi$ . Due to satisfying the vehicle kinematic constraints during node expansion, the vehicle motion observation points can be extended in the grid map, as shown in Figure 3. To ensure that only one parent node can exist in any basic grid cell, the search step must be greater than or equal to the square root of two times the resolution of the map.

Figure 3.

Vehicle motion diagram.

Extension method for variable step length child nodes

The first step in expanding child nodes is to determine the number $N_{\phi }$ of node expansion directions. The angle range [ $-{\phi }_{max},{\phi }_{max}$ ] of the front wheel is uniformly dispersed, and the expansion direction of $N_{\phi }$ nodes can be obtained. To ensure that the vehicle can move forward or backward in a straight line, $N_{\phi }$ must be an odd number and the number of front and rear extension directions must be the same. The front wheel steering angle corresponding to each extension direction can be calculated by the following equation:

$$\{\begin{array}{c}{\phi }_i=-{\phi }_{max}+(i-1)\cdot {\displaystyle \frac{2\cdot {\phi }_{max}}{N_{\phi }-1}}\\ i=1,\cdots ,N_{\phi }\end{array}$$

(7)

When expanding forward, the step sizes in each direction are not equal, as shown in Figure 4. The specific step size can be calculated using the following equations:

$$s=\{\begin{array}{c}{s}_0\cdot (i+1)/2,1\le i\le (N_{\phi }+1)/2\\ s_0\cdot (N_{\phi }+2-i)/2,i>(N_{\phi }+1)/2\end{array}$$

(8)

Figure 4.

Number of node expansion directions $N_{\phi }$ .

In the equation, $s_0$ is the basic expansion step size of the search tree. Due to the significant energy and time cost consumption caused by reversing, the step sizes in all directions are set to $s_0$ when expanding backwards. By combining the above expansion equations with the kinematic model equations (1) to (4), the position and heading angle of each simulation step, as well as the expanded nodes in each expansion direction, can be obtained.

Obstacle collision detection method

The actual top view projection shape of the vehicle is close to a rectangle. In the navigation planning stage, the environment is relatively open, and its contour does not need to be accurately described. The multicircle expansion can be used to describe the vehicle contour, as shown in Figure 5. Compared with the extremely accurate description, the computational complexity is reduced, and it can be well used for the mathematical expression of collision detection.

Figure 5.

Multicircle expansion description of vehicle contours.

Among them, $O_1,O_2,O_3,O_4$ , each has four center points. $L_1$ is the distance from the center point $O_1$ to the rear of the vehicle, $L_5$ is the distance from the center point $O_4$ to the front of the vehicle, $L_2$ , $L_3$ and $L_4$ are the distances between $O_1{O}_2$ , $O_2{O}_3$ and $O_3{O}_4$ , respectively. $R_{\mathrm{i}}$ is the radius of the outer circle of the expansion circle. The graph parameter relationship is described as follows:

$$\{\begin{array}{c}{L}_1=L_5={\displaystyle \frac{1}{8}(r+L+f)}\\ L_2=L_3=L_4={\displaystyle \frac{1}{4}(r+L+f)}\\ R_i=\sqrt{L_1^2+{(d/2)}^2}\end{array}$$

(9)

Combining the above parameter relationships and the current position $M(x,y)$ of the vehicle, four center positions can be obtained from the following equations:

$$\{\begin{array}{c}[O_{1x},O_{1y}]=[x+(L_1-r)\cdot \cos \theta ,y+(L_1-r)\cdot \sin \theta ]\\ [O_{2x},O_{2y}]=[x+(L_1+L_2-r)\cdot \cos \theta ,y+(L_1+L_2-r)\cdot \sin \theta ]\\ [O_{3x},O_{3y}]=[x+(L+f-L_4-L_5)\cdot \cos \theta ,y+(L+f-L_4-L_5)\cdot \sin \theta ]\\ [O_{4x},O_{4y}]=[x+(L+f-L_5)\cdot \cos \theta ,y+(L+f-L_5)\cdot \sin \theta ]\end{array}$$

(10)

The corresponding collision processing is performed on the grid units occupied by obstacles in the map to better perform collision detection. The condition that there is no collision between the vehicle and the obstacle is that there is no overlap between the vehicle contour circle and the obstacle contour circle, and the constraint condition can be expressed by equation (11). Collision detection is performed on each vehicle pose obtained by the expansion. If the vehicle collides with an obstacle at that node pose, stop expanding the node toward the outer layer in that direction to ensure that the path between each child node and the parent node is collision free.

$$\begin{array}{r}(O_{ix}-O_{ox}{)}^2+(O_{iy}-O_{oy}{)}^2\ge (R_i-R_o{)}^2,i=1,\cdots ,4\end{array}$$

(11)

Improved motion cost function

The overall objective function of the hybrid A-star includes two parts: the motion cost function g and the heuristic function h. The motion cost function $g(M_i)$ from the starting point to the current node $M_i$ can be expressed as follows:

$$\begin{array}{c}g(M_i)=\sum _{j=1}^{i}cost(M_{j-1},M_j)\end{array}$$

(12)

Among them, $cost(M_{j-1},M_j)$ is the motion cost for each step of expansion. In order to make the path obtained from the search allow the vehicle to keep straight as much as possible, reduce unnecessary turns and backward movement, and ensure the smoothness of the path, this article considers adding penalties for driving curvature, backward movement, and direction switching in the cost function. The specific representation is as follows:

$$\begin{array}{rl}cost(M_{j-1},M_j)& =\left(\left({\displaystyle \frac{\delta +1}{2}}\right)\cdot {\omega }_f+\left({\displaystyle \frac{1-\delta }{2}}\right)\cdot {\omega }_r\right)\\ & \times \left(s+\left|{\displaystyle \frac{1}{R}}\right|\right)+{\lambda }_{switch}\cdot |{\delta }_j-{\delta }_{j-1}|/2\end{array}$$

(13)

In the equation, ${\omega }_f$ and ${\omega }_r$ are the cost weights for forward and backward movement, and ${\lambda }_{switch}$ is the cost weight for switching between forward and backward driving directions to reduce frequent forward and reverse changes in the planned path. The overall objective function is as follows:

$$\begin{array}{c}f(M_i)=g(M_i)+\max (h_1(M_i),h_2(M_i))\end{array}$$

(14)

where $h_1$ is a nonholonomic constraint heuristic function that only considers motion without considering obstacles, using Reeds Shepp to reach the target point at a cost, and $h_2$ is a heuristic function that only considers obstacles on the map without considering kinematic constraints. Using the distance value between the current node and the target node, the maximum value of the two is selected as the final heuristic function. The former avoids vehicles reaching the target from the wrong direction, and the latter prevents vehicles from wasting time on dead ends or U-shaped obstacles.

Trajectory planning based on numerical optimization methods by solving optimal control problems

Through the improved hybrid A-star algorithm, the global optimal path in the navigation stage can be obtained, and this path satisfies the vehicle kinematic constraints. However, the executing mechanism of the vehicle chassis cannot directly track this path, and the path needs to be further optimized to obtain the corresponding trajectory, such as torque or throttle opening, vehicle speed or acceleration, front wheel angle or front wheel angular velocity, which can directly control the vehicle's parameter information. The specific method of global trajectory planning proposed in this article is to use the above path as a reference waypoint, segment the reference waypoint, and describe the trajectory planning problem as a time-optimal nonlinear control problem within each segment. The optimal trajectory is obtained by solving the optimal control problem using the numerical optimization method. The advantage of segmented solution lies in the ability to divide the previously complex global parking optimal trajectory into multiple stages of parking trajectory solving. The obstacle collision avoidance constraints and endpoint constraints during each stage of solving are simplified, thereby improving the success rate of trajectory solving and the real-time performance of the algorithm.

Description of the optimal control problem for AVP trajectory planning

The vehicle motion trajectory planning problem can be described as an optimal control problem through an accurate mathematical model. The optimal control problem generally includes the dynamic differential equation, constraint conditions, and performance index function of the controlled system. The constraint conditions include equality constraints and inequality constraints. The optimal control parameters are obtained by solving the performance index function when the controlled system reaches the minimum value under the constraint. Without loss of generality, the nonlinear optimal control Bolza problem for AVP trajectory planning can be described as:

$$min\psi =\mathrm{\Phi }\left(x(t_0),t_0,x(t_f),t_f+\underset{t_0}{\overset{t_f}{\int }}g(x(t),u(t),t)dt\right)$$

(15)

$$\begin{array}{rl}& \mathrm{s}.\mathrm{t}.\{\begin{array}{c}Differentialequationsofvehiclekinematics(5)\\ Inequalityconstraints(19),(20),(21),(22)\\ Equalityconstraints(23)\end{array}\\ & \in [t_0,t_f]\end{array}$$

(16)

Inequality constraints

When solving the trajectory problem of AVP, inequality constraints usually include vehicle collision avoidance constraints, vehicle physical constraints, and boundary condition constraints. Different from the rough description of the vehicle itself in the global path planning process, the description of the vehicle in the local trajectory planning process needs to be more precise to absolutely avoid collisions. Figure 6 shows the vehicle collision avoidance strategy during the AVP process. The actual overhead projection shape of the vehicle is close to a rectangle. Here, an enlarged rectangle is used to describe the vehicle contour, as shown in the Figrue 6. $d_{safe}$ is the safety threshold to ensure that the vehicle does not collide, and the coordinates of the four contour vertices of the enlarged vehicle can be obtained by equation (17).

$$\left[\begin{array}{c}Ax(t)\\ Ay(t)\\ Bx(t)\\ By(t)\\ Cx(t)\\ Cy(t)\\ Dx(t)\\ Dy(t)\end{array}\right]=\left[\begin{array}{c}x(t)+(L+f+d_{safe})cos\theta (t)-\left({\displaystyle \frac{d}{2}+d_{safe}}\right)sin\theta (t)\\ y(t)+(L+f+d_{safe})sin\theta (t)+\left({\displaystyle \frac{d}{2}+d_{safe}}\right)cos\theta (t)\\ x(t)+(L+f+d_{safe})cos\theta (t)+\left({\displaystyle \frac{d}{2}+d_{safe}}\right)sin\theta (t)\\ y(t)+(L+f+d_{safe})sin\theta (t)-\left({\displaystyle \frac{d}{2}+d_{safe}}\right)cos\theta (t)\\ x(t)-(r+d_{safe})cos\theta (t)+\left({\displaystyle \frac{d}{2}+d_{safe}}\right)sin\theta (t)\\ y(t)-(r+d_{safe})\sin \theta (t)-\left({\displaystyle \frac{d}{2}+d_{safe}}\right)cos\theta (t)\\ x(t)-(r+d_{safe})cos\theta (t)-\left({\displaystyle \frac{d}{2}+d_{safe}}\right)sin\theta (t)\\ y(t)-(r+d_{safe})\sin \theta (t)+\left({\displaystyle \frac{d}{2}+d_{safe}}\right)cos\theta (t)\end{array}\right]$$

(17)

As shown in the Figure 6, in order to ensure that the vehicle is always driving on the road, it does not collide with the vehicle in the parking space. In the A–B stage, each parking space is regarded as an obstacle alone, and in the B-C stage, the remaining parking spaces other than the target parking space are regarded as obstacles. In order to reduce the number of obsacles to be considered in trajectory planning, simplify the collision avoidance constraints, and improve the efficiency of the algorithm, only the parking spaces within the circular range in the graph are considered as obstacle parking spaces in each trajectory planning process. The two-stage collision avoidance method is consistent, that is, if the car does not break into the obstacle parking space and collide with other vehicles, the geometrical significance of this constraint in the two-dimensional plane is: then at any moment, no point on the obstacle parking space falls within the contour of the enlarged self-vehicle rectangle. Taking the two obstacle parking spaces $p_{i1}{p}_{i2}{p}_{i3}{p}_{i4}$ and $p_{j1}{p}_{j2}{p}_{j3}{p}_{j4}$ in the figure as examples, if the vehicle does not collide with points $p_{i1}{p}_{i2}{p}_{i3}{p}_{i4}$ and $p_{j1}{p}_{j2}{p}_{j3}{p}_{j4}$ during AVP, then the coordinates of the obstacle parking spaces $p_{i1}{p}_{i2}{p}_{i3}{p}_{i4}$ and $p_{j1}{p}_{j2}{p}_{j3}{p}_{j4}$ are always outside the rectangular ABCD area. The four sides of the rectangle are represented as straight lines $l_{AB}$ , $l_{BC},l_{CD}$ and $l_{AD}$ . The linear equations are as follows:

$$\{\begin{array}{c}{l}_{AB}=k_1\cdot x+y_1\\ l_{CD}=k_1\cdot x+y_2\\ l_{AD}=k_2\cdot x+y_3\\ l_{BC}=k_2\cdot x+y_4\end{array}$$

(18)

In the equations, $k_1$ is the slope of lines $l_{AB}$ and $l_{CD}$ , and $k_2$ is the slope of lines $l_{AD}$ and $l_{BC}$ . When the straight line $l_{AB}$ is parallel or perpendicular to the x axis, if the point $p_{mn}$ on the obstacle parking space is outside the rectangle ABCD, the coordinates of the 8 points must meet the following conditions:

$$\{\begin{array}{c}{p}_{mn}(x)⟨D_x(t)OR{p}_{mn}(x)⟩A_x(t)OR{p}_{mn}(y)⟨C_y(t)OR{p}_{mn}(y)⟩A_y(t),\theta =0\circ \\ p_{mn}(x)⟨D_x(t)OR{p}_{mn}(x)⟩C_x(t)OR{p}_{mn}(y)⟨C_y(t)OR{p}_{mn}(y)⟩A_y(t),\theta =90\circ \\ p_{mn}(x)>D_x(t)OR{p}_{mn}(x)⟨A_x(t)OR{p}_{mn}(y)⟩C_y(t)OR{p}_{mn}(y)<A_y(t),\theta =180\circ \\ p_{mn}(x)>D_x(t)OR{p}_{mn}(x)⟨C_x(t)OR{p}_{mn}(y)⟩C_y(t)OR{p}_{mn}(y)<A_y(t),\theta =270\circ \end{array}$$

(19)

When the straight line $l_{AB}$ is not parallel or perpendicular to the x axis, if the point $p_{mn}$ on the obstacle parking space is outside ABCD, the coordinates of the point $p_{mn}$ must meet the following conditions:

$$\{\begin{array}{c}(p_{mn}(y)-k_1\cdot p_{mn}(x))⟨y_{2}OR(p_{mn}(y)-k_1\cdot p_{mn}(x))⟩y_1\\ (p_{mn}(y)-k_2\cdot p_{mn}(x))⟨y_{4}OR(p_{mn}(y)-k_2\cdot p_{mn}(x))⟩y_3\end{array}$$

(20)

Similarly, the collision points of other considered obstacles need to meet the above conditions, that is, $m=1,2,\cdots ,N_o$ . $n=1,2,3,4$ . In the equation, $N_o$ represents the total number of obstacles considered. In addition to collision avoidance constraints, vehicles also need to meet their own physical condition constraints, such as equation (21):

$$\{\begin{array}{c}|v(t)|\le v_{max}\\ |a(t)|\le a_{max}\\ |j(t)|\le j_{max}\\ |\phi (t)|\le {\phi }_{max}\\ |\omega (t)|\le {\omega }_{max}\end{array},t\in [t_0,t_f]$$

(21)

In the equation, $v_{max}$ , $a_{max}$ , $j_{max}$ , ${\phi }_{max}$ , and ${\omega }_{max}$ , respectively, represent the maximum speed, maximum acceleration, maximum acceleration change rate, maximum front wheel angle, and their corresponding maximum angular velocity of the vehicle. In addition, during the AVP process, the vehicle always drives in the parking lot, so the following inequality constraints need to be met:

$$\{\begin{array}{c}0<[Ax(t),Bx(t),Cx(t),Dx(t)]<L_x\\ 0<[Ay(t),By(t),Cy(t),Dy(t)]<L_y\end{array}$$

(22)

Equality constraints

In real-time control, the shorter trajectory in the B–C stage can be directly generated, while the trajectory generation process in the A–B stage is segmented to ensure that in the event of dynamic obstacles, the generated trajectory can avoid collisions, and the shorter trajectory length is conducive to real-time calculation. The path generated by the improved hybrid A-star algorithm containing three dimensions $(x,y,\theta )$ will be segmented and sampled, with a total number of segments of q. The path points in each segment are used as the reference waypoints for the trajectory solution. Therefore, the state parameters and control parameters need to meet the following constraints when the trajectory is solved in segments:

$$\left[\begin{array}{cc}{x}_1(t_0)& x_q(t_f)\\ y_1(t_0)& y_q(t_0)\\ {\theta }_1(t_0)& {\theta }_q(t_0)\\ v_1(t_0)& v_q(t_0)\\ a_1(t_0)& a_q(t_0)\\ {\phi }_1(t_0)& {\phi }_q(t_0)\\ j_1(t_0)& j_q(t_0)\\ {\omega }_1(t_0)& {\omega }_q(t_0)\end{array}\right]=\left[\begin{array}{cc}{x}_s& x_f\\ y_s& y_f\\ {\theta }_s& {\theta }_f\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]\mathrm{\&}\mathrm{\&}\left[\begin{array}{c}{x}_p(t_f)\\ y_p(t_f)\\ {\theta }_p(t_f)\\ v_p(t_f)\\ a_p(t_f)\\ {\phi }_p(t_f)\\ j_p(t_f)\\ {\omega }_p(t_f)\end{array}\right]=\left[\begin{array}{c}{x}_{p+1}(t_0)\\ y_{p+1}(t_0)\\ {\theta }_{p+1}(t_0)\\ v_{p+1}(t_0)\\ a_{p+1}(t_0)\\ {\phi }_{p+1}(t_0)\\ j_{p+1}(t_0)\\ {\omega }_{p+1}(t_0)\end{array}\right],1\le p\le q-1$$

(23)

where $t_0$ and $t_f$ are the initial and end times of the trajectories within each segment, $(x_s,y_s,{\theta }_s)$ is the starting point of the A–B or B–C stage, and $(x_f,y_f,{\theta }_f)$ is the endpoint of the A-B or B-C stage. Replacing equations (5), (19), (20), (21), (22), and (23) with the three parts in equation (16) can obtain the optimal control problem description for AVP trajectory planning.

Transformation of NLP problems

Time domain transform

Within the sampling segments of each waypoint mentioned above, map the time domain $[t_{0,}{t}_f]$ to the time domain [−1,1] and introduce a new time variable $\tau$ :

$$\tau ={\displaystyle \frac{2t}{t_f-t_0}-{\displaystyle \frac{t_f+t_0}{t_f-t_0}}}$$

(24)

The Bolza problem can be transformed into a standard problem using equation (24), which is described as follows:

$$\psi =\mathrm{\Phi }(x(t_0),t_0,x(t_f),t_f)+{\displaystyle \frac{t_f-t_0}{2}\underset{-1}{\overset{1}{\int }}g(x(\tau ),u(\tau ),\tau )d\tau }$$

(25)

$$s.t.\{\begin{array}{c}{\displaystyle \frac{dx}{d\tau }={\displaystyle \frac{t_f-t_0}{2}f(x(\tau ),u(\tau ),\tau )}}\\ c(x(\tau ),u(\tau ),\tau ,t_0,t_f)\le 0\\ b(x(-1),u(-1),t_0,x(1),u(1),t_f)=0\end{array}$$

(26)

In the equation, x and u are the state variables and control variables, respectively.

Discretization of variables

The optimal control problem is discretized into variables at a series of Legendre Gauss points using Lagrange interpolation and Gaussian quadrature equation. The state variables are discretized into $N+1$ variables, and the control variables are discretized into $N$ variables. Using $N+1$ Lagrange polynomials as the basis function to approximate the state variables and N Lagrange polynomials as the basis function to approximate the control variables, the state variables and control variables can be represented as:

$$x(\tau )\approx X(\tau )=\sum _{i=0}^N{L}_i(\tau )\cdot X({\tau }_i)$$

(27)

$$u(\tau )\approx U(\tau )=\sum _{i=0}^N{\tilde{L}}_i(\tau )\cdot U({\tau }_i)$$

(28)

where X and U are the discrete state variables and control variables, $L_i$ and ${\tilde{L}}_i$ are the Lagrangian interpolation basis functions, namely:

$$L_i(\tau )=\prod _{j=0,j\ne i}^N{\displaystyle \frac{\tau -{\tau }_j}{{\tau }_i-{\tau }_j},i=0,1,\cdots N}$$

(29)

$${\tilde{L}}_i(\tau )=\prod _{j=1,j\ne i}^N{\displaystyle \frac{\tau -{\tau }_j}{{\tau }_i-{\tau }_j},i=1,2,\cdots N}$$

(30)

Conversion of constraints and performance indicator functions

Transforming the dynamic differential equation into an algebraic equation, the approximate derivative of the state variable can be obtained by differentiating equation (27) as follows:

$$\begin{array}{r}\dot{x}({\tau }_k)\approx \dot{X}({\tau }_k)=\sum _{i=0}^N{\dot{L}}_i({\tau }_k)\cdot X({\tau }_i)=\sum _{i=0}^N{D}_{ki}({\tau }_k)\cdot X({\tau }_i\end{array})$$

(31)

The differential matrix $D_{ki}$ in the equation represents the differentiation of the Lagrange polynomial at each collocation point, and $D_{ki}$ can be obtained by taking the derivative of equation (29):

$$D_{ki}={\dot{L}}_i({\tau }_k)=\sum _{i=0}^N\prod _{j=0,j\ne i}^N{\tau }_k-{\tau }_j/\prod _{j=0,j\ne i}^N{\tau }_i-{\tau }_j$$

(32)

In the equation, $k=1,2,\cdots N$ , $i=0,1,\cdots N$ , and $D_{ki}$ are only related to the selection of coordination points, so they can be obtained through offline calculation. By combining equations (31) and (32), the dynamic differential equation can be transformed into:

$$\begin{array}{r}\sum _{i=0}^N{D}_{ki}({\tau }_k)\cdot X({\tau }_i)-{\displaystyle \frac{t_f-t_0}{2}F(X({\tau }_k),U({\tau }_k),{\tau }_k,t_0,t_f)=0}\end{array}$$

(33)

Similarly, the discretized inequality constraints and equality constraints are represented as follows:

$$\{\begin{array}{c}C(X({\tau }_k),U({\tau }_k),{\tau }_k,t_0,t_f)\le 0\\ B(X(-1),U(-1),t_0,X(1),U(1),t_f)=0\end{array}$$

(34)

Using the Gaussian quadrature equation to approximate the performance indicator function into:

$$\psi =\mathrm{\Phi }(X(t_0),t_0,X(t_f),t_f)+{\displaystyle \frac{t_f-t_0}{2}\sum _{k=0}^N{\omega }_{k}G(X({\tau }_k),U({\tau }_k),{\tau }_k,t_0,t_f)}$$

(35)

In the equation, ${\omega }_k$ is the Gaussian integral weight coefficient, which can be expressed as:

$${\omega }_k={\displaystyle \frac{2}{(1-{\tau }_k^2)({\dot{P}}_N({\tau }_k){)}^2},k=1,2,\cdots ,N-1}$$

(36)

In summary, by discretizing the allocation points, the Bolza optimal control problem is transformed into a standard NLP problem, which provides a basis for approximate solution of subsequent optimal control problems.

Determination of endpoints for stages A–B and B–C

For parking lots that have undergone intelligent infrastructure transformation, when vehicles enter the parking lot, the intelligent parking system has already allocated idle parking spaces. Each idle parking space has a marked position $\mathrm{p}(x,y,\theta )$ on the map, where $\theta =0,\pm \frac{\pi }{2},\pi$ , as shown in the center position of the parking space by the five-pointed star in Figure 7. After identifying the marked position $\mathrm{p}(x,y,\theta )$ of the target parking space, first determine the final position status of the vehicle within the parking space, and then deduce the endpoints of the two A–B stages corresponding to the parking space in reverse. In order to facilitate boarding and alighting, as well as parking or leaving the parking space, it is required that the vehicle be at the center of the parking space, that is, the spatial distance between the left and right sides of the vehicle, and the spatial distance between the front and rear sides are equal. It is also required that the final heading angle of the vehicle and the azimuth angle of the parking space be consistent. Therefore, the coordinate value of the final position $M_3(x,y,\theta )$ of the vehicle in the parking space can be calculated as follows:

$$\begin{array}{r}\{\begin{array}{c}{M}_3(x)=p(x)-\cos (\theta )\cdot {\displaystyle \frac{1}{2}(L+f-r)}\\ M_3(y)=p(y)-\sin (\theta )\cdot {\displaystyle \frac{1}{2}(L+f-r)}\\ M_3(\theta )=p(\theta )\end{array},\theta =0,\pm {\displaystyle \frac{\pi }{2},}\end{array}\pi$$

(37)

Figure 6.

Parking space obstacle avoidance constraint diagram.

Figure 7.

Determination of the endpoints of stages A–B and B–C.

As can be seen from the figure, for each target parking space, it is assumed that it can be parked from two directions, with the left and right sides of the parking space corresponding to the endpoints $M_1(x,y,\theta )$ and $M_1^{{}^{\mathrm{\prime }}}(x,y,\theta )$ of the two A–B segments. In the hybrid A-star algorithm, the planning results with $M_1$ and $M_1^{{}^{\mathrm{\prime }}}$ as endpoints will be compared, and the shorter path will be selected as the final path output. To ensure that the vehicle can park in the parking space at once, the parking path selection for section B–C consists of a quarter arc $R_{min}$ with a minimum turning radius and a straight line $h_1$ , with the connection point between the two being $M_2(x,y,\theta )$ . According to the geometric relationship of $\mathrm{\Delta }{o}_p{o}_t{p}_1$ , it can be inferred that:

$$\begin{array}{rl}& [L_p-h_1-r-{\displaystyle \frac{1}{2}(L_p-f-L-r){]}^2+(R_{min}-{\displaystyle \frac{W_p}{2}{)}^2}}\\ & =(R_{min}-{\displaystyle \frac{d}{2}-\epsilon {)}^2}\end{array}$$

(38)

The length of $h_1$ can be derived from equation (38), as follows:

$$\begin{array}{rl}{h}_1& ={\displaystyle \frac{1}{2}(L_p+f+L-r)}\\ & -\sqrt{{(R_{min}-{\displaystyle \frac{d}{2}-\epsilon )}}^2-{(R_{min}-{\displaystyle \frac{W_p}{2})}}^2},\epsilon \in \left[0{\displaystyle \frac{W_p-d}{2}}\right]\end{array}$$

(39)

For target parking spaces in different directions, the coordinate values of connection point $M_2$ and the endpoints $M_1$ and $M_1^{{}^{\mathrm{\prime }}}$ of A–B stages can be calculated separately from the $h_1$ value, as follows:

$$\{\begin{array}{c}{M}_2(x)=M_3(x)+\cos (\theta )\cdot h_1\\ M_2(y)=M_3(y)+\sin (\theta )\cdot h_1\\ M_2(\theta )=M_3(\theta )\end{array},\theta =0,\pm {\displaystyle \frac{\pi }{2},\pi }$$

(40)

$$\{\begin{array}{c}{M}_1(x)=M_3(x)+\sin (\theta )\cdot R_{min}+\cos (\theta )\cdot (h_1+R_{min})\\ M_1(y)=M_3(y)+\sin (\theta )\cdot (h_1+R_{min})-\cos (\theta )\cdot R_{min}\\ M_1(\theta )=M_3(\theta )-{\displaystyle \frac{\pi }{2}}\end{array},\theta =0,\pm {\displaystyle \frac{\pi }{2},\pi }$$

(41)

$$\{\begin{array}{c}{M}_1^{{}^{\mathrm{\prime }}}(x)=M_3(x)-\sin (\theta )\cdot R_{min}+\cos (\theta )\cdot (h_1+R_{min})\\ M_1^{{}^{\mathrm{\prime }}}(y)=M_3(y)+\sin (\theta )\cdot (h_1+R_{min})+\cos (\theta )\cdot R_{min}\\ M_1^{{}^{\mathrm{\prime }}}(\theta )=M_3(\theta )+{\displaystyle \frac{\pi }{2}}\end{array},\theta =0,\pm {\displaystyle \frac{\pi }{2},\pi }$$

(42)

By using equations (41) and (42), the two target endpoints for stage A–B can be determined for different target parking spaces. The premise for these two points to become feasible A–B stage target endpoints is to meet the following two requirements: (1) These two points are within the range of the parking lot map and (2) The vehicle does not come into contact with any obstacles at these two points.

Framework of time-optimal global trajectory planning algorithm

The process of the time-optimal global trajectory planning algorithm for AVP is shown in Figure 8. On the premise of clarifying the target parking space, an improved hybrid A-star algorithm is first used to quickly obtain a collision-free A–B phase reference waypoint. The numerical optimization method is used to solve the optimal trajectories of the A–B stage and the B–C stage, and the above reference route points are used as the initialization conditions for the trajectory solving process of the A–B stage to improve the convergence success rate and efficiency of the numerical optimization method in solving long-distance and multiobstacle trajectories; finally, a global optimal trajectory that can be used for direct tracking control of the vehicle chassis is obtained.

Figure 8.

Time optimal global trajectory planning algorithm framework.

Simulation and verification

A–B segment path planning based on improved hybrid A-star

Based on the MATLAB platform, a large-scale parking lot map is built to simulate the global trajectory planning of AVP. The improved hybrid A-star algorithm is compared with the traditional hybrid A-star algorithm to verify the superiority of the improved algorithm. As shown in Figure 9, the parking lot contains 495 vertical parking spaces distributed around and in the center of the parking lot. The azimuth of the parking spaces included four situations: [ $-\frac{\pi }{2},0,\frac{\pi }{2},\pi$ ]. The entrance and exit of the parking lot are located in the center below the map and serve as the starting position for autonomous parking of vehicles. The parameters involved in the simulation of A-B section roadbed planning are shown in Table 1.

Figure 9.

Map of medium and large parking lot.

Table 1.

Parameters involved in planning the simulation.

Parameter	Parameter description	Value
$x\times y$	Map size	164 × 80  $\mathrm{m}$
$\xi$	Map resolution in x and y directions	0.1  $\mathrm{m}$
${\mathrm{s}}_0$	Basic extension step size of search tree	0.2  $\mathrm{m}$
$N_{\phi }$	Number of node expansion directions $N_{\phi }$	7
$R_i$	Expansion circle circumscribed circle radius $R_i$	1.1  $\mathrm{m}$
$L_p$	Parking space length	5.0  $\mathrm{m}$
$W_p$	Parking space width	2.5  $\mathrm{m}$
$L_r$	Road width	6.0  $\mathrm{m}$
$L$	Vehicle wheelbase	2.7  $\mathrm{m}$
$f$	Vehicle front suspension	0.9  $\mathrm{m}$
$r$	Vehicle rear suspension	0.9  $\mathrm{m}$
${\phi }_{max}$	Maximum front wheel steering angle	0.56 $\mathrm{rad}$

Figure 10 shows the parking lot map after binarization of 0 and 1 on the grid map. The nonobstacle area is 0 and marked in white, while the obstacle area is 1 and marked in gray. Figure 11 shows the feasible driving range of the vehicle in the parking lot after the expansion of the vehicle on the basis of the binary map. The red area represents the area where the vehicle may enter the collision area, and the white area represents the feasible driving area. Although the feasible driving area is reduced, the obtained path can provide more constrained waypoints for trajectory planning.

Figure 10.

Parking lot map after 0 and 1 binarization on raster map.

Figure 11.

The allowable driving range in the parking lot obtained by inflating the vehicle.

After determining the obstacle and drivable areas of the grid map, assuming that the vehicle is located at the entrance of the parking lot, the center point of the rear axle of the vehicle is selected as the starting node, and the starting node coordinates are (82,2, $\frac{\pi }{2}$ ). According to the marking position of the target parking space, the endpoint of the A–B stage is determined, and the improved hybrid A-star algorithm is used to search the optimal path. Figures 12 and 13 are the optimization results of the traditional hybrid A-star algorithm and different improved algorithms in the case of two different target parking spaces.

Figure 12.

(a) Traditional A-star algorithms, (b) improving the cost function, (c) node expansion using variable step size, and (d) improvement of the cost function and node expansion approach.

Figure 13.

(a) Traditional A-star algorithms, (b) improving the cost function in the objective function, (c) node expansion using variable step size, and (d) improvement of the cost function and node expansion approach.

It can be seen that for different target parking spaces, both the traditional hybrid A-star and the improved hybrid A-star algorithm can solve the path of the A–B stage in the drivable area after the expansion of the obstacle contour, which is a purple curve. However, there are significant differences in the optimization results. The dark blue and light blue lines in the figure are the extension trees of the algorithm, with dark blue indicating forward motion and light blue indicating reverse motion. In condition 1, only one of the two A–B stage destinations solved by equations (39) and (40) meets the A–B stage target endpoint requirements. The optimization results of the traditional A-star algorithm are shown in the figure. There are many turning situations on the path, and the average curvature of the path is relatively large, which does not meet the expectation of the vehicle's AVP system for optimal time because any turning may affect the vehicle's lateral stability and reduce parking safety. At the same time, during the path search process, there are more nodes to be extended, which can be directly determined by the dense extension trees in the graph, resulting in lower search efficiency. The cost function in the objective function has been improved by adding a cost term for path curvature, but the optimization results obtained without improving the search tree extension method are shown in the figure. As shown in Figure 12(a) and (b), the number of path turns obtained in Figure 12(b) is significantly reduced based on the motion cost function improvement, and the path mean curvature is reduced. Simply improving the motion cost function in the hybrid A-star algorithm achieves the goal of reducing the mean curvature, but it has to expand more nodes than the traditional A-star algorithm. Simply improving the motion cost function in the hybrid A-star algorithm achieves the goal of reducing the mean curvature, but it has to expand more nodes than the traditional A-star algorithm. This is also verified in Figure 13(b). The light blue area in the figure is the expansion tree of the node expansion process, which is significantly increased compared with Figure 13(a). Figure 12(c) shows the path search results of the hybrid A-star algorithm after only adopting the variable step node expansion mode. It can be seen that the expanded nodes are drastically reduced, the search efficiency is greatly improved, and the real-time performance of the algorithm is enhanced. However, many turning conditions in the path may result in trajectory planning results that are not globally optimal in time. Figure 12(d) and Figure 13(d) show the path search results after the above two improvements are adopted simultaneously in the hybrid A-star algorithm. It can be seen that the paths obtained not only have a low mean curvature but also a significant reduction in the number of expanded nodes, and the smaller number of expanded nodes effectively improves search efficiency. Table 2 shows the number of nodes expanded and finally saved when using the traditional hybrid A star and the improved hybrid A star to solve the final path.

As can be seen from the table, the hybrid A-star algorithm for both working conditions has significantly reduced the number of nodes expanded in obtaining the optimal path after improving the node expansion method, and the retrieval efficiency is significantly improved. However, compared to the traditional hybrid A-star algorithm, the node expansion in solving the optimal path is increased by only improving the motion cost function, which is mainly due to the penalization of turning for the optimal path, which leads to the algorithm needing to expand more nodes in order to retrieve the optimal path with the least number of turning conditions.

Obtaining the time-optimal trajectory by solving the optimal control problem

After obtaining the optimal path, the numerical optimization method is used for global trajectory planning of AVP, involving vehicle parameters and constraint parameters as shown in Table 3. In the A–B stage, the reference waypoints obtained based on the improved hybrid A-star algorithm under two operating conditions are shown in Figure 14. Among them, operating condition 1 obtained 36 reference waypoints, and operating condition 2 obtained 32 reference waypoints. When using the hybrid A-star algorithm for path planning, vehicle kinematic constraints are considered, so each waypoint contains three dimensions of information, that is, lateral displacement, longitudinal displacement, and heading angle. Segmenting the reference waypoints at intervals of every five waypoints, with the reference waypoints at the connections between the front and back sections remaining consistent to ensure the continuity of the state variables. In each section of the flight, the local trajectory planning problem within the segment is transformed into an optimal control problem, which is then solved through numerical optimization methods. By setting each waypoint as the initial value for numerical optimization, the convergence rate can be greatly improved.

Figure 14.

Reference waypoints obtained based on an improved hybrid A-star algorithm.

Table 2.

Number of nodes extended and final waypoints.

Method	Number of nodes (conditions 1 and 2)	Number of output waypoints (conditions 1 and 2)
Traditional	18,905/3199	85/61
Modified cost function	23,780/8946	145/70
Node expansion	3308/973	80/39
Cost and node expansion modified	5603/8610	70/32

Table 3.

Vehicle parameters and physical constraints.

parameter	Parameter description	Value
$d$	Vehicle width	1.8  $\mathrm{m}$
$v_{max}$	Maximum speed	5.0  $\mathrm{m}/\mathrm{s}$
$a_{max}$	Maximum acceleration	0.75  $\mathrm{m}/{\mathrm{s}}^2$
$j_{max}$	Maximum acceleration change rate	0.3  $\mathrm{m}/{\mathrm{s}}^3$
${\phi }_{max}$	Maximum front wheel equivalent swing angle	0.56 $\mathrm{rad}$
$L_{{\omega }_{max}}$	Maximum front wheel equivalent yaw rate	0.56 $\mathrm{rad}/\mathrm{s}$

The advantage of segmented solution is that when establishing obstacle avoidance constraints for vehicles within each segment, only the no target parking spaces within a certain range of the current reference waypoint are considered as independent obstacles, which greatly reduces the number of obstacles that need to be considered in global one-time trajectory planning. Due to the processing of segmented planning, the number of times obstacle parking spaces near the reference waypoint are considered in the segmented trajectory solving process varies. The number of times this obstacle was considered during the segmented iterative solution process is indicated by the different color depths in Figure 15. It can be seen that obstacle parking spaces near areas with dense waypoints are considered more frequently, resulting in darker colors. As the distance from the vehicle increases, further obstacle parking spaces are considered less frequently, and the color gradually becomes lighter. White parking spaces further away represent those that were not considered as obstacle parking spaces in the trajectory planning process. This is because the more waypoints there are, the more iterations the solution takes, and the closer the obstacle parking space is to the waypoint, the more times it is considered as an obstacle. The green box in the figure represents the vehicle trajectory in stages A–B, while the purple box represents the vehicle trajectory in stages B–C. The red curve represents the trajectory of the vehicle at the center of the latter during the entire autonomous parking phase. As can be seen from the state parameters of the vehicle in Figure 16, the planning results of the vehicle state parameters and control parameters are strictly limited to the constraints in Table 3, and the sign of the vehicle speed changes once for the two parking conditions, which verifies that the vehicle undergoes a back and forth maneuver during the vertical parking process, which is in line with the actual parking situation, and it is shown in Figure 16(a) that the total time used is 48 s, and in Figure 16(b) that the total time used is 41 s. This is because the second parking space is closer to the entrance. This is because the second parking space is closer to the entrance and takes a shorter time.

Figure 15.

Schematic diagram of the autonomous parking stage process.

Figure 16.

Simulation results of states and control variables for two different vehicle positions.

We added the types of parallel parking spaces that are common in life to further verify the effectiveness of the algorithm. Combined with the actual situation, the four weeks of the above parking lot are changed into parallel parking spaces, as the situation of parallel and perpendicular parking modes in the A–B navigation planning stage of the whole AVP process is exactly the same because the planning process of the navigation points is not given here, and the results of the multisegment global time-optimal trajectory planning are given directly, as shown in Figure 15(c) and (d). In the two figures, it can be seen that the optimal navigation reference points are solved based on the improved hybrid A* algorithm for segments A–B. The numerical optimization method quickly obtains the optimal trajectory based on the navigation points as the initial values. No vehicles collide with other parking spaces in the A–B stage. And in stage B-C, the planning results can show the powerful solution advantage of the proposed numerical optimization solution method. In the two parallel parking processes, under the premise of no collision with adjacent front and rear parking spaces, the vehicles need to be adjusted through several front and rear maneuvers before they can finally be parked into the parking spaces. This is mainly due to the simulation set up parallel parking for the length of 5 m; the space is shorter, and the orientation of the vehicle needs to be adjusted many times in the parking process. At the same time, it also serves to verify the powerful optimization capability of the proposed numerical optimization solution.

As can be seen from the vehicle parameter diagrams in Figure 16(c) and 16(d), the positive and negative repeated switching of speed and front wheel angle can verify that the vehicle needs to maneuver forward and backward several times during parallel parking to ensure that the vehicle can safely park in the parking space. This also leads to an increase in the cost of parking time when the parking space is too short compared to vertical parking. The parking completion time of the two parallel parking conditions is about 1 min. In the whole process of AVP, each state and control parameter of the vehicle strictly meets the constraint conditions, and its value is within the constraint range.

Real vehicle test

Choose a small, pure electric car to build a testing platform for the AVP system. As shown in the figure, the testing platform consists of three main parts. The first part is the environmental perception system, which consists of LiDAR, millimeter wave radar, binocular cameras, and a combination navigation system. The second part consists of computing units, including high-performance industrial control computers and embedded autonomous driving underlying software platforms. The underlying software platform uses the Baidu Applo platform. The third part is the wire-controlled chassis execution system, which consists of wire-driven, wire-controlled braking, wire-controlled steering system, and vehicle controller unit. The angle sensor of the electric power steering system provides the steering wheel angle signal, and the wheel speed sensor and the acceleration sensor provide the wheel speed pulse signal, the longitudinal acceleration signal, and the yaw rate signal, respectively. A 220 V power supply is provided to the industrial computer through the inverter. Figure 17 shows the communication architecture between the three parts. The environment awareness system communicates with 4G routers through Ethernet. As the communication gateway between the environment sensing system and the industrial computer, the 4G router collects the sensing information from each sensor and transmits it to the industrial computer, receiving the feedback information of the industrial computer in real time. The vehicle controller unit acts as a communication gateway between the wire-controlled chassis and the industrial computer, and communicates with the two through the CAN bus. The trajectory planning software based on hybrid A-star and pseudo-spectral method is embedded in Applo as application layer software in a Linux system environment. The calculation platform processes the above-mentioned sensor signals and calculates the parking target trajectory information and control variables, including the target angle, the target speed, and the target gear and sends the target control signal to each execution control system.

Figure 17.

(a) Testbed for autonomous valet parking system and (b) communication architecture between the three main parts of the testbed. VCU = vehicle controller unit.

A small parking lot is selected for the calibration test of the integrated navigation module, LiDAR, and millimeter wave radar, and an autonomous driving map suitable for AVP is established. Figure 18 is the point cloud image obtained by the real vehicle scanning the parking lot environment during the LiDAR calibration process.

Figure 18.

Point cloud view of the parking lot environment.

Considering the actual scene of the owner using the function of AVP, the parking starting point is set to the entrance outside the parking lot. It is assumed that the intelligent parking system has sent the target parking space to the vehicle end; that is, the vehicle has identified the target parking space coordinate point. The vehicle travels to the entrance of the parking lot and keeps the vehicle stationary, and then the automatic parking system takes over the vehicle. The parking system plans the parking trajectory of the A–B section and the B–C section according to the current position and the target parking space coordinates and sends it to the bottom actuator of the vehicle for execution. The actuator guides the vehicle according to the planning information. The real vehicle test process is shown in Figure 19.

Figure 19.

Realistic vehicle testing process.

As can be seen from the figure, the parking system receives the parking instruction, the decision-making layer plans the parking trajectory and sends it to the steering, driving, braking, and other actuators, and at the same time guides the vehicle to complete the valet parking and no collision occurs in the process, completing the parking time is about 21 s, the parking process is smooth speed changes, no rapid acceleration, or deceleration occurs. The vehicle easily produces a large lateral deviation when turning, and when traveling on a straight road, due to the small target corner, the lateral deviation is smaller. In the process of completing vertical parking, although the steering wheel angle is larger, the lateral deviation generated by the vehicle is still smaller due to the lower speed. The real-vehicle test shows that the trajectory information obtained using the method in this paper can effectively guide the completion of autonomous parking.

Conclusions

The innovation point of this article is that an algorithm is proposed to quickly solve the global time optimal trajectory of AVP, which solves the problem of slow convergence rate or low success rate of one-time solution of AVP global trajectory through multistage numerical optimization. At the same time, the problem of continuity of state variable and control variable in the process of multi-stage solution is solved. To solve the numerical optimization problem of AVP being sensitive to the initial value, the improved hybrid A* algorithm provides a reliable initial reference point for the multistage trajectory solution and enhances the success rate and efficiency of the solution. The algorithm has specific application value in solving practical projects.

In this article, a global time-optimal trajectory planning method based on an improved hybrid A-star algorithm and NLP is proposed for the motion planning problem involved in the decision-making layer of AVP.

By segmenting the global trajectory planning problem and comprehensively considering the vehicle physical system constraints, parking boundary condition constraints and obstacle avoidance constraints, as well as taking the shortest vehicle parking completion time as the optimal objective function, we establish a nonlinear planning NLP problem based on the (Legendre Gauss) mating point and solve it, and ultimately obtain the globally optimal trajectory based on the guarantee of the continuity of the state variables and control variables.

Simulation results show that the improved hybrid A-star algorithm can effectively reduce the number of node extensions, improve the search efficiency, and reduce the path mean curvature. The global trajectory obtained by the segmented nonlinear planning method effectively avoids collisions and improves parking safety.

As an accurate local planning method, motion planning method based on numerical optimization usually requires high computational cost. Our group has been committed to reducing the computational cost in the numerical optimization process and improving the convergence rate. The group is trying to apply advanced algorithms such as neural networks and deep learning to parking trajectory planning to reduce the computational cost problem in dynamic and complex environments.