Safe Trajectory Synthesis for Autonomous Driving in Unforeseen Environments

TL;DR

This paper presents a safety-guaranteed trajectory planning method for autonomous vehicles that accounts for model mismatch by using a conservative reachable set, enabling real-time obstacle avoidance in unforeseen environments.

Contribution

It introduces a novel approach combining low-fidelity planning with high-fidelity safety guarantees through conservative reachable sets and real-time obstacle intersection analysis.

Findings

Successfully demonstrated in simulation with Dubin's car and unicycle models.

Proves safety guarantees based on bounded computation time and sensing horizon.

Provides a framework for real-time safe trajectory synthesis in complex environments.

Abstract

Path planning for autonomous vehicles in arbitrary environments requires a guarantee of safety, but this can be impractical to ensure in real-time when the vehicle is described with a high-fidelity model. To address this problem, this paper develops a method to perform trajectory design by considering a low-fidelity model that accounts for model mismatch. The presented method begins by computing a conservative Forward Reachable Set (FRS) of a high-fidelity model's trajectories produced when tracking trajectories of a low-fidelity model over a finite time horizon. At runtime, the vehicle intersects this FRS with obstacles in the environment to eliminate trajectories that can lead to a collision, then selects an optimal plan from the remaining safe set. By bounding the time for this set intersection and subsequent path selection, this paper proves a lower bound for the FRS time horizon and sensing horizon to guarantee safety. This method is demonstrated in simulation using a kinematic Dubin's car as the low-fidelity model and a dynamic unicycle as the high-fidelity model.

Figures

Equations

$$\dot{x}(t)=f(t,x(t),u(t))$$

$$\left[\begin{array}{c}{\dot{x}}_s(t)\\ \dot{k}(t)\end{array}\right]=\left[\begin{array}{c}{f}_s(t,x_s(t),k(t))\\ 0\end{array}\right]$$

$$\left[\begin{array}{c}\dot{x}(t)\\ \dot{y}(t)\\ \dot{\theta }(t)\\ \ddot{\theta }(t)\\ \dot{v}(t)\end{array}\right]=\left[\begin{array}{c}v(t)\cos \left(\theta (t)\right)\\ v(t)\sin \left(\theta (t)\right)\\ \dot{\theta }(t)\\ u_1(t)\\ u_2(t)\end{array}\right]$$

$$\left[\begin{array}{c}\dot{x}(t)\\ \dot{y}(t)\\ \dot{\theta }(t)\end{array}\right]=\left[\begin{array}{c}{k}_2\cos \left(\theta (t)\right)\\ k_2\sin \left(\theta (t)\right)\\ k_1\end{array}\right]$$

$$K$$

$$X_0$$

$$X_s$$

