Embodied Footprints: A Safety-Guaranteed Collision-Avoidance Model for Numerical Optimization-Based Trajectory Planning

Optimization-based methods are commonly applied in autonomous driving trajectory planners, which transform the continuous-time trajectory planning problem into a finite nonlinear program with constraints imposed at finite collocation points. However, potential violations between adjacent collocation points can occur. To address this issue thoroughly, we propose a safety-guaranteed collision-avoidance model to mitigate collision risks within optimization-based trajectory planners. This model introduces an “embodied footprint”, an enlarged representation of the vehicle’s nominal footprint. If the embodied footprints do not collide with obstacles at finite collocation points, then the ego vehicle’s nominal footprint is guaranteed to be collision-free at any of the infinite moments between adjacent collocation points. According to our theoretical analysis, we define the geometric size of an embodied footprint as a simple function of vehicle velocity and curvature. Particularly, we propose a trajectory optimizer with the embodied footprints that can theoretically set an appropriate number of collocation points prior to the optimization process. We conduct this research to enhance the foundation of optimization-based planners in robotics. Comparative simulations and field tests validate the completeness, solution speed, and solution quality of our proposal.

SECTION I. Introduction

Trajectory planning, a core module in an autonomous driving system, is designated to generate spatio-temporal curves that are kinematically feasible, collision-free, passenger-friendly, and energy/time efficient [1], [2]. The existing trajectory planners for autonomous vehicles are broadly classified into search-based, sampling-based, and numerical optimal control-based methods [3], [4], [5], [6]. Search-/sampling-based planners seek a globally optimal path, while numerical optimal control-based planners aim for local optimality. This paper concentrates on numerical optimal control-based planning.

A numerical optimal control-based planner describes a nominal trajectory planning scheme as an optimal control problem (OCP). The OCP is discretized into a nonlinear program (NLP) problem before the NLP is solved by a gradient-based NLP solver [7]. This converts the continuous-time OCP into a problem with finite variables and constraints [8]. However, this conversion is imperfect because the time-continuous constraints in the original OCP are only enforced on finite collocation points, ignoring constraint satisfaction between adjacent collocation points. Increasing the density of collocation points can help, but it does not entirely solve the problem and results in high-dimensional NLPs that require extensive runtime. This constraint ignorance issue has been a common and silent drawback of numerical optimal control-based planners, particularly when constraints are highly non-convex [9], [10], [11].

Collision-avoidance constraints are widely acknowledged as the most complex type of constraint in a trajectory planning problem [12]. As depicted in Fig. 1, ignorance of collision-avoidance constraints between adjacent collocation points easily fails a planned trajectory [13]. This study aims to propose a constraint formulation method that theoretically guarantees safety between adjacent collocation points in an optimal control-based trajectory planner.

Fig. 1.

Schematics on violations of collision-avoidance constraints between adjacent collocation points. Two discretized footprints are collision-free while collisions do occur between them.

A. Related Works

This subsection reviews the previous studies that explored the violations of collision-avoidance constraints between adjacent collocation points or waypoints. For brevity, we refer to this violation as a non-collocation error throughout this paper.

Most prior works silently assume that non-collocation errors don’t occur if 1) collocation points are sufficiently dense [14], [15], 2) each polygonal obstacle is sufficiently dilated to form a buffer [16], [17], or 3) a cost function with sufficiently large penalty weights is designed to keep the planned trajectory away from obstacles [18]. However, these assumptions lack a quantitative analysis of sufficiency, preventing them from becoming standardized knowledge.

The use of a convex hull to encompass the area swept by the vehicle footprint between two adjacent points has been considered [19], [20]. Nevertheless, a convex hull does not guarantee full coverage of the swept region (Fig. 2). Schulman et al. [21] suggested expanding the convex hull, but the implementation was not detailed. They proposed a repulsive penalty in the cost function as a solution to non-collocation errors, but this does not fully eliminate collision risks. Scheuer and Fraichard [22] proposed a motion polygon to cover the swept-by region between adjacent collocation points, but part of the swept-by region is still out of the motion polygon (Fig. 3a). Additionally, their motion polygon cannot handle instances where adjacent poses overlap. Ghita and Kloetzer [23] proposed a polygonal over-approximation method, which is about building an expanded polygon with the intersection of two tangents to the swept-by region (Fig. 3b). However, that method is still too complex for a gradient-based NLP solver because the vertexes of the swept-by region are not easy to present. As seen from Fig. 3, the swept-by region is over-approximated in [22] or [23], leading to overcautious trajectories or even solution failures. To summarize, modeling the swept-by region as a compact polygon is inapplicable.

Fig. 2.

Schematics on the usage of a convex hull to cover non-collocation point footprints. The edges of the convex hull are colored blue, which does not fully cover the swept-by region.

Fig. 3.

Schematics on the usage of a polygonal region to cover the swept-by region between two adjacent vehicle footprints: (a) motion polygon proposed in [22]; (b) polygonal over-approximation method proposed in [23].

Contrary to compact polygon approaches, Li et al. [24] employed multiple polygons to cover the swept-by region. This strategy entails the interpolation of equidistant waypoints between two adjacent collocation points and the imposition of a collision-avoidance constraint at each interpolated waypoint. However, this strategy is still incomplete due to lingering minor collision risks. Zhang et al. [25] proposed a virtual protection frame (VPF) method, which covers the swept-by region by laterally expanded rectangles at each collocation point (Fig. 4). Unlike the compact polygon models, the VPF method ensures that the OCP dimension remains unchanged after addressing the non-collocation error. Nevertheless, the size of each expanded rectangle is established through trial and error, necessitating repeated iterations of the OCP solution process and consuming excessive runtime. More critically, determining the expanded rectangle’s size via trial-and-error risks failure of the iterative solution process, particularly in a tiny environment (this is demonstrated through comparative simulations in Section V). Similar to VPF, methods like the adaptive mesh refinement by Yershov and Frazzoli [26], and the moving finite element method by Chen et al. [27] also exist. In [26], the trajectory resolution is enhanced by continually adjusting the discretized mesh grids, while [27] persistently shifts the non-uniform collocation points until the non-collocation error is minimized. However, both [26] and [27] operate on a trial-and-error basis, making them challenging for quick execution.

Fig. 4.

Schematics on the usage of virtual protection frames to cover the swept-by region. Notably, the entire swept-by region is covered by two rectangles (i.e., the blue dashed boxes), which are derived by expanding the ego vehicle’s footprints laterally.

In summary, no previous study has satisfactorily addressed the non-collocation error issue considering both completeness and solution speed.

B. Motivations and Contributions

Upon reviewing previous studies, we noted that 1) increasing the density of collocation points would not completely address the issue of concern, and 2) the compact polygon-based methods are excessively complex. The VPF method is promising because it addresses the non-collocation error issue by operating only on collocation points. However, the VPF method is imperfect because it calls for an iterative planning process to determine the expansion buffers, which consumes runtime and easily makes the planner incomplete. In response to the limitations of the VPF method, this work aims to theoretically define expansion buffers, eliminating the need for time-consuming iterative computation.

The contribution of this work lies in the theoretical development of a collision-avoidance constraint model, referred to as the embodied footprint model, which ensures complete safety between adjacent collocation points in using a numerical optimal control-based planner. Specifically, the vehicle footprint at each collocation point is expanded longitudinally and laterally by buffers that fully cover the swept-by region between adjacent collocation points. These buffers, directly related to vehicle speed and curvature, are treated as decision variables along with other state/control variables within the OCP. This gives a numerical optimal control-based planner enough flexibility to find a safe trajectory without resorting to iterative computation.

C. Organization

In the rest of this paper, Section II states the concerned problem in a generic trajectory planning background. Section III proposes a safe collision-avoidance constraint modeling method. Section IV introduces a trajectory planner embedded with the modeling method introduced in Section III. Section V presents and discusses the simulation and experimental results. Finally, Section VI concludes the paper.

SECTION II. Trajectory Planning Problem Statement

This section outlines the trajectory planning problem. A generic formulation of a trajectory planning task is provided in the form of an OCP, which is then discretized into an NLP problem.

A. OCP Formulation

A trajectory planning task is cast into the following OCP: $$\begin{align*} & \mathop {\text {minimize}}\limits _{{ \mathbf{z}(t), {\mathbf{u}}(t), \textrm {}T}} J, \\ &\,\textrm {s.t.,}\quad \dot {{{\mathbf{z}}}}(t)=f\left ({ { \mathbf{z}(t), \textrm {}{\mathbf{u}}(t)} }\right ), t\in [0,T]; \\ &\hphantom {\,\textrm {s.t.,}\quad }{\underline{ \mathbf {z}}}\le {\mathbf{z}}(t)\le \bar {{{\mathbf{z}}}}, {\underline{ \mathbf {u}}}\le {\mathbf{u}}(t)\le \bar {{{\mathbf{u}}}}, t\in [0,T]; \\ &\hphantom {\,\textrm {s.t.,}\quad }{\mathbf{z}}(0)={\mathbf{z}}_{\textrm {init}}, {\mathbf{u}}(0)={\mathbf{u}}_{\textrm {init}}; \\ &\hphantom {\,\textrm {s.t.,}\quad }{\mathbf{z}}(T)={\mathbf{z}}_{\textrm {end}}, {\mathbf{u}}(T)={\mathbf{u}}_{\textrm {end}}; \\ &\hphantom {\,\textrm {s.t.,}\quad }fp({\mathbf{z}}(t))\subset \Upsilon _{\textrm {FREE}}, \textrm {}t\in [0,T]. \tag{1}\end{align*}$$ In this formulation, ${\mathbf{z}}\in \mathbb {R}{ }^{\textrm {n}_{\textrm {z}} }$ represents the vehicle state profiles, ${\mathbf{u}}\in \mathbb {R}{}^{\textrm {n}_{\textrm {u}} }$ denotes the control profiles, and $T$ represents the unfixed planning horizon length.

