Introduction

Over the past decade, substantial advancements have been made in the field of autonomous driving vehicles. Given the current surge in vehicle numbers and congested parking spaces1, there is a burgeoning interest in the field of automated parking2. Within the array of components integral to automated parking technology, trajectory planning is a fundamental technique. The intricate nature of vehicle motion, characterized by nonholonomic constraints, coupled with confined driving spaces, presents substantial challenges for the successful implementation of automated parking3. The challenge lies in navigating the narrow parking spaces, considering the nonholonomic constraints of the vehicle, and generating feasible and smooth trajectories while avoiding collisions with obstacles in the environment. At present, the most prevalent algorithms for automated parking trajectory planning include geometric methods, sampling-based methods, search-based methods, optimal control-based methods, and intelligent algorithms.

In terms of geometric methods, Kim et al.4 proposed a local path planning algorithm based on the approximate clothoid and demonstrated its stability using the Lyapunov stability theorem. Ghajar et al.5 proposed a geometric-based method for local parking path planning that considers both nonholonomic constraints and obstacle collisions and has a short computation time. Huang et al.6 combined geometric curve planning techniques, such as sixth-degree polynomial, clothoid, and Bezier curves, to plan parking paths with continuous curvature. Song et al.7 used B-spline curves for parking path planning while imposing constraints to avoid collisions but considered only collisions with parking spaces and road boundaries. Piao et al.8 employed multiple circular arcs to generate parallel parking paths without the need for initial poses to be parallel to the parking space, but the curvature is discontinuous. To address the curvature discontinuity issue in combined straight line and circular arc paths, Zhang et al.9 used fifth-degree polynomials for smoothing, Cai et al.10 applied Bezier curves for curvature smoothing, and Chen et al.11 utilized the ArcTanGent function for optimization.

Within the realm of sampling-based methods, Dong et al.12 improved the traditional Rapidly-exploring Random Tree (RRT) by directly connecting branches using Reed-Shepp curves and adjusting the RRT sampling positions based on vehicle and parking spot information. Wang et al.13 proposed an adaptive and bidirectional RRT* algorithm for narrow and multi obstacles environments, which extends the tree simultaneously from the start and end points and adjusts the sampling probability based on the number of collision detection failures. Gan et al.14 presented the 1–0 Bg-RRT algorithm with less computation time.

Regarding search-based methods, Sedighi et al.15 proposed an innovative and highly computationally efficient method that combines hybrid A* search engine visibility graph planning to find the shortest feasible nonholonomic path for valet parking in complex environments. On the basis of collision-free paths generated by RS curve, Li et al.16 employed the hybrid A* algorithm to perform offline computation of navigation points in various working scenarios. Based on hybrid A* algorithm, Zhang et al.17 came up with a layered parking path planning method and improved the search speed of hybrid A* algorithm in complex parking scenarios through dividing a complex path planning problem into several simpler problems. Meng et al.18 proposed an improved hybrid A* algorithm that integrates Voronoi fields into the path search process to enhance the safety of trajectory. By introducing anchor points, He et al.19 presented the FAST A* algorithm specifically designed for narrow parking spaces, which improves the success rate of trajectory planning while reducing computation time.

As the computed path acquired by search-based method may be far from the global optimum, the optimal control-based method is used widely for trajectory planning. According to the chosen objective function, necessary constraints and vehicle dynamics model, the computed trajectory by optimal control-based method is at least locally optimal. Zhang et al.20 utilized the strong duality of convex optimization to transform non-differentiable obstacle constraints into smooth differentiable constraints, thereby enhancing the accuracy of obstacle constraints and the safety of automatic parking. Ma et al.21 proposed a local-learning-enabled constrained Iterative Linear Quadratic Regulator (iLQR) based on hybrid dynamic optimization and machine learning, and improved trajectory planning efficiency in automatic parking compared with model-based iLQR. Duan et al.22 introduced Discrete-time Control Barrier Functions (DCBF) and slack variables into Model Predictive Control (MPC), effectively addressing the challenges of traditional methods in balancing feasibility and safety. Zhou et al.23 initially established a path set composed of multiple geometric curves, and fine-tuned the parameters of geometric curves during the parking process using Sequential Quadratic Programming (SQP) to avoid the impact of cumulative control errors on parking performance. Qiu et al.24 presented a semi-trailer automatic parking trajectory planning method based on segmented gaussian pseudospectral method, which has faster computation speed and shorter parking time compared to traditional pseudospectral method.

