Optimization-Based Trajectory Planning for Autonomous Parking With Irregularly Placed Obstacles: A Lightweight Iterative Framework

This paper is focused on planning fast, accurate, and optimal trajectories for autonomous parking. Nominally, this task should be described as an optimal control problem (OCP), wherein the collision-avoidance constraints guarantee travel safety and the kinematic constraints guarantee tracking accuracy. The dimension of the nominal OCP is high because it requires the vehicle to avoid collision with each obstacle at every moment throughout the entire parking process. With a coarse trajectory guiding a homotopic route, the intractably scaled collision-avoidance constraints are replaced by within-corridor constraints, whose scale is small and independent from the environment complexity. Constructing such a corridor sacrifices partial free spaces, which may cause loss of optimality or even feasibility. To address this issue, our proposed method reconstructs the corridor in an iterative framework, where a lightweight OCP with only box constraints is quickly solved in each iteration. The proposed planner, together with several prevalent optimization-based planners are tested under 115 simulation cases w.r.t. the success rate and computational time. Real-world indoor experiments are conducted as well.

SECTION I. Introduction

A. Background

Autonomous driving systems are making a promising change to the urban transportation modes [1]. As a critical module in an autonomous vehicle [2], [3], trajectory planning identifies a spatiotemporal curve that is free from collisions, easily tracked by the controller, and comfortable for the passengers. Trajectory planners have been widely developed for on-road autonomous driving [4]–​[6]. Compared with on-road cruising, parking in an unstructured scenario is more challenging [7]–​[9] because 1) there is no longer a navigational reference line, 2) a trajectory typically contains cusps, thus it is no longer a smooth curve, and 3) obstacles are less regularized than those on a structured road. Few of the prevalent on-road trajectory planners can be used to handle a generic parking planning scheme because of these factors. This work is focused on the autonomous parking trajectory planning problems in complex scenarios with irregularly placed static obstacles.

B. Related Works

The existing trajectory planners suitable for autonomous parking primarily include the sampling-and-search-based and optimization-based methods. A sampling-and-search-based approach discretizes the continuous state space into a graph of nodes and searches in the graph for a valid link from the starting node to the goal node. This category has two branches, that is, to sample the state space or control space. Typical methods that sample the state space primarily include the state lattice approach [10], and the rapidly-exploring random tree series [11], [12]. Meanwhile, the typical control-space samplers include the hybrid $\text{A}^\ast $ algorithm [13], and the dynamic window approach [14]. As the second category, optimization-based methods describe a trajectory planning task as an optimal control problem (OCP) and discretize it into a nonlinear programming (NLP) problem [15], [16]. Sampling-and-search- based planners are good at choosing the homotopy class globally (e.g., left or right around each of the obstacles) while optimization-based planners are superior in finding a local optimum while keeping the homotopy class not altered. Sampling-and-search-based and optimization-based planners perform better when they are combined. The rest of this subsection elaborates on the state-of-the-art optimization-based trajectory planners for autonomous parking.

An optimization-based trajectory planner is commonly used to provide optimal trajectories precisely. To the best of the authors’ knowledge, Kondak and Hommel [17] were the first to describe a parking trajectory planning problem as an OCP and then numerically solve it. Using a triangle-area criterion to describe the vehicle-to-obstacle collision-avoidance constraints analytically, Li and Shao [18] proposed a unified OCP model for parking cases with arbitrarily placed obstacles. However, such collision-avoidance constraints are nominally non-differentiable and non-convex. Shi et al. [19] formulated a nested optimization model to simplify the nominal collision-avoidance constraints. Zhang et al. [20] proposed an optimization-based collision avoidance (OBCA) method, wherein signed distance functions are used to improve the differentiability of the nominal collision-avoidance constraints. Bergman and Axehill [21] proposed a Sequential Homotopy Quadratic Programming (SHQP) strategy, which shrinks the obstacles to ease the collision-avoidance constraints and then expands them incrementally towards their nominal sizes. By solving a series of easier OCPs prior to handling the nominal one, the entire difficulties are dispersed evenly among the easier OCPs. A similar idea was proposed by Li et al. [22], wherein an arbitrary spatiotemporal restriction is first imposed to simplify the OCP formulation, and then gradually relaxed until a near-optimal parking trajectory is found.

A near-optimal or even near-feasible initial guess can largely facilitate the numerical solution process of an OCP. One may generate a coarse trajectory and then warm-start an OCP solution process with that initial guess. $\text{A}^\ast $ [23], hybrid $\text{A}^\ast $ [13], and metaheuristic algorithms [24] were used to provide an initial guess. Since the predominant NLP solvers only find local optima, an initial guess determines the homotopy class of the optimized trajectory finally [25]. The same-homotopy-class property makes an initial guess close to the final optimum, thus it is natural to consider paving a local corridor along the initial guess so that the vehicle is completely separated from the surrounding obstacles [26]. Circles [27], rectangles [28], and convex polygons [29] were considered to construct a corridor, whereby the intractably scaled collision-avoidance constraints in the nominal OCP are converted into a small scale of within-corridor constraints. However, these methods either ignore the vehicle shape when paving the corridor [27], [29] or require exhausting offline preparations [28]. Li et al [30] proposed a spiral growth strategy to construct the corridors along a reference trajectory derived by the hybrid $\text{A}^\ast $ algorithm online, thereby quickly finding a near-optimal parking trajectory. However, this method sometimes fails for the following reasons. First, the adopted hybrid $\text{A}^\ast $ method is theoretically incomplete, thus the corridors are unavailable if the hybrid $\text{A}^\ast $ algorithm becomes inefficient. Second, the reference trajectory derived by the hybrid $\text{A}^\ast $ method may not be optimal or even kinematically feasible, thus influencing the quality of the constructed corridors.

C. Contributions

This work aims to achieve fast, accurate, and high-quality parking trajectory planning performance. To ensure that the planning results are optimal and precise, we need to formulate an OCP and numerically solve it. To make the numerical solution process fast, we need to 1) identify a good initial guess via a sampling-and-search-based method quickly; and 2) replace the intractably scaled collision-avoidance constraints with within-corridor conditions. As previously mentioned, constructing corridors has the underlying risks of rendering the OCP infeasible or the derived solution far from being optimal. As a typical sampling-and-search-based algorithm for autonomous parking, the conventional hybrid $\text{A}^\ast $ algorithm is incomplete, thus sometimes cannot find a solution within a limited time. The purpose of this work is to address the aforementioned issues in planning fast, accurate, and high-quality trajectories for autonomous parking.

The contributions of this paper are two-fold. First, a fault-tolerant variant of the hybrid $\text{A}^\ast $ algorithm is proposed, which ensures that a reference trajectory is available quickly even if the conventional hybrid $\text{A}^\ast $ algorithm becomes inefficient. Second, an iterative framework that incorporates a corridor construction procedure and a lightweight NLP solution procedure is proposed to ensure that the trajectory optimization quality is not affected when the initial guess is poor, or when the free spaces are left out by the constructed corridors.

D. Organization

The rest of this paper is structured as follows. Section II presents the problem statement in the form of a nominal OCP. As a foundation, Section III introduces how to convert the nominal collision-avoidance constraints of the OCP into within-corridor constraints. A lightweight iterative framework is proposed in Section IV for providing trajectory planning results. Experimental setups and results are elaborated in Section V. Finally, some conclusions are drawn in Section VI.

SECTION II. Problem Formulation

In this section, a generic autonomous parking trajectory planning scheme is formulated as an OCP nominally.

A. Overall Formulation