We assume that the single-track bicycle model [7] is able to describe the kinematic constraints $\dot {{{\mathbf{z}}}}(t)=f\left ({ { \mathbf{z}(t), {\mathbf{u}}(t)} }\right )$ : $$\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) \mathord {\left /{ {\vphantom {v(t)\cdot \tan \phi (t) {\textrm {L}_{\textrm {W}}}}} }\right . } {\textrm {L}_{\textrm {W}}}} \\ \end{array}}} }\right ],\quad t\in [0,T]. \tag{2}\end{align*}$$ Herein, ($x$ , $y)$ represents the coordinate value of the midpoint along the rear-wheel axle of the ego vehicle (Fig. 5), $\theta$ refers to the orientation angle, $v$ is the vehicle velocity, $\phi$ represents the steering angle, $a$ is the acceleration, $\omega$ denotes the angular velocity of the steering angle, and L_W is the wheelbase. The other geometric parameters marked in Fig. 5 include L_F (front hang length plus wheelbase), L_R(rear hang length), and L_B(width).

Fig. 5.

Schematics on vehicle kinematics and geometrics.

$[{\underline{ \mathbf {z}}},\bar {{{\mathbf{z}}}}]$ and $[{\underline{ \mathbf {u}}},\bar {{{\mathbf{u}}}}]$ denote the allowable intervals for z($t)$ and u($t)$ , respectively, that is, $$\begin{align*} \left [{ {{\begin{array}{cccccccccccccccccccc} {\textrm {a}_{\textrm {min}}} \\ {0} \\ {-\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*}$$ Here, $v(t)\ge 0$ assumes that backward maneuvers are ignored in this present work. a_min, a_max, v_max, $\Omega _{\textrm {max}}$ , and $\Phi _{\textrm {max}}$ are boundary parameters. z_init and u_init denote the initial-moment values of z($t)$ and u($t)$ . z_end and u_end denote their terminal-moment values, respectively.

$fp(\cdot ): \mathbb {R}^{\textrm {n}_{\textrm {z}}}\to \mathbb {R}^{2}$ is a mapping from the vehicle’s state profile z($t)$ to the footprint. Thus, $\Upsilon _{\textrm {FREE}}$ denotes the free space in the 2D environment while $fp({\mathbf{z}}(t))\subset \Upsilon _{\textrm {FREE}}$ represents the collision-avoidance constraints. This work assumes that $\Upsilon _{\textrm {FREE}}$ is static, i.e., no moving obstacles are considered. For future usage, let us define the four vertexes of the ego vehicle’s footprint as $A=\left ({ {x_{A},y_{A}} }\right )$ , $B=\left ({ {x_{B},y_{B}} }\right )$ , $C=\left ({ {x_{C},y_{C}} }\right )$ , and $D=\left ({ {x_{D},y_{D}} }\right )$ (Fig. 5): $$\begin{align*} x_{A} (t)&=x(t)+\textrm {L}_{\textrm {F}} \cdot \cos \theta (t)-0.5\textrm {L}_{\textrm {B}} \cdot \sin \theta (t), \\ y_{A} (t)&=y(t)+\textrm {L}_{\textrm {F}} \cdot \sin \theta (t)+0.5\textrm {L}_{\textrm {B}} \cdot \cos \theta (t), \\ x_{B} (t)&=x(t)+\textrm {L}_{\textrm {F}} \cdot \cos \theta (t)+0.5\textrm {L}_{\textrm {B}} \cdot \sin \theta (t), \\ y_{B} (t)&=y(t)+\textrm {L}_{\textrm {F}} \cdot \sin \theta (t)-0.5\textrm {L}_{\textrm {B}} \cdot \cos \theta (t), \\ x_{C} (t)&=x(t)-\textrm {L}_{\textrm {R}} \cdot \cos \theta (t)+0.5\textrm {L}_{\textrm {B}} \cdot \sin \theta (t), \\ y_{C} (t)&=y(t)-\textrm {L}_{\textrm {R}} \cdot \sin \theta (t)-0.5\textrm {L}_{\textrm {B}} \cdot \cos \theta (t), \\ x_{D} (t)&=x(t)-\textrm {L}_{\textrm {R}} \cdot \cos \theta (t)-0.5\textrm {L}_{\textrm {B}} \cdot \sin \theta (t), \\ y_{D} (t)&=y(t)-\textrm {L}_{\textrm {R}} \cdot \sin \theta (t)+0.5\textrm {L}_{\textrm {B}} \cdot \cos \theta (t), \quad t\in [0,T]. \tag{4}\end{align*}$$ The concrete expression of $fp({\mathbf{z}}(t))\subset \Upsilon _{\textrm {FREE}}$ is presented later in Section III-E.

B. NLP Formulation

The previous subsection casts the trajectory planning scheme as an OCP. This subsection briefly outlines the principle for discretizing it into an NLP. For simplicity, the OCP constructed in the previous subsection is abstracted as $$\begin{align*} &\mathop {\text {minimize}}\limits _{{ \mathbf{z}(t), {\mathbf{u}}(t), \textrm {}T}} J, \\ &\textrm {s.t.,}g\left ({ { \mathbf{z}(t), \dot {{{\mathbf{z}}}}(t), {\mathbf{u}}(t)} }\right )\le 0, t\in [0,T]. \tag{5}\end{align*}$$ Herein, $g\le 0$ represents all the constraints in inequality and equality forms.

Discretizing (5) into an NLP is about sampling finite moments along the time dimension $t\in [0,T]$ such that each ${\mathbf{z}}(t)$ or ${\mathbf{u}}(t)$ could be represented by finite collocation points. In this manner, the constraints $g\le 0$ apply only to finite collocation points rather than the entire time domain $[0,T].$ Suppose that ($\text{N}_{\mathrm {fe}} +1$ ) moments are sampled from 0 to $T$ , which are collected in a set $\left \{{ {t_{i} \left |{ {i=0,\ldots ,\textrm {N}_{\textrm {fe}}} }\right .} }\right \}$ with $$\begin{equation*} 0=t_{0} < t_{1} < t_{2} < \ldots < t_{\textrm {N}_{\textrm {fe}}} =T. \tag{6}\end{equation*}$$ Each ${\mathbf{z}}(t)$ is represented by ($\text{N}_{\mathrm {fe}} +1$ ) collocation points, which are denoted by $\left \{{ {z_{i} \left |{ {i=0,\ldots ,\textrm {N}_{\textrm {fe}}} }\right .} }\right \}.$ These collocation points are used to construct an infinite-dimensional variable ${\mathbf{z}}(t)$ , which might be a piecewise constant, linear, or polynomial function. Similarly, ${\mathbf{u}}(t)$ is represented by $\left \{{{u_{i} \left |{ {i=0,\ldots ,\textrm {N}_{\textrm {fe}}} }\right .} }\right \}.$ Thus, the original OCP is discretized into the following NLP: $$\begin{align*} & \mathop {\text {minimize}}\limits _{z_{i}, u_{i}, t_{i}} \textrm {}J\left ({ {z_{i}, u_{i}, t_{i}} }\right ), \\ &\,\textrm {s.t.,}\,g\left ({ {z_{i}, u_{i}, t_{i}} }\right )\le 0, i=0,\ldots ,\textrm {N}_{\textrm {fe}}. \tag{7}\end{align*}$$

SECTION III. Embodied Footprint-Based Collision Avoidance

Nominally, one uses an NLP solver to solve (7), thereby deriving a numerical solution to the original OCP (5). However, the derived trajectory is not guaranteed to be safe because collision-avoidance constraints are only imposed at finite moments $\left \{{ {t_{i}} }\right \}$ . To resolve this issue thoroughly, we continue to impose the collision-avoidance constraints only on finite collocation points, but we expect each of them to be “responsible”, ensuring the entire time horizon $t\in [0,T]$ is completely safe. This necessitates changes in the collision-avoidance constraints.

This section introduces how to model safe collision-avoidance constraints in the NLP formulation (7). As a basic step, the modeling task is identified in Section III-A before details are provided in the next few subsections.

A. Collision-Avoidance Constraint Modeling Task Statement

This subsection states the safety-oriented collision-avoidance constraint modeling. Since the collision-avoidance constraints are used in the formulated NLP, we have to impose a finite number of collision-avoidance constraints, but we expect that each collision-avoidance constraint is modeled in a way that ensures the ego vehicle is safe within a time interval. Specifically, we expand the ego vehicle’s footprint laterally and longitudinally to define an “embodied box” and require that the swept-by region from $t_{k}$ to $t_{k+1}$ is fully covered by an embodied box over the ego vehicle’s footprint at $t_{k}$ . As shown in Fig. 6, each embodied box is aligned with the vehicle footprint, and the geometric size is determined by four variables at $t_{k}$ , which are denoted as $e_{\textrm {left}} (t_{k} ),e_{\textrm {right}} (t_{k} ),e_{\textrm {up}} (t_{k})$ , and $e_{\textrm {down}} (t_{k}).$ The central challenge in this section is defining the four variables at $t_{k}$ in a way that thoroughly eliminates collision risks between adjacent collocation points ${t} _{k}$ and $t_{k+1}$ .

Fig. 6.

Schematics on embodied box A’B’C’D’ that covers the swept-by region from $t_{k}$ to $t_{k+1}$ .

Without loss of generality, we rotate the ego vehicle in Fig. 6 to an axially aligned pose (see Fig. 7) for the convenience of analysis, i.e., $\left [{ {x(t_{k} ),y(t_{k} ),\theta (t_{k} )} }\right ]=\left [{ {0,0,\pi /2} }\right ].$ Owing to this rotation, the four variables can be easily presented as $$\begin{align*} e_{\textrm {left}} (t_{k} )&=-\frac {\textrm {L}_{\textrm {B}} }{2}-\textrm {x}_{\textrm {min}}, \\ e_{\textrm {right}} (t_{k} )&=\textrm {x}_{\textrm {max}} -\frac {\textrm {L}_{\textrm {B}}}{2}, \\ e_{\textrm {up}} (t_{k} )&=\textrm {y}_{\textrm {max}} -\textrm {L}_{\textrm {F}}, \\ e_{\textrm {down}} (t_{k} )&=-\textrm {L}_{\textrm {R}} -y_{\min }, \tag{8}\end{align*}$$ where x_min, x_max, y_min, and y_max denote the lower/upper bounds of the swept-by region, respectively. The determinations of x_min, x_max, y_min, and y_max naturally decide $e_{\textrm {left}} (t_{k} ),~e_{\textrm {right}} (t_{k} ),e_{\textrm {up}} (t_{k} )$ , and $e_{\textrm {down}} (t_{k} ).$ Therefore, the core problem in modeling the safe collision-avoidance constraints is how to determine x_min, x_max, y_min, and $\text{y}_{\mathrm {max.}}$ Our proposed idea is presented in Sections III-B and III-C.

Fig. 7.

A normalized angle of review of Fig. 6 to facilitate theoretical analysis.

B. Determination of $\text{y}_{\mathrm{min}}$

This subsection introduces how to identify $\text{y}_{\mathrm {min}}(t_{k})$ , which denotes the minimal coordinate value of the swept-by region from $t_{k}$ to $t_{k+1}$ . For brevity, we will refer to $\text{y}_{\mathrm {min}}(t_{k})$ as y_min.

Nominally, the shape of the swept-by region is determined by control variables ${\mathbf{u}}(t)$ during the time interval [$t_{k}$ , $t_{k+1}$ ]. If ${\mathbf{u}}(t)$ is not constant during [$t_{k}$ , $t_{k+1}$ ], then the swept-by region would be too complicated to analyze. We assume that the state variables ${\mathbf{z}}(t)$ are constant during [$t_{k}$ , $t_{k+1}$ ]:

Assumption 1:

Any a ${\mathbf{z}}(t)$ or ${\mathbf{u}}(t)$ in the concerned OCP (1) is a piecewise constant function.

If ${\mathbf{u}}(t)$ is assumed to be piecewise constant on a time interval, then ${\mathbf{z}}(t)$ may not be piecewise constant on that interval. Assuming both ${\mathbf{z}}(t)$ and ${\mathbf{u}}(t)$ are piecewise constant may violate the kinematic constraints (2) in the concerned OCP. Despite this, Assumption 1 is a common practice in numerical optimization that causes minor errors if the collocation points are set densely. Discretizing OCP (1) with this assumption is known as the explicit first-order Runge-Kutta method. We must uphold this assumption, as without it, analyzing the shape of a swept-by region is extremely difficult.

Assumption 1 yields that the steering angle variable $\phi (t)$ remains constant during $t\in [t_{k},t_{k+1} ]$ , thus the path segment of the rear-axle midpoint located at $\left ({ {x(t),y(t)} }\right )$ is circular during $t\in [t_{k},t_{k+1} ]$ . Similarly, the velocity variable $v(t)$ is constant during $t\in [t_{k},t_{k+1} ]$ . For simplicity, let us rewrite the constant steering angle as $\phi _{k}$ and the constant velocity as $v_{k}.$ According to (2) and the Newton-Leibniz formula, the orientation angle $\theta (t)$ is determined as $$\begin{align*} \theta (t)&=\theta (t_{k} )+\int _{\tau =t_{k}}^{t} {\frac {v(\tau )\cdot \tan \phi (\tau )}{\textrm {L}_{\textrm {W}}}\textrm {d}\tau } \\ &=\frac {\pi }{2}+\frac {v_{k} \cdot \tan \phi _{k} }{\textrm {L}_{\textrm {W}}}\cdot (t-t_{k} ), t\in [t_{k},t_{k+1} ]. \tag{9}\end{align*}$$ To further simplify the presentation, let us introduce a curvature variable $\kappa (t)\equiv \tan \phi (t)/\textrm {L}_{\textrm {W}}$ and set that $t_{k} =0$ and $t_{k+1} =\Delta T$ , then we have $$\begin{equation*} \theta (t)=\frac {\pi }{2}+v_{k} \cdot \kappa _{k} \cdot t, \textrm {}t\in [0,\Delta T], \tag{10a}\end{equation*}$$ where $\kappa _{k} =\tan \phi _{k} /\textrm {L}_{\textrm {W}}.$ Similarly, $x(t)$ and $y(t)$ are defined as $$\begin{equation*} x(t)=\frac {\cos (v_{k} \cdot \kappa _{k} \cdot t)-1}{\kappa _{k}}, \textrm {}t\in [0,\Delta T], \tag{10b}\end{equation*}$$ and $$\begin{equation*} y(t)=\frac {\sin (v_{k} \cdot \kappa _{k} \cdot t)}{\kappa _{k}}, \textrm {}t\in [0,\Delta T]. \tag{10c}\end{equation*}$$

Substituting (10) into (4) yields the trajectories of the ego vehicle’s four vertex points: $$\begin{align*} x_{A} (t)&=\frac {\cos (v_{k} \cdot \kappa _{k} \cdot t)-1}{\kappa _{k} }-\textrm {L}_{\textrm {F}} \cdot \sin (v_{k} \cdot \kappa _{k} \cdot t) \\ &\quad -\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \cos (v_{k} \cdot \kappa _{k} \cdot t), \\ y_{A} (t)&=\frac {\sin (v_{k} \cdot \kappa _{k} \cdot t)}{\kappa _{k} }+\textrm {L}_{\textrm {F}} \cdot \cos (v_{k} \cdot \kappa _{k} \cdot t) \\ &\quad -\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \sin (v_{k} \cdot \kappa _{k} \cdot t), \\ x_{B} (t)&=\frac {\cos (v_{k} \cdot \kappa _{k} \cdot t)-1}{\kappa _{k} }-\textrm {L}_{\textrm {F}} \cdot \sin (v_{k} \cdot \kappa _{k} \cdot t) \\ &\quad +\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \cos (v_{k} \cdot \kappa _{k} \cdot t), \\ y_{B} (t)&=\frac {\sin (v_{k} \cdot \kappa _{k} \cdot t)}{\kappa _{k} }+\textrm {L}_{\textrm {F}} \cdot \cos (v_{k} \cdot \kappa _{k} \cdot t) \\ &\quad +\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \sin (v_{k} \cdot \kappa _{k} \cdot t), \\ x_{C} (t)&=\frac {\cos (v_{k} \cdot \kappa _{k} \cdot t)-1}{\kappa _{k} }+\textrm {L}_{\textrm {R}} \cdot \sin (v_{k} \cdot \kappa _{k} \cdot t) \\ &\quad +\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \cos (v_{k} \cdot \kappa _{k} \cdot t), \\ y_{C} (t)&=\frac {\sin (v_{k} \cdot \kappa _{k} \cdot t)}{\kappa _{k} }-\textrm {L}_{\textrm {R}} \cdot \cos (v_{k} \cdot \kappa _{k} \cdot t) \\ &\quad +\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \sin (v_{k} \cdot \kappa _{k} \cdot t), \\ x_{D} (t)&=\frac {\cos (v_{k} \cdot \kappa _{k} \cdot t)-1}{\kappa _{k} }+\textrm {L}_{\textrm {R}} \cdot \sin (v_{k} \cdot \kappa _{k} \cdot t) \\ &\quad -\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \cos (v_{k} \cdot \kappa _{k} \cdot t), \\ y_{D} (t)&=\frac {\sin (v_{k} \cdot \kappa _{k} \cdot t)}{\kappa _{k} }-\textrm {L}_{\textrm {R}} \cdot \cos (v_{k} \cdot \kappa _{k} \cdot t) \\ &\quad -\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \sin (v_{k} \cdot \kappa _{k} \cdot t), ~~ t\in [0,\Delta T]. \tag{11}\end{align*}$$ Connecting the four vertexes $A$ , $B$ , $C$ , and $D$ forms a rectangular footprint, which moves throughout $t\in [0,\Delta T]$ to create a swept-by region. Obviously, the boundaries of the swept-by region are determined by vertices rather than edges of the footprint, thus $$\begin{align*} \textrm {x}_{\textrm {min}}& =\min \left \{{{x_{A} (t),x_{B} (t),x_{C} (t),x_{D} (t), \forall t\in \left [{ {0,\Delta T} }\right ]} }\right \}, \tag{12a}\\ \textrm {x}_{\textrm {max}}& =\max \left \{{{x_{A} (t),x_{B} (t),x_{C} (t),x_{D} (t), \forall t\in \left [{ {0,\Delta T} }\right ]} }\right \}, \tag{12b}\\ \textrm {y}_{\textrm {min}}& =\min \left \{{{y_{A} (t),y_{B} (t),y_{C} (t),y_{D} (t), \forall t\in \left [{ {0,\Delta T} }\right ]} }\right \}, \tag{12c}\\ \textrm {y}_{\textrm {max}}& =\max \left \{{{y_{A} (t),y_{B} (t),y_{C} (t),y_{D} (t), \forall t\in \left [{ {0,\Delta T} }\right ]} }\right \}. \tag{12d}\end{align*}$$

According to the definitions of $y_{A} (t),y_{B} (t),y_{C} (t)$ , and $y_{D} (t)$ in (11), one can safely state that $y_{A} (t)\ge y_{D} (t)$ and $y_{B} (t)\ge y_{C} (t)$ for any ${t}$ provided that $$\begin{equation*} \left |{ {v_{k} \cdot \kappa _{k} \cdot t} }\right |\le \frac {\pi }{2}, \forall t\in [0,\Delta T]. \tag{13}\end{equation*}$$ Given that (13) should be satisfied for any $t\in [0,\Delta T]$ , then $$\begin{equation*} \left |{ {v_{k} \cdot \kappa _{k} \cdot \Delta T} }\right |\le \frac {\pi }{2}. \tag{14}\end{equation*}$$

Assumption 2:

$v_{k}$ , $\kappa _{k}$ , and $\Delta T$ satisfy the inequality (14).

Assumption 2 ensures that $y_{A} (t)\ge y_{D} (t)$ and $y_{B} (t)\ge y_{C} (t)$ for any $t$ within $[0,\Delta T].$ Therefore, y_min would be the smaller one between $\min \left \{{{y_{C} (t)} }\right \}$ and $\min \left \{{{y_{D} (t)} }\right \}$ for $\forall t\in [0,\Delta T].$ If Assumption 2 is not held, one needs to also consider the complex case in which y_min involves vertex $A$ or $B$ .

Recall that negative velocity is not considered in this present work, thus we state the following:

Assumption 3:

$v_{k} \ge 0$ .

With Assumption 3 at hand, the subsequent analyses are divided into two branches as per the sign of $\kappa _{k}$ .

Condition 1: $\kappa _{k} \ge 0$

When $\kappa _{k}$ is non-negative, one has $y_{D} (t)\le y_{C} (t).$ Thus, y_min is the extremum of $y_{D} (t)$ on $t\in [0,\Delta T].$ Let us rewrite $y_{D} (t)$ in the following form: $$\begin{align*} y_{D} (t)&=\left({\frac {1}{\kappa _{k}}-\frac {\textrm {L}_{\textrm {B}}}{2}}\right)\cdot \sin (v_{k} \cdot \kappa _{k} \cdot t)-\textrm {L}_{\textrm {R}} \cdot \cos (v_{k} \cdot \kappa _{k} \cdot t) \\ &=\textrm {M}\cdot \sin (v_{k} \cdot \kappa _{k} \cdot t)-\textrm {N}\cdot \cos (v_{k} \cdot \kappa _{k} \cdot t) \\ &=\sqrt {\textrm {M}^{2}+\textrm {N}^{2}} \cdot \sin (v_{k} \cdot \kappa _{k} \cdot t-\alpha ), \tag{15}\end{align*}$$ where $\textrm {M}\equiv \frac {1}{\kappa _{k} }-\frac {\textrm {L}_{\textrm {B}}}{2}$ , $\textrm {N}\equiv \textrm {L}_{\textrm {R}}$ , and $$\begin{equation*} \alpha =\arccos \left({\frac {\textrm {M}}{\sqrt {\textrm {M}^{2}+\textrm {N}^{2}} }}\right). \tag{16}\end{equation*}$$ Herein, N is positive while the sign of M is pending. Let us discuss the sign of M.

If $\textrm {M} < 0$ , then ${\textrm {M}} \mathord {\left /{ {\vphantom {{\textrm {M}} {\sqrt {\textrm {M}^{2}+\textrm {N}^{2}}}}} }\right . } {\sqrt {\textrm {M}^{2}+\textrm {N}^{2}} }$ is negative, thus (16) yields that $\alpha \in [\pi /2,{\pi }].$ Let us analyze the monotonicity of $y_{D} (t)$ via its derivative ${y}'_{D} (t)$ : $$\begin{equation*} {y}'_{D} (t)=v_{k} \cdot \kappa _{k} \cdot \sqrt {\textrm {M}^{2}+\textrm {N}^{2}} \cdot \cos (v_{k} \cdot \kappa _{k} \cdot t-\alpha ).\end{equation*}$$ Given that $v_{k} \cdot \kappa _{k} \cdot \sqrt {\textrm {M}^{2}+\textrm {N}^{2}} >0$ , the sign of $y'_{D} (t)$ would be determined by $\cos (v_{k} \cdot \kappa _{k} \cdot t-\alpha ).$ Obviously, ${y}'_{D} (0)\le 0$ , which means $y_{D} (t)$ has a descending trend at $t=0^{+}.$ We expect that the entire domain $[0,\Delta T]$ is a monotonic decreasing interval. If so, the minimal extremum value is $y_{D} (\Delta T).$ The reason why we expect $[0,\Delta T]$ to be monotonic is given as follows. If $[0,\Delta T]$ is not a monotonic decreasing interval, then the extremum is achieved at some $t^{\ast }\in [0,\Delta T]$ that satisfies $$\begin{equation*} \sin (v_{k} \cdot \kappa _{k} \cdot t^{\ast }-\alpha )=-1,\end{equation*}$$ which renders the following side effects: 1) the extremum is unrelated to $v_{k}$ , thus making y_min overcautious; and 2) the extremum is a rather complex function of $\kappa _{k}.$

To make $[0,\Delta T]$ a monotonic decreasing interval, one needs to ensure that $\cos (v_{k} \cdot \kappa _{k} \cdot t-\alpha )$ remains negative throughout $[0,\Delta T]$ , which yields $\cos (v_{k} \cdot \kappa _{k} \cdot \Delta T-\alpha )\le 0$ , i.e., $$\begin{equation*} v_{k} \cdot \kappa _{k} \cdot \Delta T-\alpha \le -\frac {\pi }{2}. \tag{17}\end{equation*}$$

This inequality is further described as $$\begin{equation*} 0 < v_{k} \cdot \kappa _{k} \cdot \Delta T\le \arctan \left({-\frac {\frac {1}{\kappa _{k}}-\frac {\textrm {L}_{\textrm {B}}}{2}}{\textrm {L}_{\textrm {R}}}}\right). \tag{18}\end{equation*}$$ If (18) holds, then the extremum of $y_{D} (t)$ is $$\begin{align*} y_{D} (\Delta T)&=\left({\frac {1}{\kappa _{k}}-\frac {\textrm {L}_{\textrm {B}} }{2}}\right)\cdot \sin (v_{k} \cdot \kappa _{k} \cdot \Delta T) \\ &\quad -\textrm {L}_{\textrm {R}} \cdot \cos (v_{k} \cdot \kappa _{k} \cdot \Delta T). \tag{19}\end{align*}$$ Nominally, one can state $\textrm {y}_{\textrm {min}} =y_{D} (\Delta T)$ , but (19) is still not showing a simple enough relationship among y_min, $v_{k}$ , and $\kappa _{k}$ . Therefore, we simplify (19) further via inequality amplification skills in mathematics. According to the McLaughlin formula, one has $$\begin{equation*} \sin (v_{k} \cdot \kappa _{k} \cdot \Delta T)\le v_{k} \cdot \kappa _{k} \cdot \Delta T, \tag{20a}\end{equation*}$$ and $$\begin{equation*} \cos (v_{k} \cdot \kappa _{k} \cdot \Delta T)\le 1. \tag{20b}\end{equation*}$$ Then, we can provide a simplified lower bound for $y_{D} (\Delta T)$ : $$\begin{equation*} \left({\frac {1}{\kappa _{k}}-\frac {\textrm {L}_{\textrm {B}}}{2}}\right)\cdot v_{k} \cdot \kappa _{k} \cdot \Delta T-\textrm {L}_{\textrm {R}} \le y_{D} (\Delta T). \tag{21}\end{equation*}$$ Accordingly, one has $$\begin{equation*} \textrm {y}_{\textrm {min}} =\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \kappa _{k} \cdot s_{k} -s_{k} -\textrm {L}_{\textrm {R}}, \tag{22}\end{equation*}$$ wherein $s_{k} \equiv v_{k} \cdot \Delta T$ is deployed to simplify the presentation.

The aforementioned analysis assumes that $\textrm {M} < 0.$ Symmetrically, if $\textrm {M}\ge 0$ , then $\alpha \in [0,\pi /2]$ and $[0,\Delta T]$ is definitely a monotonic increasing interval. Therefore, the extremum value is $y_{D} (0)$ , i.e., $\textrm {y}_{\textrm {min}} =-\textrm {L}_{\textrm {R}}.$

Condition 2: $\kappa _{k} < 0$

In dealing with $\kappa _{k} < 0$ , we introduce a temporary variable $\kappa _{k}^{\ast } =-\kappa _{k}.$ Since $\kappa _{k}^{\ast } >0$ , the analysis in Condition 1 could be repeated. The concrete details are similar, thus we omit them.

Summarizing the analyses for $\kappa _{k} \ge 0$ and $\kappa _{k} < 0$ yields that

1° When $\frac {1}{\left |{ {\kappa _{k}} }\right |}-\frac {\textrm {L}_{\textrm {B}}}{2}\le 0$ , $$\begin{equation*} \textrm {y}_{\textrm {min}} =\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \left |{ {\kappa _{k}} }\right |\cdot s_{k} -s_{k} -\textrm {L}_{\textrm {R}}, \tag{23a}\end{equation*}$$ which is associated with prerequisite $$\begin{equation*} \left |{ {\kappa _{k}} }\right |\cdot s_{k} \le \arctan \left({-\frac {\frac {1}{\left |{ {\kappa _{k}} }\right |}-\frac {\textrm {L}_{\textrm {B}} }{2}}{\textrm {L}_{\textrm {R}}}}\right). \tag{23b}\end{equation*}$$ 2° When $\frac {1}{\left |{ {\kappa _{k}} }\right |}-\frac {\textrm {L}_{\textrm {B}}}{2}>0$ , $$\begin{equation*} \textrm {y}_{\textrm {min}} =-\textrm {L}_{\textrm {R}}, \tag{23c}\end{equation*}$$ which is associated with prerequisite $$\begin{equation*} \left |{ {\kappa _{k}} }\right |\cdot s_{k} \le \frac {\pi }{2}. \tag{23d}\end{equation*}$$

Assumption 4:

$2\textrm {L}_{\textrm {W}} >\textrm {L}_{\textrm {B}} \cdot \tan \phi _{\max }$ .

Empirically, Assumption 4 holds for a passenger vehicle, which means $$\begin{equation*} \frac {1}{\left |{ {\kappa _{k}} }\right |}-\frac {\textrm {L}_{\textrm {B}} }{2}=\left |{ {\frac {\textrm {L}_{\textrm {W}}}{\tan \phi _{k}}} }\right |-\frac {\textrm {L}_{\textrm {B}}}{2}\ge \frac {\textrm {L}_{\textrm {W}} }{\tan \phi _{\max }}-\frac {\textrm {L}_{\textrm {B}}}{2}>0. \tag{24}\end{equation*}$$ Therefore, the branch under $1/\left |{ {\kappa _{k}} }\right |-\textrm {L}_{\textrm {B}} /2\le 0$ is discarded under Assumption 4. The cases that involve $1/\left |{ {\kappa _{k}} }\right |-\textrm {L}_{\textrm {B}} /2\le 0$ will not be considered in the remainder of this section either.

The final conclusion of this subsection is that $\textrm {y}_{\textrm {min}} =-\textrm {L}_{\textrm {R}}$ under Assumptions 1–​4.

C. Determinations of $\text{y}_{\mathrm{max}}$ , $\text{x}_{\mathrm{min}}$ , and $\text{x}_{\mathrm{max}}$

This subsection introduces the definitions of y_max, x_min, and x_max. The analyses are similar to those in Section III-B, thus details are omitted.

y_max is written as $$\begin{equation*} \textrm {y}_{\textrm {max}} =s_{k} +\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \left |{ {\kappa _{k}} }\right |\cdot s_{k} +\textrm {L}_{\textrm {F}}, \tag{25a}\end{equation*}$$ together with the following prerequisite: $$\begin{equation*} s_{k} \cdot \left |{ {\kappa _{k}} }\right |\le \arctan \left({\frac {\frac {1}{\left |{ {\kappa _{k}} }\right |}+\frac {\textrm {L}_{\textrm {B}} }{2}}{\textrm {L}_{\textrm {F}}}}\right). \tag{25b}\end{equation*}$$ x_min is written as $$\begin{align*} \textrm {x}_{\textrm {min}} =-\frac {\textrm {L}_{\textrm {B}}}{2}-\max \left \{{ {-\textrm {L}_{\textrm {R}} \cdot \kappa _{k} \cdot s_{k}, \textrm {}\left({\textrm {L}_{\textrm {F}} +\frac {s_{k}}{2}}\right)\cdot \kappa _{k} \cdot s_{k}} }\right \}, \tag{26a}\end{align*}$$ together with $$\begin{equation*} s_{k} \cdot \left |{ {\kappa _{k}} }\right |\le \arctan \left({\frac {\textrm {L}_{\textrm {R}}}{\frac {1}{\left |{ {\kappa _{k}} }\right |}+\frac {\textrm {L}_{\textrm {B}}}{2}}}\right). \tag{26b}\end{equation*}$$ x_max is written as $$\begin{align*} \textrm {x}_{\textrm {max}} =\frac {\textrm {L}_{\textrm {B}}}{2}+\max \left \{{ {\textrm {L}_{\textrm {R}} \cdot \kappa _{k} \cdot s_{k},-\left({\textrm {L}_{\textrm {F}} +\frac {s_{k}}{2}}\right)\cdot \kappa _{k} \cdot s_{k}} }\right \}, \tag{27a}\end{align*}$$ together with $$\begin{equation*} s_{k} \cdot \left |{ {\kappa _{k}} }\right |\le \arctan \left({\frac {\textrm {L}_{\textrm {R}}}{\frac {1}{\left |{ {\kappa _{k}} }\right |}+\frac {\textrm {L}_{\textrm {B}}}{2}}}\right){.} \tag{27b}\end{equation*}$$

D. Definition of an Embodied Box

Sections III-B and III-C have defined y_min, y_max, x_min, and x_max. This subsection further forms an embodied box based on the aforementioned definitions. Eq. (8) yields that $$\begin{align*} (-\textrm {x}_{\textrm {min}} )-e_{\textrm {left}} (t_{k} )&=\frac {\textrm {L}_{\textrm {B}}}{2}, \\ \textrm {x}_{\textrm {max}} -e_{\textrm {right}} (t_{k} )&=\frac {\textrm {L}_{\textrm {B}}}{2}, \\ (-\textrm {y}_{\textrm {min}} )-e_{\textrm {down}} (t_{k} )&=\textrm {L}_{\textrm {R}}, \\ \textrm {y}_{\textrm {max}} -e_{\textrm {up}} (t_{k} )&=\textrm {L}_{\textrm {F}}. \tag{28}\end{align*}$$ Eq. (28), Section III-B, and III-C yield that $$\begin{align*} e_{\textrm {left}} (t_{k} )&=\max \left \{{{-\textrm {L}_{\textrm {R}} \cdot \kappa _{k} \cdot s_{k}, \left({\textrm {L}_{\textrm {F}} +\frac {s_{k} }{2}}\right)\cdot \kappa _{k} \cdot s_{k}} }\right \}, \\ e_{\textrm {right}} (t_{k} )&=\max \left \{{{\textrm {L}_{\textrm {R}} \cdot \kappa _{k} \cdot s_{k},-\left({\textrm {L}_{\textrm {F}} +\frac {s_{k}}{2}}\right)\cdot \kappa _{k} \cdot s_{k}} }\right \}, \\ e_{\textrm {up}} (t_{k} )&=s_{k} +\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \left |{ {\kappa _{k}} }\right |\cdot s_{k}, \\ e_{\textrm {down}} (t_{k} )&=0, \tag{29}\end{align*}$$ subject to the intersection of prerequisites (23d), (25b), (26b), and (27b). Since (26b) and (27b) are identical, one only needs to consider (23d), (25b), and (26b).

Eq. (25b) is rewritten as $$\begin{equation*} \tan \left ({ {s_{k} \cdot \left |{ {\kappa _{k}} }\right |} }\right )\le \frac {\frac {1}{\left |{ {\kappa _{k}} }\right |}+\frac {\textrm {L}_{\textrm {B}} }{2}}{\textrm {L}_{\textrm {F}}}, \tag{30a}\end{equation*}$$ that is, $$\begin{equation*} \left |{ {\kappa _{k}} }\right |\cdot \textrm {L}_{\textrm {F}} \cdot \tan \left ({ {s_{k} \cdot \left |{ {\kappa _{k}} }\right |} }\right )\le 1+\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \left |{ {\kappa _{k}} }\right |. \tag{30b}\end{equation*}$$ Similarly, (26b) is rewritten as $$\begin{equation*} \left ({ {1+\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \left |{ {\kappa _{k}} }\right |} }\right )\cdot \tan \left ({ {s_{k} \cdot \left |{ {\kappa _{k}} }\right |} }\right )\le \textrm {L}_{\textrm {R}} \cdot \left |{ {\kappa _{k}} }\right |. \tag{31}\end{equation*}$$

Based on the aforementioned analyses, the four vertexes of the ego vehicle at time instance $t_{k}$ are expanded as the rectangle $A ' B ' C ' D '$ shown in Fig. 6, where $e_{\textrm {left}} (t_{k} )$ , $e_{\textrm {right}} (t_{k} )$ , $e_{\textrm {up}} (t_{k} )$ , and $e_{\textrm {down}} (t_{k} )$ are defined in (29). Simultaneously, prerequisites (23d), (30b), and (31) should hold.

According to (29), the embodied box becomes large when $\left |{ {\kappa _{k}} }\right |$ and/or $s_{k}$ are large, which is intuitively reasonable. Prerequisites (23d), (30b), and (31) are inherently setting an upper bound for $(t_{k+1} -t_{k} )$ , i.e., the duration between adjacent collocation points.

E. Formulation of Safe Collision-Avoidance Constraints

This subsection presents the safety-oriented collision-avoidance constraints with the defined embodied box at the collocation point $t_{k}$ .

The four vertices of the embodied box are defined as ${A}'=\left ({ {{\stackrel {{{\unicode{0x2322}}}} {x}} _{A},{\stackrel {{{ \unicode{0x2322}}}} {y}}_{A}} }\right ),{B}'=\left ({ {{\stackrel {{{ \unicode{0x2322}}}} {x}} _{B},{\stackrel {{{ \unicode{0x2322}}}} {y}}_{B}} }\right ),{C}'=\left ({ {{\stackrel {{{ \unicode{0x2322}}}} {x}} _{C},{\stackrel {{{ \unicode{0x2322}}}} {y}}_{C}} }\right )$ , and ${D}'=\left ({ {{\stackrel {{{ \unicode{0x2322}}}} {x}} _{D},{\stackrel {{{ \unicode{0x2322}}}} {y}}_{D}} }\right )$ : $$\begin{align*} {\stackrel {{{ \unicode{0x2322}}}} {x}} _{A} (t_{k} )&=x(t_{k} )+\left ({ {\textrm {L}_{\textrm {F}} +e_{\textrm {up}} (t_{k} )} }\right ) \\ &\quad \cdot \cos \theta (t_{k} )-\left ({ {\frac {\textrm {L}_{\textrm {B}}}{2}+e_{\textrm {left}} (t_{k} )} }\right )\cdot \sin \theta (t_{k} ), \\ {\stackrel {{{ \unicode{0x2322}}}} {y}} _{A} (t_{k} )&=y(t_{k} )+\left ({ {\textrm {L}_{\textrm {F}} +e_{\textrm {up}} (t_{k} )} }\right ) \\ &\quad \cdot \sin \theta (t_{k} )+\left ({ {\frac {\textrm {L}_{\textrm {B}}}{2}+e_{\textrm {left}} (t_{k} )} }\right )\cdot \cos \theta (t_{k} ), \\ {\stackrel {{{ \unicode{0x2322}}}} {x}} _{B} (t_{k} )&=x(t_{k} )+\left ({ {\textrm {L}_{\textrm {F}} +e_{\textrm {up}} (t_{k} )} }\right ) \\ &\quad \cdot \cos \theta (t_{k} )+\left ({ {\frac {\textrm {L}_{\textrm {B}}}{2}+e_{\textrm {right}} (t_{k} )} }\right )\cdot \sin \theta (t_{k} ), \\ {\stackrel {{{ \unicode{0x2322}}}} {y}} _{B} (t_{k} )&=y(t_{k} )+\left ({ {\textrm {L}_{\textrm {F}} +e_{\textrm {up}} (t_{k} )} }\right ) \\ &\quad \cdot \sin \theta (t_{k} )-\left ({ {\frac {\textrm {L}_{\textrm {B}}}{2}+e_{\textrm {right}} (t_{k} )} }\right )\cdot \cos \theta (t_{k} ), \\ {\stackrel {{{ \unicode{0x2322}}}} {x}} _{C} (t_{k} )&=x(t_{k} )-\left ({ {\textrm {L}_{\textrm {R}} +e_{\textrm {down}} (t_{k} )} }\right ) \\ &\quad \cdot \cos \theta (t_{k} )+\left ({ {\frac {\textrm {L}_{\textrm {B}}}{2}+e_{\textrm {right}} (t_{k} )} }\right )\cdot \sin \theta (t_{k} ), \\ {\stackrel {{{ \unicode{0x2322}}}} {y}} _{C} (t_{k} )&=y(t_{k} )-\left ({ {\textrm {L}_{\textrm {R}} +e_{\textrm {down}} (t_{k} )} }\right ) \\ &\quad \cdot \sin \theta (t_{k} )-\left ({ {\frac {\textrm {L}_{\textrm {B}}}{2}+e_{\textrm {right}} (t_{k} )} }\right )\cdot \cos \theta (t_{k} ), \\ {\stackrel {{{ \unicode{0x2322}}}} {x}} _{D} (t_{k} )&=x(t_{k} )-\left ({ {\textrm {L}_{\textrm {R}} +e_{\textrm {down}} (t_{k} )} }\right ) \\ &\quad \cdot \cos \theta (t_{k} )-\left ({ {\frac {\textrm {L}_{\textrm {B}}}{2}+e_{\textrm {left}} (t_{k} )} }\right )\cdot \sin \theta (t_{k} ), \\ {\stackrel {{{ \unicode{0x2322}}}} {y}} _{D} (t_{k} )&=y(t_{k} )-\left ({ {\textrm {L}_{\textrm {R}} +e_{\textrm {down}} (t_{k} )} }\right ) \\ &\quad \cdot \sin \theta (t_{k} )+\left ({ {\frac {\textrm {L}_{\textrm {B}}}{2}+e_{\textrm {left}} (t_{k} )} }\right )\cdot \cos \theta (t_{k} ). \tag{32}\end{align*}$$

It is required that the embodied box $A ' B ' C ' D '$ does not overlap with obstacles at (N_fe–1) moments $\left \{{ {t_{k} \left |{ {k=1,\ldots ,\textrm {N}_{\textrm {fe}} -1} }\right .} }\right \}.$ Before ending this section, we briefly present the principle to describe $fp({\mathbf{z}}(t))\subset \Upsilon _{\textrm {FREE}}$ as algebraic inequalities via a triangle-area criterion introduced in [7].

A collision between the rectangular embodied box $A ' B ' C ' D '$ and each obstacle should be avoided at the (N_fe–1) collocation points. Without loss of generality, the collision-avoidance constraint between the $j$ th obstacle and the embodied box $A ' B ' C ' D '$ at $t_{k}$ is examined. If the $j$ th obstacle has N_j vertices denoted as $\textrm {V}_{j1},\ldots ,\textrm {V}_{j\textrm {N}_{j}}$ , a collision begins when a vertex of the obstacle hits the ego vehicle’s embodied box $A ' B ' C ' D '$ or vice versa, a vertex of the embodied box hits in the obstacle. Hence, collisions will not occur if 1) vertexes $A '$ , $B '$ , $C '$ , and $D '$ are always located out of the $j$ th obstacle, and 2) each $\textrm {V}_{jk}$ ($k=1,\ldots ,\textrm {N}_{j} )$ always locate out of $A ' B ' C ' D '$ .