In addition, several intelligent algorithms have also been applied in the field of automatic parking trajectory planning. Daniali et al.25 employed the Multi-Objective Particle Swarm Optimization (MOPSO) to minimize parking time and parking space length. Yu et al.26 transformed the parking path planning problem into an optimization problem with maximum curvature and curvature at both ends as objective functions, and used the simulated annealing algorithm for path optimization. Su et al.27 proposed a genetic algorithm-based endpoint partitioning quadratic parallel automatic parking method to enhance the accuracy of automatic parking in narrow spaces. Han et al.28 combined the grid method with an improved ant colony algorithm to find the destination by setting the ant’s movement rules in the grid, and identified the shortest path for automatic parking. Tang et al.29 introduced a segmented parking training framework based on Soft Actor-Critic (SAC) and incorporated policy entropy into the objective function to comprehensively consider environmental information during the learning process. Zhang et al.30 proposed a reinforcement learning-based end-to-end automatic parking algorithm, enabling vehicle to learn the optimal steering angles for different parking spaces through continuous learning. Zhang et al.31 achieved rapid and autonomous learning of automatic parking strategies without relying on expert data or prior knowledge through a model-driven reinforcement learning approach.

The comparison of various algorithms is summarized in Table 1.

Table 1 Comparison of commonly used algorithms.
Full size table

In narrow spaces, both sampling-based and search-based planning methods have high success rates of trajectory planning. The sampling-based methods have strong randomness and unstable effect, so that hybrid A* algorithm is employed to deal with automatic parking in narrow space. The planning accuracy of hybrid A* algorithm is constrained by resolution of grid map. Although the hybrid A* algorithm considers vehicle kinematic constraints, path curvature changes calculated by this algorithm are discontinuous, which is unfavorable for control tracking. Furthermore, decoupling path and velocity obtained by the hybrid A* algorithm may lead to unnecessary shifting behaviors. To overcome these drawbacks of the hybrid A* algorithm, an optimal control algorithm called NMPC with longitudinal and lateral coupling is proposed. However, the pure NMPC algorithm is time-consuming to solve and prone to local optima. Based on above analysis, an improved trajectory planning method composed of hybrid A* and NMPC is presented in this study. The contributions of this paper are as follows:

  1. 1.

    The reverse hybrid A* search algorithm is utilized to improve the success rate of searching in narrow spaces, followed by the adoption of cubic polynomials and linear interpolation to obtain rough trajectories.

  2. 2.

    Based on precise obstacle contours described by OBB bounding box, the collision-free constraint is established to incorporated into optimal control of automatic parking trajectory planning, in order to make full use of the parking space.

  3. 3.

    The coupled NMPC algorithm is employed to optimize the path, smooth the rough trajectory and significantly reduce the number of gear shifts, making the parking maneuver more reasonable.

The remaining structure of this paper is as follows: "Vehicle model" introduces the vehicle kinematic model. "Automated parking planning strategy" provides a detailed explanation of the automated parking algorithm and designs scenarios for parallel parking and vertical parking to demonstrate the effectiveness of trajectory planning in narrow spaces. "Algorithm validation on actual vehicles" presents the experimental platform and communication mechanism for real-world vehicle experiments, validating the adaptability and feasibility of the algorithm. Finally, "Conclusions" concludes this paper.

Vehicle model

The vehicle model can be divided into two types: kinematic model and dynamic model. The kinematic model does not consider the vehicle’s dynamics but focuses on the motion of the mass center. It is mainly used for vehicle planning and control in low-speed scenarios. In the parking scenario, where the vehicle speed is normally below 10 km/h, to simplify the system complexity, the kinematic model can be used to describe the vehicle’s motion trajectory. The vehicle is considered as a rigid body moving in a two-dimensional plane. Assuming that the front wheels’ steering angles and speeds are equal, furthermore, longitudinal and lateral load transfer is not considered, the vehicle kinematic model can be presented as Fig. 1. The state-space equations of the model are as follows:

$$\left[ {\begin{array}{*{20}c} {\dot{x}} \\ {\dot{y}} \\ {\dot{\theta }} \\ {\dot{v}} \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {cos\left( \theta \right)v} \\ {sin\left( \theta \right)v} \\ {tan\left( \delta \right)v/L} \\ a \\ \end{array} } \right]$$
(1)