Let us denote the vehicle state profile as ${\mathbf { z}}\in {\mathbb {R}}{ }^{\textrm {n}_{\textrm {z}}}$ , the control profile as ${\mathbf { u}}\in {\mathbb {R}}{}^{\textrm {n}_{\textrm {u}}}$ , the workspace as $\Upsilon $ , the obstacle space as $\Upsilon _{\textrm {OBS}} \subset \Upsilon $ , and the free space as $\Upsilon _{\textrm {FREE}} =\Upsilon \backslash \Upsilon _{\textrm {OBS}} $ . The trajectory planning scheme is written as \begin{align*}&\min \limits _{{\mathbf { z}}(t), {\mathbf { u}}(t), T} J\left ({{{\mathbf { z}}(t),{\mathbf { u}}(t)} }\right), \\&\quad \textrm {s.t.,}~{\dot {\mathbf {z}}}(t)=f\left ({{{\mathbf { z}}(t),{\mathbf { u}}(t)} }\right), \\&\hphantom { \quad \textrm {s.t.,}~}{\mathbf {z}}\le {\mathbf { z}}(t)\le {\bar {\mathbf {z}}},\quad {\mathbf {u}}\le {\mathbf { u}}(t)\le { \bar {\mathbf {u}}}, ~t\in [0,T]; \\&\hphantom { \quad \textrm {s.t.,}~}{\mathbf { z}}(0)={\mathbf { z}}_{\textrm {init}}, \quad {\mathbf { u}}(0)={\mathbf { u}}_{\textrm {init}}; \\&\hphantom { \quad \textrm {s.t.,}~}g_{\textrm {end}} \left ({{{\mathbf { z}}(T), \quad {\mathbf { u}}(T)} }\right)\le 0; \\&\hphantom { \quad \textrm {s.t.,}~} fp({\mathbf { z}}(t))\subset \Upsilon _{\textrm {FREE}},\quad t\in [0,T].\tag{1}\end{align*}

Herein, $T$ denotes the parking process duration in seconds (not known $a$ priori), and $J$ is the cost function to be minimized. We use the common shorthand ${\dot {\mathbf {z}}}$ to denote the derivative w.r.t. time, i.e., ${\dot {\mathbf {z}}}=\partial {\mathbf { z}}/\partial t$ [31], [42]. Function $f$ describes the vehicle kinematics. $[{\mathbf {z}},{\bar {\mathbf {z}}}]$ and $[{\mathbf {u}},{\bar {\mathbf {u}}}]$ denote the allowable intervals where ${\mathbf { z}}(t)$ and ${\mathbf { u}}(t)$ stay. ${\mathbf { z}}_{\textrm {init}} $ and ${\mathbf { u}}_{\textrm {init}} $ denote the initial values of ${\mathbf { z}}(t)$ and ${\mathbf { u}}(t)$ , respectively. The inequality $g_{\textrm {end}} \le 0$ models the implicit end-point conditions at $t=T\,.\,fp(\cdot)$ :${\mathbb {R}}^{\textrm {n}_{\textrm {z}}}\to {\mathbb {R}}^{2}$ is a mapping from vehicle state to its footprints, thus $fp({\mathbf { z}}(t))\subset \Upsilon _{\textrm {FREE}}, \forall t$ represents the collision- avoidance constraints. In the remainder of this section, we elaborate on the details behind the aforementioned abstract symbols.

B. Vehicle Kinematic Constraints

This subsection formulates the differential equations ${\dot {\mathbf {z}}}(t)=f\left ({{{\mathbf { z}}(t), {\mathbf { u}}(t)} }\right)$ in (1). The well-known bicycle model is used to describe the vehicle kinematics during the low-speed parking process [18]:\begin{align*} \frac {\textrm {d}}{\textrm {d}t}\left [{ {{\begin{array}{cccccccccccccccccccc} {{\begin{array}{cccccccccccccccccccc} {x(t)} \\ {y(t)} \\ \end{array}}} \\ {v(t)} \\ {\phi (t)} \\ {\theta (t)} \\ \end{array}}} }\right]=\left [{ {{\begin{array}{cccccccccccccccccccc} {{\begin{array}{cccccccccccccccccccc} {v(t)\cdot \cos \theta (t)} \\ {v(t)\cdot \sin \theta (t)} \\ \end{array}}} \\ {a(t)} \\ {\omega (t)} \\ {v(t)\cdot \tan \phi (t)/ {\textrm {L}_{\textrm {W}}}} \\ \end{array}}} }\right], \quad t\in [0,T].\tag{2}\end{align*}

Herein, ($x$ , $y$ ) is the rear-wheel axle mid-point (point $P$ in Fig. 1), $\phi $ is the steering angle, $\omega $ is the angular velocity, $v$ is the velocity of $P$ , $a$ stands for the acceleration, $\theta $ is the orientation angle, and L_W denotes the wheelbase. According to (1), the state profiles ${\mathbf { z}}$ refer to the variables that are differentiated. Thus ${\mathbf { z}}(t)=[x(t),y(t),\theta (t),v(t),\phi (t)]$ and ${\mathbf { u}}(t)=[a(t),\omega (t)]$ .

Fig. 1.

Schematics on vehicle kinematics and geometric sizes.

Besides L_W, other geometric parameters such as L_F (front overhang length), L_R (rear overhang length), and L_B (width) are also depicted in Fig. 1.

The aforementioned state/control variables have allowable intervals, which reflect the physical or mechanical limitations related to vehicle kinematics:\begin{align*} \left [{ {{\begin{array}{cccccccccccccccccccc} {\textrm {a}_{\textrm {min}}} \\ {\textrm {v}_{\textrm {min}}} \\ {-\Omega _{\textrm {max}}} \\ {-\Phi _{\textrm {max}}} \\ \end{array}}} }\right]\le \left [{ {{\begin{array}{cccccccccccccccccccc} {a(t)} \\ {v(t)} \\ {\omega (t)} \\ {\phi (t)} \\ \end{array}}} }\right]\le \left [{ {{\begin{array}{cccccccccccccccccccc} {\textrm {a}_{\textrm {max}}} \\ {\textrm {v}_{\textrm {max}}} \\ {\Omega _{\textrm {max}}} \\ {\Phi _{\textrm {max}}} \\ \end{array}}} }\right],\quad t\in [0,T].\tag{3}\end{align*}

C. Boundary Constraints

Boundary constraints ${\mathbf { z}}(0)={\mathbf { z}}_{\textrm {init}} $ , ${\mathbf { u}}(0)={\mathbf { u}}_{\textrm {init}}$ , and $g_{\textrm {end}} \le 0$ in (1) are deployed to specify the vehicle’s configurations at $t=0$ and $T$ . At $t=0$ , we have \begin{align*}&\hspace {-0.5pc} [x(0),y(0),\theta (0),v(0),\phi (0),a(0),\omega (0)] \\& \qquad\qquad\qquad\qquad\qquad =\, [\textrm {x}_{0},\textrm {y}_{0},{\theta }_{0},\textrm {v}_{0},\phi _{0},\textrm {a}_{0},\textrm {w}_{0}],\tag{4}\end{align*}