The generic constraint that a point $\textrm {Q}=(x_{\textrm {Q}},y_{\textrm {Q}} )$ locates outside a convex polygon $\textrm {W}_{1} \textrm {W}_{2} {\ldots\textrm { W}}_{\textrm {m}}$ can be expressed as an analytical inequality via the triangle-area criterion [7]: $$\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_{\Box \textrm {W}_{1} \textrm {W}_{2} {\ldots\textrm { W}}_{\textrm {m}}}, \tag{33}\end{equation*}$$ where $S_{\Delta }$ denotes the triangle area, and $S_{\Box }$ denotes the area of polygon $\textrm {W}_{1} \textrm {W}_{2} {\ldots\textrm { W}}_{\textrm {m}}.$ Suppose that the coordinate of $W_{l}$ is $(x_{\textrm {W}l},y_{\textrm {W}l} )$ , each $S_{\Delta }$ is computed according to $$\begin{align*} S_{\Delta {\textrm {QW}_{l}} {\textrm {W}_{l+1}}} &=0.5\cdot \left |{ {x_{\textrm {Q}} y_{\textrm {W}l} +x_{\textrm {W}l} y_{\textrm {W}l+1} +x_{\textrm {W}l+1} y_{\textrm {Q}}} }\right . \\ &\quad \left .{ - {x_{\textrm {Q}} y_{\textrm {W}l+1} -x_{\textrm {W}l} y_{\textrm {Q}} -x_{\textrm {W}l+1} y_{\textrm {W}l}} }\right |. \tag{34}\end{align*}$$ $S_{\Box \textrm {W}_{1} \textrm {W}_{2} {\ldots\textrm { W}}_{\textrm {m}}}$ is a constant calculated by summing up multiple triangle areas offline. The aforementioned conditions 1) and 2) are summarized into the following inequalities using (34): $$\begin{align*} &\hspace {-1pc} S_{\Delta \textrm {QV}_{j\textrm {N}_{j}} \textrm {V}_{j1}} +\sum \limits _{l=1}^{\textrm {N}_{j} -1} {S_{\Delta \textrm {QV}_{jl} \textrm {V}_{j(l+1)}}} >S_{\Box \textrm {V}_{j1} \textrm {V}_{j2} {\ldots\textrm { V}}_{j\textrm {N}_{j}}}, \\ &\quad \forall \textrm {Q}\in \left \{{{A',B',C',D'} }\right \}. \tag{35a}\\ &\hspace {-1pc} S_{\Delta \textrm {Q}A'B'} +S_{\Delta \textrm {Q}B'C'} +S_{\Delta \textrm {Q}C'D'} +S_{\Delta \textrm {Q}D'A'} >S_{\Box A'B'C'D'}, \\ &\quad \forall \textrm {Q}\in \left \{{{V_{j1},V_{j2},\ldots ,V_{j\textrm {N}_{j}}} }\right \}. \tag{35b}\end{align*}$$ In (35b), $S_{\Box A'B'C'D'}$ denotes the area of the embodied box, thus $$\begin{align*} S_{\Box A'B'C'D'} &=\left ({ {e_{\textrm {up}} (t_{k} )+e_{\textrm {down}} (t_{k} )+\textrm {L}_{\textrm {R}} +\textrm {L}_{\textrm {F}}} }\right ) \\ &\quad \times \left ({ {e_{\textrm {left}} (t_{k} )+e_{\textrm {right}} (t_{k} )+\textrm {L}_{\textrm {B}}} }\right ). \tag{35c}\end{align*}$$ Applying (35) to all $k=1,\ldots ,\textrm {N}_{\textrm {fe}} -1$ and $j=1,\ldots ,\textrm {N}_{\textrm {obs}}$ yields the complete collision-avoidance constraints. By enforcing the ego vehicle’s (N_fe–1) embodied footprints to be collision-free, the ego vehicle’s actual footprint will definitely be safe at any a moment throughout $t\in [0,T].$

