Spatio-temporal Motion Planning for Autonomous Vehicles with Trapezoidal Prism Corridors and Bézier Curves

Speed planning techniques can be classified into three categories: 1) search and optimization; 2) sampling lattices and selecting the minimum-cost trajectory; 3) approximated optimization. The Search and optimization method searches for the best candidate speed profile and optimizes the curve for smoothness; see the post-optimization method[3], the Baidu EM motion planner [8], and the Piecewise-Jerk Speed Optimization [9]. The Sampling approach samples different speed lattices combined with path lattices. The generated local spatial-temporal trajectories are evaluated and the one with minimum cost is selected. Related works can be found in [2, 3, 4]. Most works in the first and the second categories conduct search and optimization directly in the $S-T$ graph. Approximated optimization considers a vehicle dynamic model in a sequential optimization problem; see convex feasible set algorithm [10] and optimal control methods, such as model predictive control (MPC)[11] and constrained iterative linear quadratic regulator (CiLQR)[12]. The advantage of these approaches is that they mitigate the planning and control inconsistency problem since the dynamic model has already been considered in the planning layer. However, the disadvantage is the high computation cost.

The spatial corridor is widely used in trajectory generation. Some previous works generate the corridors in a static environment and cannot deal with dynamic obstacles [13], [14]. Liu et al. [15] find a convex feasible set around the reference trajectory, but the computation complexity restricts the method for real-time applications. Our previous work proposes the use of trapezoidal corridors for convexifying 2D space in the $S-T$ graph [16]. Zhang et al. present a general convex spatio-temporal corridors-based approach [17]. Xu et al. propose using a modified vertical cell decomposition approach for speed planning in [18]. All these methods suffer from the limitations of the layered planning approach discussed in the previous section. Ding et al. use the spatio-temporal semantic corridor (SSC) method to uniformly express obstacles and traffic rules in the 3D $S-L-T$ space [5]. However, restricting the shape of the corridors to simple cuboids drastically limits the search space for optimization in complex scenarios. Our proposed method of extending trapezoid-shaped 2D corridors in $S-T$ to 3D $S-L-T$ space significantly enlarges the solution space for trajectory optimization. This enables us to extend the spatial corridor to the spatio-temporal domain to cope with dynamic obstacles while meeting the real-time requirement.

Previously, monomial basis polynomials have been used to generate trajectories [1], [8]. However, these methods often fail to represent highly constrained maneuvers in the presence of dynamic obstacles. They also fail to give safety guarantees between sample points, as the constraints are only enforced/checked on a finite set of sampled points. In [19], a smooth and continuous speed profile is computed by proper curve concatenation without optimization and dynamic obstacles. Bézier polynomials combined with rectangular corridors was originated in the area of unmanned aerial vehicles (UAVs) [20], [21]. Ding et al. extended it for motion planning of unmanned ground vehicles (UGVs) [5]. The significant limitation is that the proposed cuboidal corridor representation fails to make the most of free space for optimization. In our work, we propose to use time-dependent trapezoidal prism-shaped corridors and give sufficient conditions to enforce Bézier curves in these time-dependent corridors for safety. It is theoretically proved that the trapezoidal prism-shaped corridors can enlarge the solution space for improved optimization.

The $S-L-T$ graph represents all traffic participants’ positions at each time step including the past, current, and prediction. As an example, in Fig. 1a, we take the case of two cars moving at constant speeds for simplicity. The scenario is described as follows: car A and the ego vehicle are driving in a lane which has a static obstacle (e.g., a construction site). A lane change maneuver is enforced for both vehicles. We list the following typical entities:

1) Static obstacle (6-zero-slope-faces type): As the most simple entity, all the six faces have zero slopes, see the bird’s-eye view of the yellow block in Fig. 1a and the cuboid in the $S-L-T$ plot in Fig. 1b.

