Safe Trajectory Synthesis for Autonomous Driving in Unforeseen Environments  Shreyas Kousik 1 , Sean Vaskov 1 , Matthew Johnson-Roberson 2 , Ram Vasudevan 1  Abstract — Path planning for autonomous vehicles in arbi- trary 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.  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 e ffi ciency, 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.  1. University of Michigan - Ann Arbor, Department of Mechanical Engineering. Contact   { skousik, skvaskov, ramv } @umich.edu . 2. University of Michigan - Ann Arbor, Department of Naval Architecture and Marine Engineering. Contact   mattjr@umich.edu . 1  2  1  2  Fig.   1:   An   illustration   of   the   Forward   Reachable   Set   in   the physical world (right) and the trajectory parameter space (left). Each trajectory parameter on the left corresponds to a trajectory in the physical world. The FRS (the funnel beginning from the blue car) is intersected with obstacles (the road boundaries and the red car) at run-time to identify trajectory parameters that lead to collisions (orange and red sets in left picture). In this sketch, the parameters labeled 1 produce a guaranteed-safe trajectory, whereas the parameters labeled 2 result in a collision. Optimal parameters can be selected from the safe set,   K safe .  Various methods exist in the literature for mid-level con- troller 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 Ob- servable 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 Pre- dictive 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  arXiv:1705.00091v1 [cs.SY] 28 Apr 2017
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 em- ployed 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 o ff -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 uncer- tainty [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 dynam- ics, 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 o ff - 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 de- scribe the problem of interest. This paper makes substantial use of semidefinite programming theory and sums of squares programming [12].  A. Notation  The following notation is adopted throughout the remain- der of the paper: Sets are italicized and capitalized. The boundary of a set   K   is   ∂ K . Finite truncations of the set of natural numbers are   N n   : =   { 1 , . . . ,   n } . The set of continuous functions on a compact set   K   are   C ( K ). The Lebesgue measure on   K   is denoted by   λ K   . We let   y K   denote the Lebesgue moments of   λ K   . The   i -th component of a vector  v   ∈   R n   is denoted by   v i . The ring of polynomials in   x   is  R [ x ], and the degree of a polynomial is the degree of its largest multinomial; the degree of the multinomial   x α , α   ∈   N n  is   | α |   =   ‖ α ‖ 1 ; and   R d [ x ] is the set of polynomials in   x   with degree   d . Let vec( p ) denote the vector of coe ffi cients of  p   ∈   R [ x ].  B. Low and High-Fidelity Vehicle Models  Let an autonomous vehicle by described by:  ̇ x ( t )   =   f   ( t ,   x ( t ) ,   u ( t ))   (1) where   f   : [0 ,   T   ]   ×   X   ×   U   →   R n ,   T   >   0,   X   ⊂   R n   and   U   ⊂   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   ⊂   R n s   with   X s   ⊂   X . This model is described as:  [    ̇ x s ( t )  ̇ k ( t )  ]  =  [   f s ( t ,   x s ( t ) ,   k ( t )) 0  ]  (2) where the parameters,   k ( t ), do not evolve after initialization and are drawn from a set   K   ⊂   R p .  Example 1:   Consider a dynamic unicycle model:                       ̇ x ( t )  ̇ y ( t )  ̇ θ ( t )  ̈ θ ( t )  ̇ v ( t )                      =                      v ( t ) cos ( θ ( t ))  v ( t ) sin ( θ ( t ))  ̇ θ ( t )  u 1 ( t )  u 2 ( t )                      (3) where   x   and   y   represent position;   θ   and  ̇ θ   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 [ x   y   θ ] T   such that:              ̇ x ( t )  ̇ y ( t )  ̇ θ ( t )              =             k 2   cos ( θ ( t ))  k 2   sin ( θ ( t ))  k 1              (4)
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   ⊂   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:  K   =   { k   ∈   R p   |   h K i   ( k )   ≥   0 ,   ∀ i   ∈ { 1 , . . . ,   n K   } }   ,   (5)  X 0   =   { x s   ∈   R n s   |   h 0 i   ( x s )   ≥   0 ,   ∀ i   ∈ { 1 , . . . ,   n 0 } }   ,   (6)  X s   =   { x s   ∈   R n s   |   h X i   ( x s )   ≥   0 ,   ∀ i   ∈ { 1 , . . . ,   n X s   } }   .   (7) 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. Unfortu- nately this tracking cannot be done perfectly. The higher fidelity model can select a controller,   u k   : [0 ,   T   ]   ×   X   →   R m , to follow the generated trajectory which is parameterized by  k   ∈   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   ] ×  X s   →   X s   such that: max  x ∈ A xs  ∣ ∣ ∣   f i ( t ,   x ,   u k ( t ,   x )   −   f s , i ( t ,   x s ,   k ) ∣ ∣ ∣   ≤   g i ( t ,   x s ) ,   (8) for all   x s   ∈   X s ,   t   ∈   [0 ,   T   ],   k   ∈   K , and   i   ∈ { 1 , . . . ,   n s } , where  A x s   : =   { x   ∈   X   |   x i   =   x s , i ∀ i   ∈ { 1 , . . . ,   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:  ̈ θ ( t )   =   u 1 ( t )   =   20   ·   ( ̇ θ ( t )   −    ̇ θ des )   (9)  ̇ v ( t )   =   u 2 ( t )   =   10   ·   ( v ( t )   −   v des )   (10) where ( ̇ θ des ,   v 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  ̇ θ   dimension. Such a  g , illustrated in Figure 2, could be:  g ( t ,   x ,   y , θ )   =             v err ( t )   ·   (1   −   1 2   θ 2 )  v err ( t )   ·   ( θ   −   1 6   θ 3 )  ̇ θ err              (11) where   v err ( t )   =   ( t   −   1) 2   and  ̇ θ err ( t )   =   ( t   −   1) 4 . -1.5   -1   -0.5   0   0.5   1   1.5  x  -0.2  0  0.2  0.4  y  0   0.5   1   1.5   2  t  0  0.5  1  v  0   0.5   1   1.5   2  t  -0.5  0  0.5  d   /dt  Fig. 2: An example of the worst-case error dynamics. The top subplot shows the ( x ,   y ) trajectories of a Dubins car (grey) and unicycle model (blue), beginning at ( − 0 . 75 ,   0) and ending at the corresponding stars. Both trajectories begin at full speed (1 [m / s]) and then emergency brake at   t   =   1. The resulting velocity profile, with the same color scheme, is shown in the middle subplot. In addition, the Dubins car begins its trajectory with  ̇ θ   = + 0 . 5 [rad / s], whereas the unicycle begins at 0 [rad / s] and must catch up. At   t   =   1, the Dubins car switches instantaneously to turning at   − 0 . 5 [rad / s], as shown in the bottom subplot. The red dashed line in the middle and bottom subplots is the error function   g   that overapproximates the worst-case possible error between the Dubins car and unicycle with a polynomial.  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:  ̇ x s ( t )   =   f s ( t ,   x s ( t ) ,   k )   +   g ( t ,   x s ( t ) ,   k )   d ( t )   (12) where   d ( t )   ∈   [ − 1 ,   1] for each   t   ∈   [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   L   f s   :  C 1 ( [0 ,   T   ]   ×   X s   ×   K )   →   C ( [0 ,   T   ]   ×   X s   ×   K )   on a test function  v   as:  L   f s   v ( t ,   x s ,   k )   =   ∂ v  ∂ t   ( t ,   x s ,   k )   +  n s ∑  i = 1  ∂ v  ∂ x i , s  ( t ,   x s ,   k )   f i , s ( t ,   x s ,   k ) .   (13) Next, define the linear operator   L g   :   C 1 ( [0 ,   T   ]   ×   X s   ×   K )   →  C ( [0 ,   T   ]   ×   X s   ×   K )   as:  L g v ( t ,   x s ,   k )   =  n s ∑  i = 1  ∂ v  ∂ x i , s  ( t ,   x s ,   k ) g i ( t ,   x s ) .   (14) Note that these linear operators are useful in summarizing the evolution of a system as we describe in Section III-A.2.
C. Sensing, Planning, and Braking Time Horizons  Next, we define a sensing, planning, and braking time hori- zon 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:  X obs   =   { x s   ∈   R n s   |   h obs i   ( x s )   ≥   0 ,   ∀ i   ∈ { 1 , . . . ,   n obs } }   .   (15) 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,   ∂ X s . In addition, let the system be limited to a scalar top speed, denoted  ̇ x s , max . Then, this assumption requires that obstacles are sensed at a minimum sensor horizon   D sense   : =    ̇ x s , max   ·  T sense .  T 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 informa- tion 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  τ 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   τ 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   =   τ plan . The initial condition for this next trajectory is computed by the vehicle by forward integrating   f   over the time interval [0 , τ 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   ⊆   K   such that, without deviating from the spatial component of a pre-planned trajectory,    ̇ x s   =   0 in a finite amount of time   τ b ( x s )   ∈   R +   for each   x s   ∈   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   τ stop   ∈   R +   such that   τ b ( x s )   ≤   τ stop   for all   x s   ∈   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 sens- ing time horizons to ensure the safety of the trajectory design procedure. The approach is broken into the following steps: 1) Precompute a   Forward Reachable Set   (FRS) that cap- tures all possible trajectories and associated parame- ters, subject to error function   g , over a time interval [0 ,   T   ] (Section III-A). 2) During operation, perform   set intersection   of the FRS with obstacles in the vehicle’s local environment to identify trajectory parameters,   K safe   ⊂   K , which could cause collisions using a convex optimization technique (Section III-B). 3) During operation, perform   trajectory optimization   over  K 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.  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).  X FRS   =   { ( x s ,   k )   ∈   X s   ×   K   | ∃   x 0   ∈   X 0 , τ   ∈   [0 ,   T   ] ,   and  d   : [0 ,   T   ]   →   [ − 1 ,   1] s.t.   x ( τ )   =   x s ,   where  ̇ x ( t )   =   f s ( t ,   x ( t ) ,   k )   +   g ( t ,   x ( t ) ,   k )   d ( t ) a.e.   t   ∈   [0 ,   T   ] and   x (0)   =   x 0  }  (16) Notice that in this definition the error function   g   along with  d ( t )   ∈   [ − 1 ,   1] accounts for the di ff erence between a trajectory, parameterized by   k   ∈   K , of the lower dimensional model and the higher-dimensional model which follows this trajectory using a control   u k .  1) 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   X FRS , we first describe how to select the time horizon   T   to ensure safety while planning:  Theorem 10:   Let   T sense ,   τ plan , and   τ stop   be as in Assump- tions 6,7, and 9, respectively. Suppose the vehicle plans a trajectory every   τ plan   and there exists a   T   ≥   0 such that:  τ plan   +   τ stop   ≤   T   (17)  T   +   τ plan   ≤   T sense ,   (18) 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 assump- tions, 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 su ffi cient 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   ∈  ( T ,   T sense ].   These   obstacles   are   sensed   according   to   As- sumption 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   τ plan , so the plan beginning from time   t   =   τ 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   τ plan   because the FRS uses Assumption 3. Therefore, at time   t   =   τ plan , no obstacle is reachable within a time horizon less than   T   −  τ plan , which is greater than or equal to   τ stop . Consequently, while traversing the first time interval [0 , τ plan ], the vehicle determines whether or not it must begin stopping, or if a non-braking trajectory exists, at time   t   =   τ plan . As a result, at  t   =   τ plan , the obstacle falls into Case 1.  Case 3:   Any obstacle that can be reached at a time   t   >   T 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 sense   −   τ plan   at time   t   =   τ plan . The obstacle thus falls into Case 2 for the trajectory starting at   t   =   τ plan .  2) 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 )]: inf  v , w , q  ∫  X s × K  w ( x s ,   k )   d λ X s × K   ( D ) s.t.   L   f s   v ( t ,   x s ,   k )   +   q ( t ,   x s ,   k )   ≤   0 ,   on [0 ,   T   ]   ×   X s   ×   K  L g v ( t ,   x s ,   k )   +   q ( t ,   x s ,   k )   ≥   0 ,   on [0 ,   T   ]   ×   X s   ×   K  − L g v ( t ,   x s ,   k )   +   q ( t ,   x s ,   k )   ≥   0 ,   on [0 ,   T   ]   ×   X s   ×   K q ( t ,   x s ,   k )   ≥   0 ,   on [0 ,   T   ]   ×   X s   ×   K  −   v (0 ,   x s ,   k )   ≥   0 ,   on   X 0   ×   K w ( x s ,   k )   ≥   0 ,   on   X s   ×   K w ( x s ,   k )   +   v ( t ,   x s ,   k )   −   1   ≥   0 ,   on [0 ,   T   ]   ×   X s   ×   K  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 )   ∈   C 1   ([0 ,   T   ]   ×   X s )   × C ( X s )   ×  C ([0 ,   T   ]   ×   X s ). Notice first that the 1-superlevel set of feasible   w ’s in ( D ) are outer approximations of   X FRS :  Lemma 11:   Let ( v ,   w ,   q ) be feasible functions to ( D ), then  X FRS   ⊂   { ( x s ,   k )   ∈   X s   ×   K   |   w ( x s ,   k )   ≥   1 } .  Proof:   Suppose   ( x s ,   k )   ∈ X FRS ,   then   ∃   τ   ∈   [0 ,   T   ],  d   : [0 ,   T   ]   7   →   [ − 1 ,   1] and   x   : [0 ,   T   ]   7   →   X s   such that    ̇ x ( t )   =  f s ( t ,   x ( t ) ,   k )   +   g ( t ,   x ( t ) ,   k ) d ( t ) for a.e.   t   ∈   [0 ,   T   ] with   x ( τ )   =   x s  and   x (0)   ∈   X 0 . Observe that:  v ( τ,   x ( τ ) ,   k )   =   v (0 ,   x (0) ,   k )   +  τ ∫  0  ( L   f s   v ( t ,   x ( t ) ,   k )   +   L g v ( t ,   x ( t ) ,   k ) d ( t ) )   dt  ≥   v (0 ,   x (0) ,   k )   +  ∫   τ  0  ( L   f s   v ( t ,   x ( t ) ,   k )   +   q ( t ,   x ( t ) ,   k ) )   dt  ≥   v (0 ,   x (0) ,   k ) ,  where the first equality follows from the Fundamental Theo- rem of Calculus, the second inequality follows from noticing that sup d ( t ) ∈ [ − 1 , 1] |L g v ( t ,   x ( t ) ,   k ) d ( t ) |   =   |L g v ( t ,   x ( t ) ,   k ) |   and that  |L g v ( t ,   x ( t ) ,   k ) | ≥   q ( t ,   x ( t ) ,   k ) due to the constraints in ( D ), and the final inequality follows from the constraints in ( D ). No- tice the desired result follows by noticing that   v (0 ,   x (0) ,   k )   ≥   0 for   x (0)   ∈   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   X 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   X FRS   in the   L 1   norm and almost uniformly. In Section IV-A we describe how to generate feasible solu- tions to ( D ) using semidefinite programming over polynomial representations of continuous functions. Once this computa- tion is completed once o ffl ine, the result can be used online by performing real-time set intersection as described next.  B. Set Intersection  Any outer-approximation to   X 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 safe   ⊂   K   by solving the following optimization problem: sup  h  ∫  K  h ( k ) d λ K   (19) s.t.   1   −   w ( x s ,   k )   −   h ( k )   ≥   0 ,   on   X obs   ×   K h ( k )   ≤   1 ,   on   K  where   w   is the given data and the supremum is taken over  h   ∈   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 obs   such that   w ( x s ,   k )   ≥   1. In that instance the first constraint in the previous optimization
-1   0   1  x  -1  -0.5  0  0.5  1  y  0   0.5   1  speed  -0.5  0  0.5  steering rate Fig. 3: An example of set intersection, with   X obs   chosen as three points in   X s . On the right is the ( x ,   y ) subspace of   X s   with each point obstacle shown, and the vehicle plotted in blue. On the left is the trajectory parameter space   K , with three dashed-line contours containing an outer approximation of the trajectory parameters that would cause a collision with each point shown on the right (the colors match between the points and contours). The set intersection step returns the subset of   K   shown by the green contour, which outer approximates all trajectory parameters that could result in collisions with any of the three points in   X obs . Therefore,   K safe   is inner-approximated.  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   ∈   K   |   h ( k )   ≥   0 } ⊂   K safe . By exploiting the polynomial outer approximation of the  X FRS   which is generated in Section IV-A one can formulate a convex optimization method to compute a closed, inner ap- proximation to   K 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 safe   ,   ∅ .  C. Trajectory Optimization  Once   K safe   is determined for an obstacle, one can optimize the original dynamics directly over the set. Specifically, if a user specifies a continuously di ff erentiable cost function,   J   :  K   →   R   then one can optimize this cost function with a con- straint that enforces that   k   ∈   K 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 di ff erent ways via collocation, solving a variational equation, sampling, or using convex relaxations [26]. If this optimization program is unable to conclude within   τ 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  R [ t ,   x s ,   k ], and the error function   g   is in   R [ t ,   x s ]. In addition, the sets   K ,   X 0 ,   X s , and   X obs   are semialgebraic, defined by polynomials in their respective spaces as in Equations (5) - (7) and (15).  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 2 l . 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 τ   =   t ( T   −   t ) and define   Q 2 l ( h τ ,   h X 1   , . . . ,   h X nXs   ,   h K 1   , . . . ,   h K nK   )   ⊂   R 2 l [ t ,   x s ,   k ] to be the set of polynomials   q   ∈   R 2 l [ t ,   x s ,   k ] (i.e. of total degree less than 2 l ) expressible as:  q   =   s 0   +   s 1 h τ   +  n Xs ∑  i = 1  s i + 2 h X i   +  n K ∑  i = 1  s i + n XS   + 2 h K i   ,   (20) for   some   polynomials   { s i } n Xs   + n K   + 1  i = 0   ⊂   R 2 k [ 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   ]   ×   X s   ×   K   [12, Theorem   2.14].   Define   Q 2 l ( h 0 1   , . . . ,   h 0 n 0   ,   h K 1   , . . . ,   h K nK   )   ⊂  R 2 l [ x s ,   k ],   Q 2 l ( h X 1   , . . . ,   h X nXs   ,   h K 1   , . . . ,   h K nK   )   ⊂   R 2 l [ x s ,   k ],  Q 2 l ( h obs 1   , . . . ,   h obs n obs   ,   h K 1   , . . . ,   h K nK   )   ⊂   R 2 l [ x s ,   k ],   and  Q 2 l ( h K 1   , . . . ,   h K nK   )   ⊂   R 2 l [ k ] similarly. Employing   this   notation,   the   l -th   relaxed   semidefinite programming representation of ( D ), denoted ( D l ), is defined as: inf  v , w , q   y T X s × K   vec( w )   ( D l ) s.t.   − L   f s   v   −   q   ∈   Q 2 l ( h τ ,   h X 1   , . . . ,   h X nXs   ,   h K 1   , . . . ,   h K nK   )  L g v   +   q   ∈   Q 2 l ( h τ ,   h X 1   , . . . ,   h X nXs   ,   h K 1   , . . . ,   h K nK   )  − L g v   +   q   ∈   Q 2 l ( h τ ,   h X 1   , . . . ,   h X nXs   ,   h K 1   , . . . ,   h K nK   )  q   ∈   Q 2 l ( h τ ,   h X 1   , . . . ,   h X nXs   ,   h K 1   , . . . ,   h K nK   )  −   v (0 ,   · )   ∈   Q 2 l ( h 0 1   , . . . ,   h 0 n 0   ,   h K 1   , . . . ,   h K nK   )  w   ∈   Q 2 l ( h X 1   , . . . ,   h X nXs   ,   h K 1   , . . . ,   h K nK   )  w   +   v   −   1   ∈   Q 2 l ( h τ ,   h X 1   , . . . ,   h X nXs   ,   h K 1   , . . . ,   h K nK   ) where the infimum is taken over the vector of polynomials ( v ,   w ,   q )   ∈   R l [ t ,   x s ,   k ]   ×   R l [ x s ,   k ]   ×   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 func- tion on   X 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   X FRS .  B. Set Intersection  In this section, we present a semidefinite program to generate a closed inner-approximation to   K safe   and comment on computing   τ plan . Using the notation developed in Section
IV-A, consider the following semidefinite formulation of Equation (19): sup  h  y T K   vec( h )   (21) s.t.   1   −   w   −   h   ∈   Q 2 l ( h obs 1   , . . . ,   h obs n obs   ,   h K 1   , . . . ,   h K nK   ) 1   −   h   ∈   Q 2 l ( h K 1   , . . . ,   h K nK   ) where the given data is any feasible   w   ∈   R l [ x s ,   k ] to ( D l ) and the supremum is taken over polynomials   h   ∈   R l [ k ]. Note again that since the constraints in this optimization problem are su ffi cient to ensure positivity on the required sets in Equation (19), one can apply the result of Lemma 13 to prove that the 0-superlevel set of   h l   is an inner approximation to  K safe . Solving such an SDP, while possible in real time, is still computationally expensive. Compared to the trajectory opti- mization step, which takes milliseconds, the set intersection step can take several tenths of a second per obstacle, and is thus the primary contributor to   τ plan . The planning time increases with the number of dimensions of   X obs   and with the number of obstacles. We find   τ plan   by precomputing an FRS over an arbitrary, short time horizon   T   , then empiri- cally determining   τ plan   by performing set intersection with representative obstacles and environments.  C. Trajectory Optimization  We implement trajectory optimization by directly passing  h l   from solving (21) as a nonlinear constraint to an o ff -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.  A. FRS Computation  We computed the FRS for a 3 rd   order Taylor-expanded Dubins car as the low-fidelity model   f s . Trajectories pro- duced by this model were tracked by the unicycle model from Equation (1) as the high-fidelity model   f   . The vehi- cle’s representation as an initial distribution   X 0   ⊂   X s , was a rectangle of length 0 . 2 [m] in   x   and width 0 . 1 [m] in   y , at 0 ◦  initial heading, and centered at   x   =   − 0 . 75 and   y   =   0. This is the same vehicle representation shown in all previous figures. We chose   τ stop   =   τ 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  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 -1   -0.5   0   0.5   1   1.5   2   2.5  x [m]  -0.5  0  0.5  y [m]  -1   -0.5   0   0.5  x [m]  -0.5  0  0.5  y [m]  -1   -0.5   0   0.5  x [m]  -0.5  0  0.5  Fig. 4: The top subplot shows an example result out of the 1000 tri- als. This trial used eight randomly-generated obstacles. The vehicle begins on the left at   x   =   − 0 . 75 and reaches a randomly-generated goal near (2 . 5 ,   0 . 5), plotted as a blue circle. Every   τ plan   =   0 . 5[s], the vehicle replans its trajectory, shown by an asterisk plotted on the global trajectory in blue. The bounding box of the vehicle at each planning step is shown as a grey rectangle. In the bottom- left subplot, an obstacle was constructed between the vehicle and the goal, forcing an emergency braking maneuver. In the bottom- right subplot, an obstacle was constructed with a hole that would allow the vehicle to pass, but the set intersection result is overly conservative, resulting in a braking maneuver.  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 circum- scribing 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 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  τ 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   τ plan   is exceeded, then the vehicle must emergency brake; recall that a safe braking trajectory is always available. As shown in Figure 5,   τ plan  scales linearly with the number of obstacles.
0   2   4   6   8   10 0 1 2 3 set int mean [s] 0   2   4   6   8   10 Number of Obstacles 0.04 0.06 0.08 0.1 traj opt mean [s] Fig. 5: The mean set intersection time (top) and trajectory opti- mization time (bottom) versus the number of obstacles. Over the 1000 trials, each number of obstacles from 1 to 10 was used for 100 trials. Notice that set intersection takes up to 3[s], and scales with the number of obstacles. On the other hand, the trajectory optimization takes around 80 [ms] and has low correlation with 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 e ff ective- ness 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 ex- tension 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]. R eferences  [1]   Matthias Altho ff   and John M Dolan. Online verification of automated road vehicles using reachability analysis.   IEEE Transactions on Robotics , 30(4):903–918, 2014. [2]   Sebastian Brechtel, Tobias Gindele, and R ̈ udiger Dillmann. Probabilis- tic decision-making under uncertainty for autonomous driving using continuous pomdps.   In   Intelligent Transportation Systems (ITSC), 2014 IEEE 17th International Conference on , pages 392–399. IEEE, 2014. [3]   Martin Buehler, Karl Iagnemma, and Sanjiv Singh.   The DARPA urban challenge: autonomous vehicles in city tra ffi c , volume 56.   springer, 2009. [4]   Aparna Dhinakaran, Mo Chen, Glen Chou, Jennifer C Shih, and Claire J Tomlin.   A hybrid framework for multi-vehicle collision avoidance.   arXiv preprint arXiv:1703.07375 , 2017. [5]   Jerry   Ding,   Eugene   Li,   Haomiao   Huang,   and   Claire   J   Tomlin. Reachability-based synthesis of feedback policies for motion planning under bounded disturbances.   In   Robotics and Automation (ICRA), 2011 IEEE International Conference on , pages 2160–2165. IEEE, 2011. [6]   Paolo Falcone, Francesco Borrelli, Jahan Asgari, Hongtei Eric Tseng, and Davor Hrovat. Predictive active steering control for autonomous vehicle systems.   IEEE Transactions on control systems technology , 15(3):566–580, 2007. [7]   Yiqi Gao, Andrew Gray, H Eric Tseng, and Francesco Borrelli. A tube- based robust nonlinear predictive control approach to semiautonomous ground vehicles.   Vehicle System Dynamics , 52(6):802–823, 2014. [8]   Andrew Gray, Yiqi Gao, Theresa Lin, J Karl Hedrick, H Eric Tseng, and Francesco Borrelli. Predictive control for agile semi-autonomous ground vehicles using motion primitives. In   American Control Con- ference (ACC), 2012 , pages 4239–4244. IEEE, 2012. [9]   Patrick Holmes, Shreyas Kousik, Shankar Mohan, and Ram Vasude- van. Convex estimation of the   α -confidence reachable set for systems with parametric uncertainty.   In   Decision and Control (CDC), 2016 IEEE 55th Conference on , pages 4097–4103. IEEE, 2016. [10]   Matthew   Johnson-Roberson,   Charles   Barto,   Rounak   Mehta, Sharath   Nittur   Sridhar,   and   Ram   Vasudevan.   Driving   in   the matrix: Can virtual worlds replace human-generated annotations for real world tasks?   arXiv preprint arXiv:1610.01983 , 2016. [11]   Sertac Karaman and Emilio Frazzoli.   Sampling-based algorithms for optimal motion planning.   The international journal of robotics research , 30(7):846–894, 2011. [12]   Jean Bernard Lasserre.   Moments, positive polynomials and their applications , volume 1. World Scientific, 2009. [13]   Steven M LaValle and James J Ku ff ner Jr. Randomized kinodynamic planning.   The international journal of robotics research , 20(5):378– 400, 2001. [14]   Alexander   Liniger   and   John   Lygeros.   Real-time   control   for autonomous   racing   based   on   viability   theory.   arXiv   preprint arXiv:1701.08735 , 2017. [15]   Wei Liu, Dragomir Anguelov, Dumitru Erhan, Christian Szegedy, Scott Reed, Cheng-Yang Fu, and Alexander C Berg.   Ssd: Single shot multibox detector. In   European Conference on Computer Vision , pages 21–37. Springer, 2016. [16]   Johan Lofberg.   Yalmip: A toolbox for modeling and optimization in matlab.   In   Computer Aided Control Systems Design, 2004 IEEE International Symposium on , pages 284–289. IEEE, 2004. [17]   Anirudha Majumdar and Russ Tedrake. Funnel libraries for real-time robust feedback motion planning.   arXiv preprint arXiv:1601.04037 , 2016. [18]   Anirudha Majumdar, Ram Vasudevan, Mark M Tobenkin, and Russ Tedrake.   Convex optimization of nonlinear feedback controllers via occupation measures.   The International Journal of Robotics Research , 33(9):1209–1230, 2014. [19]   Shankar Mohan and Ram Vasudevan.   Convex computation of the reachable set for hybrid systems with parametric uncertainty.   In  American Control Conference (ACC), 2016 , pages 5141–5147. IEEE, 2016. [20]   Petter Nilsson, Omar Hussien, Ayca Balkan, Yuxiao Chen, Aaron D Ames,   Jessy   W   Grizzle,   Necmiye   Ozay,   Huei   Peng,   and   Paulo Tabuada.   Correct-by-construction adaptive cruise control: Two ap- proaches.   IEEE   Transactions   on   Control   Systems   Technology , 24(4):1294–1307, 2016.
[21]   Pablo A Parrilo.   Structured semidefinite programs and semialgebraic geometry methods in robustness and optimization . PhD thesis, Cali- fornia Institute of Technology, 2000. [22]   Victor Shia, Ram Vasudevan, Ruzena Bajcsy, and Russ Tedrake. Convex computation of the reachable set for controlled polynomial hybrid systems.   In   Decision and Control (CDC), 2014 IEEE 53rd Annual Conference on , pages 1499–1506. IEEE, 2014. [23]   Victor A Shia, Yiqi Gao, Ramanarayan Vasudevan, Katherine Driggs Campbell, Theresa Lin, Francesco Borrelli, and Ruzena Bajcsy. Semi- autonomous vehicular control using driver modeling.   IEEE Transac- tions on Intelligent Transportation Systems , 15(6):2696–2709, 2014. [24]   Li Wang, Aaron D Ames, and Magnus Egerstedt.   Safety barrier certificates for collisions-free multirobot systems.   IEEE Transactions on Robotics , 2017. [25]   Xiangru Xu, Jessy W Grizzle, Paulo Tabuada, and Aaron D Ames. Correctness   guarantees   for   the   composition   of   lane   keeping   and adaptive cruise control.   arXiv preprint arXiv:1609.06807 , 2016. [26]   Pengcheng Zhao, Shankar Mohan, and Ram Vasudevan.   Control synthesis for nonlinear optimal control via convex relaxations.   arXiv preprint arXiv:1610.00394 , 2016. [27]   Pengcheng Zhao, Shankar Mohan, and Ram Vasudevan.   Optimal control for nonlinear hybrid systems via convex relaxations.   arXiv preprint arXiv:1702.04310 , 2017.