For brevity, the collision-avoidance constraints between the embodied footprint $A ' B ' C ' D '$ at $t_{k}$ and environmental obstacles are abstracted as $\textsf {EmbodiedFootprints}(t_{k} )\le 0.$

SECTION IV. Trajectory Planning With Embodied Footprints

This section introduces a trajectory planner based on numerical optimal control, embedded with the embodied footprints proposed in the preceding section. We begin with an overall algorithm architecture in Section IV-A before entering into the detailed functions.

A. Overall Framework

The steps in the proposed trajectory planner are summarized in the following pseudo-code.

In Line 1 of Alg. 1, the function $\textsf {PlanCoarsePath}{()}$ is used to search a coarse path that connects the initial and goal poses. The inputs of $\textsf {PlanCoarsePath}{()}$ include task and map: task refers to the initial and goal configurations while map presents the environmental layout and obstacle location information. The output of $\textsf {PlanCoarsePath}{()}$ is a coarse path $path_{\textrm {coarse}}$ presented by a sequence of waypoints.

Algorithm 1 Optimization-Based Trajectory Planning With Embodied Footprints

Function $traj\leftarrow PlanTrajectory(task,map)$

1.

$path_{\textrm {coarse}} \leftarrow \textsf {PlanCoarsePath}(task,map)$ ;

