On Infusing Reachability-Based Safety Assurance within Probabilistic Planning Frameworks for Human-Robot Vehicle Interactions

TL;DR

This paper presents a real-time reachability-based safety controller for autonomous vehicles that ensures collision avoidance in interactive scenarios with human-driven cars, maintaining safety without significantly compromising planned trajectories.

Contribution

It introduces a minimally-interventional safety controller using reachability analysis within an autonomous vehicle control stack for collision-free human-robot interactions.

Findings

Successfully avoids collisions in traffic weaving experiments.

Maintains safety even when human driver actions are unpredictable or dangerous.

Operates at 100Hz in real-time to ensure safety constraints are always met.

Abstract

Action anticipation, intent prediction, and proactive behavior are all desirable characteristics for autonomous driving policies in interactive scenarios. Paramount, however, is ensuring safety on the road --- a key challenge in doing so is accounting for uncertainty in human driver actions without unduly impacting planner performance. This paper introduces a minimally-interventional safety controller operating within an autonomous vehicle control stack with the role of ensuring collision-free interaction with an externally controlled (e.g., human-driven) counterpart. We leverage reachability analysis to construct a real-time (100Hz) controller that serves the dual role of (1) tracking an input trajectory from a higher-level planning algorithm using model predictive control, and (2) assuring safety through maintaining the availability of a collision-free escape maneuver as a persistent constraint regardless of whatever future actions the other car takes. A full-scale steer-by-wire platform is used to conduct traffic weaving experiments wherein the two cars, initially side-by-side, must swap lanes in a limited amount of time and distance, emulating cars merging onto/off of a highway. We demonstrate that, with our control stack, the autonomous vehicle is able to avoid collision even when the other car defies the planner's expectations and takes dangerous actions, either carelessly or with the intent to collide, and otherwise deviates minimally from the planned trajectory to the extent required to maintain safety.

Equations

$$\mathcal{A}(t):=\{{\bar{x}}_{\mathcal{R}}\in {\mathbb{R}}^n:\exists u_H(\cdot ),\forall u_R(\cdot ),\exists s\in [t,0],x_{\mathcal{R}}(t)={\bar{x}}_{\mathcal{R}}\wedge {\dot{x}}_{\mathcal{R}}=f(x_{\mathcal{R}},u_R,u_H)\wedge x_{\mathcal{R}}(s)\in \mathcal{T}\}$$

$$u_R^{\ast }=\arg \underset{u_R}{\max }\underset{u_H}{\min }\nabla V(x_{\mathcal{R}})\cdot f(x_{\mathcal{R}},u_R,u_H)$$

$${\mathcal{U}}_R(x_{\mathcal{R}})=\{u_R:\underset{u_H}{\min }\nabla V(x_{\mathcal{R}})\cdot f(x_{\mathcal{R}},u_R,u_H)\ge 0\}.$$

$$\begin{array}{c}\left[\begin{array}{c}{x}_{rel}\\ y_{rel}\end{array}\right]=R_{-{\psi }_R}\left[\begin{array}{c}{x}_H-x_R\\ y_H-y_R\end{array}\right]\\ \\ {\psi }_{rel}={\psi }_H-{\psi }_R\end{array}{x}_{\mathcal{R}}=\left[\begin{array}{c}{\dot{x}}_{rel}\\ {\dot{y}}_{rel}\\ {\dot{\psi }}_{rel}\\ {\dot{U}}_{x_R}\\ {\dot{U}}_{y_R}\\ {\dot{v}}_H\\ {\dot{r}}_R\end{array}\right]=\left[\begin{array}{c}{v}_H\cos {\psi }_{rel}-U_{x_R}+y_{rel}{r}_R\\ v_H\sin {\psi }_{rel}-U_{y_R}-x_{rel}{r}_R\\ \omega -r_R\\ \frac{1}{m}(F_x+F_{x_{drag}})-r_R{U}_{x_R}\\ \frac{1}{m}(F_{y_f}+F_{y_r})-r_R{U}_{x_R}\\ a\\ \frac{1}{I_{zz}}(\bar{a}{F}_{y_f}-\bar{b}{F}_{y_r})\end{array}\right]$$

$$\underset{q,u,\sigma ,{\sigma }_{HJI},\Delta \delta ,\Delta F_x}{\mathrm{minimize}}$$