where \(\dot{x}\) and \(\dot{y}\) are longitudinal velocity and lateral velocity of the mass center respectively; \(\theta\) is the yaw angle; \(v\) is the speed; \(\delta\) is the front wheel steering angle; L is the wheelbase; \(a\) is the acceleration.

Fig. 1
figure 1

Schematic diagram of the kinematic model.

Full size image

Automated parking planning strategy

For the automatic parking scenario, a coarse path is planned using the hybrid A* algorithm. Cubic polynomial is then used to fit the vehicle’s velocity and acceleration. The vehicle’s pose, velocity, acceleration, and steering angle are considered as the initial values for the optimization variables in the optimal control problem. While the hybrid A* algorithm merges vehicle states occupying the same grid in discrete space, it cannot guarantee finding the solution with the minimum cost. However, the solution obtained by the hybrid A* algorithm is often in the vicinity of the global optimum. Therefore, a linear interpolation method is used to densify the trajectory, and then NMPC is applied to refine the trajectory, approaching the global optimum. The process is illustrated in the following Fig. 2:

Fig. 2
figure 2

Automatic parking trajectory planning strategy diagram.

Full size image

Coarse trajectory planning based on hybrid A* algorithm

Compared to traditional A* algorithm, the hybrid A* algorithm takes into account the vehicle’s kinematic constraints in node expansion and the planning result better adheres to the vehicle’s motion characteristics. It has been widely used for path planning in open spaces.

Hybrid A* path search

The hybrid A* algorithm considers the vehicle’s maximum steering angle constraint during node expansion. The vehicle’s maximum steering angle is uniformly discretized into 7 steering angles, and the expansion directions are divided into forward and backward. Therefore, each parent node generates 14 child nodes. When the search step size is particularly small, it is possible to assume a constant heading angle during node expansion. To ensure that the child nodes are not in the same grid as the parent node, the search step size \(d_{size}\) is set to be \(\sqrt 2\) times the grid resolution.

In addition, each node in the search process contains some data for the A* algorithm, which can be defined by the following equation:

$$node = \left( {X,\tilde{X},np,g,h} \right)$$
(2)

where \(X = \left( {x,y,\theta } \right)\) represents the vehicle pose, x is the abscissa, y is the ordinate, \(\theta\) is the heading angle; \(\tilde{X}\) indicates the index number of the vehicle pose in the grid map; \(np\) is a pointer to the parent node; \(g\) is the sum of the costs between all nodes from the starting point to the current point, including the distance cost, direction conversion cost, front wheel angle change cost, and obstacle distance cost; \(h\) is the heuristic cost.

Assuming that the cost of the parent node is \(g_{pre}\), the cost of the current node \(g\) is calculated by the following equation:

$$\left\{ \begin{gathered} g = g_{pre} + cost_{1} + cost_{2} + cost_{3} \\ cost_{1} = \omega_{1} *d_{size} \\ cost_{2} = \omega_{2} *\left| {\Delta steer} \right| \\ cost_{3} = \omega_{3} *max\left( {f\left( {d_{1} } \right),f\left( {d_{2} } \right)...f\left( {d_{N} } \right)} \right) \\ \end{gathered} \right.$$
(3)
$$f\left( {d_{i} } \right) = \left\{ {\begin{array}{*{20}c} { 0{ ,}\quad d_{i} \ge d_{0} } \\ { \frac{\varepsilon }{{\varepsilon + d_{N} }} ,\quad d_{i} < d_{0} } \\ \end{array} } \right.$$
(4)

where \(\omega_{1} ,\omega_{2} ,\omega_{3}\) are the corresponding weight coefficients; \(cost_{1}\) is the travel distance cost, \(d_{size}\) is the search step size; \(cost_{2}\) denotes the cost of steering angle change, which is introduced to avoid frequent steering adjustments that can affect the smoothness and comfort of the trajectory, the \(\Delta steer\) is the change in steer angle; \(cost_{3}\) is the penalty for ego vehicle being close to obstacles. \(d_{i}\) is the distance between the node and the i-th obstacle. For the i-th obstacle, the penalty term can be described by Eq. (4), where \(d_{0}\) represents the obstacle distance threshold and \(\varepsilon\) is the attenuation coefficient for the obstacle cost. If the current node’s vehicle bounding box collides with an obstacle, the node is discarded.

The heuristic cost is calculated based on the grid map, as shown in Eq. (5).

$$h = max\left( {h_{1} ,h_{2} } \right)$$
(5)

where \(h_{1}\) represents the heuristic cost considering only the nonholonomic constraints of the vehicle, calculated by the length of the RS curve; \(h_{2}\) denotes the heuristic cost considering only the path length, calculated using the Dijkstra algorithm.

The pseudocode for the hybrid A* algorithm is shown in the Table 2 below.

Table 2 Pseudocode for the hybrid A* algorithm.
Full size table

Speed planning based on cubic polynomial

A cubic polynomial is used to roughly fit the time-distance curve. The rough path is divided into several segments based on forward and backward movements. The variations of distance, velocity, and acceleration over time are described by the following equations:

$$\left\{ {\begin{array}{*{20}c} {s\left( t \right) = bt^{3} + ct^{2} + dt + e} \\ {v\left( t \right) = 3bt^{2} + 2ct + d} \\ {a\left( t \right) = 6bt + 2c} \\ \end{array} {, }} \right.\forall t \in [0,t_{f} ]$$
(6)

where b, c, d, and e are the coefficients of the distance polynomial; t is the current time; s(t), v(t), a(t) respectively represent the distance traveled, velocity, and acceleration of the vehicle at time t; \(t_{f}\) is the total time required for this path segment. The equality constraints satisfied by Eq. (6) are shown in the following expression:

$$\left\{ {\begin{array}{*{20}c} {s\left( 0 \right) = 0} \\ {v\left( 0 \right) = 0} \\ {s\left( {t_{f} } \right) = S} \\ {v\left( {t_{f} } \right) = 0} \\ {a\left( {t_{vmax} } \right) = 0} \\ {v\left( {t_{vmax} } \right) = v_{max} } \\ \end{array} } \right.$$
(7)

where S represents the total length of this path segment; \(v_{\max }\) is the maximum speed; \(t_{v\max }\) indicates the time corresponding to the maximum speed.

According to Eqs. (6) and (7), the velocity and acceleration can be assigned to the path points, resulting in an initial solution for numerical optimization.

Trajectory optimization based on optimal control

Although the trajectory obtained through hybrid A* direct search is drivable, the path formed by node expansion and the RS curve consists of arc and straight line, resulting in discontinuous curvature. Therefore, the optimal control method is used to optimize the initial trajectory.

State space equation of the optimal control problem

To ensure the continuity of the optimized trajectory and satisfy the kinematic constraints, the vehicle’s motion state must adhere to the following state space equation:

$$\mathop {Z\left( t \right)}\limits^{ \cdot } {\mkern 1mu} = f\left( {Z\left( t \right),u\left( t \right)} \right)$$
(8)

where Z is the state variable, \(Z = \left( {x,y,\theta ,v} \right)\); u is the control variable, \(u = \left( {\delta ,a} \right)\).

The discretized state space equation can be described by the following equation:

$$\left[ {\begin{array}{*{20}c} {x\left( {t + 1} \right)} \\ {y\left( {t + 1} \right)} \\ {\theta \left( {t + 1} \right)} \\ {v\left( {t + 1} \right)} \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {cos\left( \theta \right)v} \\ {sin\left( \theta \right)v} \\ {tan\left( \delta \right)v/L} \\ a \\ \end{array} } \right]dt + \left[ {\begin{array}{*{20}c} {x\left( t \right)} \\ {y\left( t \right)} \\ {\theta \left( t \right)} \\ {v\left( t \right)} \\ \end{array} } \right]$$
(9)

Objective function of the optimal control problem

The trajectory is discretized into a series of trajectory points according to time, which can be expressed as \(V = \left( {x_{ti} ,y_{ti} ,\theta_{ti} ,v_{ti} ,\delta_{ti} ,a_{ti} } \right), \, ti \in [0,T]\). The optimization is performed on these trajectory points. The objective cost function is composed of parking space occupancy cost, control variables cost, control variables change cost, and parking trajectory length cost, as described by the Eq. (10):

$$\left\{ {\begin{array}{*{20}c} {\min J(V, \, t) = J_{1} + J_{2} + J_{3} + J_{4} } \\ {J_{1} = w_{1} \mathop \sum \limits_{i = 0}^{T} (x_{ti}^{2} + y_{ti}^{2} )} \\ {J_{2} = \mathop \sum \limits_{i = 0}^{T - 1} (w_{21} a_{ti}^{2} + w_{22} \delta_{ti}^{2} )} \\ {J_{3} = \mathop \sum \limits_{i = 0}^{T - 1} (w_{31} \left( {a_{ti + 1} - a_{ti} } \right)^{2} + w_{32} \left( {\delta_{ti + 1} - \delta_{ti} } \right)^{2} )} \\ {J_{4} = w_{4} \mathop \sum \limits_{i = 0}^{T} (\left| {v_{ti} } \right|*dt)} \\ \end{array} } \right.$$
(10)

where the \(J\) is the total cost; \(J_{1}\) is the space occupation cost;\(J_{2}\) is control variables cost; \(J_{3}\) represents the control variables change cost; \(J_{4}\) represents the parking trajectory length cost;\(w_{1} ,w_{21} ,w_{22} ,w_{31} ,w_{32} ,w_{4}\) are the corresponding weights; \(x_{ti} ,y_{ti} ,\theta_{ti} ,v_{ti} ,\delta_{ti} ,a_{ti}\) respectively represents the abscissa, ordinate, heading angle, speed, steering angle and acceleration of the i-th discrete trajectory point; T is the number of the discrete trajectory points.

Constraints of the optimal control problem

The constraints of proposed optimal control method are composed of three parts.

The first part is limit constraints for v, δ, and a, considering speed limitation in parking scenario and vehicle kinematics, as shown in Eq. (11):

$$\left\{ {\begin{array}{*{20}c} \begin{gathered} \left| {v\left( t \right)} \right| \le v_{max} \hfill \\ \left| {\delta \left( t \right)} \right| \le \delta_{max} \hfill \\ \end{gathered} \\ {\left| {a\left( t \right)} \right| \le a_{max} } \\ \end{array} } \right.{ , }\forall t \in [0,t_{f\_all} ]$$
(11)

where \(\delta_{\max }\) is the max steering angle, \(a_{\max }\) is the max acceleration, \(t_{f\_all}\) is the total parking time.

The second part is the pose constraints and velocity constraints when the vehicle is at the start points and end points for parking, as depicted in Eq. (12):

$$\left\{ {\begin{array}{*{20}c} {x_{0} = x_{start} } \\ {y_{0} = y_{start} } \\ {\theta_{0} = \theta_{start} } \\ {v_{0} = v_{start} } \\ {x_{T} = x_{end} } \\ {y_{T} = y_{end} } \\ {\theta_{T} = \theta_{end} } \\ {v_{T} = v_{end} } \\ \end{array} } \right.$$
(12)

where \([x_{start} ,y_{start} ,\theta_{start} ]\) is the starting pose; \([x_{end} ,y_{end} ,\theta_{end} ]\) is the final pose; \(v_{start}\) is the starting velocity and \(v_{end}\) is the final velocity.

As collisions with obstacles must be avoided during vehicle movement, the collision-free constraints should be considered as the third part of constraints, which are obtained by the following method.

Obstacle collision detection typically utilizes bounding boxes to describe the contours of the vehicle and obstacles. Common types of bounding boxes include axis-aligned bounding boxes (AABB), oriented bounding boxes (OBB), and sphere bounding boxes. AABB does not change the direction of the bounding box with the heading, OBB employs directed bounding boxes to better describe the original shape of objects, and sphere bounding boxes envelop the vehicle using multiple circles. The specific forms of these bounding boxes are illustrated in Fig. 332. For parking scenarios, the environment is usually narrow and complex. Therefore, OBB are adopted to describe the obstacle contours for automatic parking.

Fig. 3
figure 3

Common bounding box types.

Full size image

The ego vehicle and obstacle outlines are described as rectangles using OBB. A simple and effective method for detecting collisions between rectangles is employed by projecting them into each other.

When there is no collision between the ego vehicle and the obstacle, the four corners of the obstacle are outside the rectangle of the ego vehicle, and the four corners of the ego vehicle are also outside the rectangle of the obstacle. Therefore, the collision-free constraint with obstacles can be transformed into constraints where points are outside rectangles. For one obstacle in a state of the ego vehicle, such constraints amount to 8. The constraint for points outside a rectangle can be described by the following Eq. (13):

$$\left( {\left| x \right| - L_{half} } \right) + \left( {\left| y \right| - W_{half} } \right) + \left| {\left| x \right| - L_{half} } \right| + \left| {\left| y \right| - W_{half} } \right| > d_{safe}$$
(13)

where \(W_{half}\) is half of the width of the vehicle, and \(L_{half}\) is half of the length of the vehicle.

Figure 4 illustrates the above method by means of determining if a point is outside a rectangle. As shown in Fig. 4a, the obstacle is projected into the ego vehicle’s coordinate system, this corner of the obstacle is outside the rectangle of the ego vehicle when \(\Delta x\) or \(\Delta y\) is greater than 0. When the ego vehicle is projected into the obstacle’s coordinate system, as shown in Fig. 4b, this corner of the ego vehicle is outside the rectangle of the obstacle while \(\Delta x\) or \(\Delta y\) is greater than 0. If all corners meet the above condition, there is no collision between the ego vehicle and the obstacle. In order to improve the safety, the safe distance without collision is defined as a positive number which is greater than 0.

Fig. 4
figure 4

Schematic diagram of collision avoidance.

Full size image

When the obstacle is projected into the ego vehicle’s coordinate system, the rectangular coordinate system is established based on the ego vehicle’s pose. The transformation matrix is as follows:

$$\left[ {\begin{array}{*{20}c} {x_{obs\_in\_ego} } \\ {y_{obs\_in\_ego} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {cos\left( {\theta_{i} } \right)} & { - sin\left( {\theta_{i} } \right)} \\ {sin\left( {\theta_{i} } \right)} & {cos\left( {\theta_{i} } \right)} \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {x_{par} - x_{i} } \\ {y_{par} - y_{i} } \\ \end{array} } \right]$$
(14)
$$\theta_{obs\_in\_ego} = \theta_{par} - \theta_{i}$$
(15)

where \(x_{obs\_in\_ego}\), \(y_{obs\_in\_ego}\) and \(\theta_{obs\_in\_ego}\) are the abscissa, ordinate and heading of the obstacle in the ego vehicle coordinate system; \(x_{par}\), \(y_{par}\) and \(\theta_{par}\) are the abscissa, ordinate and heading of the obstacle in the parking coordinate system; \(x_{i}\), \(y_{i}\), and \(\theta_{i}\) are current abscissa, ordinate and heading of the ego vehicle in the parking coordinate system.

Numerical simulation and discussion

The program is implemented in C++ and utilizes the IPOPT library to solve the nonlinear optimization problem. The vehicle parameters are set as shown in Table 3.

Table 3 Main kinematic parameters of the vehicle.
Full size table

In practical scenarios, parking spaces often do not have sufficient length to allow a vehicle to park in one attempt. For parallel parking, in extremely narrow parking spaces, multiple maneuvers may be required to complete the parking. Similarly, for vertical parking, in situations with small road boundaries, multiple adjustments may be necessary at the parking entrance to successfully park the vehicle.

In the case of meeting the calculation time requirements, it is desirable to keep the grid size and search step size as small as possible in the hybrid A* search, as well as expanding more child nodes based on steering angle for avoiding search failure in narrow space. Furthermore, reverse search is utilized to accelerate the search process and help the vehicle swiftly navigate out of narrow parking slot.

Considering both the success rate and computational time in narrow parking scenarios, the proposed algorithm’s main parameters are defined as Table 4.

Table 4 Main parameters of the proposed algorithm.
Full size table

Parallel parking simulation

The parallel parking space has a length of 8.0 m and a width of 3.5 m. The comparision of hybrid A* path and the NMPC optimized path are shown in Fig. 5a. Figure 5b shows the space occupied by the vehicle in the NMPC optimized path. Figure 6 illustrates the motion state of the vehicle in the NMPC optimized path and displays the variations of heading angle, velocity, acceleration, and front wheel angle over time. The planning process takes a total of 2.867 s.

Fig. 5
figure 5

Parallel parking path.

Full size image
Fig. 6
figure 6

Variation of heading angle, velocity, front wheel angle and acceleration over time for parallel parking.

Full size image

Vertical parking simulation

The vertical parking space is the same as the parallel parking space, which has a length of 8.0 m and a width of 3.5 m. Figure 7a displays the comparison of hybrid A* path and NMPC optimized path, while Fig. 7b illustrates the space occupied by the vehicle with NMPC optimized path. The heading angle, velocity, acceleration and front wheel angle of the vehicle for vertical parking are presented in Fig. 8. The planning process lasts 2.604 s.

Fig. 7
figure 7

Vertical parking path.

Full size image
Fig. 8
figure 8

Heading angle, velocity, front wheel angle and acceleration of the vehicle for vertical parking.

Full size image

Algorithm comparison and discussion

It can be observed from above simulation experiments that in parallel parking scenarios, the pure hybrid A* algorithm requires 13 gear shifts with a path length of 34.08 m, while the proposed hybrid A* with NMPC algorithm only requires 5 gear shifts with a path length of 29.06 m. The proposed algorithm not only significantly reduces the number of gear shifts in complex scenarios but also shortens the trajectory length by 14.73%. In vertical parking scenarios, the hybrid A* algorithm requires 17 gear shifts with a path length of 32.51 m, whereas the hybrid A* with NMPC algorithm only requires 3 gear shifts with a path length of 25.89 m, similarly reducing the number of gear shifts in complex scenarios and shortening the trajectory length by 20.36%.

Therefore, the hybrid A* with NMPC algorithm exhibits better performance in complex narrow parking scenarios compared to the pure hybrid A* algorithm. It not only generates a shorter path but also optimally utilizes space. The unnecessary gear shifting behavior is avoided through adjusting the vehicle pose in advance. Additionally, the planning process is within 3 s, meeting the time requirements for full parking scenarios.

Parking scenes are generally stationary, so only static obstacles is considered in this study. When dynamic obstacles are incorporated into the proposed algorithm, a prediction module is required to provide accurate trajectories for these dynamic obstacles, and real-time planning is conducted at a fixed frame rate. However, in real driving scenario, precise trajectories of dynamic obstacles cannot be acquired, the planning process should be conducted at each frame, thus leading to huge calculation amount of proposed algorithm. Due to limited computing power of on-board controller, feasible trajectories cannot be obtained by proposed algorithm when considering dynamic obstacles. Based on above analysis, the dynamic obstacles are not included in the proposed algorithm.

The proposed algorithm describes the obstacles based on rectangular bounding boxes. The accuracy of bounding boxes is affected by accurate perception of the environment. In real driving scenarios, when there exists sensor noise or occlusions in the environmental perception, the bounding boxes acquired by proposed algorithm may differ from actual bounding boxes through actual obstacles. This will result in collision risks. Therefore, the reliability of proposed algorithm can be improved further by enhancing the environment perception accuracy.

Algorithm validation on actual vehicles

To validate the algorithm’s practicability in real vehicle, real vehicle experiments are conducted using the EV18 platform. The experimental vehicle is equipped with a drive-by-wire chassis, a ___domain controller, a laser radar, millimeter-wave radar, surround-view cameras, and an integrated inertial navigation system, as shown in Fig. 9.

Fig. 9
figure 9

EV18 experimental platform.

Full size image

The software architecture of the vehicle’s autonomous driving system can be represented as shown in Fig. 10. The bottom layer is the hardware layer, which includes various sensors, actuators, V2X devices, and other hardware components. The middle layer consists of the firmware, which utilizes the MCU, SOC, UDP, CAN, and various interfaces to communicate and process data from the hardware layer. Based on Ubuntu 20.04 and the Robot Operating System (ROS), the top layer which is called the application layer, include map, localization, perception, planning and control modules.

Fig. 10
figure 10

Vehicle software architecture diagram.

Full size image

Due to the slow driving speed and low number of traffic participants, all obstacles are treated as static obstacles. In the application layer, the planning module receives upstream information including perception, localization, chassis messages, and parking space information. Then the planning module sends the trajectory point sequence and gear information to the control module at a frequency of 10 Hz. The control module utilizes MPC to track the trajectory, and control command are sent to the chassis at a frequency of 100 Hz.

The kinematic parameters of the vehicle and the main algorithm parameters remain consistent with Tables 3 and 4. The size of the parallel parking spot and vertical parking spot remains 8.0 m in length and 3.5 m in width. All obstacles are described using OBB rectangular envelopes. The boundaries of parking spots and road boundaries can also be represented using rectangular boxes of zero length or width. The safety distance is determined by the inflation distance of the obstacle envelope as specified in Table 3. The vehicle experiment is conducted on autonomous driving test road within the Wuhan Economic Development Zone.

Parallel parking experiment

In the parallel parking scenario, the planned trajectory and the actual vehicle trajectory are shown in Fig. 11. The vehicle first moves forward to approach the parking space, then reverses into the parking space, and finally adjusts its position by changing gears twice within the parking spot to complete parallel parking.

Fig. 11
figure 11

Parallel parking planning trajectory and actual trajectory.

Full size image

Figure 12 shows the real-time monitoring visualization of parallel parking using the ROS-based RVIZ tool. As depicted in Fig. 12, the ego vehicle’s heading and outline are denoted by yellow arrows and a rectangular bounding box respectively, whereas obstacles are depicted by light blue rectangular bounding boxes. The trajectory of the rear axle center point throughout the parking process is depicted by the purple curve, while the white curve represents the real-time trajectory sent by the planning module. The white dashed lines are road boundaries.

Fig. 12
figure 12

Parallel parking monitoring diagram.

Full size image

The experimental results indicate that for narrow parallel parking spaces, the vehicle can still plan reasonable trajectories with minimal gear shifts, effectively executing the parallel parking maneuver. successfully accomplishing the parallel parking maneuver. The vehicle first moves to the front of the parking space and starts reversing, then adjusts its position through two gear shifts in the parking space. The planning duration amounts to 2.412 s, and the trajectory length is 42.52 m.

Vertical parking experiment

In the vertical parking scenario, the planned trajectory and the actual vehicle trajectory are shown in Fig. 13. The vehicle first moves forward and adjusts its heading angle and then returns to the vertical parking spot. Throughout the process, there are no collisions with obstacles or parking spot. Figure 14 displays the real-time monitoring visualization of the actual vehicle’s operation process using RVIZ.

Fig. 13
figure 13

Vertical parking planning trajectory and actual trajectory.

Full size image
Fig. 14
figure 14

Vertical parking monitoring diagram.

Full size image

For vertical parking scenarios with obstacles and limited road boundaries, the proposed algorithm can plan excellent trajectories. The vehicle first drives to the obliquely above the parking space and adjusts its rear direction, then reverses to complete the vertical parking maneuver. The duration of the planning process is 1.967 s, and the trajectory spans a length of 30.48 m.

It can be seen from Figs. 11, 12, 13 and 14 that the proposed algorithm can effectively complete the parking task in both parallel and vertical parking.

Conclusions

To enable parallel and perpendicular parking in narrow spaces, this study proposes an improved trajectory planning method composed of hybrid A* and NMPC. The proposed algorithm initially utilizes the hybrid A* algorithm and backward search for initial path planning. Subsequently, speed planning is carried out based on a cubic polynomial. Vehicle kinematic constraints and precise collision avoidance constraints are then integrated into the environment. The NMPC with longitudinal and lateral coupling is employed to further optimize the trajectory. In parallel parking simulations, the path length is reduced by 14.73%, with gear shifts decreasing from 13 to 5. In vertical parking, the path length is reduced by 20.36%, with gear shifts decreasing from 17 to 3. The simulation and real-world experiments results demonstrate that proposed algorithm performs well in various scenarios, exhibiting strong adaptability, smooth trajectories and good computation efficiency within 3 s.

Based on precise obstacle contours described by OBB bounding box, the proposed algorithm can make full use of the parking space by establishing the collision-free constraint. Furthermore, proposed hybrid A* combining NMPC effectively reduces path length and gear shift frequency. Therefore, proposed algorithm can enable successful planning of reasonable and smooth trajectories during automatic parking planning in narrow spaces, as well as improving efficiency and comfort of automatic parking. However, on one hand, proposed algorithm does not consider dynamic obstacles, thus there is a risk of collision with dynamic obstacles under complex and variable driving environment; on the other hand, perception errors caused by sensor noise and occlusions is not involved in proposed algorithm, hence obstacle envelope may fluctuate in real driving environment, there is also a risk of collision when the vehicle is very close to the obstacle. For future research, dynamic obstacles and perception errors caused by sensor noise should be incorporated into the trajectory planning, in order to improve the reliability of trajectory planning in real driving environment.