2.

$[traj_{\textrm {coarse}},\textrm {N}_{\textrm {fe}} ]\leftarrow \textsf {AttachVelocity}(path_{\textrm {coarse}} )$ ;

3.

$ig\leftarrow \textsf {ConvertTrajToInitialGuess}(traj_{\textrm {coarse}} \textrm {, N}_{\textrm {fe}} )$ ;

4.

$NLP\leftarrow \textsf {BuildNLP}(task, map,\textrm {N}_{\textrm {fe}} )$ ;

5.

$sol\leftarrow \textsf {SolveNLP}(NLP, ig)$ ;

6.

$traj\leftarrow \textsf {ConvertSolutionToTraj}(sol)$ ;

7.

return;

In Line 2 of Alg. 1, $\textsf {AttachVelocity}{()}$ attaches a time course along $path_{\textrm {coarse}}$ to build a coarse trajectory $traj_{\textrm {coarse}}.$ It is worth emphasizing that $\textsf {AttachVelocity}{()}$ also decides N_fe, i.e., the number of collocation points. This is a significant highlight and innovation of this study because our method provides a quantitative estimation of the collocation point scale before solving the NLP problem, which means we resolve the non-collocation error issue without any need to deploy a trial-and-error strategy. Concrete principle of the function $\textsf {AttachVelocity}{()}$ is introduced in Section IV-B.