$$\Delta F_{x,k}^T{R}_{\Delta F_x}\Delta F_{x,k}+W_{\beta }{\sigma }_{\beta ,k}+W_r{\sigma }_{r,k}+W_{HJI}{\sigma }_{HJI,k}$$

$${\delta }_{k+1}-{\delta }_k=\Delta {\delta }_k,\Delta {\delta }_{min}\le \Delta {\delta }_k\le \Delta {\delta }_{max},{\delta }_{min}\le {\delta }_k\le {\delta }_{max}$$

$$F_{x,k+1}-F_{x,k}=\Delta F_{x,k},V_{min}\le U_{x,k}\le V_{min},F_{x,min}\le F_{x,k}\le F_{x,max}$$

$${\sigma }_{1,k}\ge 0,{\sigma }_{2,k}\ge 0,{\sigma }_{HJI,j}\ge 0$$

$$H_k\left[\begin{array}{c}{U}_{y,k}\\ r_k\end{array}\right]-G_k\le \left[\begin{array}{c}{\sigma }_{\beta ,k}\\ {\sigma }_{r,k}\end{array}\right],A_k{q}_k+B_k^{-}{u}_k+B_k^{+}{u}_{k+1}+c_k=q_{k+1}$$

$$q_1=q_{curr},u_1=u_{curr},M_{HJI}{u}_j+b_{HJI}\ge -{\sigma }_{HJI}$$

$$\text{for }j=1,...,T_{HJI},k=1,...,T$$

Full text

1 Introduction

Decision-making and control for mobile robots is typically stratified into levels. A high-level planner, informed by representative dynamics of a robot and its environment, might be responsible for selecting a coarse trajectory plan, which is implemented through a low-level controller that respects more accurate models of the robot’s dynamics and control constraints. While additional components may be required to flesh out a robot’s full control stack from model to motor commands, selecting the right “division of responsibilities” is fundamental to system design.

One consideration that defies clear classification, however, is how to ensure a mobile robot’s safety when operating in close proximity with a rapidly evolving and stochastic environment. Safety is a function of uncertainty in both the robot’s dynamics and those of its surroundings; high-level planners typically do not replan sufficiently rapidly to ensure split-second reactivity to threats, yet low-level controllers are typically too short-sighted to ensure safety beyond their local horizon. Hand-designed artificial potential fields have been employed at the planning level WolfBurdick2008 to encourage the selection of safer nominal trajectories, while reactive collision avoidance techniques, e.g., based on online optimal control FunkeBrownEtAl2017 or precomputed emergency maneuver libraries AroraChoudhuryEtAl2015 , have been applied at the controller level to avoid previously unseen static obstacles, but these approaches do not account for interactive scenarios in which another sentient agent is a key environmental consideration.

In this work we implement a control stack for a full-scale autonomous car (the “robot”) engaging in close proximity interactions with a human-controlled vehicle (the “human”). Freedom of motion is essential for the planner to carry out the driving task while conveying future intent to the other vehicle. Our primary tool for designing a controller that does not needlessly impinge upon the planner’s choices is Hamilton-Jacobi (HJ) reachability. Backward HJ reachability, in particular, has been studied extensively and applied successfully in a variety of safety-critical interactive settings BokanowskiForcadelEtAl2010 ; ChenHuEtAl2017 ; DhinakaranChenEtAl2017 ; GattamiAlAlamEtAl2011 ; MargellosLygeros2011 ; MitchellBayenEtAl2005 due to its flexibility with respect to system dynamics, and its optimal (i.e., non-overly-conservative) avoidance maneuvers stemming from its equivalence to an exhaustive search over joint system dynamics. Previously reachability-based controllers have largely been used in a switched fashion: the robot is normally allowed to apply any control, but switches to the optimal avoidance controller when near safety violation. While this is sometimes effective FisacAkametaluEtAl2017 , to the best of our knowledge there has not been any work that explicitly addresses the integration of reachability-based safety controllers as a component within a robot’s control stack, i.e., with safety as a constraint upon a primary planning objective.