Nonetheless, at the endpoint $t=T$ , simply imposing ${\mathbf { z}}(T)={\mathbf { z}}_{\textrm {end}} $ may cause problems because of $\theta (T)$ . Concretely, $\theta (t)$ is differentiable, thus it is continuous w.r.t. $t$ . Since $\theta (t)$ cannot jump, directly setting $\theta (T)$ to a point would fix the phase difference between $\theta (0)$ and $\theta (T)$ , which might misleadingly restrict the behavior of the vehicle. For example, the trajectories of a vehicle that travels from $\theta (0)=0$ to $\theta (T)=0$ or $\theta (T)=2\pi $ are plotted in Fig. 2, respectively. Typically in an on-road local trajectory planning scheme, the phase difference between $\theta (0)$ and $\theta (T)$ is not large, thus setting $\theta (T)$ to a point never excites challenges. But free-space parking allows backward maneuvers, thus a winding trajectory may be necessary to avoid collisions. Due to this reason, we use the following equalities to describe the end-point constraints: \begin{align*}&\hspace {-0.5pc}[x(T),y(T),v(T),\phi (T),a(T),\omega (T)] \\& \qquad\qquad\qquad\qquad\qquad =\,[\textrm {x}_{\textrm {F}},\textrm {y}_{\textrm {F}},\textrm {v}_{\textrm {F}},\phi _{\textrm {F}},\textrm {a}_{\textrm {F}},\textrm {w}_{\textrm {F}}],\tag{5a}\end{align*} and \begin{align*} \sin \theta (T)=&\sin (\theta _{\textrm {F}}), \\ \cos \theta (T)=&\cos (\theta _{\textrm {F}}).\tag{5b}\end{align*}

Fig. 2.

An example reflecting the difference between setting $\theta (T)$ to 0 and $2\pi $ .

D. Collision-Avoidance Constraints

This subsection presents the collision-avoidance constraints $fp({\mathbf { z}}(t))\subset \Upsilon _{\textrm {FREE}} $ in (1). Suppose that the ego vehicle moves in a static environment with N_OBS convex polygonal obstacles. Collisions between the ego vehicle and each obstacle should be avoided at every instance during $[0,T]$ . Suppose that the ego vehicle does not collide with any of the obstacles at the initial moment $t=0$ , collisions will not occur during $t\in (0,T]$ if 1) every vertex of the rectangular ego vehicle stays out of each polygonal obstacle, and 2) every vertex of each polygonal obstacle stays out of the rectangular ego vehicle. It deserves to emphasize that the aforementioned conditions are true only when the polygonal obstacles are convex. Concave polygonal obstacles need to be decomposed into convex ones. The requirement to keep one point $\textrm {Q}=(x_{\textrm {q}},y_{\textrm {q}})$ out of a polygon $\textrm {W}_{1} \textrm {W}_{2} \ldots \textrm {W}_{\textrm {m}} $ can be modeled analytically via a triangle-area criterion [32]:\begin{equation*} S_{\Delta \textrm {QW}_{\textrm {m}} \textrm {W}_{1}} +\sum \limits _{l=1}^{\textrm {m}-1} {S_{\Delta \textrm {QW}_{l} \textrm {W}_{l+1}}} >S_{\unicode{0x25A1} \textrm {W}_{1} \textrm {W}_{2} \ldots \textrm {W}_{\textrm {m}}},\tag{6}\end{equation*} where $S_{\Delta } $ denotes a triangle area, and $S_{\unicode{0x25A1} } $ denotes the area of a polygon. Each $S_{\Delta } $ is computed based on the vertex coordinates. Concretely, suppose that the coordinate of W_l is $(\textrm {x}_{\textrm {W}l},\textrm {y}_{\textrm {W}l})$ , then \begin{align*}&\hspace {-0.5pc} S_{\Delta \textrm {QW}_{l} \textrm {W}_{(l+1)}} = \frac {1}{2}\cdot \Big |{ {x_{\textrm {q}} \textrm {y}_{\textrm {W}l} +\textrm {x}_{\textrm {W}l} \textrm {y}_{\textrm {W}(l+1)} +\textrm {x}_{\textrm {W}(l+1)} y_{\textrm {q}}} } \\& \qquad\qquad\qquad -\, { {x_{\textrm {q}} \textrm {y}_{\textrm {W}(l+1)} -\textrm {x}_{\textrm {W}l} y_{\textrm {q}} -\textrm {x}_{\textrm {W}(l+1)} \textrm {y}_{\textrm {W}l}} }\Big |.\tag{7}\end{align*}

As a constant value, $S_{\unicode{0x25A1} \textrm {W}_{1} \textrm {W}_{2} \ldots \textrm {W}_{\textrm {m}}} $ is calculated by summing up multiple triangle areas.

Suppose that the four vertexes of the ego vehicle at $t$ are denoted as $A(t), B(t), C(t),~\textrm {and}~D(t)$ , and that the $j$ th obstacle has NP_j vertexes $\{\textrm {V}_{jk} \left |{ {k=1,\ldots,\textrm {NP}_{j}} }\right.\}$ , then the conditions 1) and 2) are summarized into the following inequalities:\begin{align*}&\hspace {-2pc}S_{\Delta Q\textrm {V}_{j\textrm {NP}_{j}} \textrm {V}_{j1}} +\sum \limits _{k=1}^{\textrm {NP}_{j} -1} {S_{\Delta Q\textrm {V}_{jk} \textrm {V}_{j(k+1)}}} \\>&S_{\unicode{0x25A1} \textrm {V}_{j1} \textrm {V}_{j2} \ldots \textrm {V}_{j\textrm {NP}_{j}}}, \\&\times \, \forall Q\in \left \{{{A(t),B(t),C(t),D(t)} }\right \},\tag{8}\end{align*} and \begin{align*}&\hspace {-2pc}S_{\Delta QA(t)B(t)} +S_{\Delta QB(t)C(t)} +S_{\Delta QC(t)D(t)} +S_{\Delta QD(t)A(t)} \\>&S_{\unicode{0x25A1} A(t)B(t)C(t)D(t)}, \\&\times \,\forall Q\in \left \{{{\textrm {V}_{j1},\textrm {V}_{j2},\ldots,\textrm {V}_{j\textrm {NP}_{j}}} }\right \}.\tag{9}\end{align*}

Applying (8) and (9) to $\forall t\in [0,T]$ and $j=1,\ldots,\textrm {N}_{\textrm {OBS}} $ yields the complete collision-avoidance constraints.

This work assumes that all of the obstacles are static because predicting the future maneuver of a moving obstacle in a parking scenario is typically not as easy as it is on a structured road. Also, some extra computational time is needed before the vehicle can really react to the moving obstacles. In parking an autonomous vehicle, the primary challenge is not the capability to consider moving obstacles, but to handle the irregularly placed obstacles in a cluttered environment regardless of they are static or not.

E. Cost Function

In this work, a time-energy cost function is defined as follows:\begin{equation*} J=\textrm {w}\cdot \int _{\tau =0}^{T} {\left ({{a^{2}(\tau)+v^{2}(\tau)\cdot \omega ^{2}(\tau)} }\right)\cdot \textrm {d}\tau +T},\tag{10}\end{equation*} where w > 0 is a weighting parameter. The first term models the passenger comfort according to the ISO 2631–1 standard mentioned in [33], the second term is deployed to show our preference to finish the parking process early.

As a summary of the whole section, a generic autonomous parking trajectory planning scheme is described as the following OCP:\begin{align*}&\min \limits _{{\mathbf { z}}(t), {\mathbf { u}}(t), T}~ (10), \\&\quad \textrm {s.t.}~\textrm {Kinematic constraints (2) and (3);} \\&\hphantom {\quad \textrm {s.t.}~}\textrm {Boundary conditions (4) and (5);} \\&\hphantom {\quad \textrm {s.t.}~}\textrm {Collision-avoidance constraints (8) and (9).}\tag{11}\end{align*}

SECTION III. Corridor Construction

A. Motivations

The nominal OCP (11) formulated in the preceding section is intractable because it requires the ego vehicle to avoid collision with each obstacle at every time instant. However, the ego vehicle does not have the chance to collide with each obstacle at every moment. Regarding the task to travel from A to B in Fig. 3, if the ego vehicle travels via route 1, then it has slim chances to collide with obstacles $O_{1}$ or $O_{2}$ at the bottom; if the vehicle chooses route 2 instead, it may not collide with $O_{5}$ . Therefore, simultaneously avoiding collisions with $O_{1}$ , $O_{2}$ , and $O_{5}$ renders redundant constraints. If a guiding route is given $a$ priori, the scale of the collision-avoidance constraints can be reduced. To consider further, if a corridor is constructed along a selected route to separate the ego vehicle from the surrounding obstacles, then the scale of the environment-related constraints becomes fully independent from the number of obstacles, i.e., being irrelevant to the complexity of the environment (Fig. 4).