In Line 3, the coarse trajectory is converted to an initial guess ig, which contains all state and control variables in their discretized forms.

An NLP is built via $\textsf {BuildNLP}{()}$ in Line 4 of Alg. 1. The detailed principles of this function will be introduced in Section IV-B.

After solving the formulated NLP via a gradient-based optimizer in Line 5, we convert the derived solution sol to an optimized trajectory traj, which is the final output of Alg. 1.

B. Velocity Generation in Initial Guess

This subsection presents the principle of AttachVelocity In the first step, we assign a kinematically feasible and time-optimal velocity profile to the coarse path. This is achieved by solving a one-dimensional OCP via Pontryagin’s Maximum Principle. Using this method, we obtain a coarse trajectory preliminarily.

The second step starts from resampling along the derived coarse trajectory densely. The values of state and control profiles, together with the time stamp, are recorded in each densely resampled waypoint. The first waypoint wp₀ refers to the one at $t=0$ , i.e., wp₀.t $=0.$ Thereafter, one checks if the second waypoint wp₁ satisfies a relaxed version of the prerequisites (23d), (30b), and (31) with $$\begin{align*} \kappa _{k} &=\textrm {wp}_{0}.\kappa , \\ s_{k}& =\textrm {wp}_{0}.v\cdot \left ({ {\textrm {wp}_{1}.\textrm {t}-\textrm {wp}_{0}.\textrm {t}} }\right ){.} \tag{36}\end{align*}$$ Herein, the prerequisites (23d), (30b), and (31) are relaxed via a user-specified slack variable $0 < {\lambda } < \textrm {1:}$ $$\begin{align*} \left |{ {\kappa _{k}} }\right |\cdot s_{k} &\le \lambda \cdot \frac {\pi }{2}, \tag{37a}\\ \left |{ {\kappa _{k}} }\right |\cdot \textrm {L}_{\textrm {F}} \cdot \tan \left ({ {s_{k} \cdot \left |{ {\kappa _{k}} }\right |} }\right )&\le {\lambda }\cdot \left ({ {1+\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \left |{ {\kappa _{k} } }\right |} }\right ), \tag{37b}\\ \left ({ {1+\frac {\textrm {L}_{\textrm {B}}}{2}\cdot \left |{ {\kappa _{k}} }\right |} }\right )\cdot \tan \left ({ {s_{k} \cdot \left |{ {\kappa _{k}} }\right |} }\right )&\le \lambda \cdot \left ({ {\textrm {L}_{\textrm {R}} \cdot \left |{ {\kappa _{k}} }\right |} }\right ). \tag{37c}\end{align*}$$ The usage of ${\lambda }$ serves to relax the right sides of inequalities (23d), (30b), and (31). As a result, (37) becomes a stricter version of these prerequisites (the reason to use ${\lambda }$ is explained later).