Statement of Contributions: The contributions of this paper are twofold. First, we propose a method for formally incorporating reachability-based safety within an existing optimization-based control framework. The main insight that enables our approach is the recognition that, near safety violation, the set of safety-preserving controls often contains more than just the optimal avoidance control. Instead of directly applying this optimal avoidance control when prompted by reachability considerations, as in a switching control approach, we quantify the set of safety-preserving controls and pass it to the broader control framework as a constraint. Our intent is to enable truly minimal intervention against the direction of a higher-level planner when evasive action is required. Second, we evaluate the benefits and performance of this safe control methodology in the context of a probabilistic planning framework for the traffic weaving scenario studied at a high level in SchmerlingLeungEtAl2018 , wherein two cars, initially side-by-side, must swap lanes in a limited amount of time and distance. Experiments with a full-scale steer-by-wire vehicle reveal that our combined control stack achieves better safety than applying a tracking controller alone to the planner output, and smoother operation (with similar safety) compared with a switching control scheme; in our discussion we provide a roadmap towards improving the level of safety assurance in the face of practical considerations, i.e., unmodeled dynamics, as well as towards generalizations of the basic traffic weaving scenario.

2 Technical Approach

To bring safe traffic weaving to the road, we combine a model-based interaction planner SchmerlingLeungEtAl2018 with a real-time trajectory tracking model predictive control (MPC) controller inspired by BrownFunkeEtAl2017 , modified to include an additional invariant set constraint detailed in this section. An outline of our control framework is shown in Figure 1.

*Interaction Planner: *The traffic-weaving interaction planner SchmerlingLeungEtAl2018 uses a predictive model of future human behavior to select a desired trajectory for the robot car to follow, updated at $\sim$3Hz. We extend this work by using a hindsight optimization policy YoonFernEtAl2008 instead of the limited-lookahead action policy used in SchmerlingLeungEtAl2018 in order to encourage more information-seeking actions from the robot. Though this interaction planner optimizes an objective that weighs safety considerations (e.g., distance between cars) relative to other concerns (e.g., control effort), it reasons anticipatively with respect to a probabilistic interaction dynamics model. That is, safety is not enforced as a deterministic constraint at the planning level.

*MPC Tracking Controller: *The MPC tracking controller, operating at 100Hz, computes optimal controls to track a desired trajectory by solving a quadratic program (QP) at each iteration. This optimization problem is based on a single track vehicle model (also known as the bicycle model) and incorporates friction and stability control constraints while minimizing a combination of tracking error and steering effort. We incorporate additional constraints computed from HJ reachability analysis into this QP to ensure that at each control step the robot car does not enter a state of possible inevitable collision.

2.1 Backward Reachability Analysis

We use infinite time horizon backward reachability analysis to compute additional safety constraints for the MPC tracking controller. Critically, backward reachability is computed backwards in time allowing for closed-loop robot reactions to avoid future collisions (see Figure 2a), while forward reachability is computed forwards in time assuming open-loop control for the robot (see Figure 2b) which leads to an overly-conservative robot outlook. Though more expensive to compute, backward reachability solutions may be precomputed offline and cached for online use. We briefly review relevant HJ reachability definitions in the remainder of this section; see ChenTomlin2018 for a more in-depth treatment.

Let the relative dynamics of the robot and human be given by $\dot{x}_{\mathcal{R}}=f(x_{\mathcal{R}},u_{R},u_{H})$, and the collision set be $\mathcal{T}$. The BRS $\mathcal{A}(t)$ represents the set of “avoid states” from which if the human followed an adversarial strategy $u_{H}(\cdot)$, any robot action $u_{R}(\cdot)$ would lead to the relative state trajectory $x_{\mathcal{R}}(\cdot)$ being inside $\mathcal{T}$ within a time horizon $|t|$ (note that $t<0$ when propagating backwards in time):

[TABLE]