Fig. 3.

A simple case illustrating partial collision-avoidance constraints in the nominal OCP formulation are redundant.

Fig. 4.

Schematics of the constructed corridors. A corridor is paved along each route and each corridor consists of five local boxes. Note that the scale of the within-corridor constrains is irrelevant to the number of obstacles.

In the remainder of this section, we first identify a guiding route, then construct the corridors, finally formulate the within-corridor constraints.

B. Generation of a Guiding Route

A guiding route like the ones presented in Fig. 4 should be identified before we construct the corridors. This subsection introduces how to generate such a guiding route. Searching in the $x-y-\theta \,\,3$ -dim mesh grids, the hybrid $\text{A}^\ast $ algorithm [13] generates a route that connects the initial and goal configurations.

Since the hybrid $\text{A}^\ast $ algorithm is not complete, it may fail when the environment is complex. We propose a fault-tolerant hybrid $\text{A}^\ast $ (FTHA) algorithm, which guarantees to have an output even if the conventional hybrid $\text{A}^\ast $ search fails. In FTHA, a best-so-far node is maintained to record the explored node with the smallest cost-to-go value during the iterations. Recall that a hybrid $\text{A}^\ast $ search process is regarded as failed when the maximum iteration number is reached or the openlist becomes empty. When a hybrid $\text{A}^\ast $ search process fails, one can still get a path from the initial node to the best-so-far node. At the same time, another path from the best-so-far node to the goal node can be identified via a 2-dim $\text{A}^\ast $ search. Connecting the two paths renders a guiding route from the initial point to the goal point, although the end-point orientation angle may not satisfy the desired constraint (5b). The identified guiding route determines from which side the ego vehicle bypasses each of the N_OBS static obstacles.

When the guiding route is derived, we attach a minimum-time velocity profile to form a coarse trajectory. This is achieved by solving a one-dimensional OCP via Pontryagin’s Maximum Principle (PMP). The derived coarse trajectory is resampled evenly in time as ($\text{N}_{\mathrm {FE}}+1$ ) elements $\chi _{P} =\left \{{{\sigma _{0},\sigma _{1},\ldots,\sigma _{N_{FE}}} }\right \}$ , wherein each element $\sigma _{i} $ is a vector containing the vehicle location, orientation angle, and time stamp, i.e., $\sigma _{i} =[x(t_{i}),y(t_{i}),\theta (t_{i}),t_{i}]$ . The time-stamp in the last element $t_{N_{FE}} $ reflects the initially guessed $T$ , which is specified by the aforementioned PMP.

Note that a coarse trajectory derived by our FTHA algorithm may not satisfy the vehicle kinematic constraints, but as we will introduce later, $\chi _{P} $ is used to construct the corridors at the very beginning. This means that the underlying low quality of $\chi _{P}$ would not influence the entire trajectory optimization performance provided that the selected homotopy class is unpromising.

C. Construction of Corridors

This subsection introduces the construction of corridors along the coarse trajectory such that the ego vehicle can safely avoid all of the obstacles if it stays in the corridors. When a corridor is available, requiring a mass point to stay in the corridor is easier than requiring a rectangular vehicle. Thus the well-known safe flight corridor (SFC) based planners in the community of unnamed aerial vehicle (UAV) (e.g. [34], [35]) are not directly applicable. In our study, the SFC concept is extended such that a rectangular vehicle body can be dealt with.

Two discs are used to evenly cover the vehicle body (Fig. 5a). The disc centers, namely $P_{\textrm {f}} =(x_{\textrm {f}},y_{\textrm {f}})$ and $P_{\textrm {r}} =(x_{\textrm {r}},y_{\textrm {r}})$ , are quartile points along the vehicle’s longitudinal axle:\begin{align*} x_{\textrm {f}} (t)=&x(t)+\frac {1}{4}(3\textrm {L}_{\textrm {W}} +3\textrm {L}_{\textrm {F}} -\textrm {L}_{\textrm {R}})\cdot \cos \theta (t), \\ y_{\textrm {f}} (t)=&y(t)+\frac {1}{4}(3\textrm {L}_{\textrm {W}} +3\textrm {L}_{\textrm {F}} -\textrm {L}_{\textrm {R}})\cdot \sin \theta (t), \\ x_{\textrm {r}} (t)=&x(t)+\frac {1}{4}(\textrm {L}_{\textrm {W}} +\textrm {L}_{\textrm {F}} -3\textrm {L}_{\textrm {R}})\cdot \cos \theta (t), \\ y_{\textrm {r}} (t)=&y(t)+\frac {1}{4}(\textrm {L}_{\textrm {W}} +\textrm {L}_{\textrm {F}} -3\textrm {L}_{\textrm {R}})\cdot \sin \theta (t).\tag{12}\end{align*} R_D, the radius of either disc, is determined by \begin{equation*} \textrm {R}_{\textrm {D}} =\frac {1}{2}\sqrt {\left({\frac {\textrm {L}_{\textrm {R}} +\textrm {L}_{\textrm {W}} +\textrm {L}_{\textrm {F}} }{2}}\right)^{2}+(\textrm {L}_{\textrm {B}})^{2}}.\tag{13}\end{equation*}

Fig. 5.

Schematics of vehicle shape shrinking and obstacle dilatation: (a) presenting the vehicle shape with two discs; (b) shrinking the vehicle shape and dilating the environmental obstacles.

The condition that each disc does not overlap with the obstacles is the same as that each disc center keeps a distance of R_D from the obstacles at least. Therefore, an equivalent conversion can be made by simultaneously shrinking the two discs to their centers and dilating the obstacles by R_D (see Fig. 5b). The new map with the dilated obstacles is denoted as a dilated map.

The next step is to construct two corridors in the dilated map for $P_{\textrm {f}} $ and $P_{\textrm {r}}$ , respectively. Note that the corridors we build for the disc centers are based on the dilated map, thus keeping $P_{\textrm {f}} $ and $P_{\textrm {r}} $ in their own series of corridors ensures that the rectangular vehicle does not collide with the obstacles in the original environment.

Let us focus on the corridor construction process of $P_{\textrm {f}} $ first. With the coarse trajectory $\chi _{P} $ at hand, we compute the coarse trajectory of point $P_{\textrm {f}} $ via (12). The resultant trajectory is presented in the form of ($\text{N}_{\mathrm {FE}}+1$ ) elements $\chi _{P_{\textrm {f}}} =\left \{{{\sigma _{0}^{\textrm {F}},\sigma _{1}^{\textrm {F}},\ldots,\sigma _{N_{FE}}^{\textrm {F}}} }\right \}$ . We need to pave a corridor which 1) fully covers $\chi _{P_{\textrm {f}}}$ , and 2) does not overlap with the dilated obstacles in the dilated map. We require that the corridor consists of multiple regularly placed local boxes like the ones shown in Fig. 6a. We use regularly placed local boxes because they make the within-corridor constraints box constraints (we will introduce this idea in the next subsection). Except for the first element, each of the rest N_FE elements in ${\chi }_{P_{\textrm {f}}} $ represents a point, which is called a waypoint. Each waypoint should be covered by one local box, which means a corridor consists of N_FE local boxes. Without losing generality, we focus on how to identify the $k$ th local box.

Fig. 6.

Principle to identify a corridor: (a) a corridor consisting of local boxes; (b) expansion trial direction order and sampled waypoints along the coarse trajectory; (c) expansion trials conducted in a sequence.