2) Moving car with only longitudinal motion (4-zero-slope-faces type): Car C moving straight forward is an example shown in Fig. 1a. The top and bottom faces of Car C in Fig. 1b have zero slopes. The two faces (see another angle of view for left and right faces in Fig. 1c) are perpendicular to the $S-L$ plane without slopes. The side length of the parallelogram along the $L$ axis is the width of the vehicle plus the safety region. The side length of the parallelogram along the $S$ axis is the length of the vehicle plus half the length of the ego vehicle as a safety region.

3) Moving car with longitudinal and lateral motions (2-zero-slope-faces type): Car A moving left and forward is an example shown in Fig. 1a. As we can see in Fig.1b, only the top and down faces are zero-slope.

The 3D free space in the $S-L-T$ graph is non-convex in general. We propose an over-approximation of the 2-zero-slope-faces type parallelepiped, e.g., car A in Fig. 1b, into a 4-zero-slope-faces type parallelepiped, see the pink inflated space in Fig. 1b and Fig. 1d. There are two significant benefits of doing so: 1) we will show that in the presence of such parallelepipeds, we can extend our 2D corridor construction algorithm [16] to construct 3D trapezoidal prism-shaped corridors efficiently; 2) though we pay the cost of losing some space due to over-approximation, the safety corridor is still significantly larger than the state-of-the-art cuboidal corridors. Note that the transformation between 2D and 3D in our method is without the loss of search space. Thus, in summary, we make a good trade-off between exactness and efficiency. The faces chosen for the over-approximation step are decided by a simple minimization of the volume of search space compromised in the process. Intuitively, if the lateral velocity is less than the longitudinal velocity of the vehicle, the corresponding faces are chosen for over-approximation.

A Bézier polynomial is a polynomial function represented by linear combinations of Bernstein bases. The $n$th-order Bézier polynomial is written as

where the Bernstein bases satisfy $b_{n}^{i}(t)=C_{i}^{n}\cdot t^{i}\cdot(1-$ $t)^{n-i},t\in[0,1]$. The coefficients of the polynomial $c_{i}(i=$ $0,1,\ldots,n$) are also called control points. Compared to monomial polynomials, Bézier curves have the following properties:

• •

  The time interval is defined on $t\in[0,1]$.

• •

  The Bézier polynomial starts at control point $B(0)=c_{0}$ and ends at $B(1)=c_{n}$.

• •

  Convex hull property: The Bézier curve $B(t)$ is confined within the convex hull of control points.

• •

  Hodograph property: By the hodograph property, the derivative of $B(t),\dot{B}(t)$, can also be written as a Bézier polynomial with control points $c_{i}^{1}=n\cdot\left(c_{i+1}-c_{i}\right),i=$ $0,1,\ldots,n-1$. By applying the convex hull property to the derivative Bézier curve, the entire dynamical profile of the original curve $B(t)$ can be confined within a given dynamical range.

To mitigate the numerical instability issue, piecewise Bézier polynomials with lower orders are used instead of using a high-order Bézier polynomial for the whole planning horizon. Each piece of the trajectory is associated with one trapezoidal-prism corridor. Note that $B(t)$ is defined on a fixed time interval $[0,1]$. For a whole trajectory with $m+1$ pieces, in each piece $\left[T_{k},T_{k+1}\right](k=0,1,\ldots,m)$, we use a scaling transformation and translation transformation in the time domain to map it into the interval $[0,1]$ [13]. Then, the whole piece-wise trajectory in one dimension $\sigma\in\{s,l\}$ is:

where $h_{i}$ is the scaling transformation factor and $T_{i}$ is the translation transformation factor for $i=0,1,\ldots,m$ with setting $T_{0}=0$.

Suppose the whole safe region is divided into $m+1$ pieces with time intervals $\left[T_{0},T_{1}\right],\ldots,\left[T_{m},T\right]$ and $T=T_{m+1}$, with each interval corresponding to a convex safe region. The details of such a convexification algorithm will be introduced in the next section. The $k$-th convex safe region in $S-L-T$ space can be represented as