Assuming optimal human actions, $\mathcal{A}(t)$ can be computed by solving the Hamilton-Jacobi-Isaacs (HJI) partial differential equation MitchellBayenEtAl2005 ; FisacChenEtAl2015 , whose solution $V(t,x_{\mathcal{R}})$ gives the BRS as its zero sublevel set: $\mathcal{A}(t)=\{x_{\mathcal{R}}:V(t,x_{\mathcal{R}})\leq 0\}$.111The HJI PDE is solved starting from the boundary condition $V(0,x_{\mathcal{R}})$, the sign of which reflects set membership of $x_{\mathcal{R}}$ in $\mathcal{T}$; see Section 5 for discussion of specific choices of $V(0,x_{\mathcal{R}})$. When the control capabilities of the human are no greater than those of the robot, one can take the limit $t\rightarrow-\infty$ and obtain the infinite time horizon BRS $\mathcal{A}_{\infty}$ with corresponding value function $V_{\infty}(x_{\mathcal{R}})$.222For ease of notation going forward we will often write $V:=V_{\infty}$. Illustrative slices of the value function and the BRS for the vehicle-vehicle relative dynamics considered in this work are shown in Figure 3.

The optimal robot avoidance control offers the greatest increase in $V$ assuming optimal (worst-case) actions by the human:

[TABLE]

Previous applications of HJI solutions have switched to this control when near the boundary of the BRS FisacAkametaluEtAl2017 , i.e., when safety is nearly violated. In an interactive scenario where, for example, we may want to let a robot planner convey intent by nudging towards the human car to the extent that is safe, we prefer a less extreme control strategy. Rather than switching to the optimal avoidance controller (2), we add the set of safety-preserving controls defined below as an additional constraint in the MPC optimal control problem.

[TABLE]

$\mathcal{U}_{R}(x_{\mathcal{R}})$ represents the set of robot controls that ensure the value function is nondecreasing. We compute the BRS offline using the BEACLS toolkit TanabeChen2018 , and cache the value function $V$ and its gradient $\nabla V$. Online we employ a safety buffer $\epsilon>0$ so that when the condition $V(x_{\mathcal{R}})\leq\epsilon$ holds, indicating that the robot is nearing safety violation, we add a half-space constraint $\mathcal{U}_{R}(x_{\mathcal{R}})=\{u_{R}:M_{HJI}\cdot u_{R}+b_{HJI}\geq 0\}$ to the list of MPC constraints in the tracking control problem. This constraint is a linearized approximation of (3); specifically $M_{HJI}=\nabla V\frac{\partial f}{\partial u_{R}}(x_{\mathcal{R}},u_{H}^{*},u_{R})$ and $b_{HJI}=\nabla V\cdot f(x_{\mathcal{R}},u_{H}^{*},u_{R})-M_{HJI}\cdot u_{R}$ evaluated at the current state and robot control ($u_{H}^{*}$ denotes the optimal, i.e., worst-case, human action defined analogously to (2)).

3 Methodology

In this section we describe how we tailor the technical approach described above to the particular application of ensuring safety for human-robot pairwise vehicle interactions. We detail the relative dynamics model for HJI computation and outline the MPC tracking controller; all the details of the traffic weaving interaction planner can be found in SchmerlingLeungEtAl2018 . All code for the planner, MPC tracking controller, and BRS computation is available at https://github.com/StanfordASL/safe_traffic_weaving.

3.1 Relative Dynamics

We use a six-state single track model to describe the robot car’s dynamics (see BrownFunkeEtAl2017 for details) and assume the human car obeys a dynamically extended unicycle model. This represents a compromise between model fidelity and the number of state dimensions in the relative dynamics; reducing the latter is essential since solving the HJI PDE suffers greatly from the curse of dimensionality.333The computation becomes notoriously expensive past five or more states without compromising on grid discretization or employing some decoupling strategy. We use a grid size of $13\times 13\times 9\times 9\times 9\times 9\times 9$ for our 7D system uniformly spaced over $(x_{rel},y_{rel},\psi_{rel},U_{x_{R}},U_{y_{R}},v_{H},r_{R})\in[-15,15]\times[-5,5]\times[-\pi/2,\pi/2]\times[1,12]\times[-2,2]\times[1,12]\times[-1,1]$; computing the BRS with this discretization takes approximately 16 hours on a 3.6GHz octocore AMD Ryzen 1800X CPU.

We define the relative dynamics (4) in a coordinate system centered on and aligned with the robot, where $R_{\theta}$ is the counterclockwise rotation matrix over $\theta$. $x_{rel}$, $y_{rel}$ and $\psi_{rel}$ are the relative position and heading angle between the human and robot, while the other states are needed to completely describe the human and robot car dynamics. The robot control inputs $u_{R}=[\delta,\,F_{x_{f}},F_{x_{r}}]$ are steering angle and front and rear longitudinal tire force (we write $F_{x}=F_{x_{f}}+F_{x_{r}}$ to denote total longitudinal force), while the human control inputs $u_{H}=[\omega,\,a]$ are yaw rate and longitudinal acceleration.

