Time-optimal trajectory planning for autonomous parking of tractor-semitrailer vehicle based on piecewise Gauss pseudospectral method

Abstract

For the autonomous parking problem of the tractor-semitrailer vehicle, a piecewise Gauss pseudospectral method is proposed as a trajectory planning scheme in this paper. Firstly, the trajectory planning problem is transformed into an optimal control problem by establishing a vehicle kinematics model for the low-velocity scene, the dynamic constraints including the physical constraints, the collision avoidance constraints, and the endpoint constraints. The optimal control problem are discretized into a nonlinear programming problem by piecewise Gauss pseudospectral method according to the movement area during parking process. The shortest time to complete parking is taken as the objective function. The Gauss pseudospectral method is further adopted to solve the nonlinear programming problem in the two parking stages. Three parallel parking conditions with different lengths of parking spaces and three vertical parking conditions with different widths of parking spaces are set for simulation verification based on Matlab/Simulink. The simulation results show that the proposed method can uniformly and effectively solve the trajectory planning problem of the tractor-semitrailer vehicle. Compared with the traditional pseudospectral method, a shorter parking time and a significantly higher convergence rate can be got with the method, which further indicates that the proposed method has good effectiveness and robustness.

Introduction

Trucks with large cargo capacity occupy a major position in the freight transport industry. Tractor-semitrailer are more and more widely used because of their large carrying capacity and low transportation cost. Due to the large outline size of the tractor-semitrailer and the hinge joint constraint between the tractor and the semitrailer, the tractor-semitrailer has higher requirements for the driver’s experience and driving technology in the parking condition. Therefore, the development of autonomous parking systems is very important to improve driving safety and parking efficiency. An autonomous parking system mainly includes three layers: the perception layer, decision-planning layer, and execution layer. Motion planning belongs to the decision-planning level and its main role is to generate the best route for vehicles from the starting point to the destination, including path planning and trajectory planning.

Generally speaking, the current motion planning methods can be roughly divided into four categories. The first is the graph search method, which includes two steps: graph construction and path search. For an unstructured environment, literature ¹ proposed a graph-based search algorithm and a discrete kinematic vehicle model to find a primitive path that is non-holonomic, collision-free, and safe. Literature ² redesigned the Dijkstra algorithm to tackle situations in which the parameters of the networks may be uncertain by the form of special picture fuzzy numbers. Literature ³ presented Morphological Dilation Voronoi Diagram Roadmap (MVDRM) algorithm to address unsafe path computation accompanied by high time and space computation complexity problems of roadmap path planning methods in complex environments for mobile robots. The graph search method can quickly obtain the driving path and is widely used in global path planning. However, the planning results are relatively rough. The graph search method is usually used as a preprocessing for other methods.

The second kind is the random sampling method. The PRM method proposed in the literature ⁴ make full use of terrain and threat information, so that the UAV’s route map can not only avoid terrain and threat, but also automatically track the terrain. A modified RRT* algorithm named Quick-RRT* was proposed in the literature, ⁵ which can generate better initial solutions and converge to the optimal solution faster than RRT*. Further, to improve the algorithm efficiency, many kinds of literature proposed some improvement strategies, such as PBG RRT, ⁶ EBG-RRT, ⁷ RRT-Connect, ⁸ Extend RRT, ⁹ etc. Compared to graph search methods, random sampling methods are more efficient and can be found as long as the solution exists. However, the shortcomings of the random sampling method are also obvious, that is, the solutions generated each time have a certain degree of randomness and may not be the same.

The third category is the geometric line construction method. The Dubins curve in literature^10,11 and the Reeds-Shepp curve in literature^12–14 are typical geometric line construction methods, which can obtain the shortest path but cannot deal with complex collision avoidance constraints. Literature ¹⁵ adopted a Clothoid curve, while the literature ¹⁶ used a S-shaped curve to generate smooth and continuous paths. Simple combinations of circles and straight lines can also be used to generate paths, such as in literature.^17,18 The parking paths obtained by the spline curves, cubic spline interpolation, and polynomial fitting curves used in literature^19–21 are smoother and collision-free. The advantages of this method are low computational complexity and easy path tracking of the output path.

The fourth category is the one based on machine learning or numerical optimization methods. In literature,^22,23 the Random Forest (RF) model and neural network algorithms was applied to learn the mapping relationship between control parameters and trajectories, and the trained model is used for parking trajectory planning. Literature ²⁴ presented a path planning approach for robot social navigation by making use of Fully Convolutional Neural networks (FCNs) to learn from experts’ path demonstrations. The use of FCNs allowed us to overcome the problem of manually designing/identifying the cost-map and relevant features for the task of robot navigation. Literature ²⁵ realized path planning of mobile robot based on Q-learning algorithm of Markov decision process. Machine learning methods have fast computational speed, but are highly dependent on sample size and quality. In terms of numerical optimization methods, a search-resampling-optimization (SRO) framework is proposed in literature. ²⁶ A hybrid A* algorithm is employed to generate a coarse path and a resampling process is implemented to pave a series of safe dispatch corridors (SDCs) along the coarse path. The resampled result is further fed into the optimization stage to facilitate the numerical solution. Literature ²⁷ proposed a two-stage hierarchical method for trajectory planning of unmanned surface vehicle. This method included coarse path generation based on hybrid A* and trajectory solving based on numerical optimization. Literature ²⁸ proposed an efficient hierarchical initialization technique based on the Dubins curve method when using numerical optimization methods for trajectory solving, which effectively improves convergence efficiency. In literature, ²⁹ a homogenization layer was proposed to reduce the dimension and nonlinearity of the formulated optimal control problem in the planning layer and consequently improves the robustness and efficiency of the solution process. Literature ³⁰ adopted Reeds-Shepp curve to provide initial guesses for trajectory planning of the entire system in the second layer. The above literatures used different initialization techniques to accelerate the convergence rate of trajectory solving process, which provides a high academic reference value. Literature ³¹ proposed a Successive Linearization in Feasible Set (SLiFS) algorithm to address the nonconvex collision avoidance constraints and the nonlinear vehicle dynamic constraints. For the trajectory planning of heavy mining trucks near the loading/dumping site of open-pit mines, literature ³² established a coarse to fine framework. Under the rough planning by an enhanced hybrid a* search algorithm, a small-scale nonlinear programming was used to obtain the optimal trajectory. Literature ³³ proposed a parallel stitching strategy to solve the replanning problem caused by dynamic obstacles during the parking process and the proposed replanner also supported switching to a better homotopy class online. In literature, ³⁴ a method of reconstructing corridors in an iterative framework is proposed to quickly solve the lightweight optimal control problem with only box constraints. An iterative computation framework is proposed to accumulatively handle the collision avoidance constraint and nonconvex kinematics constraint in curve trajectory planning task in literature. ³⁵ In literature, ³⁶ an optimization-based trajectory planning method is proposed to solve the motion planning problem of tractor-trailer vehicle in a curvy and tiny tunnel. This method has significant advantages over popular sampling and search-based algorithms in solving high spatial dimensions or harsh constraint conditions. In the literature,^37,38 Li et al., divided the parallel parking process of a single car into two stages. Interior-point method is used to solve the trajectory planning problem of parallel parking in each stage. Piecewise solution can effectively improve the success rate of solving single car parking trajectory. The numerical optimization method can directly and accurately describe the vehicle motion planning problem, and the solution process can be combined with the various intelligent optimization algorithms. Therefore, it has been favored by academic circles at home and abroad in recent years and it is also a research hotspot. However, in the numerical optimization method, as the number of state/control variables and constraints increases, the solution maybe unsuccessful or convergence rate is slow. This is also one of the focuses of research.