Since the waypoints are densely resampled, wp₁ may not violate the prerequisites (37). If so, one continues to check wp₂, wp₃, etc. until a violating waypoint wp_m is found, which means wp$_{\mathrm {k-1}}$ is the last valid one $(m>1{).}$ Let us discard the intermediate waypoints between wp₀ and wp$_{m-1}$ . After that, we treat wp$_{m-1}$ like wp₀ and repeat the aforementioned operations until the last resampled waypoint is reached. The waypoints that survive from the discard operation are no longer equidistant along the time dimension, but they do form a coarse trajectory $traj_{\textrm {coarse}}.$ The number of waypoints in $traj_{\textrm {coarse}}$ is regarded as the number of collocation points ($\text{N}_{\mathrm {fe}} +1$ ).

Before the end of this subsection, the reason for introducing ${\lambda }$ in (37) is presented. Recall that the criterion to discard resampled waypoints in $\textsf {AttachVelocity}{()}$ influences the setting of N_fe. If one uses the nominal criterion (23d), (30b), and (31), then each $(t_{k+1} -t_{k} )$ is close to its maximum allowable value. This results in the gross waypoint number of the initial guess being nearly minimized. If one sets N_fe to such a low value, then the flexibility in the NLP would be insufficient. More specifically, every change made in $\kappa$ or $v$ during the optimization process may need a smaller bound on $(t_{k+1} -t_{k} )$ , which requires a sufficiently large N_fe. Therefore, setting N_fe too small easily causes an NLP solution failure. The introduction of ${\lambda }\in \textrm {(0,1)}$ in (37) sets a stricter condition than the nominal one, which makes N_fe larger and thus brings more flexibility to the NLP solution process.

C. NLP Formulation

This subsection presents the NLP formulation, which is comprised of a cost function $J$ and the discretized state and control variables located on ($\text{N}_{\mathrm {fe}} +1$ ) collocation points. The NLP problem is written in the following form: $$\begin{align*} &\mathop {\text {minimize}}\limits _{z_{i}, u_{i}, t_{i}}~ \textrm {}J\left ({ {z_{i}, u_{i}, t_{i}} }\right ), \\ &\textrm {s.t.,}~\textrm {vehicle kinematic constraints;} \\ &\hphantom {\textrm {s.t.,}~}\textrm {two-point boundary value constraints;} \\ &\hphantom {\textrm {s.t.,}~}\textrm {embodied-footprint constraints.} \tag{38}\end{align*}$$ Herein, the vehicle kinematic constraints refer to the discretized version of the differential equations (2) together with bounding constraints (3). For example, the differential equations in (2) are discretized in the following form: $$\begin{equation*} z_{i} =f\left ({ {z_{i-1}, u_{i-1}, t_{i-1}} }\right ), i=1,\ldots ,\textrm {N}_{\textrm {fe}}. \tag{39}\end{equation*}$$ The two-point boundary-value constraints are imposed on the first and last collocation points for the state/control profiles. The remainder of this subsection elaborates on the embodied-footprint constraints, which are used to replace the nominal collision-avoidance constraints $fp(z_{i} )\subset \Upsilon _{\textrm {FREE}}$ .

The embodied-footprint constraints are only applied to the collocation points indexed from 1 to (N_fe–1). This is because the collocation points indexed 0 and N_fe correspond to the initial and final positions, which are assumed to be free from collisions. It is worth noting that the collocation points are not forced to be equidistant along $[0,T]$ in our formulated NLP. Instead, one only requires the collocation points to be ordered: $$\begin{align*} t_{0} &=0, t_{\textrm {N}_{\textrm {fe}}} =T, \tag{40a}\\ t_{k-1}& \le t_{k}, k=1,\ldots ,\textrm {N}_{\textrm {fe}}. \tag{40b}\end{align*}$$ Additional decision variables $t_{k},\kappa _{k},s_{k},e_{\textrm {left}k},e_{\textrm {right}k},e_{\textrm {up}k},~e_{\textrm {down}k},{\stackrel {{{ \unicode{0x2322}}}} {x}} _{Ak},{\stackrel {{{ \unicode{0x2322}}}} {x}}_{Bk},{\stackrel {{{ \unicode{0x2322}}}} {x}} _{Ck},{\stackrel {{{ \unicode{0x2322}}}} {x}}_{Dk},{\stackrel {{{ \unicode{0x2322}}}} {y}} _{Ak}$ ,…, ${\stackrel {{{ \unicode{0x2322}}}} {y}}_{Dk}$ are introduced in the NLP together with constraints (29), (23d), (30b), (31), $$\begin{align*} \kappa _{k} &=1/\textrm {L}_{\textrm {W}} \cdot \tan \phi _{k}, \tag{40c}\\ s_{k} &=v_{k} \cdot (t_{k+1} -t_{k} ), \tag{40d}\end{align*}$$ and $$\begin{equation*} \textsf {EmbodiedFootprints}(t_{k} )\le 0. \tag{40e}\end{equation*}$$

In summary, the embodied-footprint constraints consist of (23d), (30b), (29), (31), and (40), which enable our method to plan safe trajectories. It is worth noting that the prerequisites (23d), (30b), and (31) must be satisfied, as our definition of embodied footprints would otherwise be incorrect. However, these prerequisites, which we actively incorporate as constraints in the NLP to ensure they are met, should not be viewed as a deficiency in our planner. In other words, the NLP solver will automatically adjust the decision variables to meet these prerequisites, so users need not verify their fulfillment prior to utilizing our planner. In fact, the efficiency of our proposed trajectory planner only hinges on three assumptions:

1. In discretizing the OCP into an NLP, the state and control variables are piecewise constant.

2. The ego vehicle does not go backward during the driving process.

3. The geometrics and kinematics of the ego vehicle satisfy $2\textrm {L}_{\textrm {W}} >\textrm {L}_{\textrm {B}} \cdot \tan \phi _{\max }$ .

SECTION V. Experimental Results and Discussions

Simulations and field tests were conducted to validate the completeness, solution optimality, solution speed, application scope, and closed-loop tracking performance of our proposed trajectory planner.

A. Simulation Setup

Simulations were performed in MATLAB (R2023a) and executed on an i9–9900 CPU that runs at $2\times 3.10$ GHz. IPOPT [29] is applied in the MATLAB+AMPL environment [30] as the NLP solver. The linear solver embedded in IPOPT is chosen as MA27 from the Harwell Subroutine Library (HSL) [31]. Hybrid $\text{A}\ast$ search algorithm [4] is adopted as the sampling-based path planner in the function $\textsf {PlanCoarsePath}{()}$ of Alg. 1.

The OCP’s cost function is defined as the completion time of the driving process. Typically, one might define the cost function as a weighted sum of both time efficiency and trajectory smoothness. However, by focusing purely on time optimality in the OCP, planned trajectories move closer to barriers and/or vertices of obstacles, providing a better demonstration of our proposal’s collision-avoidance ability. In seeking time optimality, an intuitive idea is to define the cost function as $J = T$ , but such a definition makes the Hessian matrix of the cost function too sparse because $T$ is only related to $t_{\textrm {N}_{\textrm {fe}} }.$ As a remedy for this issue, we define $J$ quadratically so that it optimizes the gross time while also minimizing the differences between adjacent collocation points: $$\begin{equation*} J=\sum \nolimits _{k=0}^{\textrm {N}_{\textrm {fe}} -1} {\left ({ {t_{k+1} -t_{k} } }\right )^{2}}. \tag{41}\end{equation*}$$

A 30m$\times 30\text{m}$ virtual workspace filled with static polygonal obstacles was constructed. Parametric settings are outlined in Table I. Notably, the basic assumption $2\textrm {L}_{\textrm {W}} >\textrm {L}_{\textrm {B}} \cdot \tan \phi _{\max }$ holds, thereby justifying the use of our proposed planner in this context.

TABLE I Parametric Settings for Simulations

B. Simulation Results and Discussions