[TABLE]

We assume that the robot and human car share the same power, steering, and friction limits. These limits are chosen to reflect the same physical constraints assumed for the MPC tracking controller’s dynamic bicycle model BrownFunkeEtAl2017 . Due to its simpler dynamics representation, the human car has a transient advantage in control authority over the robot car (it may change its path curvature discontinuously, while the robot may not), but by equating the steady state control limits we ensure that the infinite time horizon BRS computation converges.

3.2 The MPC+HJI Tracking Controller

Both the trajectory tracking objective and safety-preserving control constraint rely on optimizing over the robot steering and longitudinal force inputs simultaneously. The associated tracking optimization problem is non-convex; we apply an approximate variant of sequential quadratic programming (SQP) in which we time-discretize and linearize the dynamics over a lookahead horizon and solve one corresponding QP at each MPC step. We interpolate along each solution trajectory to compute the linearization nodes for the QP at the next MPC step.444Only a single iteration of SQP is solved for the tracking problem at each MPC step, rather than the usual iteration until convergence. Since the tracking problems are so similar from one MPC step to the next, we find that this approach yields sufficient performance for our purposes.

We use the ForwardDiff.jl automatic differentiation (AD) package implemented in the Julia programming language RevelsLubinEtAl to linearize the trajectory tracking dynamics as well as the HJI relative dynamics to facilitate the computation of $M_{HJI}$ and $b_{HJI}$. We call the Operator Splitting Quadratic Program (OSQP) solver StellatoBanjacEtAl2017 through the Parametron.jl modeling framework Koolencontributors ; this combination of software enables us to solve the following MPC optimization problem at 100Hz.

Let $q_{k}=[\Delta s_{k},U_{x,k},U_{y,k},r_{k},\Delta\psi_{k},e_{k}]^{T}$ be the state of the robot car with respect to a nominal trajectory at discrete time step $k$. $\Delta s_{k}$, $e_{k}$ and $\Delta\psi_{k}$ denote longitudinal, lateral, and heading angle error; $U_{x,k}$, $U_{y,k}$, and $r_{k}$ are body-frame longitudinal and lateral velocity, and yaw rate respectively. Let $u_{k}=[\delta_{k},F_{x,k}]$ be the controls at step $k$ and let $A_{k}q_{k}+B_{k}^{-}u_{k}+B_{k}^{+}u_{k+1}+c_{k}=q_{k+1}$ denote linearized first-order-hold dynamics. We adopt the varying time steps method and stable handling envelope constraint ($H_{k}$, $G_{k}$) from BrownFunkeEtAl2017 ; $\sigma_{\beta,k}$, $\sigma_{r,k}$, and $\sigma_{HJI}$ are slack variables to ensure the existence of a feasible solution. The constraint $M_{HJI}u_{j}+b_{HJI}\geq-\sigma_{HJI}$ is added only when $V(x_{\mathcal{R}})\leq\epsilon$. Although HJI theory suggests that applying this constraint on the next action alone is sufficient, we apply it over the next 3 timesteps (30ms lookahead) to account for the many approximations inherent in our QP formulation.

[TABLE]

4 Experiments

4.1 Experimental Vehicle Platform

X1 is a flexible steer-by-wire, drive-by-wire, and brake-by-wire experimental vehicle developed by the Stanford Dynamic Design Lab (see Figure 4). To control X1, desired steering ($\delta$) and longitudinal tire force ($F_{x_{f}},F_{x_{r}}$) commands are sent to the dSpace MicroAutoBox (MAB) which handles all sensor inputs except LiDAR (handled by the onboard PC) and implements all low level actuator controllers. We use the Robot Operating System (ROS) to communicate with the MAB; the planning/control stack described in this work is running onboard X1 on a consumer desktop PC running Ubuntu 16.04 equipped with a quadcore Intel Core i7-6700K CPU and an NVIDIA GeForce GTX 1080 GPU.