where $s_{i}$ and $l_{i}$ are the longitudinal and lateral coordinates of the $i^{th}$ control point respectively, $\underline{p_{0}^{k}},\underline{p_{1}^{k}}$ are bias and skew of the lower bound and $\overline{p_{0}^{k}},\overline{p_{1}^{k}}$ are those of the upper bound. $h_{k}$ denotes the length of the $k$-th time interval and satisfies $h_{k}=T_{k+1}-T_{k},k=0,1,\ldots,m$.

Then, the whole safe region is the union of a set of piecewise-safe sub-regions: $\mathcal{S}=\mathcal{S}_{0}\cup\cdots\cup\mathcal{S}_{m}$. The speed planning is safe if $\forall t_{0}\in[0,T],s\left(t_{0}\right)\in\mathcal{S},l\left(t_{0}\right)\in\mathcal{S}$, which is equivalent to for $t_{0}\in\left[T_{k},T_{k+1}\right],s\left(t_{0}\right)\in\mathcal{S}_{k},l\left(t_{0}\right)\in\mathcal{S}_{k},k=0,1,\ldots,m$, i.e.,

Algo. 1 outlines the 3D trapezoidal corridor generation process. The original non-convex space is sliced along the $L$ axis at the starting or ending $L$ coordinates of any obstacles in the $S-L-T$ graph. This generates 3D chunks of the non-convex space which can be projected in a 2D $S-T$ graph without the loss of any search space. As an example, in Fig. 2a, any slices at $L$ coordinates in the range $[1,3)$ will give us the 2D $S-T$ cross-section as seen in Fig. 2c. Similarly, any slice at an $L$ coordinate between $[3,6.7)$ will give us the $S-T$ graph as seen in Fig. 2d. The inputs to Algo. 1 are the upper and lower bounds in the $S$ and $L$ direction w.r.t. the ego vehicle and the road. The bounds are measured over a time horizon using a discrete time interval $\Delta$. For each slice, we construct 2D convex corridors in the corresponding $S-T$ graph. In this work, we extend the 2D convex trapezoidal corridor generation algorithm in our previous work [16]. The new algorithm is presented as Algo. 2.

Algo. 2 outlines the construction of 2D piecewise-convex safe regions in any given $S-T$ cross-section along the $L$ axis. The lower and upper bounds in the $S$ direction serve as inputs to this algorithm. We refer readers to [16] for further details about the working of Algo. 2. A key modification in our work is that in the subroutine $SingleRegionCaculate()$, we also initialize the upper and lower boundaries of the regions in the $L$ direction (Algo. 3. Lines 6,7). Our over-approximation step and the design of Algo. 1 guarantee that these boundaries are the same for all 2D convex regions generated by Algo. 2. Thus, we essentially get 2D trapezoidal-shaped corridors dragged along the $L$ axis to form 3D trapezoidal prism-shaped convex corridors. Finally, $RegionSplit()$ is used to check the length of each 2D convex region. If it is above a user-defined threshold (e.g., $1\ s$ in our experimental setting), it will be split into multiple sub-regions, for which the time intervals are all below the threshold. This refinement operation aims to avoid underfitting.

This 2D corridor generation process is repeated for all distinct obstacle boundaries in our $S-L-T$ graph (Line 1, Algo. 1). The initialization of bounds along the $L$ axis ensures that we get 3D trapezoidal-shaped convex corridors. Since the length of the corridors in the $L$ direction is given by the starting or ending of the obstacles in the $S-L-T$ space, we can guarantee the safety of all the corridors generated using Algo. 1. Note that for the space divided by obstacles, we select the unique space enclosing the reference trajectory using the $SelectCorridors()$ method. For yielding to car A, the corridors lying in the green region of Fig. 2c and 2d are chosen according to the 3D reference waypoints. Note that once the corridors are selected, the whole optimization is solved as a single problem and not decomposed into individual optimizations for separate corridors.