The above studies have made a positive contribution to the solution of the motion planning problem of unmanned vehicles. As a numerical optimization method, Gauss pseudospectral Method (GPM) has many advantages, such as a fast convergence rate and low sensitivity to the initial value, which make this method much more suitable for autonomous parking trajectory planning. However, due to extremely narrow parking environments relative to the larger body size of the tractor-semitrailer vehicle, more contour vertex than that of the ordinary vehicle, and complex constraints, the traditional GPM often has the phenomenon of non-convergence or slow convergence rate in the solution of parking trajectory.

In this paper, a trajectory planning method based on piecewise GPM is proposed to solve the problem of autonomous parking of the tractor-semitrailer vehicle. In order to improve the convergence success rate and efficiency of the algorithm when solving the trajectory, the parallel parking and vertical parking of the tractor-semitrailer vehicle are divided into two stages according to the parking area. The piecewise algorithm can automatically filter a suitable intermediate configuration as the connection point of the two stages. Reasonable constraints are set at the end of the first stage and the beginning of the second stage to ensure that the state variables and control variables of the two stages remain numerically continuous. Taking the shortest parking time as the optimization goal at each stage, the vehicle kinematics model, dynamic constraints, collision avoidance constraints, and endpoint constraints of two stages are established, and the autonomous parking trajectory planning problem is further described as the optimal control problem. By linearizing and discretizing the optimization problem, the above optimal control problem is transformed into a nonlinear programming problem within the framework of GPM. The optimal trajectory can be obtained by using piecewise GPM to solve the nonlinear programming problem. The simulation results of trajectory planning under vertical and parallel parking conditions indicate that this method can obtain an effective parking trajectory for the tractor-semitrailer vehicle, and the convergence success rate and efficiency are significantly improved.

Mathematical model of parking motion planning

Simplified kinematics model of the tractor-semitrailer vehicle

The outline of the tractor-semitrailer is simplified, and the tractor and semitrailer are equivalent to rectangular rigid bodies that can be rotated. The hinged joint is located at the center of the rear axle of the tractor. Under the condition of autonomous parking on a flat road, the dynamic characteristics such as sideslip caused by tire lateral force can be ignored due to the low driving velocity. The established tractor-semitrailer autonomous parking kinematics model is shown in Figure 1.

Figure 1.

Kinematics model of the tractor-semitrailer vehicle.

The golden rectangular block represents the tractor, the front axle is the steering axle, and the rear axle adopts the form of a double-axle arrangement. The pink rectangular block represents the semitrailer, the rear axle adopts the form of a triple-axle arrangement. $O_1$ and $O_2$ are the rear-axle center points of the tractor and semitrailer, respectively. $L_1$ and $L_2$ refer to the length of the tractor and semitrailer. Both the width of the tractor and semitrailer are $W$ . $L_{f1}$ and $L_{f2}$ indicate the front suspension of the tractor and the semitrailer, $L_{r1}$ and $L_{r2}$ are the corresponding rear suspension. $L_w$ and $L_c$ are the wheelbase. ${\theta }_1$ and ${\theta }_2$ refer to the heading angle. $O_3$ indicates the instantaneous turning center of the tractor and semitrailer, $r_c$ and $r_t$ denote the corresponding turning radius. $\phi$ refers to the front wheel steering angle. A tractor-semitrailer vehicle is described as a tractor towing one semitrailer. Kinematics of the concerned tractor is expressed by:

$$\{\begin{array}{c}\stackrel{.}{x}=v_c\left(t\right)cos{\theta }_1\left(t\right)\\ \stackrel{.}{y}=v_c\left(t\right)sin{\theta }_1\left(t\right)\\ {\stackrel{.}{v}}_c=a\left(t\right)\\ \stackrel{.}{a}=j\left(t\right)\\ {\stackrel{.}{\theta }}_1=\frac{v_c\left(t\right)·tan\phi }{L_w}\\ \stackrel{.}{\phi }=\omega \left(t\right)\end{array}$$

(1)

Where (x, y) refers to the mid-point of the rear-wheel axis of the tractor (see point $O_1$ in Figure 1), $v_c$ and $a$ indicate the corresponding velocity and acceleration along with the vehicle’s forward direction, respectively. $j$ and $\omega$ are the differential of acceleration and front-wheel steering angle, respectively. The tractor’s yaw rate vector ${\omega }_c={[\begin{array}{ccc}0\hfill & 0\hfill & {\stackrel{·}{\theta }}_1\hfill \end{array}]}^T$ , velocity vector $v_c={[\begin{array}{ccc}{v}_x\hfill & v_y\hfill & 0\hfill \end{array}]}_c^T$ . The semitrailer’s yaw rate vector ${\omega }_t={[\begin{array}{ccc}0\hfill & 0\hfill & {\stackrel{·}{\theta }}_2\hfill \end{array}]}^T.$ According to the velocity equal relationship between tractor and semitrailer at the hinged point. The relationship between $v_c$ and $v_t$ can be deduced as follows:

$$v_c=v_t+w_t\times [r_c-r_t]$$

$$=[\begin{array}{c}{v}_{t}cos{\theta }_2\\ v_{t}sin{\theta }_2\\ 0\end{array}]+[\begin{array}{c}0\\ 0\\ {\stackrel{.}{\theta }}_2\end{array}]\times [\begin{array}{c}{L}_{c}cos{\theta }_2\\ L_{c}sin{\theta }_2\\ 0\end{array}]$$

(2)

$v_c={[\begin{array}{ccc}{v}_x\hfill & v_y\hfill & 0\hfill \end{array}]}_c^T=[v_c\cos {\theta }_1{v}_c\sin {\theta }_{1}0{]}^T$ . By equation (2) and using (1) with the trigonometric identities, the sought semitrailer yaw dynamic can be converted into the following equation:

$${\stackrel{·}{\theta }}_2=\frac{v_c}{L_c}\sin ({\theta }_1-{\theta }_2)$$

(3)

As a result, $v_t$ can be expressed by the following $v_c$ -related equation:

$$v_t=v_c\cos ({\theta }_1-{\theta }_2)$$

(4)

The coordinate values of the tractor contour vertexes ABCD can be calculated by the center point $O_1$ according to the geometric position relationship, as follows:

$$[\begin{array}{cc}{A}_x\hfill & A_y\hfill \\ \multicolumn{1}{c}{B_x}& B_y\hfill \\ \multicolumn{1}{c}{C_x}& C_y\hfill \\ \multicolumn{1}{c}{D_x}& D_y\hfill \end{array}]=[\begin{array}{ccc}{L}_w+L_{f1}\hfill & -\frac{w}{2}\hfill & 1\hfill \\ \multicolumn{1}{c}{L_w+L_{f1}}& \frac{w}{2}\hfill & 1\hfill \\ \multicolumn{1}{c}{-L_{r1}}& \frac{w}{2}\hfill & 1\hfill \\ \multicolumn{1}{c}{-L_{r1}}& -\frac{w}{2}\hfill & 1\hfill \end{array}][\begin{array}{cc}\cos {\theta }_1\hfill & \sin {\theta }_1\hfill \\ \multicolumn{1}{c}{\sin {\theta }_1}& -\cos {\theta }_1\hfill \\ \multicolumn{1}{c}{x}& y\hfill \end{array}]$$

(5)

The relationship between the rear-axle center points $O_1$ and $O_2$ of the tractor and semitrailer can be further expressed as:

$$O_2(x,y)=(O_1\left(x\right)-L_c*\cos {\theta }_2\left(t\right),O_1\left(y\right)-L_c*\sin {\theta }_2\left(t\right))$$

(6)

Similar to the tractor, the coordinate values of the semitrailer contour vertexes EFGH can be obtained from the coordinate value of the center point $O_2$ . The calculation equation is as follows:

$$[\begin{array}{cc}{E}_x\hfill & E_y\hfill \\ \multicolumn{1}{c}{F_x}& F_y\hfill \\ \multicolumn{1}{c}{G_x}& G_y\hfill \\ \multicolumn{1}{c}{H_x}& H_y\hfill \end{array}]=[\begin{array}{ccc}{L}_c+L_{f2}\hfill & -\frac{w}{2}\hfill & 1\hfill \\ \multicolumn{1}{c}{L_c+L_{f2}}& \frac{w}{2}\hfill & 1\hfill \\ \multicolumn{1}{c}{-L_{r2}}& \frac{w}{2}\hfill & 1\hfill \\ \multicolumn{1}{c}{-L_{r2}}& -\frac{w}{2}\hfill & 1\hfill \end{array}][\begin{array}{cc}\cos {\theta }_2\hfill & \sin {\theta }_2\hfill \\ \multicolumn{1}{c}{\sin {\theta }_2}& -\cos {\theta }_2\hfill \\ \multicolumn{1}{c}{x}& y\hfill \end{array}]$$

(7)

Constraint conditions

Physical constraints

The tractor will be constrained by its own kinematic and dynamic characteristics during the parking process. Consequently, inequality constraints are defined for some state variables and control variables of the tractor-semitrailer vehicle as follows:

$$\{\begin{array}{c}|v\left(t\right)|\le v_{max}\\ |a\left(t\right)|\le a_{max}\\ |j\left(t\right)|\le j_{max}\\ |\phi \left(t\right)|\le {\phi }_{max}\\ |\omega \left(t\right)|\le {\omega }_{max}\\ \left|{\theta }_1-{\theta }_2\right|\le \mathrm{\Delta }{\theta }_{max}\end{array},t\in [t_0,t_f]$$

(8)

Where $v_{max}$ , $a_{\max }$ , $j_{\max }$ , ${\phi }_{\max }$ , ${\omega }_{\max }$ denote the maximum velocity, the maximum acceleration, maximum change rate of acceleration, the maximum steering angle of the front wheel, and the corresponding maximum angular velocity allowed during the parking process. $\mathrm{\Delta }{\theta }_{\max }$ is the maximum included angle between the tractor and the semitrailer allowed to prevent folding loss during the movement.

Collision avoidance constraints

The process of autonomous parking is actually the process in which the vehicle avoids collision near the parking space and finally enters the target parking space. In the whole parking time domain [ $t_0{t}_f$ ], collision not only with obstacles but also with the edge of the road should be avoided. Li^38,39 used rectangles to describe the contours of vehicles and obstacles, and the main idea of collision avoidance is that the two rectangles do not overlap. In order to limit the vehicle vertex always outside the obstacle contour or the obstacle vertex always outside the vehicle contour, an algebraic inequality based on the graphic area was used to establish the constraint. The simulation results in the literature show that this method can effectively avoid collision. However, there are errors in the actual trajectory tracking process of the vehicle chassis actuator, and the continuous accumulation of such errors may lead to collisions. Therefore, in this paper, the expansion coefficient $\mathrm{\alpha }$ of the object is introduced into the algebraic inequality constraint equations proposed by Li, and the expansion coefficient $\mathrm{\alpha }$ is set to be greater than 1, which means that the vehicle and the obstacle are expanded to ensure that there is enough gap between the vehicle and the obstacle to avoid collision. The constraint diagram after adding the collision avoidance idea in literature^38,39 to the expansion of vehicles and obstacles is shown in Figure 2 to make the paper coherent.

Figure 2.

Diagram of collision avoidance constraints during the parking process based on literature.^38,39

Taking the following situation as an example: the tractor contour vertex A and the semitrailer contour vertex G do not collide with the obstacle parking space ${\mathrm{p}}_1{\mathrm{p}}_2{\mathrm{p}}_3{\mathrm{p}}_4$ and the contour vertex ${\mathrm{p}}_4$ of the obstacle parking space does not collide with the tractor ABCD and the semi-semitrailer EFGH as shown in Figure 2, the updated equation after adding the expansion coefficient $\mathrm{\alpha }$ to the inequality constraint in literature^38,39 is as follows:

$$\begin{array}{c}{S}_{\mathrm{\Delta }A{P}_1{P}_2}+S_{\mathrm{\Delta }A{P}_2{P}_3}+S_{\mathrm{\Delta }A{P}_3{P}_4}+S_{\mathrm{\Delta }A{P}_1{P}_4}>\alpha ·S_{P_1{P}_2{P}_3{P}_4}\hfill \\ S_{\mathrm{\Delta }G{P}_1{P}_2}+S_{\mathrm{\Delta }G{P}_2{P}_3}+S_{\mathrm{\Delta }G{P}_3{P}_4}+S_{\mathrm{\Delta }G{P}_1{P}_4}>\alpha ·S_{P_1{P}_2{P}_3{P}_4}\hfill \\ S_{\mathrm{\Delta }{P}_4\mathrm{AB}}+S_{\mathrm{\Delta }{P}_4\mathrm{BC}}+S_{\mathrm{\Delta }{P}_4\mathrm{CD}}+S_{\mathrm{\Delta }{P}_4\mathrm{DA}}>\alpha ·S_{\mathrm{ABCD}}\hfill \\ S_{\mathrm{\Delta }{P}_4\mathrm{EF}}+S_{\mathrm{\Delta }{P}_4\mathrm{FG}}+S_{\mathrm{\Delta }{P}_4\mathrm{GH}}+S_{\mathrm{\Delta }{P}_4\mathrm{HE}}>\alpha ·S_{\mathrm{EFGH}}\hfill \end{array}$$

(9)

Where $\mathrm{\alpha }$ refers to the expansion coefficient of the tractor, the semitrailer, or obstacles. According to the inequality constraint of literature,^38,39 the collision avoidance constraint of the whole parking process can be updated to the equation (10) after adding the expansion coefficient $\mathrm{\alpha }$ :

$$\{\begin{array}{c}S(C_c,P_{i,1},P_{i,2},P_{i,3},P_{i,4})>\alpha ·S_{P_i};c=1\cdots 4,i=1\cdots n\hfill \\ S(D_d,P_{i,1},P_{i,2},P_{i,3},P_{i,4})>\alpha ·S_{P_i};d=1\cdots 4,i=1\cdots n\hfill \\ S(P_{i,j},C_1,C_2,C_3,C_4)>\alpha ·S_c;j=1\cdots 4,i=1\cdots n\hfill \\ S(P_{i,j},D_1,D_2,D_3,D_4)>\alpha ·S_d;j=1\cdots 4,i=1\cdots n\hfill \end{array}$$

(10)

Where, $S(C_c,P_{i,1},P_{i,2},P_{i,3},P_{i,4})$ and $S(D_d,P_{i,1},P_{i,2},P_{i,3},P_{i,4})$ represent the sum of the triangle areas composed of the tractor contour vertex $C_c$ and the semitrailer contour vertex $D_d$ with the four contour vertices of the i-th obstacle. On the contrary, $S(P_{i,j},C_1,C_2,C_3,C_4)$ or $S(P_{i,j},D_1,D_2,D_3,D_4)$ respectively denotes the sum of the triangular areas composed of the contour vertex $P_{i,j}$ of the i-th obstacle with the tractor or semitrailer contour vertices. $S_{P_i}$ , $S_c$ , and $S_d$ are the corresponding rectangular area.

In addition to meeting the above collision avoidance constraints, the vehicle should always be within the road during the entire parking process and cannot exceed the road boundary.

Endpoint constraints

Based on the above kinematic model and constraint conditions, the tractor-semitrailer vehicle can complete collision-free parking from the start time $t_0$ to the end time $t_f$ . At moments $t_0$ and $t_f$ , the tractor-semitrailer vehicle is considered to be in a static state, and the steering angle of the front wheel is zero. The center point $O_1$ of the rear-axle of the tractor is at the specified position at the time $t_0$ , and both the tractor and semitrailer heading angles are zero. At time $t_f$ , all vertices of the tractor-semitrailer vehicle shall be in the parking space. However, the heading angle at the parking completion time needs to be based on whether it is a vertical parking condition or a parallel parking condition. The former is $\frac{\pi }{2}$ , and the latter is 0. Endpoint constraints can be summarized as follows:

$$\{\begin{array}{c}0<[A_x\left(t_f\right),B_x\left(t_f\right),C_x\left(t_f\right),D_x\left(t_f\right),E_x\left(t_f\right),F_x\left(t_f\right),G_x\left(t_f\right),H_x\left(t_f\right)]<L_P\hfill \\ \multicolumn{1}{c}{-w_p<[A_y\left(t_f\right),B_y\left(t_f\right),C_y\left(t_f\right),D_y\left(t_f\right),E_y\left(t_f\right),F_y\left(t_f\right),G_y\left(t_f\right),H_y\left(t_f\right)]<0}\end{array}$$

(11)

$$\{\begin{array}{c}v\left(t_0\right)=0m/s;v\left(t_f\right)=0m/s\hfill \\ a\left(t_0\right)=0m/s^2;a\left(t_f\right)=0m/s^2\hfill \\ j\left(t_0\right)=0m/s^3;j\left(t_f\right)=0m/s^3\hfill \\ \theta \left(t_0\right)=0\mathrm{rad};\theta \left(t_f\right)=0\mathrm{rad}\left(\mathrm{parallel}\right)\mathrm{or}\frac{\pi }{2}\mathrm{rad}\left(\mathrm{vertical}\right)\hfill \\ \phi \left(t_0\right)=0\mathrm{rad};\phi \left(t_f\right)=0\mathrm{rad}\hfill \\ \omega \left(t_0\right)=0\mathrm{rad}/s;\omega \left(t_0\right)=0\mathrm{rad}/s\hfill \end{array}$$

(12)

Where $L_P$ indicates the length of the parking space along the road, $w_p$ refers to the length of the parking space along the Y-axis. In inequality (11), a minus sign is added before $w_p$ , mainly because the parking space is placed below the x-axis of the coordinate system, as shown in Figure 3. If the parking space is sufficient, the heading angle is allowed to deviate from the target heading angle to a certain extent.

Figure 3.

Schematic diagram of the autonomous parking area: (a) vertical parking and (b) parallel parking.

Optimal trajectory solution of autonomous parking based on piecewise GPM

Description of autonomous parking problem of the tractor-semitrailer vehicle

Without losing generality, the Bolza problem of nonlinear optimal control includes the following aspects:

The differential equation of system dynamics is:

$$\stackrel{·}{x}\left(t\right)=f(x\left(t\right),\mu \left(t\right),t)t\in [t_0,t_f]$$

(13)

The path constraints are:

$$\{\begin{array}{c}{c}_1(x\left(t\right),\mu \left(t\right),t)=0\hfill \\ \multicolumn{1}{c}{c_2(x\left(t\right),\mu \left(t\right),t)\le 0}\end{array}t\in [t_0,t_f]$$

(14)

Boundary constraints are:

$$b(x\left(t_0\right),\mu \left(t_0\right),t_0,x\left(t_f\right),\mu \left(t_f\right),t_f)=0$$

(15)

The objective function is:

$$\min \psi =\varphi (x\left(t_0\right),t_0,x\left(t_f\right),t_f)+\int \begin{array}{c}{t}_f\hfill \\ \multicolumn{1}{c}{t_0}\end{array}g(x\left(t\right),\mu \left(t\right),t)\mathrm{dt}$$

(16)

Where: $x$ and $\mu$ are the state variables and control variables of the control system respectively, and satisfy the following conditions: $x\left(t\right)\in R^n$ , $\mu \left(t\right)\in R^m$ . $\min \psi$ is the objective function of the optimal control problem. The state variable $x\left(t\right)$ is continuously differentiable in the time domain [ $t_0,t_f$ ]. However, the control variable $\mu \left(t\right)$ can be discontinuous in the time domain [ $t_0,t_f$ ]. $c_1$ and $c_2$ are inequality path constraints and equality path constraints respectively. The objective function can be formulated according to the actual demand of the controlled system. In a controlled system whose initial state and termination state has been determined, the objective function is usually set to an optimal time or minimum energy consumption.

Autonomous parking technology is to help drivers improve parking efficiency, reduce road occupation time, and avoid traffic congestion on the premise of absolute safety. Hence, the shortest autonomous parking time is set as the objective function of the optimal control problem in this paper, that is, $\min (t_f-t_0)$ . Comprehensively considering the equations (1), (8), (10), (11) the nonlinear optimal control problem of autonomous parking optimal trajectory planning can be described as:

$$\min (t_f-t_0)$$

(17)

$$s.t.\{\begin{array}{c}\mathrm{Equation}\left(1\right)\hfill \\ \multicolumn{1}{c}{\mathrm{Equation}\left(8\right),\left(10\right)}\\ \multicolumn{1}{c}{\mathrm{Equation}\left(12\right)}\end{array},\left(11\right)$$

(18)

Piecewise GPM for solution

GPM can transform the above optimal control problem into a nonlinear programming problem through variable discretization, and then solve it by numerical optimization method. The advantages of GPM such as being insensitive to initial values, high accuracy, and fast convergence make it very suitable for the solution of trajectory planning. However, due to extremely narrow parking environments relative to the larger body size of the tractor-semitrailer vehicle, more contour vertex than that of an ordinary vehicle, and complex constraints, the traditional GPM often has the phenomenon of non-convergence or slow convergence rate in the solution of parking trajectory. Piecewise GPM is proposed to solve the above-mentioned problems. The algorithm is derived as follows:

Time-domain transformation

The time-domain of the optimal control problem of trajectory planning is $t\in [t_0,t_f]$ . The first premise of Piecewise GPM is to divide the time domain into $p$ subintervals, that is $0=t_0<t_1<\cdots t_q<\cdots <t_{p-1}<t_p=t_f$ . Hence, the step length of each time domain $t^{\left(p\right)}$ is:

$$\mathrm{\Delta }t=t_q-t_{q-1},q\in [1,p]$$

(19)

Since the interval of discrete points selected in the GPM is $\tau \in [-1,1]$ , the subintervals need to be transformed in the time domain, that is, the variable $t$ is converted to $\tau$ as follows:

$$\tau =\frac{2t-(t_q+t_{q-1})}{\mathrm{\Delta }t},\tau \in [-1,1]$$

(20)

After transformation, the Bolza problem of each subinterval is transformed into a standard objective function problem, which is described as follows:

$$\begin{array}{c}{\psi }^{\left(q\right)}={\varphi }^{\left(q\right)}(x^{\left(q\right)}\left({\tau }_0\right),{\tau }_0,x^{\left(q\right)}\left({\tau }_f\right),{\tau }_f)\hfill \\ +\frac{\mathrm{\Delta }t}{2}\int \begin{array}{c}{\tau }_f\hfill \\ \multicolumn{1}{c}{{\tau }_0}\end{array}g(x^{\left(q\right)}\left(\tau \right),{\mu }^{\left(q\right)}\left(\tau \right),\tau )d\tau \hfill \end{array}$$

(21)

$$s.t.\{\begin{array}{c}\frac{d{x}^{\left(q\right)}}{d\tau }=\frac{\mathrm{\Delta }t}{2}f(x^{\left(q\right)}\left(\tau \right),{\mu }^{\left(q\right)}\left(\tau \right),\tau )\hfill \\ c_1(x^{\left(q\right)}\left(\tau \right),{\mu }^{\left(q\right)}\left(\tau \right),\tau )=0\hfill \\ c_2(x^{\left(q\right)}\left(\tau \right),{\mu }^{\left(q\right)}\left(\tau \right),\tau )\le 0\hfill \\ b(x^{\left(q\right)}\left({\tau }_0\right),{\mu }^{\left(q\right)}\left({\tau }_0\right),{\tau }_0,x^{\left(q\right)}\left({\tau }_f\right),{\mu }^{\left(q\right)}\left({\tau }_f\right),{\tau }_f)=0\hfill \\ x^{\left(q\right)}\left({\tau }_0\right)-x^{(q-1)}\left({\tau }_f\right)=0\hfill \end{array}$$

(22)

Where $x^{\left(q\right)}$ and ${\mu }^{\left(q\right)}$ are the state variables and control variables to be optimized in each time domain $t^{\left(q\right)}$ respectively.

Discretization of variables

The control variables and state variables in GPM need to be discretized at the position of the configuration points. Then the Lagrange interpolation function is constructed based on the configuration points to approximate the original function. In the interval $[{\tau }_0,{\tau }_f]$ the zero point of Legendre polynomial of order $N$ is set as the configuration points, and its polynomial is:

$$P_N\left(\tau \right)=\frac{1}{2^N·N!}\times \frac{d^N}{d{\tau }^N}·\left[{({\tau }^2-1)}^N\right]$$

(23)

After discretization, the state variables can form $N+1$ variables. Using $N+1$ Lagrange polynomial as the basis function to approximate the original variable, the state variables can be expressed as:

$$x^{\left(q\right)}\left(\tau \right)\approx X^{\left(q\right)}={\sum }_{i=0}^N{L_i}^p\left(\tau \right)·X^q\left({\tau }_i\right)$$

(24)

The basic function of the Lagrange polynomial in equation (23) is:

$${L_i}^p\left(\tau \right)={\Pi }_{j=0,j\ne i}^N\frac{\tau -{\tau }_j^p}{{\tau }_i-{\tau }_j^p},i=0,1,\cdots ,N$$

(25)

However, the control variables can be approximated by $N$ Lagrange polynomials as basis functions. After approximation, the control variable and the corresponding basis function of the Lagrange polynomial can be expressed as:

$${\mu }^{\left(q\right)}\left(\tau \right)\approx U^{\left(q\right)}\left(\tau \right)={\sum }_{i=0}^N{\widetilde{L_i}}^p\left(\tau \right)·U^{\left(q\right)}\left({\tau }_i\right)$$

(26)

$${\widetilde{L_i}}^p\left(\tau \right)={\Pi }_{j=0,j\ne i}^N\frac{\tau -{\tau }_j^p}{{\tau }_i-{\tau }_j^p},i=0,1,\cdots ,N$$

(27)

Constraints and objective function transformation

GPM can convert differential equation constraints into algebraic constraints. The derivative of the state variable at $\tau ={\tau }_k$ can be obtained by deriving from equation (24), which can be approximately expressed as:

$${\stackrel{·}{x}}^{\left(q\right)}\left({\tau }_k\right)\approx {\stackrel{·}{X}}^{\left(q\right)}\left({\tau }_k\right)={\sum }_{i=0}^N{\stackrel{·}{L}}_i^p({\tau }_k)·X^{\left(q\right)}\left({\tau }_i\right)$$

(28)

Combined with equations (22) and (28), the algebraic constraints of the kinematic equation can be obtained as follows:

$${\sum }_{i=0}^N{\stackrel{·}{L}}_i^p\left({\tau }_k\right)·X^{\left(q\right)}\left({\tau }_i\right)-\frac{\mathrm{\Delta }t}{2}f(X\left({\tau }_k\right),U\left({\tau }_k\right),{\tau }_k)=0$$

(29)

The path constraints and boundary constraints after discretization in each time domain are expressed as:

$$\{\begin{array}{c}{C}_1(X^{\left(q\right)}\left({\tau }_k\right),U^{\left(q\right)}\left({\tau }_k\right),{\tau }_k)=0\hfill \\ \multicolumn{1}{c}{C_2(X^{\left(q\right)}\left({\tau }_k\right),U^{\left(q\right)}\left({\tau }_k\right),{\tau }_k)\le 0}\\ \multicolumn{1}{c}{B(X^{\left(q\right)}\left({\tau }_0\right),U^{\left(q\right)}\left({\tau }_0\right),{\tau }_0,X^{\left(q\right)}\left({\tau }_f\right),U^{\left(q\right)}\left({\tau }_f\right),{\tau }_f)=0}\end{array}$$

(30)

In addition, the objective function can be approximately transformed into the following equation by Gauss integration:

$$\begin{array}{c}{\psi }^{\left(q\right)}={\varphi }^{\left(q\right)}(X^{\left(q\right)}\left({\tau }_0\right),{\tau }_0,X^{\left(q\right)}\left({\tau }_f\right),{\tau }_f)\hfill \\ +\frac{\mathrm{\Delta }t}{2}{\sum }_{k=0}^N{ϵ}_k·G(X\left({\tau }_k\right),U\left({\tau }_k\right),{\tau }_k,{\tau }_0,{\tau }_f)\hfill \end{array}$$

(31)

Where ${ϵ}_k$ is the Gaussian integral weight coefficient. The above is the theoretical basis of the piecewise GPM. By transforming the Bolza optimal control problem into a standard NLP problem, it provides the feasibility for solving the optimal trajectory.

The piecewise GPM in autonomous parking of the tractor-semitrailer vehicle

Compared with the single vehicle parking, the parking process of the tractor-semitrailer has the following characteristics: the addition of semitrailer leads to a longer overall vehicle body, the more contours, and the more calculation of the trajectory solution. The kinematic constraint between the tractor and semitrailer, and the collision avoidance constraints of the semitrailer make the constraints more complicated. The traditional Gauss pseudospectral method for solving the parking trajectory may not converge or converge slowly. Li et al.,^36,37 divided the automatic parking process of a single vehicle into two stages for extremely small parking space conditions, and used the interior point method to solve the optimal trajectory in each stage. The simulation results show that the method can effectively improve the success rate and efficiency of the solution. Referring to the division of single car parking area in different stages in the literature,^36,37 this paper divides the autonomous parking process of a tractor-semitrailer into two stages according to the parking area. At each stage, the pseudo spectral method is used to solve the optimal collision-free trajectory with the goal of minimizing the parking time. Corresponding endpoint constraints need to be added at the boundary between the two stages to maintain continuity in the values of vehicle state variables and control variables. The advantage of the segmented solution is that the one-time solution of the original complex whole parking trajectory can be divided into two stages of relatively simple parking trajectory solution. The reason is that the collision avoidance constraint and the endpoint constraint are simplified in the process of piecewise solution, so as to improve the success rate and convergence efficiency of the trajectory solution of the autonomous parking of the tractor-semitrailer.

The first stage is the movement of the vehicle from the initial position to the vicinity of the parking space, while the vehicle position and heading angle will be adjusted to facilitate further entry into the parking space. During this phase, the vehicle runs longitudinally without boundary constraints, so the driving area of the vehicle corresponding to this stage is called the free motion region. At the end of this stage, part of the semitrailer body may has entered the parking space. The second stage is that the vehicle moves until the vehicle body is fully into the parking space and the heading angle reaches the target attitude. At this stage, the vehicle has a range limit in the longitudinal direction and strictly meets the collision avoidance constraints during the movement. Therefore, the vehicle needs to continuously adjust its position and attitude to meet the final goal, and the corresponding allowable driving area is called the adjustment region. Two parking phases correspond to two parking areas, as shown in Figure 4.

Figure 4.

Schematic diagram of autonomous parking area division.

The state variables at the junction of the two stages shall be further constrained to ensure its continuity. At the same time, the position of the contour vertices of the tractor and semitrailer also shall be constrained according to the parking area. The constraint equations are as follows:

$$R_f:\{\begin{array}{c}-L_{d1}\le [A_x,B_x,C_x,D_x,E_x,F_x,G_x,H_x,]\le L_p+L_{d2}\hfill \\ \multicolumn{1}{c}{-W_p\le [A_y,B_y,C_y,D_y,E_y,F_y,G_y,H_y,]\le W_s}\end{array}$$

(32)

$$R_a:\{\begin{array}{c}0\le [A_x,B_x,C_x,D_x,E_x,F_x,G_x,H_x,]\le L_p\hfill \\ \multicolumn{1}{c}{-W_p\le [A_y,B_y,C_y,D_y,E_y,F_y,G_y,H_y,]\le W_s}\end{array}$$

(33)

$$\mathrm{Tractor}:\{\left[\begin{array}{c}{x}_1\left(t_f\right)-x_2\left(t_0\right)\hfill \\ \multicolumn{1}{c}{y_1\left(t_f\right)-y_2\left(t_0\right)}\\ \multicolumn{1}{c}{{\theta }_1\left(t_f\right)-{\theta }_2\left(t_0\right)}\\ \multicolumn{1}{c}{a_1\left(t_f\right)-a_2\left(t_0\right)}\\ \multicolumn{1}{c}{{\phi }_1\left(t_f\right)-{\phi }_2\left(t_0\right)}\end{array}\right]=0$$

(34)

$$\mathrm{Semitrailer}:\{\left[\begin{array}{c}{x}_1\left(t_f\right)-x_2\left(t_0\right)\hfill \\ \multicolumn{1}{c}{y_1\left(t_f\right)-y_2\left(t_0\right)}\\ \multicolumn{1}{c}{{\theta }_1\left(t_f\right)-{\theta }_2\left(t_0\right)}\end{array}\right]=0$$

(35)

The pseudo-code of the piecewise GPM to solve the autonomous parking of the tractor-semitrailer vehicle is shown in the Table 1 below:

Table 1.

The pseudo-code of the piecewise GPM.

Algorithm flow
1. Initialization and discretization of state variables and control variables for trajectory planning optimal control problem;
2. Ignoring the collision avoidance constraints of the parking path, the GPM is used to obtain the global optimal collision-free trajectory;
3. According to the two parking regions, the above trajectory data is divided into two parts ${\mathit{G}}_{\mathit{f}}$ and ${\mathit{G}}_{\mathit{a}}$ , which are used as the initial values of the piecewise GPM in Regions ${\mathit{R}}_{\mathit{f}}$ and ${\mathit{R}}_{\mathit{a}}$ respectively;
4. The solution to the NLP problem in the region ${\mathit{R}}_{\mathit{f}}$ with the collision avoidance constraints based on ${\mathit{G}}_{\mathit{f}}$ ;
5. If the above solution is successful, update the initial value ${\mathit{G}}_{\mathit{f}}$ and return to step 4, otherwise, return the step 6;
6. The time variable ${\mathit{t}}_{1\mathit{f}}$ in the above optimal solution obtained in the region ${\mathit{R}}_{\mathit{f}}$ is added by the relaxation variable ${\mathit{t}}_{\mathit{s}}$ , and the result is used to update the initial value range of time in ${\mathit{G}}_{\mathit{f}}$ , such as [ ${\mathit{t}}_{1\mathit{f}},{\mathit{t}}_{1\mathit{f}}+{\mathit{t}}_{\mathit{s}}$ ]. Go ahead with step 7;
7. The solution to the NLP problem in the region ${\mathit{R}}_{\mathit{f}}$ and ${\mathit{R}}_{\mathit{a}}$ with the collision avoidance constraints based on ${\mathit{G}}_{\mathit{f}}$ and ${\mathit{G}}_{\mathit{a}}$ ;
8. Update the initial values ${\mathit{G}}_{\mathit{f}}$ and ${\mathit{G}}_{\mathit{a}}$ based on the solution and return to step 7; otherwise, go ahead with step 9;
9. Save the last optimal solution and proceed to step 10;
10. Output the optimal solution and the algorithm ends.

Simulation and validation

According to the autonomous parking kinematics model of the tractor-semitrailer vehicle, the trajectory solving program based on piecewise GPM was written on the Matlab platform. Taking an actual tractor-semitrailer vehicle as the simulation object, the tractor-semitrailer vehicle parameters and physical constraints are shown in Table 2.

Table 2.

Vehicle parameters and physical constraints.

Parameters	Parameter description	Value
${\mathrm{L}}_{\mathrm{w}}$	Tractor wheelbase	3.98 m
$L_c$	Semitrailer wheelbase	6.25 m
$L_{f1}$	Tractor front suspension	1.48 m
$L_{f2}$	Semitrailer front suspension	2.25 m
$L_{r1}$	Tractor rear suspension	1.72 m
$L_{r2}$	Semitrailer rear suspension	2.50 m
$\mathrm{W}$	Vehicle width	2.62 m
$L_1$	Total length of the tractor	7.18 m
$L_2$	Total length of the semitrailer	11.00 m
$v_{\max }$	Maximum driving velocity	2.00 $\mathrm{m}/\mathrm{s}$
$a_{\max }$	Maximum acceleration	0.75 $\mathrm{m}/s^2$
$j_{\max }$	Maximum change rate of acceleration	0.3 $0\mathrm{m}/s^3$
${\phi }_{\max }$	Maximum steering angle of the front wheel	0.56 rad
${\omega }_{\max }$	Maximum angular velocity of steering angle	0.56  $\mathrm{rad}/\mathrm{s}$
$\mathrm{\Delta }{\theta }_{\max }$	Maximum included angle between the tractor and the semitrailer	1.48 rad

The simulation of the vertical parking condition

Three vertical parking spaces with different widths are designed to verify the robustness and universal applicability of the piecewise GPM. The width of parking space is 3.14, 3.90, and 4.68 m, respectively, that is, 1.2, 1.5, and 1.8 times the vehicle width. The length of the parking space under three working conditions is 16 m. The rear-axle center point of the tractor is regarded as the observation point of the parking process and the initial state is set to $x_0=20,y_0=4.5$ , which is consistent in three cases. Considering the minimum turning radius of cars and trains, the width of the road is set as 10 m. The planning results are shown in Figures 5 and 6.

Figure 5.

Planning results of longitudinal and lateral displacement for autonomous vertical parking in three parking spaces: $\left(\mathrm{a}\right)L_p=4.68m$ , $\left(\mathrm{b}\right)L_p=3.90m$ , and $\left(\mathrm{c}\right)L_p=3.14m$ .

Figure 6.

Planning results of state parameters and control parameters for autonomous vertical parking in three parking spaces: (a) L_p = 4.68m, (b) L_p = 3.90m, and (c) L_p = 3.14m.

As can be seen from Figure 5 and the partially enlarged figure, in the case of three parking spaces, there is no collision between the vehicle and the road boundary, or between the vehicle and the obstacles during the parking process. There is no in situ steering under the three parking conditions. The heading angle of the vehicle at the beginning and completion time of parking meets the constraints. When the width of the parking space is 1.8 times that of the vehicle, the vehicle can complete the parking process with one reverse, as the curve of the rear-axle center is shown in Figure 5(a). In the parking space, the vehicle makes a small adjustment in the left and right directions, so that the termination position is in the center of the parking space, and the total time is 19.59 s. When the width of the parking space is reduced to 3.90 m, the vehicle completes parking by adjusting the body posture before entering the parking space through a forward-backward maneuver. The sign of velocity in Figure 6(b) changes once, which is consistent with the maneuver, and the total time is 26.62 s. When the width of the parking space is only 3.14 m, the vehicle completes parking after fine-tuning the body attitude through 13 times of forward-backward maneuvers, and the total time is 139.93 s. With the gradual reduction of the width of the parking space, the vehicle completes the attitude adjustment by increasing the number of forward-backward maneuvers, and finally parks into the parking space, which is consistent with the actual situation of manned driving. However, the parking time increases accordingly. It can be seen in Figure 6 that the planning results of state parameters and control parameters are strictly limited in the constraint range of Table 2, which indicates that GPM strictly implements the constraints in the solution process.

The simulation of the parallel parking condition

Similar to the vertical condition, the length of parking spaces is set to 1.2 times, 1.5 times, and 1.8 times the total length of the tractor-semitrailer vehicle respectively in parallel parking conditions, that is, 25.58, 21.32, and 17.10 m. The width of the parking space is 3.5 m, and the road width is also 3.5 m according to relevant legal standards. The initial position is set to $x_0=40,y_0=1.75$ . The simulation results are shown in Figures 7 and 8.

Figure 7.

Planning results of longitudinal and lateral displacement for autonomous parallel parking in three parking spaces: $\left(\mathrm{a}\right)L_p=25.58m$ , $\left(\mathrm{b}\right)L_p=21.32m$ , and $\left(\mathrm{c}\right)L_p=17.10m$ .

Figure 8.

Planning results of state parameters and control parameters for autonomous parallel parking in three parking spaces: (a) L_p = 25.58m, (b) L_p = 21.32m, and (c) L_p = 17.10m.

The completion time of parking under the three parallel conditions is 24.31, 37.17, and 148.87 s. When the length of the parking space is large and medium, the vehicle stops in the center of the parking space through a forward-backward maneuver. As can be seen from Figure 7(a) and (b), the signs of speed have changed once, as shown in Figure 8(a) and (b). However, when the length of parking spaces becomes extremely small, the vehicle also adjusts its posture through a large number of forward-backward maneuvers for collision-free and finally stops in the center of the parking space. With the decrease in parking space length, the number of forward-backward maneuvers and parking time increase.

The vertexes of the vehicle contour in the planning result diagram seem to be in contact with the edge of the parking space or the road boundary, but in fact, the vehicle and the obstacle have been expanded, so there is enough distance between them to ensure no collision, and the value of the safety distance can be adjusted by the expansion coefficient $\alpha$ in equation (10). In the algorithm, the vehicle is required to be in the center of the parking space as far as possible when parking is completed, which has made it convenient for passengers to get up and down and the movement of adjacent vehicles, and at the same time, and make the vehicle itself have enough maneuvering space when leaving the parking space.

The GPM method can be successfully and effectively applied to the trajectory solution of autonomous parking of tractor-semitrailer vehicle. Meanwhile, the advantages of the piecewise GPM method in the convergence success rate, CPU calculation time, and optimal goal can be demonstrated through comparative analysis. The comparison results of the two methods are shown in Figures 9 and 10. When the parking space is large or medium, both methods can converge. The difference is that the piecewise GPM can obtain a shorter parking completion time. However, when the length or width of the parking space is extremely small, the traditional GPM fails to converge, and the piecewise GPM can still converge. Piecewise GPM is superior in terms of calculation time. With the increase in parking space width, the vertical parking time of piecewise GPM is 51.36% and 41.13% of the traditional GPM. With the increase in parking space length, the parallel parking time of piecewise GPM is 48.40% and 39.27% of the traditional GPM. This is mainly because the number of constraints in the divided parking area is reduced by the piecewise GPM to improve the convergence rate.

Figure 9.

Comparison of $t_f-t_0$ and CPU time based on traditional GPM and piecewise GPM for autonomous vertical parking in three parking spaces.

Figure 10.

Comparison of $t_f-t_0$ and CPU time based on traditional GPM and piecewise GPM for autonomous parallel parking in three parking spaces.

A numerical animation demonstration video of the planned trajectory can be obtained at the following website:

https://www.bilibili.com/video/BV1aS4y1Y7fY/.

Conclusions

In this paper, an autonomous parking trajectory planning method using piecewise GPM in different parking areas for the tractor-semitrailer vehicle is proposed. By establishing the tractor-semitrailer vehicle kinematics model, physical constraints, endpoint constraints, and obstacle avoidance, the parking trajectory planning problem are transformed into an optimal control problem. Taking the shortest parking completion time as the optimal objective function, the reasonable initial value of the problem is obtained by the solution without constraints. The constraints are reset according to different parking areas, and the final trajectory is obtained by a faster convergence rate.

The vertical parking conditions of three parking spaces with different widths and the parallel parking conditions of three parking spaces with different lengths are selected for trajectory planning simulation. The results show that the obtained trajectory meets the constraints in the process of autonomous parking. Compared with the traditional GPM, the proposed method has a faster convergence speed.