4.2 Results

To evaluate our proposed control stack — a synthesis of a high-level probabilistic interaction planner with the MPC+HJI tracking controller — we perform full-scale human-in-the-loop traffic-weaving trials with X1 taking on the role of the robot car.555We scaled the highway traffic-weaving scenario (mean speed $\sim$28m/s) in SchmerlingLeungEtAl2018 down to a mean speed of $\sim$8m/s by shortening the track (reducing longitudinal velocity by a constant) and scaling time by a factor of 4/3 (with the effect of scaling speeds by 3/4 and accelerations by 9/16). We investigate and evaluate the effectiveness of our control stack by allowing the human car to act carelessly (i.e., swerving blindly towards the robot car) during the experiments. We compare our proposed controller (MPC+HJI) against a tracking-only MPC controller (MPC) and a controller that switches to the HJI optimal avoidance controller when near safety violation (switching). To ramp up towards testing with two full-scale vehicles in the near future, we investigate two types of human car: (1) a virtual human-driven car and (2) a 1/10-scale LiDAR-visible human-driven RC car.

4.3 Virtual Human-Driven Vehicle

To ensure a completely safe experimental environment, our first tier of experimentation uses a joystick-controlled virtual vehicle for the human car and allows the robot control stack to have perfect observation of the human car state. Experimental trials of the probabilistic planning framework for the MPC+HJI and switching controllers are shown in Figure 6a and 6b respectively, along with a simulated comparison between the two safety controllers and the tracking-only controller.666For comparative purposes, the controllers were simulated with the displayed nominal trajectory held fixed, but in reality, the nominal trajectory in these experiments was updated at $\sim 4Hz$. As expected, we see that the MPC+HJI controller represents a middle ground between the tracking-only MPC which does not react to the human car’s intrusion, and the switching controller which arguably overreacts with a large excursion outside the lane boundaries. Evident from Figure 6a, our proposed controller tries to be minimally interventional — the robot car swerves/brakes but only to an extent that is necessary.

4.4 1/10-Scale Human-Driven Vehicle

To begin to investigate the effects of perception uncertainty on our safety assurance framework we use three LiDARs onboard X1 to track a human-driven RC car, and implement a Kalman filter for human car state estimation (position, velocity, and acceleration). Even with imperfect observations, we show some successful preliminary results (an example is shown in Figure 7) at mean speeds of  4m/s, close to the limits of the RC car + LiDAR-visible mast in crosswinds at the test track. We observe similar behavior as in the virtual human car experiments, including the fact that the value function dips briefly below zero before the MPC+HJI controller is able to arrest its fall; we discuss this behavior in the next section.

5 Discussion

Beyond the qualitative confirmation of our design goals, our experimental results reveal two main insights.

Takeaway 1: The reachability cache is underly-conservative with respect to robot car dynamics and overly-conservative with respect to human car dynamics.

In all cases — hardware experiments as well as simulation results — the HJI value function $V$ dips below zero, indicating that neither the HJI+MPC nor even the optimal avoidance switching controller are capable of guaranteeing safety in the strictest sense. The root of this apparent paradox is in the computation of the reachability cache used by both controllers as the basis of their safety assurance. Though the 7-state relative dynamics model (4) subsumes a single-track vehicle model that has proven successful in predicting the evolution of highly dynamic vehicle maneuvers FunkeBrownEtAl2017 ; BrownFunkeEtAl2017 , the way it is employed in computing the value function $V$ omits relevant components of the dynamics. In particular, when computing the optimal avoidance control (2) as part of solving the HJI partial differential equation, we assume total freedom over the choice of robot steering angle $\delta$ and longitudinal force command $F_{x}$, up to maximum control limits. This does not account for, e.g., limits on the steering slew rate (traversing $[-\delta_{max},\delta_{max}]$ takes approximately 1 second), and thus the value function is computed under the assumption that the robot can brake/swerve far faster than it actually can.