As discussed in Sec. III C, the convex hull property of the Bézier curves is used to enforce that the trajectory in the $S-L-T$ graph stays in the safe region $\mathcal{S}$. We first formally define a corridor for our trajectory generation:

Ding et al. presented the construction of cuboidal corridors in the $S-L-T$ graph [5]. Constraints of the control points of cuboidal corridors are given by the following proposition:

The proof of safety enforcement in rectangular corridors can be found in [16], and can be straightforwardly extended to the third dimension $L$ for cuboidal corridors.

The optimization fails if any lower bound ($\underline{p}_{0}^{k}+h_{k}\underline{p_{1}^{k}}$) is greater than the upper bound ($\overline{p_{0}^{k}}$). In order to avoid this, the time interval of the $k$-th corridor must satisfy $h_{k}\leq\frac{\overline{p_{0}^{k}}-p_{0}^{k}}{\underline{p_{1}^{k}}}$.

In [5], Ding et al. propose a seed generation and cube inflation method to adjust time intervals. However, this method generates a significant number of corridors and optimized variables, which leads to a high computation cost. In common driving scenarios (Fig. 1), we have $\underline{p_{1}^{k}}>0$ or $\overline{p_{1}^{k}}>0$, due to which the cuboidal corridors fail to cover all the safe regions. As a result, the search space is sub-optimal and the constraints on control points to enforce the trajectory are overtightened.

The sufficient conditions of control points $c_{i}$ to keep the longitudinal and the lateral trajectory safe and in our proposed trapezoidal-prism corridors are built upon the following lemma.

The proof can be found in [16]. We leverage the following theorem meant for 2D trapezoidal corridors to construct 3D trapezoidal prism-shaped corridors.

The proof for the 2D case of the above theorem can be found in [16], and can be easily extended to another dimension $L$.

In Theorem 1, conditions on $c_{i}^{s}$ are $\underline{p_{0}^{k}}+h_{k}\underline{p_{1}^{k}}M_{i,1}\underline{p_{1}^{k}}\leq c_{i}^{k,s}\leq\overline{p_{0}^{k}}+h_{k}\overline{p_{1}^{k}}M_{i,1}$. Compared to the safety enforcement in cuboidal corridors in Proposition 1, we have $\underline{p_{0}^{k}}+h_{k}\underline{p_{1}^{k}}M_{i,1}\leq\underline{p_{0}^{k}}+h_{k}\underline{p_{1}^{k}}$ and $\overline{p_{0}^{k}}+h_{k}\overline{p_{1}^{k}}M_{i,1}\geq\overline{p_{0}^{k}}$. The advantage of having trapezoidal corridors is twofold: i) By the proof of $\underline{p_{0}^{k}}+h_{k}\underline{p_{1}^{k}}M_{i,1}\underline{p_{1}^{k}}\leq c_{i}^{k,s}\leq\overline{p_{0}^{k}}+h_{k}\overline{p_{1}^{k}}M_{i,1}$, the lower boundaries are guaranteed to be smaller than the upper boundaries all the time. Recall that for the rectangular corridors, we need to always check $h_{k}\leq\frac{\overline{p_{0}^{k}}-p_{0}^{k}}{\underline{p_{1}^{k}}}$; ii) The constraints are relaxed, therefore the solution space is enlarged compared with the rectangular corridors (see the illustration for the comparison in Fig. 3).

The objective function is established as

where $s^{r}(t)$ and $l^{r}(t)$ are the reference longitudinal and lateral trajectories, and $v^{r}_{s}$ and $v^{r}_{l}$ are the reference velocities in the two directions. For $J_{s}$ and $J_{l}$, their first terms penalize the deviation from the reference; the second ones penalize the deviation between the actual and reference speed; the third and the fourth terms penalize acceleration and jerk, respectively. The last terms penalize the deviation of the ending position from the reference. We used Optuna[22] for tuning all the 10 parameters.