The $k$ th local box is responsible for covering the element indexed $k$ in $\chi _{P_{\textrm {f}}} $ . The $k$ th local box is initially constituted by a zero-width-zero-length rectangle and is expanded in each edge in turn until an overlap with the dilated map is detected. The expansion order is defined in Fig. 6b. In Fig. 6c, each expansion trial pushes the involved edge of the currently approved region outwards by a constant size $\Delta \textrm {s}$ . We check, at each time, whether the expanded region (see each small block labeled with a unique number 1,2,3,…,11 in Fig. 6c) causes collisions in the dilated map [36]. If no collision occurs, the expanded region is merged into the currently approved region; otherwise, it is rejected and no further expansions are conducted in that direction. In each direction, a maximum expansion length L_limit is introduced to avoid excessive growth. The principle to identify a local box over a given waypoint $(\textrm {x}_{\textrm {c}},\textrm {y}_{\textrm {c}})$ is summarized by the pseudo-codes in Alg. 1 as follows:

Algorithm 1 Local Box Generation

Function $\textrm {GenerateLocalBox}(\textrm {x}_{\textrm {c}},\textrm {y}_{\textrm {c}})$

2.

$\Omega _{\textrm {ID}} \leftarrow \{1,2,3,4\}$

2.

$\Omega _{\textrm {DIRECTION}} \leftarrow \left \{{{\pi /2, \pi, 3\pi /2,0} }\right \}$ ;

3.

$\Omega _{\textrm {LENGTH}} \leftarrow \left \{{{0, 0, 0,0} }\right \}$ ;

4.

$\Upsilon _{\textrm {APPROVED}} \leftarrow (\textrm {x}_{\textrm {c}},\textrm {y}_{\textrm {c}} {);}$

5.

while $\Omega _{\textrm {ID}} \ne \varnothing $ do

6.

for each $i\in \Omega _{\textrm {ID}} $ do

7.

$\Upsilon _{\textrm {TRIAL}} \! \leftarrow \! \textrm {ExpandBox(}\Upsilon _{\textrm {APPROVED}},\Omega _{\textrm {DIRECTION}} [i])$ ;

8.

if $\textrm {IsBoxValid(}\Upsilon _{\textrm {TRIAL}})$ then

9.

$\Omega _{\textrm {LENGTH}} [i]\leftarrow \Omega _{\textrm {LENGTH}} [i]+\Delta \textrm {s}$ ;

10.

$\Upsilon _{\textrm {APPROVED}} \leftarrow \Upsilon _{\textrm {TRIAL}} \cup \Upsilon _{\textrm {APPROVED}} {;}$

11.

if $\Omega _{\textrm {LENGTH}} [i]\ge \textrm {L}_{\textrm {limit}} $ then

12.

$\Omega _{\textrm {ID}} \leftarrow \Omega _{\textrm {ID}} \backslash i$ ;

13.

end if

14.

else

15.

$\Omega _{\textrm {ID}} \leftarrow \Omega _{\textrm {ID}} \backslash i$ ;

16.

end if

17.

end for

18.

end while

19.

$\begin{aligned} \begin{array}{l} \textrm {x}_{\textrm {min}} \leftarrow \textrm {x}_{\textrm {c}} -\Omega _{\textrm {LENGTH}}\,\,\text {[3]}, \\ \textrm {x}_{\textrm {max}} \leftarrow \textrm {x}_{\textrm {c}} +\Omega _{\textrm {LENGTH}}\,\,\text {[1]}, \\ \textrm {y}_{\textrm {min}} \leftarrow \textrm {y}_{\textrm {c}} -\Omega _{\textrm {LENGTH}}\,\,\text {[2]}, \\ \textrm {y}_{\textrm {max}} \leftarrow \textrm {y}_{\textrm {c}} +\Omega _{\textrm {LENGTH}}\,\,\text {[4]};\\ \end{array} \end{aligned}$

20.

return with $\left [{ {\textrm {x}_{\textrm {min}},\textrm {x}_{\textrm {max}},\textrm {y}_{\textrm {min}},\textrm {y}_{\textrm {max}}} }\right]$ .

The output of GenerateLocalBox() is a vector containing four elements, which records the ranging intervals of the identified local box in the x and y axes, respectively. In line 7, the function $trial\_{}area\leftarrow \textrm {ExpandBox}(box,dir)$ is used to identify an additional region $trial\_{}area$ by pushing the edge of a regularly placed box $box$ in the direction of $dir$ by a certain step $\Delta \textrm {s}$ . The function $\textrm {IsBoxValid}(trial\_{}area)$ is deployed to check if the aforementioned region $trial\_{}area$ overlaps with the dilated obstacles in the dilated map.

Applying Alg. 1 to all of the waypoints yields two corridors for $P_{\textrm {f}} $ and $P_{\textrm {r}}$ , respectively:

The output of GenerateCorridors() is a vector containing 8N_FE elements, all of which determine the two corridors. In line 1 of Alg. 2, the function $\textrm {GenerateReferenceTrajectory}(\chi _{P})$ calculates two reference trajectories according to the coarse trajectory $\chi _{P} $ we identify in Section III.B.

Algorithm 2 Corridor Generation

Function $\textrm {GenerateCorridors}(\chi _{P})$

1.

$[\chi _{P_{\textrm {f}}},\chi _{P_{\textrm {r}}}]\leftarrow \textrm {GenerateReferenceTrajectory}(\chi _{P} {);}$

2.

$\Gamma \leftarrow \emptyset $ ;

3.

for $k=1\ldots \textrm {N}_{\textrm {FE}} $ do

4.

$\textrm {[xr}_{\textrm {min}}^{k},\textrm {xr}_{\textrm {max}}^{k},\textrm {yr}_{\textrm {min}}^{k},\textrm {yr}_{\textrm {max}}^{k}]\leftarrow \textrm {GenerateLocalBox}(\chi _{P_{\textrm {r}}} [k].x,\chi _{P_{\textrm {r}}} [k].\textrm {y})$ ;

5.

$\Gamma \leftarrow \Gamma \cup \textrm {[xr}_{\textrm {min}}^{k},\textrm {xr}_{\textrm {max}}^{k},\textrm {yr}_{\textrm {min}}^{k},\textrm {yr}_{\textrm {max}}^{k}]$ ;

6.

$\textrm {[xf}_{\textrm {min}}^{k},\textrm {xf}_{\textrm {max}}^{k},\textrm {yf}_{\textrm {min}}^{k},\textrm {yf}_{\textrm {max}}^{k}]\leftarrow \textrm {GenerateLocalBox(}\chi _{P_{\textrm {f}}} [k].x,\chi _{P_{\textrm {f}}} [k].\textrm {y})$ ;

7.

$\Gamma \leftarrow \Gamma \cup \textrm {[xf}_{\textrm {min}}^{k},\textrm {xf}_{\textrm {max}}^{k},\textrm {yf}_{\textrm {min}}^{k},\textrm {yf}_{\textrm {max}}^{k}]$ ;

8.

end for

9.

return with $\Gamma $ .

D. Within-Corridor Constraint Formulation

With the two constructed corridors at hand, we formulate the within-corridor constraints, which are designated to replace the nominal collision-avoidance constraints (8) and (9).

The within-corridor constraints require that the points $P_{\textrm {f}} $ and $P_{\textrm {r}} $ should stay in the corresponding local boxes at specified time instances:\begin{align*}&P_{\textrm {f}} (t)~\textrm {stays in}k\textrm {th local box for }P_{\textrm {f}},~ \textrm {and} \\&P_{\textrm {r}} (t)~\textrm {stays in}k\textrm {th local box for }P_{\textrm {r}}, \\&t=T/\textrm {N}_{\textrm {FE}} \cdot k,k=1,\ldots,\textrm {N}_{\textrm {FE}}.\tag{14}\end{align*}