We note that simply tuning the safety buffer $\epsilon$ is insufficient to account for these unmodeled dynamics. In Figure 6 we see that $V$ may drop from approximately 0.5 (the value when the two cars traveling at 8m/s start side-by-side in lanes) to -0.3 in the span of a few tenths of a second. Selecting $\epsilon>0.5$ might give enough time for the steering to catch up, but such a selection would prevent the robot car from accomplishing the traffic weaving task even under nominal conditions, i.e., when the human car is equally concerned about collision avoidance. This is because the safety controller would push the robot car outside of its lane from the outset to maintain the buffer. This behavior follows from wide level sets associated with the transient control authority asymmetry (recall that in the HJI relative dynamics the human car may adjust its trajectory curvature discontinuously), assumed as a conservative safety measure as well as a way to keep the relative state dimension manageable.

The simplest remedy for both of these issues is to increase the fidelity of the relative dynamics model by incorporating additional integrator states $\dot{\delta}$ for the robot and $\dot{\omega}$ for the human. Naively increasing the state dimension to 8 or 9, however, might not be computationally feasible (even offline) without devising more efficient HJI solution techniques or choosing an extremely coarse discretization over the additional states.777By literature standards we already use a relatively coarse discretization grid for solving the HJI PDE; associated numerical inaccuracies may be another source of the observed safety mismatch. We believe that simulation, accounting for slew rates, could be a good tool to prototype such efforts, noting that as it stands we have relatively good agreement between simulation and experimental platform in our testing.

Takeaway 2: Interpretability of the value function $V$ should be a key consideration in future work.

In this work the terminal value function $V(0,x_{\mathcal{R}})$ is specified as the separation/pene-tration distance between the bounding boxes of the two vehicles, a purely geometric quantity dependent only on $x_{rel}$, $y_{rel}$, and $\psi_{rel}$. Recalling that V ($:=V_{\infty}$) represents the worst-case eventual outcome of a differential game assuming optimal actions from both robot and human, we may interpret the above results through the lens of worst-case outcomes, i.e., a value of -0.3 may be thought of as 30cm of collision penetration assuming optimal collision seeking/avoidance from human/robot. When extending this work to cases with environmental obstacles (e.g., concrete highway boundaries that preclude large deviations from lane), or multi-agent settings where the robot must account for the uncertainty in multiple other parties’ actions, for many common scenarios it may be the case that guaranteeing absolute safety is impossible. Instead of avoiding a BRS of states that might lead to collision, we should instead treat the value function inside the BRS as a cost. In particular we should specify more contextually relevant values of $V(0,x_{\mathcal{R}})$ for states in collision, e.g., negative kinetic energy or another notion of collision severity as a function of the velocity states $U_{x_{R}}$, $U_{y_{R}}$, $v_{H}$, and $r_{R}$ in addition to the relative pose. This would lead to a controller that prefers, in the worst case, collisions at lower speed, or perhaps “glancing blows” where the velocities of the two cars are similar in magnitude and direction. Computationally we could use such a value function in an MPC formulation similar to that presented in this work, with static obstacles either incorporated in $V(0,x_{\mathcal{R}})$ or represented as additional MPC constraints as done in BrownFunkeEtAl2017 , and accommodating multiple other agents by considering them pairwise with the robot and taking the minimum over the corresponding value functions.

6 Conclusions

We have investigated a control scheme for providing real-time safety assurance to underpin the guidance of a probabilistic planner for human-robot vehicle-vehicle interactions. By essentially projecting the planner’s desired trajectory into the set of safety-preserving controls whenever safety is threatened, we preserve more of the planner’s intent than would be achieved by adopting the optimal control with respect to separation distance. Our experiments show that with our proposed minimally interventional safety controller, we accomplish the high level objective (traffic weaving) despite the human car swerving directly onto the path of the robot car, and accomplish this relatively smoothly compared to using a switching controller that results in the robot car swerving more violently off the road. We note that this work represents only a promising first step towards the integration of reachability-based safety guarantees into a probabilistic planning framework. We have already discussed the concrete modifications to this controller we believe are necessary to improve the impact of these guarantees; further study should also consider better fitting of the planning objective at the controller level. That is, instead of performing a naive projection, i.e., the one that minimizes trajectory tracking error, it is likely that a more nuanced selection informed by the planner’s prediction model would represent a better “backup choice” in the case that safety is threatened. We recognize that ultimately, guaranteeing absolute safety on a crowded roadway may not be realistic, but we believe that in such situations value functions derived from reachability may provide a useful metric for near-instantly evaluating the future implications of a present action choice.