$$\underset{x\in A_{x_s}}{\max }\left|f_i(t,x,u_k(t,x)-f_{s,i}(t,x_s,k)\right|\le g_i(t,x_s),$$

$$\ddot{\theta }(t)=u_1(t)=20\cdot (\dot{\theta }(t)-{\dot{\theta }}_{\text{des}})$$

$$\dot{v}(t)=u_2(t)=10\cdot (v(t)-v_{\text{des}})$$

$$g(t,x,y,\theta )=\left[\begin{array}{c}{v}_{\text{err}}(t)\cdot (1-\frac{1}{2}{\theta }^2)\\ v_{\text{err}}(t)\cdot (\theta -\frac{1}{6}{\theta }^3)\\ {\dot{\theta }}_{\text{err}}\end{array}\right]$$

$${\dot{x}}_s(t)=f_s(t,x_s(t),k)+g(t,x_s(t),k)d(t)$$

$${\mathcal{L}}_{f_s}v(t,x_s,k)=\frac{\partial v}{\partial t}(t,x_s,k)+\sum _{i=1}^{n_s}\frac{\partial v}{\partial x_{i,s}}(t,x_s,k)f_{i,s}(t,x_s,k).$$

$${\mathcal{L}}_{g}v(t,x_s,k)=\sum _{i=1}^{n_s}\frac{\partial v}{\partial x_{i,s}}(t,x_s,k)g_i(t,x_s).$$

$$X_{\text{obs}}=\left\{x_s\in {\mathbb{R}}^{n_s}\mid h_{{\text{obs}}_i}(x_s)\ge 0,\forall i\in \{1,\dots ,n_{\text{obs}}\}\right\}.$$

$$\displaystyle\mathcal{X}_{\text{FRS}}=\Big{\{}(x_{s},k)\in X_{s}\times K\ |$$

$$d:[0,T]\to [-1,1]\text{ s.t. }x(\tau )=x_s,\text{ where}$$

$$\dot{x}(t)=f_s(t,x(t),k)+g(t,x(t),k)d(t)$$

$$\displaystyle\text{a.e. }t\in[0,T]\text{ and }x(0)=x_{0}\Big{\}}$$

$${\tau }_{\text{plan}}+{\tau }_{\text{stop}}$$

$$T+{\tau }_{\text{plan}}$$

$$\underset{v,w,q}{\inf }$$

$${\mathcal{L}}_{f_s}v(t,x_s,k)+q(t,x_s,k)\le 0,$$

$${\mathcal{L}}_{g}v(t,x_s,k)+q(t,x_s,k)\ge 0,$$

$$-{\mathcal{L}}_{g}v(t,x_s,k)+q(t,x_s,k)\ge 0,$$

$$q(t,x_s,k)\ge 0,$$

$$-v(0,x_s,k)\ge 0,$$

$$w(x_s,k)\ge 0,$$

$$w(x_s,k)+v(t,x_s,k)-1\ge 0,$$

$$v(\tau ,x(\tau ),k)$$

$$\ge v(0,x(0),k)+{\int }_0^{\tau }\left({\mathcal{L}}_{f_s}v(t,x(t),k)+q(t,x(t),k)\right)dt$$

$$\ge v(0,x(0),k),$$

$$\underset{h}{\sup }$$

$$1-w(x_s,k)-h(k)\ge 0,$$

$$h(k)\le 1,$$

$$q=s_0+s_1{h}_{\tau }+\sum _{i=1}^{n_{X_s}}{s}_{i+2}{h}_{X_i}+\sum _{i=1}^{n_K}{s}_{i+n_{X_S}+2}{h}_{K_i},$$

$$\underset{v,w,q}{\inf }$$

$$-{\mathcal{L}}_{f_s}v-q\in Q_{2l}(h_{\tau },h_{X_1},\dots ,h_{X_{n_{X_s}}},h_{K_1},\dots ,h_{K_{n_K}})$$

$${\mathcal{L}}_{g}v+q\in Q_{2l}(h_{\tau },h_{X_1},\dots ,h_{X_{n_{X_s}}},h_{K_1},\dots ,h_{K_{n_K}})$$

$$-{\mathcal{L}}_{g}v+q\in Q_{2l}(h_{\tau },h_{X_1},\dots ,h_{X_{n_{X_s}}},h_{K_1},\dots ,h_{K_{n_K}})$$

$$q\in Q_{2l}(h_{\tau },h_{X_1},\dots ,h_{X_{n_{X_s}}},h_{K_1},\dots ,h_{K_{n_K}})$$

Full text

I INTRODUCTION

Safe control of autonomous vehicles for use on public roads is a challenging problem that directly impacts both public safety and development costs for auto manufacturers. To be safe, autonomous vehicles must be able to account for errors in their sensors and dynamic models. Moreover, since it is impractical to preplan an obstacle avoidance trajectory for every plausible environment and configuration of obstacles, controllers need to be constructed, evaluated for correctness, and selected in real-time.

Autonomous vehicles typically apply a hierarchical control design structure with three levels [6, 3, 8]. The highest-layer in this architecture is responsible for route planning using Dijkstra’s algorithm or its variants. The mid-level controller generates trajectories to perform actions such as lane-keeping or obstacle avoidance. The low-level controller follows these generated trajectories by applying throttle, brake, and steering inputs. For computational efficiency, the mid-level controller often uses a lower degree of freedom model than the low-level controller. As a result, there is typically a non-trivial gap between the trajectories generated by the mid-level controller and what a vehicle is capable of executing. This paper develops a real-time optimization based scheme for the mid-level controller that can provably design trajectories which can be safely followed by the low-level controller in arbitrary environments with static obstacles.

Various methods exist in the literature for mid-level controller design, which rely upon either temporal or spatial discretization, or both. Rapidly Random Exploring Trees, for example, compute safe trajectories by sampling from a vehicle’s input space and numerically integrating trajectories forward using a dynamic model [13]. These methods can handle nonlinear dynamics and non-convex constraints and can even be made asymptotically optimal [11]. Partially Observable Markov Decision Processes are also able to handle nonlinear dynamics, non-convex constraints, and bounded uncertainty, while planning trajectories in a spatially and temporally discretized space[2]. Though each of these mid-level control design methods is powerful, they struggle to account for the uncertainty introduced by discretization, which can result in trajectories that are impossible to execute. For this reason, others have focused solely on the real-time verification of the safety of a generated mid-level controller using approaches grounded in zonotopes [1].

Several approaches have recently been pursued to perform control design with safety guarantees. Robust Model Predictive Control (MPC), for example, has been demonstrated to successfully account for uncertainty in the dynamics and can be used for safe control design for nonlinear vehicle models in the presence of non-convex constraints [7, 23]. This requires linearizing the dynamics about a pre-specified, spatially discretized trajectory to synthesize a controller that is able to safely follow this trajectory while avoiding obstacles. Thus, this approach requires solving a scenario-specific nonlinear program, which can be challenging to solve at run-time and, as a result, has few associated safety guarantees. To try to address the more general safe control design problem, others have employed the Hamilton-Jacobi-Bellman Equation, which is typically solved using state-space discretization, to compute the set of safe feedback controllers in the presence of bounded disturbance [5]. These approaches rely upon a complete characterization of obstacle locations, which can make real-time applications challenging due to the computational expense associated with state-space discretization. Despite this limitation, extensions of this approach have been successfully employed to perform cooperative control for connected vehicles [4]. Similarly, the viability kernel, which is also computed using state-space discretization, has been employed to evaluate the set of possible trajectories for autonomous vehicles on pre-defined scenarios [14].

To avoid this curse of dimensionality, others have employed barrier functions using Lyapunov theory to devise safety-preserving controllers for adaptive cruise control and lane-keeping [20, 25, 24]. These approaches can quickly solve a quadratic program at run-time to guarantee safety, but the barrier function, which is computed off-line, is scenario specific. To overcome this situation-specific limitation, an approach relying upon funnel libraries was recently proposed to perform real-time control design in the presence of uncertainty [17]. This approach relies upon pre-computing a rich-enough finite family of trajectories, which is then searched at run-time to ensure safety.

This paper presents an approach for mid-level control design that simultaneously accounts for nonlinear dynamics, non-convex constraints, and uncertainty without pre-specifying the scenario, pre-computing a finite family of trajectories, or relying on spatial or temporal discretization. The proposed method, which is depicted in Figure 1, begins by computing the set of trajectories that can be reached by a high-fidelity vehicle model under a parameterized controller. This Forward Reachable Set (FRS), which is computed off-line, is then intersected with obstacles at run-time using convex optimization to exclude controllers that could cause collisions. Finally, the remaining safe set of controllers is optimized over to minimize an arbitrary cost function, such as following a nominal trajectory, minimizing total acceleration, etc.

This paper makes the following pair of contributions: first, a convex optimization based method to synthesize trajectories which can be safely tracked by a high fidelity vehicle model; and second, a method to select braking, sensing, and planning time horizons to guarantee that a safety preserving trajectory always exists in an environment with static obstacles. The rest of the paper is structured as follows: Section II introduces the notation and assumptions utilized throughout the paper. Section III outlines methods to determine the FRS in the presence of bounded uncertainty, to compute the set of safe trajectory parameters in the presence of obstacles, and to compute an optimal trajectory from this set. Section IV describes the implementation of the methods formulated in Section III. Section V illustrates the performance of our method on an example.

II PRELIMINARIES

This section introduces necessary notation for this paper, and formalizes the dynamics and assumptions using to describe the problem of interest. This paper makes substantial use of semidefinite programming theory and sums of squares programming [12].

II-A Notation

The following notation is adopted throughout the remainder of the paper: Sets are italicized and capitalized. The boundary of a set $K$ is $\partial K$. Finite truncations of the set of natural numbers are $\mathbb{N}_{n}:=\{1,\ldots,n\}$. The set of continuous functions on a compact set $K$ are $\mathcal{C}(K)$. The Lebesgue measure on $K$ is denoted by $\lambda_{K}$. We let $y_{K}$ denote the Lebesgue moments of $\lambda_{K}$. The $i$-th component of a vector $v\in\mathbb{R}^{n}$ is denoted by $v_{i}$. The ring of polynomials in $x$ is $\mathbb{R}[x]$, and the degree of a polynomial is the degree of its largest multinomial; the degree of the multinomial $x^{\alpha},\,\alpha\in\mathbb{N}^{n}$ is $|\alpha|=\|\alpha\|_{1}$; and $\mathbb{R}_{d}[x]$ is the set of polynomials in $x$ with degree $d$. Let $\text{vec}(p)$ denote the vector of coefficients of $p\in\mathbb{R}[x]$.

II-B Low and High-Fidelity Vehicle Models

Let an autonomous vehicle by described by:

[TABLE]

where $f:[0,T]\times X\times U\to\mathbb{R}^{n}$, $T>0$, $X\subset\mathbb{R}^{n}$ and $U\subset\mathbb{R}^{m}$. The objective of this paper is to design safe trajectories for this autonomous vehicle which may be high dimensional and challenging to optimize over in real-time. Our approach relies upon generating parameterized trajectories using a second, lower-dimensional model for this autonomous vehicle that shares a common set of states, $X_{s}\subset\mathbb{R}^{n_{s}}$ with $X_{s}\subset X$. This model is described as:

[TABLE]

where the parameters, $k(t)$, do not evolve after initialization and are drawn from a set $K\subset\mathbb{R}^{p}$.

Example 1

Consider a dynamic unicycle model:

[TABLE]

where $x$ and $y$ represent position; $\theta$ and $\dot{\theta}$ represent heading and yaw rate, both relative to the initial condition of the vehicle; and $v$ represents the longitudinal speed. The inputs are $u_{1}$ for steering and $u_{2}$ for throttle or braking. Consider a low-fidelity, trajectory-producing model with shared states $\begin{bmatrix}x&y&\theta\end{bmatrix}^{T}$ such that:

[TABLE]

with the trajectory parameters that are steering rate $k_{1}$ and speed $k_{2}$. The unicycle model “adds” mass to the Dubins car.

For the sake of convenience, the dynamics of the shared states, $X_{S}$, in the high dimensional model are assumed to be described by the first $n_{s}$ coordinates of $f$ throughout this paper. Let the controlled vehicle be represented as a compact set $X_{0}\subset X_{s}$, typically a bounding box around the vehicle. We make the following assumptions to ensure the well-posedness of the subsequent problem formulation.

Assumption 2

The sets $U$ and $X$ are compact and suppose $K,X_{0},$ and $X_{s}$ have the following representation:

[TABLE]

The objective of this paper is to optimize over trajectories using the lower complexity dynamical model which are then followed by the high dimensional vehicle model. Unfortunately this tracking cannot be done perfectly. The higher fidelity model can select a controller, $u_{k}:[0,T]\times X\to\mathbb{R}^{m}$, to follow the generated trajectory which is parameterized by $k\in K$ (e.g. using a PID control loop); however there may still be a gap between the generated pair of trajectories. To describe this error as a function of time, we make the following assumption:

Assumption 3

There exists a bounded function $g:[0,T]\times X_{s}\to X_{s}$ such that:

[TABLE]

for all $x_{s}\in X_{s}$, $t\in[0,T]$, $k\in K$, and $i\in\{1,\ldots,n_{s}\}$, where $A_{x_{s}}:=\{x\in X\mid x_{i}=x_{s,i}\forall i\in\{1,\ldots,n_{s}\}\}$.

In other words, the error in the shared states is bounded by $g$. Though this paper does not describe a formal method to construct such a $g$, in the instance of polynomial, rational, or trigonometric dynamics, such a $g$ can be found numerically by applying Sums-of-Squares programming as we describe in Section IV.

Example 4

For the unicycle model in Example 1, $u_{k}$ can be a proportional velocity controller for the yaw rate and speed:

[TABLE]

where $(\dot{\theta}_{\text{des}},v_{\text{des}})$ map to a $k_{1}$ and $k_{2}$ in the parameter space for the Dubins car model described in Equation (4). Given this $u_{k}$ one can generate a $g$ that overapproximates the error when transitioning from top speed to a complete stop in the $v$ dimension; and when switching from turning full-left to turning full-right (or vice-versa) in the $\dot{\theta}$ dimension. Such a $g$, illustrated in Figure 2, could be:

[TABLE]

*where $v_{\text{err}}(t)=(t-1)^{2}$ and $\dot{\theta}_{\text{err}}(t)=(t-1)^{4}$. *

As a result of this assumption, the dynamics can be rewritten in a form amenable to computing the flow of the system $f_{s}$ while subject to error dynamics:

[TABLE]

where $d(t)\in[-1,1]$ for each $t\in[0,T]$ can be chosen to describe the worst-case error behavior. This use of $d$ is discussed further in Section III-A. To understand the gap between lower and higher dimensional models, we rely upon a pair of linear operators. First, the linear operator \mathcal{L}_{f_{s}}:C^{1}\big{(}[0,T]\times X_{s}\times K\big{)}\to C\big{(}[0,T]\times X_{s}\times K\big{)} on a test function $v$ as:

[TABLE]

Next, define the linear operator \mathcal{L}_{g}:C^{1}\big{(}[0,T]\times X_{s}\times K\big{)}\to C\big{(}[0,T]\times X_{s}\times K\big{)} as:

[TABLE]

Note that these linear operators are useful in summarizing the evolution of a system as we describe in Section III-A2.

II-C Sensing, Planning, and Braking Time Horizons

Next, we define a sensing, planning, and braking time horizon whose relationships are formalized in the next section to ensure the safety of the autonomous vehicle. To construct these definitions, we begin by formalizing obstacles:

Definition 5

An obstacle is any portion of $X_{s}$ that must be avoided by the vehicle. In general, obstacles are represented as closed subsets of $X_{s}$. Obstacles are assumed to be static in the system’s local reference frame and are defined as:

[TABLE]

Obstacles can be other vehicles, the edges of the road, pedestrians, etc. A trajectory is safe if it does not intersect with an obstacle. These obstacles are assumed to enter the scene as follows:

Assumption 6

During operation, obstacles always enter $X_{s}$ from outside, i.e., any obstacle’s trajectory relative to the system must begin outside of, and may pass through, $\partial X_{s}$. In addition, let the system be limited to a scalar top speed, denoted $\dot{x}_{s,\text{max}}$. Then, this assumption requires that obstacles are sensed at a minimum sensor horizon $D_{\text{sense}}:=\dot{x}_{s,\text{max}}\cdot T_{\text{sense}}$.

$T_{\text{sense}}$ is a sensor time horizon that is to be determined, as a safety condition that we describe in the next section.

Next, we assume that the time to process sensor information and plan a trajectory is bounded and known:

Assumption 7

The time required to process sensor data and perform trajectory planning has a finite upper bound $\tau_{\text{plan}}$.

Specific sensor processing to detect obstacles is outside of the scope of this paper, but in practice most modern obstacle detectors, working from camera, lidar, or radar, have a bounded processing time [10, 15]. Though we do not prove that the trajectory planning time is bounded, the convex optimization based implementation that we utilize has a processing time that takes several seconds, at most, in practice as we describe in Section IV.

Remarks 8

To understand how this $\tau_{\text{plan}}$ can be used, suppose the vehicle is planning as quickly as possible and executing a trajectory beginning at $t=0$ . While executing this trajectory, the vehicle can plan its next trajectory which is applied beginning at $t=\tau_{\text{plan}}$. The initial condition for this next trajectory is computed by the vehicle by forward integrating $f$ over the time interval $[0,\tau_{\text{plan}}]$. This process can be repeated recursively to replan trajectories.

Finally, in certain instances it may be impossible to reach a desired position safely. For these scenarios, we must assume that there exists a braking maneuver which can stop the car:

Assumption 9

There exists a family of braking trajectory parameters $K_{b}\subseteq K$ such that, without deviating from the spatial component of a pre-planned trajectory, $\dot{x}_{s}=0$ in a finite amount of time $\tau_{b}(x_{s})\in\mathbb{R}_{+}$ for each $x_{s}\in X_{s}$. In addition, this stopped state can be held indefinitely.

In other words, the vehicle is able to brake and stop along any trajectory it is following. It follows from Assumption 9 that there is a maximum braking time for the system, i.e. there exists $\tau_{\text{stop}}\in\mathbb{R}_{+}$ such that $\tau_{b}(x_{s})\leq\tau_{\text{stop}}$ for all $x_{s}\in X_{s}$. This braking time is used to construct planning and sensing time horizons to guarantee safety.

III PROBLEM FORMULATION

This section outlines a method for trajectory design and presents a theorem relating the braking, planning, and sensing time horizons to ensure the safety of the trajectory design procedure. The approach is broken into the following steps:

Precompute a Forward Reachable Set (FRS) that captures all possible trajectories and associated parameters, subject to error function $g$, over a time interval $[0,T]$ (Section III-A). 2. 2.

During operation, perform set intersection of the FRS with obstacles in the vehicle’s local environment to identify trajectory parameters, $K_{\text{safe}}\subset K$, which could cause collisions using a convex optimization technique (Section III-B). 3. 3.

During operation, perform trajectory optimization over $K_{\text{safe}}$ of a user-specified cost function to drive the system towards an objective while guaranteeing safety (Section III-C).

These steps are described next.

III-A Forward Reachable Set

This subsection describes our approach to compute the FRS, which contains all points that can be reached from $X_{0}$ under the dynamics of $f$. The dynamics of $f$ may be high-dimensional in general as a result, which can make computing the FRS for $f$ impractical. To overcome this challenge, we compute the FRS of the lower-dimensional model described in Equation (12).

[TABLE]

Notice that in this definition the error function $g$ along with $d(t)\in[-1,1]$ accounts for the difference between a trajectory, parameterized by $k\in K$, of the lower dimensional model and the higher-dimensional model which follows this trajectory using a control $u_{k}$.

III-A1 Selecting $T$ to Ensure Safety

Identifying the time horizon $T$ for the FRS is the critical first step. If $T$ is too short, then safe behavior cannot be guaranteed; if it is too long, then computing the FRS may be impractical. Before describing how to compute $\mathcal{X}_{\text{FRS}}$, we first describe how to select the time horizon $T$ to ensure safety while planning:

Theorem 10

Let $T_{\text{sense}}$, $\tau_{\text{plan}}$, and $\tau_{\text{stop}}$ be as in Assumptions 6,7, and 9, respectively. Suppose the vehicle plans a trajectory every $\tau_{\text{plan}}$ and there exists a $T\geq 0$ such that:

[TABLE]

*and a safe trajectory over the time horizon $[0,T]$. Then, there exists a safe trajectory for all $t>T$. *

Proof:

This theorem follows directly from the assumptions, which we can use to construct the worst-case scenario. At time $t=0$, let the vehicle be following a trajectory that has been previously identified as safe for the time interval $[0,T]$. Suppose trajectories are planned as described in Remark 8. Obstacles that could cause collisions fall into one of three cases. It is sufficient to show that none of these cases can cause a collision.

Case 1: Any obstacle that can be reached within time $T$. These obstacles will already be avoided since the trajectory starting at time $t=0$ is safe for the entire duration $[0,T]$ by definition, even if the vehicle must come to a stop.

Case 2: Any obstacle that can be reached at a time $t\in(T,T_{\text{sense}}]$. These obstacles are sensed according to Assumption 6. The worst-case scenario, in this instance, is an obstacle that can be reachable infinitesimally after $t=T$. Recall that the vehicle is replanning while traveling a duration of $\tau_{\text{plan}}$, so the plan beginning from time $t=\tau_{\text{plan}}$ incorporates the positions of all sensed obstacles by adjusting their relative position using the initial condition for the upcoming trajectory. In addition, this plan incorporates the error that accumulates over the trajectory before $\tau_{\text{plan}}$ because the FRS uses Assumption 3. Therefore, at time $t=\tau_{\text{plan}}$, no obstacle is reachable within a time horizon less than $T-\tau_{\text{plan}}$, which is greater than or equal to $\tau_{\text{stop}}$. Consequently, while traversing the first time interval $[0,\tau_{\text{plan}}]$, the vehicle determines whether or not it must begin stopping, or if a non-braking trajectory exists, at time $t=\tau_{\text{plan}}$. As a result, at $t=\tau_{\text{plan}}$, the obstacle falls into Case 1.

Case 3: Any obstacle that can be reached at a time $t>T_{\text{sense}}$. We assume that the vehicle is detecting obstacles continually, as per Assumption 6. Therefore, by similar logic to Case 2, the obstacle is no closer than $T_{\text{sense}}-\tau_{\text{plan}}$ at time $t=\tau_{\text{plan}}$. The obstacle thus falls into Case 2 for the trajectory starting at $t=\tau_{\text{plan}}$. ∎

III-A2 FRS Computation

To compute the FRS one can solve the following linear program over the space of functions, which is adapted from [18, Section 3.3, Program $(D)$]:

[TABLE]

The given data in this problem are $f,g,X_{s},X_{0},K$ and $T$ and the infimum is taken over $(v,w,q)\in C^{1}\left([0,T]\times X_{s}\right)\times C(X_{s})\times C([0,T]\times X_{s})$.

Notice first that the $1$-superlevel set of feasible $w$’s in $(D)$ are outer approximations of $\mathcal{X}_{\text{FRS}}$:

Lemma 11

Let $(v,w,q)$ be feasible functions to $(D)$, then \mathcal{X}_{\text{FRS}}\subset\big{\{}(x_{s},k)\in X_{s}\times K\mid w(x_{s},k)\geq 1\big{\}}.

Proof:

Suppose $(x_{s},k)\in\mathcal{X}_{\text{FRS}}$, then $\exists~{}\tau\in[0,T]$, $d:[0,T]\mapsto[-1,1]$ and $x:[0,T]\mapsto X_{s}$ such that $\dot{x}(t)=f_{s}(t,x(t),k)+g(t,x(t),k)d(t)$ for a.e. $t\in[0,T]$ with $x(\tau)=x_{s}$ and $x(0)\in X_{0}$. Observe that:

[TABLE]

where the first equality follows from the Fundamental Theorem of Calculus, the second inequality follows from noticing that $\text{sup}_{d(t)\in[-1,1]}|\mathcal{L}_{g}v(t,x(t),k)d(t)|=|\mathcal{L}_{g}v(t,x(t),k)|$ and that $|\mathcal{L}_{g}v(t,x(t),k)|\geq q(t,x(t),k)$ due to the constraints in $(D)$, and the final inequality follows from the constraints in $(D)$. Notice the desired result follows by noticing that $v(0,x(0),k)\geq 0$ for $x(0)\in X_{0}$ and applying the last constraint in $(D)$. ∎

The result of this lemma is that the $1$-superlevel set of any feasible $w$ contains all possible trajectories of the high fidelity vehicle model $f$ beginning from $X_{0}$. In fact, one can prove that the solution to this infinite dimensional linear program allows one to compute $\mathcal{X}_{\text{FRS}}$:

Theorem 12

[18, Theorem 3.5]** There is a sequence of feasible solutions to $(D)$ whose $w$-component converges from above to an indicator function on $\mathcal{X}_{\text{FRS}}$ in the $L^{1}$ norm and almost uniformly.

In Section IV-A we describe how to generate feasible solutions to $(D)$ using semidefinite programming over polynomial representations of continuous functions. Once this computation is completed once offline, the result can be used online by performing real-time set intersection as described next.

III-B Set Intersection

Any outer-approximation to $\mathcal{X}_{\text{FRS}}$ can be intersected with obstacles in the state space to return the trajectory parameters that could result in a collision. The complement of the set returned by this intersection are then parameters which can be safely followed by the high fidelity vehicle model. To describe this process, consider a $w$ generated as a solution to $(D)$. One can construct a closed inner approximation to the set of safe gains, denoted $K_{\text{safe}}\subset K$ by solving the following optimization problem:

[TABLE]

where $w$ is the given data and the supremum is taken over $h\in C(K)$.

To appreciate how his optimization problem generates an inner approximation to the set of safe gains, consider a trajectory parameterization, $k$, that generates a trajectory which intersects an obstacle as illustrated in Figure 3. In this instance, there exists some $x_{s}$ in $X_{\text{obs}}$ such that $w(x_{s},k)\geq 1$. In that instance the first constraint in the previous optimization problem requires that $h(k)$ be less than zero. This observation is formalized in the next lemma:

Lemma 13

Let $h$ be a feasible function to Equation (19). Then $\{k\in K\mid h(k)\geq 0\}\subset K_{\text{safe}}$.

By exploiting the polynomial outer approximation of the $\mathcal{X}_{\text{FRS}}$ which is generated in Section IV-A one can formulate a convex optimization method to compute a closed, inner approximation to $K_{\text{safe}}$. This optimization method is described in Section IV-B. Note that if $T$ is chosen as in Theorem 10, then as a result of Assumption 9, $K_{\text{safe}}\neq\emptyset$.

III-C Trajectory Optimization

Once $K_{\text{safe}}$ is determined for an obstacle, one can optimize the original dynamics directly over the set. Specifically, if a user specifies a continuously differentiable cost function, $J:K\to\mathbb{R}$ then one can optimize this cost function with a constraint that enforces that $k\in K_{\text{safe}}$. Since each $k$ corresponds to a $u_{k}$ one can optimize directly over safe trajectories of the high fidelity model that travel to some waypoint, minimize total acceleration along a trajectory, match a desired system speed, etc.

This constrained nonlinear optimal control problem can be solved in a variety of different ways via collocation, solving a variational equation, sampling, or using convex relaxations [26]. If this optimization program is unable to conclude within $\tau_{\text{plan}}$, then one can always apply a braking maneuver which always exists as described in Section III-B.

IV IMPLEMENTATION

This section describes the specific implementations of the FRS Computation, the set intersection, and the trajectory optimization described in Section III. For implementation, we require the following assumption:

Assumption 14

The functions $f_{s}$ is a polynomial in $\mathbb{R}[t,x_{s},k]$, and the error function $g$ is in $\mathbb{R}[t,x_{s}]$. In addition, the sets $K$, $X_{0}$, $X_{s}$, and $X_{\text{obs}}$ are semialgebraic, defined by polynomials in their respective spaces as in Equations (5) - (7) and (15).

IV-A FRS Computation

To solve $(D)$, we construct a sequence of convex sums-of-squares programs by relaxing the continuous function in $(D)$ to polynomial functions with degree truncated to $2l$. The inequality constraints in $(D)$ then transform into SOS constraints which then transforms $(D)$ into a semidefinite program[21] To formulate this problem, let $h_{\tau}=t(T-t)$ and define $Q_{2l}(h_{\tau},h_{X_{1}},\ldots,h_{X_{n_{X_{s}}}},h_{K_{1}},\ldots,h_{K_{n_{K}}})\subset\mathbb{R}_{2l}[t,x_{s},k]$ to be the set of polynomials $q\in\mathbb{R}_{2l}[t,x_{s},k]$ (i.e. of total degree less than $2l$) expressible as:

[TABLE]

for some polynomials $\{s_{i}\}_{i=0}^{n_{X_{s}}+n_{K}+1}\subset\mathbb{R}_{2k}[t,x,k]$ that are sums of squares of other polynomials, where we have dropped the dependence on $t,x,$ and $k$ in each polynomial for the sake of convenience. Note that every such polynomial is non-negative on $[0,T]\times X_{s}\times K$ [12, Theorem 2.14]. Define $Q_{2l}(h_{0_{1}},\ldots,h_{0_{n_{0}}},h_{K_{1}},\ldots,h_{K_{n_{K}}})\subset\mathbb{R}_{2l}[x_{s},k]$, $Q_{2l}(h_{X_{1}},\ldots,h_{X_{n_{X_{s}}}},h_{K_{1}},\ldots,h_{K_{n_{K}}})\subset\mathbb{R}_{2l}[x_{s},k]$, $Q_{2l}(h_{\text{obs}_{1}},\ldots,h_{\text{obs}_{n_{\text{obs}}}},h_{K_{1}},\ldots,h_{K_{n_{K}}})\subset\mathbb{R}_{2l}[x_{s},k]$, and $Q_{2l}(h_{K_{1}},\ldots,h_{K_{n_{K}}})\subset\mathbb{R}_{2l}[k]$ similarly.

Employing this notation, the $l$-th relaxed semidefinite programming representation of $(D)$, denoted $(D_{l})$, is defined as:

[TABLE]

where the infimum is taken over the vector of polynomials $(v,w,q)\in\mathbb{R}_{l}[t,x_{s},k]\times\mathbb{R}_{l}[x_{s},k]\times\mathbb{R}_{l}[t,x_{s},k]$.

If $w_{l}$ denotes the $w$-component of the solution to $(D_{l})$, one can prove that $w_{l}$ converges from above to an indicator function on $\mathcal{X}_{\text{FRS}}$ in the $L^{1}$ norm [18, Theorem 6]. Importantly since each such $w_{l}$ is feasible with respect to the constraints in $(D)$, one can apply the result of Lemma 11 to prove that the $1$-superlevel set of $w_{l}$ is an outer approximation to $\mathcal{X}_{\text{FRS}}$.

IV-B Set Intersection

In this section, we present a semidefinite program to generate a closed inner-approximation to $K_{\text{safe}}$ and comment on computing $\tau_{\text{plan}}$. Using the notation developed in Section IV-A, consider the following semidefinite formulation of Equation (19):

[TABLE]

where the given data is any feasible $w\in\mathbb{R}_{l}[x_{s},k]$ to $(D_{l})$ and the supremum is taken over polynomials $h\in\mathbb{R}_{l}[k]$. Note again that since the constraints in this optimization problem are sufficient to ensure positivity on the required sets in Equation (19), one can apply the result of Lemma 13 to prove that the [math]-superlevel set of $h_{l}$ is an inner approximation to $K_{\text{safe}}$.

Solving such an SDP, while possible in real time, is still computationally expensive. Compared to the trajectory optimization step, which takes milliseconds, the set intersection step can take several tenths of a second per obstacle, and is thus the primary contributor to $\tau_{\text{plan}}$. The planning time increases with the number of dimensions of $X_{\text{obs}}$ and with the number of obstacles. We find $\tau_{\text{plan}}$ by precomputing an FRS over an arbitrary, short time horizon $T$, then empirically determining $\tau_{\text{plan}}$ by performing set intersection with representative obstacles and environments.

IV-C Trajectory Optimization

We implement trajectory optimization by directly passing $h_{l}$ from solving (21) as a nonlinear constraint to an off-the-shelf gradient-descent solver, such as MATLAB’s fmincon.

V SIMULATION RESULTS

This section presents simulation results of the proposed method implemented on the unicycle model example. Each semidefinite program was prepared using a custom software toolbox and the modeling tool YALMIP [16]. The programs are run with commercial solver MOSEK on a machine with $1$ TB availabe memory.

V-A FRS Computation

We computed the FRS for a 3${}^{\text{rd}}$ order Taylor-expanded Dubins car as the low-fidelity model $f_{s}$. Trajectories produced by this model were tracked by the unicycle model from Equation (1) as the high-fidelity model $f$. The vehicle’s representation as an initial distribution $X_{0}\subset X_{s}$, was a rectangle of length $0.2$ [m] in $x$ and width $0.1$ [m] in $y$, at $0^{\circ}$ initial heading, and centered at $x=-0.75$ and $y=0$. This is the same vehicle representation shown in all previous figures.

We chose $\tau_{\text{stop}}=\tau_{\text{plan}}=0.5$ [s], so $T=1$ [s]. The stopping time can be seen in Figure 2. The FRS computation took 79 hours and used a maximum of 150 GB of memory

V-B Set Intersection and Trajectory Planning

We used the precomputed FRS for safe trajectory planning in $1000$ simulated trials in MATLAB on the aforementioned machine. For each trial, the vehicle began at the same initial location and heading, surrounded by $1-10$ randomized obstacles and a randomly-located goal to reach. The vehicle’s initial speed, and the desired speed to maintain for the duration of the trial, were randomly chosen between $0.25$ and $0.75$ [m/s].

Obstacles were represented as line segments between $0.1$ and $0.2$[m] in length, with random location and orientation. The obstacles were always placed between the vehicle and the goal. We checked for crashes conservatively for each trial, by inspecting if any obstacle was within a circle circumscribing the rectangular vehicle at any point of the vehicle’s trajectory. Using this method, no crashes were detected in any trial. Out of all the trials, $82\%$ reached the goal, and $15\%$ performed an emergency braking maneuver (by setting $v_{\text{des}}=0$). The remaining 3% hit a simulation iteration limit. Examples of the vehicle’s path from a randomly-generated trial and from two constructed emergency braking cases are shown in Figure 4.

Currently, our implementation cannot consistently achieve $\tau_{\text{plan}}=0.5$ [s]. Consequently, instead of replanning and driving simultaneously, we pause time every 0.5 [s] of the simulation to guarantee that the vehicle can finish replanning. In a physical implementation, if $\tau_{\text{plan}}$ is exceeded, then the vehicle must emergency brake; recall that a safe braking trajectory is always available. As shown in Figure 5, $\tau_{\text{plan}}$ scales linearly with the number of obstacles.

VI CONCLUSION

This paper presents a method to plan safe trajectories with obstacle avoidance for autonomous vehicles. This approach is able to guarantee safety in arbitrary environments for multiple, static obstacles. The method begins with computing the forward reachable set (FRS) of parameterized trajectories that a vehicle can realize. This set is computed in continuous space and time, and is robust to model uncertainty between the dynamics of the vehicle’s mid- and low-level controllers.

As an example, we use a kinematic Dubin’s car and dynamic unicycle model as low- and high-fidelity models. At runtime, the FRS is intersected with obstacles to eliminate unsafe trajectories, and an optimal trajectory is chosen from the remaining, safe trajectories. This method is proven to ensure vehicle safety for present and future static obstacles by considering the time required for path planning, the time required to stop the vehicle, and the error between the low- and high-fidelity models. One thousand simulations with randomly-located obstacles were run to show the effectiveness of this method.

The next step in applying this method is to expand the error function $g$ beyond modeling uncertainty. To reduce the conservativeness of the approach, we plan to explore extension to the FRS computation that incorporate confidence level sets [19, 9]. The error function $g$ can also be improved by considering time variation of trajectory parameters across planning steps by posing the FRS computation as a hybrid problem [22]. Finally, we plan to use convex optimization to find a global solution to the nonlinear trajectory optimization problem at each planning step [26, 27].