Since each local box is regularly placed, (14) is explicitly written as \begin{align*} x_{\textrm {f}} (t)~\in&\left [{ {\textrm {xf}_{\textrm {min}}^{k},\textrm {xf}_{\textrm {max}}^{k}} }\right], \quad y_{\textrm {f}} (t)\in \left [{ {\textrm {yf}_{\textrm {min}}^{k}, \textrm {yf}_{\textrm {max}}^{k}} }\right], \\ x_{\textrm {r}} (t)~\in&\left [{ {\textrm {xr}_{\textrm {min}}^{k},\textrm {xr}_{\textrm {max}}^{k}} }\right], \quad y_{\textrm {r}} (t)\in \left [{ {\textrm {yr}_{\textrm {min}}^{k}, \textrm {yr}_{\textrm {max}}^{k}} }\right], \\ t=&T/\textrm {N}_{\textrm {FE}} \cdot k, \quad k=1,\ldots,\textrm {N}_{\textrm {FE}}.\tag{15}\end{align*}

The within-corridor constraints (15) are box constraints w.r.t. $x_{\textrm {f}} (t)$ , $y_{\textrm {f}} (t)$ , $x_{\textrm {r}} (t)$ , and $y_{\textrm {r}} (t)$ . Since box constraints are the simplest type of linear constraints, replacing (8) and (9) with (12) and (15) makes the entire OCP tractably scaled and less complex. Now the modified OCP is presented as \begin{align*}&\min \limits _{{\mathbf { z}}^{\ast }(t), {\mathbf { u}}(t), T} (10), \\&\quad ~\!\textrm {s.t.}~\textrm {Kinematic constraints (2), (3) and (12);} \\&\hphantom {\quad ~\!\textrm {s.t.}~}\textrm {Boundary conditions (4) and (5);} \\&\hphantom {\quad ~\!\textrm {s.t.}~}\textrm {Within-corridor constraints (}15{).}\tag{16}\end{align*}

Herein, ${\mathbf { z}}^{\ast }$ is defined as the expanded state profile, i.e., $\left [{ {{\mathbf { z}},x_{\textrm {f}},y_{\textrm {f}},x_{\textrm {r}},y_{\textrm {r}}} }\right]$ ; the geometric relation equation (12) is taken as part of the interior kinematic principle of the ego vehicle. In contrast with (11), the dimension of (16) is fully irrelevant to the complexity of the environment, which means that the number of constraints in (16) does not alter no matter how large N_OBS is. By solving (16) numerically, collocation points that represent a parking trajectory are derived.

SECTION IV. Lightweight Iterative Optimization Framework

A. Motivations

The preceding section has presented how to simplify the nominal OCP (11) into an easier version (16). However, (16) is subject to the following issues. First, the identified corridors inevitably leave some corner regions in the free space uncovered (see the blue regions in Fig. 6a). Second, the quality of the coarse trajectory (derived by FTHA introduced in Section III.B) may not be high, which influences the construction of the corridors. Due to the first issue, solving (16) renders a near-optimal rather than an optimal solution when the optimum falls into the uncovered regions. The second issue may make (16) fail to be solved if the constructed corridors do not cover any kinematically feasible trajectory. To summarize, directly solving (16) is not always reliable.

B. Iterative Optimization Framework

To address the issues pointed out in the preceding subsection, a natural idea is to adjust the corridors adaptively if they are found to be inappropriate. We establish an iterative framework, in which the corridors are built once per iteration and an intermediate OCP is formulated accordingly. The solution to the intermediate OCP in one iteration is used as the coarse trajectory $\chi _{P} $ for re-constructing the corridors in the next iteration. The entire iterative optimization process is initialized by the proposed FTHA algorithm, which guarantees to have an output within a limited time.

Intuitively, the iterative framework would work well to gradually find an optimal parking trajectory. However, one needs to ensure that each intermediate OCP is always solved with success, otherwise, an intermediate failure blocks any further evolution. To fix this underlying issue, we simplify (16) so that it never becomes infeasible. One may notice that the only nonlinear constraints in (16) include the kinematic equalities (2) and (12), as well as the end-point boundary condition (5b). If these nonlinear equalities are softened via external penalty functions and merged into the cost function $J$ , (16) becomes a problem with purely box constraints. Eq. (2) is softened as \begin{equation*} J_{\textrm {EQ2}} =\int _{\tau =0}^{T} {\left \|{ {f\left ({{{\mathbf { z}}(\tau),{\mathbf { u}}(\tau)} }\right)} }\right \|^{2}} \cdot \textrm {d}\tau.\tag{17}\end{equation*}

Herein, $\left \|{ {f\left ({{{\mathbf { z}}(\tau),{\mathbf { u}}(\tau)} }\right)} }\right \|^{2}$ measures the violation degree of $f\left ({{{\mathbf { z}}(\tau),{\mathbf { u}}(\tau)} }\right)=0$ at $t=\tau $ , which is integrated from 0 to $T$ so as to form $J_{\textrm {EQ2}} $ . Eq. (12), abstracted as $g\left ({{{\mathbf { z}}^{\ast }(t)} }\right)=0$ , is softened as \begin{equation*} J_{\textrm {EQ12}} =\int _{\tau =0}^{T} {\left \|{ {g\left ({{{\mathbf { z}}^{\ast }(\tau)} }\right)} }\right \|^{2}} \cdot \textrm {d}\tau.\tag{18}\end{equation*}

The end-point boundary condition (5b) is softened as \begin{equation*} J_{\textrm {EQ5B}} \!=\! \left ({{\sin \theta (T)-\sin (\theta _{\textrm {F}})} }\right)^{2} \!+\! \left ({{\cos \theta (T)-\cos (\theta _{\textrm {F}})} }\right)^{2}.\tag{19}\end{equation*}

The aforementioned three terms are summed up as $\psi _{\textrm {infeasibility}} $ :\begin{equation*} \psi _{\textrm {infeasibility}} =J_{\textrm {EQ2}} +J_{\textrm {EQ12}} +J_{\textrm {EQ5B}}.\tag{20}\end{equation*}

For a candidate solution, $\psi _{\textrm {infeasibility}} =0$ iff the constraints (2), (12), and (5b) are strictly satisfied. The compound cost function is presented as \begin{equation*} J_{\textrm {INT}} =J+\textrm {w}_{\textrm {penalty}} \cdot \psi _{\textrm {infeasibility}},\tag{21}\end{equation*} wherein $J$ is defined in (10), and $\textrm {w}_{\textrm {penalty}} >0$ should be set relatively large to ensure that the penalty for the violations of softened nonlinear constraints is dominant. But please note that setting $\textrm {w}_{\textrm {penalty}} $ too high results in an ill-conditioned optimization problem.

The complete intermediate OCP is presented in the following form:\begin{align*}&\min \limits _{{\mathbf { z}}^{\ast }(t), {\mathbf { u}}(t), T} (21), \\&\quad ~\! \textrm {s.t.} ~\textrm {Kinematic constraints (3);} \\&\hphantom {\quad ~\! \textrm {s.t.} ~} \textrm {Boundary conditions (4) and (5a);} \\&\hphantom {\quad ~\! \textrm {s.t.} ~} \textrm {Within-corridor constraints} (15).\tag{22}\end{align*}

Theoretically, (22) is never subject to the risk of being infeasible because every vector that lies between the box boundaries would be a feasible solution that satisfies all of the box constraints. As (22) does not contain complex constraints, its numerical solution process is extremely fast.

We use the following pseudo-codes to present our proposed autonomous parking trajectory planning method.