The optimization considers the following constraints:

• •

  Boundary Constraints: The piecewise curve starts from fixed position, speed, and acceleration, i.e.,

  $$c_{i}^{0,l}h_{k}^{(1-l)}=\left.\frac{\mathrm{d}^{l}f(t)}{\mathrm{d}t^{l}}\right|_{t=0},\quad l=0,1,2,$$

  where $c_{i}^{k,l}$ is the control point for the $l$th-order derivative of the $k$-th Bézier curve. Note that $c_{i}^{k,l}$ has two dimensions: $\{S,L\}$.

• •

  Continuity Constraints: The piecewise curve must be continuous at the connected time points for position, speed, and acceleration.

  $$c_{n}^{k,l}h_{k}^{(1-l)}=c_{0}^{k+1,l}h_{k+1}^{(1-l)},l=0,1,2,k=0,1,\ldots,m-1$$

• •

  Safety Constraints: With our proposed trapezoidal-prism corridors, safety constraints for the longitudinal dimension of the control point can be given as

  $$\underline{p_{0}^{k}}+h_{k}\underline{p_{1}^{k}}M_{i,1}\leq c_{i}^{k,0}\leq\overline{p_{0}^{k}}+h_{k}\overline{p_{1}^{k}}M_{i,1},k=0,1,\ldots,m$$

  and those for the lateral dimension of the control point can be given as

  $$l_{beg}\leq c_{i}^{k,0}\leq l_{end}$$

• •

  Physical Constraints: The physical constraints under consideration include the limit of a vehicle’s velocity, acceleration, and jerk. We can use the hodograph property of a Bézier curve to calculate velocity, acceleration, and jerk. The constraints are given by

  	$\displaystyle\beta^{k,1}$	$\displaystyle\leq c_{i}^{k,l}\leq\overline{\beta^{k,1}}$
  	$\displaystyle\beta^{l}$	$\displaystyle\leq c_{i}^{k,l}\leq\overline{\beta^{l}},l=2,3$

  where $k=0,1,\ldots,m$ and it follows that $c_{i}^{k,l+1}=(n-$ $l)\left(c_{i+1}^{k,l}-c_{i}^{k,l}\right)$. The upper bounds $\overline{\beta^{k,1}}$ are determined by speed limits on road and centripetal acceleration constraints. Let $a_{cm}$ be the maximum acceleration permitted and $\kappa_{k}$ the maximum curvature of the path for $t\in\left[T_{k},T_{k+1}\right]$ (see [23] for details). The lateral acceleration constraints are given by

  $$c_{i}^{k,l}\leq\overline{\beta^{k,1}}=\sqrt{\frac{a_{cm}}{\kappa_{k}}}.$$

  The bounds on longitudinal and lateral accelerations and jerks are constant for different pieces of speed profiles.

Then, the trajectory optimization process can be formulated as a quadratic programming (QP) problem as

We refer readers to the appendix of our previous work [24] for the detailed formulation process. This problem can be solved in real-time by a modern solver such as OSQP[25].

We conduct numerical simulations to compare the proposed approach with cuboidal corridors [5]. The planning horizon is 7 s. Different road scenarios are as follows:

The simulations in this part are conducted on the CommonRoad platform [26], which provides an interactive simulated and non-interactive real traffic environment for validating motion planning algorithms. A given scenario is considered “solved” when the ego vehicle reaches the desired goal region while satisfying all the constraints. We visualize the bird’s-eye view simulation of the lane change scenario in Fig. 1. The results obtained using our approach can be seen in Fig. 6. The cuboidal corridor approach did not yield a collision-free trajectory as it failed to replan owing to the lack of search space. It also had significantly high acceleration as can be observed in Fig. 3c.