Three distinct simulation cases were established, each characterized by environmental obstacles with sharp vertices. These cases were designed specifically to probe the ability of our planner to successfully avoid obstacles throughout the driving process. Fig. 8 depicts the optimized trajectories together with the footprints. These footprints, densely resampled between adjacent collocation points, do not overlap with the environmental obstacles, validating the efficacy of our proposed trajectory planner.

Fig. 8.

Footprints associated with optimized trajectories derived by our proposed trajectory planner: (a) Case 1 with $\text{N}_{\mathrm{fe}}=117$ ; (b) Case 2 with $\text{N}_{\mathrm{fe}}=124$ ; (c) Case 3 with $\text{N}_{\mathrm{fe}}=105.$

Within Fig. 8, the collocation points are marked as red dots along the optimized trajectory. Notably, these points are not equally spaced in the time horizon. Concentrating on Case 3, we provide a further depiction of the optimized $\Delta t_{k} \equiv t_{k+1} -t_{k}$ alongside their initially guessed values in Fig. 9. As can be seen from this figure, our trajectory planner generates collocation points that are not uniformly distributed along the time dimension. Additionally, the trends observed for the optimized $\Delta t_{k}$ and their initially guessed values are similar, suggesting the effectiveness of AttachVelocity Additionally, the optimized $\{\Delta t_{k} \}$ exhibits better uniformity, showing the effectiveness of the employed cost function (41).

Fig. 9.

Comparison between initially guessed and optimized time subinterval durations $\left\{{ {\Delta t_{k} \left|{ {\Delta t_{k} \equiv t_{k+1} -t_{k}} }\right.} }\right\}$ in Case 3.

Case 2 serves as an example in our examination of how the user-specified parameter ${\lambda }$ influences trajectory planning performance. Fig. 10 displays the optimized ${T}$ values under various ${\lambda }$ ranging from 0.65 to 1.00. As we observe, $T$ generally increases with ${\lambda }$ . The rationale for this trend is as follows. Recall that ${\lambda }$ is utilized in $\textsf {AttachVelocity}{()}$ to estimate the distribution of collocation points [refer to (37) in Section IV-B] and N_fe is also determined then. Consequently, a larger ${\lambda }$ results in a smaller N_fe, and vice versa. When N_fe is larger, $\Delta t_{k}$ is generally smaller, bringing the size of the embodied footprint closer to the actual footprint. This makes the proposed planner less conservative and enhances time optimality, which explains why a descending trend is observed in the optimized $T$ as ${\lambda }$ decreases.

Fig. 10.

Migration of optimized T with user-specified slack multiplier ${\lambda}$ in Case 2.

Our proposed trajectory planner is evaluated alongside existing optimization-based trajectory planners, including the naive NLP method [7], the Lightweight Iterative Optimization Method (LIOM) [1], and VPF [25]. The naïve NLP method is identical to our approach except that the embodied footprint constraints in the NLP (38) are replaced with the nominal collision-avoidance constraints applied to the collocation points. LIOM employs multiple discs to cover the rectangular vehicle body, thus creating buffers to mitigate non-collocation errors. The VPF method was mentioned in Section I-A. When implementing VPF, we strictly follow the pseudo codes and parametric settings in [25]. To ensure fairness in comparison, the number of collocation points in the naïve NLP method, LIOM, or VPF is set to match the value identified by our proposed planner in each case. Thus, the collocation point number remains identical throughout all four planners for each trajectory planning case.

The results of the comparative simulations are summarized in Table II. Here, “completeness” refers to whether a planned trajectory can successfully mitigate non-collocation errors. As shown in the table, the naïve NLP method fails to achieve completeness in two cases. Using Case 2 as an example, Fig. 11 highlights minor collisions between adjacent collocation points, thus showing the limitation of the naïve NLP method and reaffirming our motivation for this research. Although LIOM operates swiftly, it produces overly conservative trajectories, as reflected by the optimized $T$ in each case. VPF is less conservative because its optimized $T$ closely matches that of the naïve NLP method. However, VPF runs slowly as it seeks to determine a valid enlarged box around the ego vehicle’s actual footprint in a trial-and-error mode by solving intermediate NLPs iteratively. We showcase the trajectories optimized by the four planners in Fig. 12 using Cases 2 and 3 as examples, which are in line with our aforementioned analysis.

TABLE II Comparative Simulation Results Among Four Planners

Fig. 11.

Footprints associated with an optimized trajectory that is derived by naïve NLP planner in Case 2. Note that collocation points distribute uniformly along time horizon.

Fig. 12.

Comparison among trajectories optimized by four planners.

Upon examining Fig. 12, some readers might argue that VPF offers better solution optimality than our planner because VPF is less conservative. To respond to this concern, we define a new case by adding several static obstacles to Case 3, thus making the environment tighter. Fig. 13a shows the optimized trajectory along with the densely resampled footprints from our proposed planner, whereas VPF, LIOM, and the naïve NLP method fail in that case. Recall that LIOM deploys multiple discs to model the ego vehicle’s rectangular footprint. This would render a too large buffer region around the ego vehicle to hinder a narrow passage traverse. That is why LIOM did not work in tiny scenarios. Conversely, the naïve NLP method exits with an optimal trajectory successfully, but the derived trajectory is found to be invalid because it involves non-collocation errors. The failure of VPF is explained as follows. VPF employs a trial-and-error strategy, which starts with solving the naïve NLP problem. If the derived trajectory leads to collisions between adjacent collocation points, then the ego vehicle’s footprint is laterally expanded quantitatively in hopes of reducing collisions in the next iteration with this enhanced buffer. In VPF, a variable $\alpha _{\textrm {gcf}} \ge 1.0$ is deployed to describe the quotient of the laterally expanded width divided by the nominal width. According to the monotony policy listed in [25], VPF exits once the calculated $\alpha _{\textrm {gcf}}$ is found to be smaller than that derived in a preceding cycle. In our simulation, $\alpha _{\textrm {gcf}}$ used to be 1.0016 and later got updated to 1.0012, which made VPF exited with a derived optimal solution. However, we find that the derived trajectory involves non-collocation errors, thus the output of VPF is invalid (Fig. 13b). Even without such a monotony policy, VPF is still inefficient to deal with narrow passages when a passage width happens to be smaller than the laterally expanded buffer. At this point, VPF lacks enough flexibility to fight against non-collocation errors. By contrast, our planner does not suffer from such a limitation.

Fig. 13.

Comparison between this work and VPF in an enhanced version of Case 3. Note that the black obstacles are newly added to make this new case distinct from Case 3. (a) Footprints associated with an optimized trajectory derived by our proposed planner; (b) Footprints associated with an output trajectory (invalid) derived by VPF.

Fig. 13a plots the distribution of collocation points along the optimized trajectory, indicating that the collocation points are densely packed near the bottleneck regions. This is not determined by users but by the NLP solution process itself. In other words, our planner knows how to distribute collocation points to navigate narrow passages safely. If the passages are really tiny, the NLP solution process knows to slow down with slim steering angles there so as to make the embodied footprints close to the actual footprints there. This unique advantage of our proposed planner clearly demonstrates that tackling narrow passages in a trial-and-error mode is not a complete solution.

The results of the simulation cases discussed in this subsection can be found at bilibili.com/video/BV1J94y1W7JY/. In these simulations, the ego vehicle is bordered in green and the embodied footprint is marked in red.

C. Field Test Setup, Results, and Discussions

Field tests were conducted on a small-sized autonomous vehicle platform within a $1.75\textrm {m}\times 1.20\textrm {m}$ indoor workspace, as shown in Fig. 14. For the purpose of obstacle detection and localization, six infrared sensors were deployed. Specifically, reflective markers were affixed to the vertices of each polygonal obstacle as well as the top of the self-driving vehicle. The procedure involves infrared light sources emitting beams, which are then captured by the infrared sensors after reflection from the markers. This setup allows for the precise localization of each marker [32].

Fig. 14.

Workspace layout and indoor localization solution for field tests.

The indoor infrared solution, provided by NOKOV®, was implemented in this study. The infrared sensors from this solution operated at a frequency of 60Hz. The data collected for perception and localization was processed on a desktop computer. This is where the trajectory planning codes, written in C++, were compiled and executed. The trajectory planning module ran once to generate an offline trajectory before the ego vehicle initiated its movement. The precalculated trajectory, along with the localization information, was transmitted from the computer to the autonomous vehicle platform via ZigBee. In regards to onboard tracking control, a Proportional-Integral-Derivative controller was employed for longitudinal tracking, while a Pure Pursuit controller was used for lateral tracking. Each controller was set to a frequency of 10 Hz.

Parameters relevant to the vehicle’s kinematics and geometry are detailed in Table III. These parameters satisfy $2\textrm {L}_{\textrm {W}} >\textrm {L}_{\textrm {B}} \cdot \tan \phi _{\max }$ , thus confirming the suitability of the proposed planner for the field tests.

TABLE III Parametric Settings for Field Tests

Representative results of the field tests can be accessed via the video link provided in Section V-B. Fig. 15 showcases snapshots of the closed-loop tracking performance, signifying that the planned open-loop trajectory is safe and easily trackable, as further illustrated in Fig. 16.

Fig. 15.

Snapshots of closed-loop control performance when the ego vehicle is tracking a planned open-loop trajectory, which is derived by a trajectory planner with safe collision-avoidance constraints.

Fig. 16.

Comparison between open-loop and closed-loop trajectories in field test.

SECTION VI. Conclusion

This paper has proposed a theoretical model for safe collision-avoidance constraints for numerical optimal control-based trajectory planners. We have named the proposed model embodied footprint, drawing inspiration from the emerging concept embodied intelligence from the AI research field. We aim to capture the idea that the footprint model of the ego vehicle proactively knows how to flexibly and autonomously set its buffer scale rather than reactively making adjustments in a feedback or trial-and-error manner.

The proposed method can be further extended to deal with precision-aware motion planning problems in other robotics-related fields other than autonomous driving. The proposed modeling method may also work as a fast collision checker that only needs to examine a small number of waypoints along a to-be-checked path or trajectory.

Our future work is to further extend the current study so that reverse driving conditions are enabled, which are simply symmetric to the analyses in this paper, though.