The inputs of ParkingTrajectoryPlanning() include map and task. map presents the environmental setup, i.e. the geometric size and location of each obstacle, as well as the scenario boundaries. task presents the parking scheme, i.e. the initial and terminal configurations of the ego vehicle. The output of the function is the optimized parking trajectory in the form of a sequence of timed configuration points $\chi _{P} =\left \{{{\sigma _{0},\sigma _{1},\ldots,\sigma _{N_{FE}}} }\right \}$ . In line 1 of Alg. 3, GenerateCoarseTrajectoryViaFTHA() is used to generate a coarse trajectory. In line 2, FormInitialGuess() approximates the control profiles and the rest state profiles based on $\chi _{P} $ to form an initial guess. In line 4, the function DilateMap() is deployed to inflate the original obstacles so that a dilated map is built for corridor construction. In line 5, ${\varepsilon }_{\textrm {tol}} \to 0^{+}$ is a user-specified parameter denoting the convergence acceptance threshold, i.e., the softened nonlinear constraints are regarded as satisfied if $\psi _{\textrm {infeasibility}} < \varepsilon _{\textrm {tol}} $ . The parameter iter_max in line 7 denotes the maximum allowable iteration number, and exceeding this number renders a solution failure. GenerateOCP() formulates an intermediate OCP in the form of (22). $\textrm {SolveOCP(}OCP_{\textrm {INT}},sol)$ stands for solving $OCP_{\textrm {INT}} $ numerically. Concretely, it is about converting $OCP_{\textrm {INT}} $ into an NLP problem and solving it via an NLP solver, which is warm-started by $sol$ . With a solution vector sol at hand, we calculate its infeasibility degree $\psi _{\textrm {infeasibility}} $ via MeasureInfeasibility() according to (20). ExtractTrajectory() is a simple function to extract the trajectory part from the entire solution vector.

Algorithm 3 Autonomous Parking Trajectory Planning

Function $\textrm {ParkingTrajectoryPlanning}(map,task)$

1.

$\chi _{P} \!\leftarrow \!\textrm {GenerateCoarseTrajectoryViaFTHA(}map,task)$ ;

2.

$sol\leftarrow \textrm {FormInitialGuess(}\chi _{P})$ ;

3.

$iter\leftarrow 0, \psi _{\textrm {infeasibility}} \leftarrow +\infty $ ;

4.

$dilated\_{}map\leftarrow \textrm {DilateMap(}map)$ ;

5.

while $\psi _{\textrm {infeasibility}} \ge \varepsilon _{\textrm {tol}} $ do

6.

$iter\leftarrow iter+1$ ;

7.

if $iter>\textrm {iter}_{\max } $ then

8.

return with failure;

9.

end if

10.

$\Gamma =\textrm {GenerateCorridors(}\chi _{P})$ ;

11.

$OCP_{\textrm {INT}} \leftarrow \textrm {GenerateOCP(}task,\Gamma)$ ;

12.

$sol\leftarrow \textrm {SolveOCP(}OCP_{\textrm {INT}},sol)$ ;

13.

$\psi _{\textrm {infeasibility}} \leftarrow \textrm {MeasureInfeasibility(}sol)$ ;

14.

$\chi _{P} \leftarrow \textrm {ExtractTrajectory(}sol)$ ;

15.

end while

16.

return with $\chi _{P} $ .

Remark 1:

The final output of Alg. 3 is feasible w.r.t. the nominal planning scheme (11).

Recall that the output of Alg. 3, denoted as $\chi _{P}$ , is derived by solving an intermediate OCP in the form of (22). Thus $\chi _{P} $ satisfies all of the constraints in (22), including the boundary conditions (4) and (5a), and kinematics-related box constraints (3).

According to the criterion to exit the while loop, the finally derived $\chi _{P} $ satisfies that $\psi _{\textrm {infeasibility}} < \varepsilon _{\textrm {tol}}$ . According to the semi-positive definitions in (17)–​(20), $\psi _{\textrm {infeasibility}} < \varepsilon _{\textrm {tol}} $ indicates that \begin{equation*} J_{\textrm {EQ2}} < \varepsilon _{\textrm {tol}}, \quad J_{\textrm {EQ12}} < \varepsilon _{\textrm {tol}},~ J_{\textrm {EQ5B}} < \varepsilon _{\textrm {tol}}.\tag{23}\end{equation*}

When $\varepsilon _{\textrm {tol}} \to 0^{+}$ , $J_{\textrm {EQ2}}, J_{\textrm {EQ12}}$ , and $J_{\textrm {EQ5B}} $ approaches $0^{+}$ as well, which means the constraints (2), (12), and (5b) are satisfied.

Besides that, the usage of two disks to cover the rectangular vehicle body, the establishment of two tunnels, and the satisfaction of the within-tunnel constraints (15) ensure that $\chi _{P} $ satisfies the nominal collision-avoidance constraints (8) and (9). Since $\chi _{P} $ does not violate any of the constraints in the nominal OCP (11), Alg. 3 always provides a feasible solution to it.

Remark 2:

The final output of Alg. 3 is near-optimal w.r.t. the nominal planning scheme (11) if $\textrm {w}_{\textrm {penalty}} $ is sufficiently large.

If we set $\textrm {w}_{\textrm {penalty}} \gg {\varepsilon }_{\textrm {tol}}^{-1}$ , then $\textrm {w}_{\textrm {penalty}} \cdot \psi _{\textrm {infeasibility}} \to 0^{+}$ w.r.t. the final output $\chi _{P}$ . In that case, the intermediate cost function $J_{\textrm {INT}} \to J$ , which means the cost function of the last intermediate OCP is consistent with the nominal OCP (11).

Let us denote the feasible region of an OCP as ${\Gamma }_{ID}$ , where ID denotes the equation index. ${\Gamma }_{22}$ is a subset of ${\Gamma }_{11}$ because the establishment of the within-corridor constraints defiantly indicates that the vehicle body is presented by two dicks rather than a rectangle. The usage of two discs wastes free spaces inevitably. Since ${\Gamma }_{22}$ is slightly smaller than ${\Gamma }_{11}$ , ${\chi }_{P} $ is not an optimal solution if the optimum lies in ${ \Gamma }_{22}\cap ^{\neg } {\Gamma }_{11}$ .

SECTION V. Experimental Setups, Results, and Discussions

A. Simulation Setup

Simulations were conducted in C++ and executed on an i9-9900 CPU with 32 GB RAM that runs at 3.10 $\times $ 2 GHz. Regarding the function SolveOCP() defined in Section IV.B, we use the first-order explicit Runge-Kutta method to form the NLP problem and use the primal-dual interior-point solver IPOPT [37] with the linear solver MA97 [38] called through the AMPL interface [39]. Basic parametric settings are listed in Table I.

TABLE I Parametric Settings Regarding Model and Approach

A set containing 115 simulation cases is built to evaluate the performance of the proposed parking trajectory planner. Manually selected from 6,000 randomly generated cases, the 115 cases are classified as maneuvering, detouring, and narrow passage traverse. Each case contains 5 polygonal obstacles; the vertex number of each obstacle randomly ranges from 4 to 7; the location of each vertex obeys the uniform distribution within a $40\textrm {m}\,\,\times \,\,40\textrm {m}$ workspace; each polygonal obstacle’s area ranges randomly from 5m² to 50m²; the initial and terminal locations of the parking vehicle are uniformly distributed in the workspace; the initial and goal orientations are uniformly distributed in $[-2\pi,2\pi]$ ; and the other boundary constraints are set to \begin{align*} \left [{ {{\begin{array}{cccccccccccccccccccc} {v(0)} \\ {a(0)} \\ {\phi (0)} \\ {\omega (0)} \\ \end{array}}} }\right]=\left [{ {{\begin{array}{cccccccccccccccccccc} {v(T)} \\ {a(T)} \\ {\phi (T)} \\ {\omega (T)} \\ \end{array}}} }\right]=\left [{ {{\begin{array}{cccccccccccccccccccc} 0 \\ 0 \\ 0 \\ 0 \\ \end{array}}} }\right].\end{align*}

B. Simulation Results and Discussions

To evaluate the performance of the proposed trajectory planner, comparable algorithms are defined in Table II. The performances of Alg. 3 and its competitors on the 115 simulation cases are summarized in Tables III.

TABLE II Definitions of Comparable Trajectory Planners

TABLE III Comparative Simulation Results

Directly solving the nominal OCP (11) is time-consuming. Given that the nominal collision-avoidance constraints (8) and (9) are highly non-convex and almost non-differentiable, the Naïve NLP solution process cannot easily get correct gradient information when the initial guess is not close to being optimal. As the result, 10 of the gross 115 cases fail to be solved by the Naïve NLP method.

The STC method is featured by solving the OCP (16) once. Since the collision-avoidance constraints in the OCP (16) are linear, the numerical solution process is faster. However, as we mentioned in Section IV.A, the STC method relies on the initial guess. In each of the 4 failed cases, the corridor construction operation leaves out essential free spaces to form a feasible solution.

Regarding the TEB method, the nominal OCP is softened as an unconstrained optimization problem and then solved quickly via the $\text{g}^{2}\text{o}$ solver. TEB is featured by running fast, but the finally derived solution may violate the kinematic constraints or the collision-avoidance constraints, which is regarded as a solution failure.

The TDR-OBCA method is a variant of the conventional OBCA method [20]. In contrast with the OBCA method, the TDR-OBCA method refines the coarse trajectory before using it as the initial guess. Also, it reformulates the constraints/cost function to enhance the planning robustness and efficiency. As seen in Table III, all of the 115 cases are solved with success using the TDR-OBCA method, but the CPU runtime is longer because that method, like the basic OBCA method, does not consider the intractability issue of the collision-avoidance constraints.

Our proposed planning method scales tractably w.r.t the environmental obstacles, and is insensitive to the quality of the initial guess at the very beginning. It builds a lightweight iterative framework to recover from kinematical infeasibility. We name the framework as lightweight because each intermediate OCP only contains a tractable number of box constraints. These are the reasons that our proposal outperforms its competitors listed in Table II.

In our proposed Alg. 3, L_limit is a critical parameter that decides the maximum length/width of each local box. By setting L_limit to different values, the changes in the success rate and the average CPU time are depicted in Fig. 7. From that figure, one may conclude that setting L_limit smaller makes the corridor construction faster but leads to more iterations; setting L_limit larger consumes more time in constructing the corridors within one iteration, but fewer iterations are needed before the trajectory planning process is completed. The success rate is not affected unless L_limit is set overly small ($\text{L}_{\mathrm {limit}} =0.1$ ). This is a merit of our proposed iterative framework. As a comparison, in using the STC method to solve the 115 cases with $\text{L}_{\mathrm {limit}} =0.1$ , we find that the success rate drastically reduces to 6.09%.

Fig. 7.

On the performance of Alg. 3 under different settings of L_limit.

A video containing the simulation results is available at bilibili.com/video/BV1n7411q7iv/. A typical simulation result is depicted in Fig. 8.

Fig. 8.

Typical simulation results: (a) footprints of planned parking trajectory; (b) constructed corridors consisting of two series of local boxes.

C. Real-World Experimental Setups and Results

Besides simulations, we have also carried out real-world experiments based on a small car-like robot in a $1.8\textrm {m}\,\,\times 1.2\,\,\textrm {m}$ indoor scenario.

As depicted in Fig. 9, the vehicle localization is done visually. Concretely, the location and orientation of the car-like robot are identified through tracking the AprilTag placed on the top of the robot via a bird-eye-view camera. AprilTags are also used to “percept” the location and orientation of each registered obstacle. The perception and localization information is sent to a desktop PC, in which our proposed autonomous parking trajectory planning method would be implemented once to generate an open-loop trajectory before the robot begins to move. In using the proposed planner, we add a buffer of 0.01m to the radius of each covering disc such that collisions caused by small tracking errors would be avoided [41]. Parametric settings different from Table I are listed in Table IV. The derived open-loop trajectory is sent to the robot platform via ZigBee communication. Thereafter, the open-loop trajectory is used for closed-loop control. Particularly in the closed-loop trajectory control module, we use a PID controller for longitudinal tracking and use a pure pursuit controller for lateral tracking. We set the execution frequency of the control module to 10.0 Hz.

TABLE IV Updated Parametric Settings for Experiments

Fig. 9.

Experimental platform based on an ackermann-steering car-like robot.

Fig. 10 shows the closed-loop tracking performances under various conditions in a typical vertical parking scenario. We generate an extremely precise open-loop trajectory under $\text{N}_{\mathrm {FE}} =200$ (N_FE, as listed in Table I, decides the discretization accuracy when converting an OCP to an NLP problem). Thereafter, the open-loop trajectory is down-sampled by setting N_FE to smaller values and then sent to the low-level controller for closed-loop tracking. The results show that the numerical discretization accuracy is not a dominant factor that influences the whole parking performance. Also, Fig. 10 shows that our planner can generate easy-to-track trajectories.

Fig. 10.

Closed-loop parking trajectory tracking performances under various discretization accuracy conditions.

SECTION VI. Conclusion

This paper has proposed an autonomous parking trajectory planning method in a generic unstructured environment. To get precise and optimal solutions, we build an OCP to describe the concerned scheme. To solve it efficiently, we convert the nominal collision-avoidance constraints to within-corridor conditions via STC. The performance of the adopted STC method depends on the quality of the initial guess, thus we 1) propose the FTHA method to enhance the time efficiency of the coarse trajectory generation process, and 2) build an iterative framework to gradually recover from a poor initial guess.

Generally speaking, the optimization-based planners run more slowly than the sampling-and-search-based planners, thus how to enhance the runtime efficiency is a primary concern. This work has confirmed that simply softening the hard constraints via external penalty functions does not guarantee solution feasibility, although it makes the optimization process faster. Linearizing the original collision-avoidance constraints without making their scale tractable, as the OCBA-related methods do, renders a low calculation speed. Relying highly on the initial guess, as the STC method does, is not a promising approach especially in dealing with intricate scenarios with narrow passages. By contrast, our proposed method balances the solution accuracy, optimality, solution speed, and robustness (weak reliance on the initial guess). All of the difficulties in the planning scheme can be dispersed into the intermediate problems defined in our iterative framework. We expect that the kinematics infeasibility and the solution optimality can evolve in cooperation during the iteration. In this sense, there is no need to make the intermediate procedures too strict, thus we make each intermediate problem lightweight, i.e., a simple optimization problem with fixed-scale box constraints. Comparative simulation results have shown that our proposal outperforms the other few optimization-based strategies w.r.t the CPU runtime and success rate. The proposed iterative framework can be extended to handle generic high-dimensional state transfer problems with nonlinear system dynamics and large-scale non-convex exterior constraints.

It deserves to point out that our proposed planner cannot alter the homotopy class after a coarse trajectory is identified at the very beginning via FTHA. If the coarse trajectory is homotopically misleading, then our proposed method is not able to find a different homotopy class. As our future work, fault-recovery strategies should be designed in the outer loop of our iterative framework so that imperfect homotopy classes can be altered, which makes the entire optimization-based planner insensitive to the initial guess.

ACKNOWLEDGMENT

Bai Li would like to thank Prof. Li Li, Dr. Weitian Sheng, Dr. Oskar Ljungqvist, Prof. Changliu Liu, Muyuan Niu, Peng Song, Yu Zheng, and Shiliang Jin for their support during this study.