Delay-aware Robust Control for Safe Autonomous Driving

A polytope is defined as a convex hull of finite points in $d$-dimensional space $\mathbb{R}^{d}$. The Minkowski sum of two polytopes is defined as $P\oplus Q:=\{x+q\in\mathbb{R}^{d}:x\in P,q\in Q\}$. The Pontryagin difference of two polytopes is defined as $P\ominus Q:=\{x\in\mathbb{R}^{d}:x+q\in P,q\in Q\}$.

A kinematic bicycle model describes the dynamics of the vehicle with the state variables being position ($p_{x}$, $p_{y}$), heading angle ($\theta$), and velocity ($v$), and the control variables acceleration ($a$) and steering angle ($\delta$). It is commonly assumed in the literature that steering angle $\delta$ and acceleration $a$ are applied instantaneously by the actuators. But in reality, there is a certain lag between the command and when the actuator physically modifies the steering angle state, which is called the actuator dynamic delay. We modify the system dynamics to include the actual steering angle as a state denoted as $\delta_{a}$. The control command is now the desired steering angle $\delta$. We approximate the change in the steering angle state by a first-order ODE similar to [3], i.e., $\dot{\delta_{a}}=K_{\delta}(\delta-\delta_{a})$, where $K_{\delta}$ is the inverse of the time constant for the steering actuator. For acceleration, pedal dynamics are assumed to be instantaneous for the experiments in this paper. However, they can also be approximated in the same way. After discretization, the vehicle state is now modified as $x_{k}=[p_{x,k},p_{y,k},\theta_{k},v_{k},\delta_{a,k}]$. The discrete dynamics are given in Eq. 1.

For a discrete linear system with system matrices $A\in\mathbb{R}^{n\times n}$ and $B\in\mathbb{R}^{n\times m}$, let the control gain $K\in\mathbb{R}^{m\times n}$ be such that the feedback system of $A_{K}=A+BK$ is stable. Let $\mathcal{Z}$ be the disturbance-invariant set for the controlled uncertain system $x^{+}=A_{K}x+w$, satisfying $A_{K}\mathcal{Z}\oplus\mathcal{W}\subseteq\mathcal{Z}$, where the disturbance $w$ is assumed to be bounded ($w\in\mathcal{W}$) by a polyhedron that contains the origin in its interior. The following finite optimization problem is solved at each step for $\bar{X}=\{\bar{x}_{0},\bar{x}_{1}...,\bar{x}_{N}\}$, $\bar{U}=\{\bar{u}_{0},\bar{u}_{1}...,\bar{u}_{N-1}\}$ and reference state sequence $X_{ref}=\{x_{ref,0},x_{ref,1}...,x_{ref,N}\}$ obtained from the path planner, where $N$ is the horizon length, and $\bar{X}$ and $\bar{U}$ are the state and control sequences of a nominal system ignoring $w$:

where $Q$, $R$ and $Q_{N}$ are the state, control and terminal state cost matrices, respectively. The control command given is $u=\bar{u}+K(x-\bar{x})$, where $x$ is the current state. This guarantees $x^{+}\in\bar{x}^{+}+\mathcal{Z}$ for any $w\in\mathcal{W}$, i.e., all states $x_{k}$ will be inside the constraint set $\mathcal{X}$. However, for a nonlinear system as in our case, we use the equivalent LTV system, where the system matrices $A$ and $B$ are replaced with Jacobian matrices $A_{k}$ and $B_{k}$ at the current state for the system dynamics used in Eq. 1. For more details as well as a detailed proof of the feasibility and the stability of the above controller, see [7]. Also, a mismatch between the linearized and the actual model can be compensated by adding an additional disturbance $\hat{w}$ assuming the model non-linear function to be a Lipschitz function [8]. For the experiments in this work, we assume the disturbance margin is large enough to cover this extra disturbance.

For the above formulation, we assumed delay time to be zero, meaning that the computed command is delivered to the system at the same time the observation is made for the current state $x$. But in practice, there is computation time denoted as $t_{c}$ and an actuator command processing delay $t_{a}$ after the calculated command is delivered, resulting in a total delay time $t_{d}=t_{c}+t_{a}$. Hence, if the current state $x$ is observed at time $t$, the computed command influences the actuator state at $t+t_{d}$ time. This may lead to instability of the system if $t_{d}$ is large and the robust tube assumptions no longer hold true. In order to tackle this problem, [2] proposes a bi-level control scheme to deal with time delay and also proves robustness using the tube MPC. A buffer block of commands is used for communication between higher- and lower-level units, as depicted in Fig. 1. At time $t$, the set of possible states at time $t+t_{h}$, where $t_{h}$ is the horizon length, is predicted. The high-level tube MPC updates the buffer with nominal states and control commands from time $t+t_{h}$ to $t+2t_{h}$. If the higher-level MPC requires a delay time $t_{c}$ that is less than $t_{h}$, the system waits for the remaining time $t_{h}-t_{c}$ for time alignment. However, we believe this is only suited for systems when the objective is nearly constant. For dynamically changing objectives as well as state constraints, it is necessary to update the reference path more frequently, since waiting for the full horizon path to be followed may lead to inconsistencies. In the case of autonomous driving, for dynamic scenarios where the reference path has to be updated frequently, it would not be feasible to wait for $t_{h}$ to get a new updated path.

Hence, we propose to get a local probabilistic upper-bound estimate $\hat{t}_{c}$ of the computation time. We update the buffer from ($t+\hat{t}_{c}$ to $t+\hat{t}_{c}+t_{h}$) instead of from ($t+t_{h}$ to $t+2t_{h}$), as shown in Fig. 1. This increases the controller plan update rate for the higher-level MPC and also makes the controller robust to changing computation times. For estimation of the local upper bound $\hat{t}_{c}$, we use an adaptive Kalman filter, as further described in Section III-E. Considering $t_{a}$ to be the extra delay due to actuator command processing, we thus get a new local upper-bound delay time estimate $\hat{t}_{d}=\hat{t}_{c}+t_{a}$, which we use to find the initial state estimate $x_{\hat{t}_{d}|t}$ assuming no disturbance after $\hat{t}_{d}$ time given the current state $x_{t}$. It can be calculated by piece-wise integration of the system dynamics using the control commands from the buffer. Hence, a command issued at $t+\hat{t}_{c}$ will be executed at $t+\hat{t}_{c}+t_{a}=t+\hat{t}_{d}$, where we consider our initial state to be for optimization, as depicted in Fig. 1. Mathematically, the updated objective function is described in Eq. 3, where $x_{\hat{t}_{d}+t}$ is the actual state after time $\hat{t}_{d}$. The calculated nominal discrete states ($\bar{X}$) and controls ($\bar{U}$) are used to fill the buffer B from time $t+\hat{t}_{c}$ to $t+\hat{t}_{c}+t_{h}$ as $\bar{u}_{[t+\hat{t}_{c}+k\Delta t,t+\hat{t}_{c}+(k+1)\Delta t]}=\bar{u}_{k}$ for $k\in\{0,1,2...N-1\}$, as shown in Fig. 1. The pre-compensator unit is a low-level process which executes commands in the buffer at a higher frequency than the high-level MPC.

We propose INFLUENCE for estimating a local upper bound on computation time. The conventional Kalman filter faces a crucial challenge when the dynamic model and noise covariance are unknown. On the subject of adaptive filters addressing this challenge, existing approaches either assume an unknown dynamic model but known covariance [11] or unknown covariance but a known dynamic model [12, 13]. INFLUENCE assumes the process model $\gamma$, process noise variance $q$ and measurement noise variance $r$ are all unknown. Since measuring time is direct but noisy, it is reasonable to assume the measurement matrix to be the identity matrix. However, if the assumption does not hold in other applications, the identification can be facilitated using the similar approach for process model identification in the INFLUENCE algorithm. We assume both noise distributions to be Gaussian, independent and mutually uncorrelated throughout. To make the optimization tractable, INFLUENCE iteratively fixes $\gamma$ first to identify $q$ and $r$ (see Eq. 6c to Eq. 6k), which are then fixed to update $\gamma$ (see Eq. 6l to Eq. 6m). For the identification of $q$ and $r$, an exponential moving average is maintained to estimate prediction error $e$ and measurement error $w$, respectively. Parameters $N_{q}$ and $N_{r}$ determine the influence of older values on the moving average: the greater their values, the greater the weighting of older values. Thus, an incremental update for $q$ and $r$ can be done in Eq. 6d and Eq. 6j. These update rules have been adapted from [12, 13], while the update rules for state transition parameters $\theta$ have been adapted from [11]. $F$ is the learning gain, and $\lambda=\frac{N_{\theta}-1}{N_{\theta}}$ denotes the forgetting factor for estimation of $\theta$, where $N_{\theta}$ is the buffer size for the process model’s identification. The initialization of INFLUENCE is in Eq. 6a.

where $t_{c,n}$ is the observed computation time at step $n$, and $\gamma_{n-1,0}$ is the first element of the vector at step $n-1$.

For the local upper bound estimate, we use the predicted value and variance to get an upper-bound estimate on computation time, i.e., $\hat{t}_{c,n}=x_{n|n-1}+\beta p_{n|n-1}$. We choose the parameter $\beta$ accordingly to get sufficiently high confidence as an upper bound assuming a Gaussian distribution. It is important to note here that the addition of an adaptive Kalman filter for estimation of computation time doesn’t significantly affect the computation time of the overall algorithm, as augmenting by a fixed number of computations in the cycle leads to an insignificant increase in computation time as opposed to window-based estimation methods whose computational complexity depends on the window size.

We present a standalone controller (called plan A) as depicted in Fig. 2. We compensate for the actuator’s steering dynamics by modelling a first-order ODE. For the compensation of the computation and actuator command processing delays, we use initial state shift by the estimated local upper bound on the net delay time. The optimization problem updates the robust tube buffer from $t+\hat{t}_{c}$ to $t+\hat{t}_{c}+t_{h}$ with the nominal commands and states. The pre-compensator unit runs as a low-level process to refine the control with a higher frequency (see Fig. 2).

As an alternate plan, we compensate for the actuator and computation delay of a nominal controller (see Fig. 3). It can be a black-box controller, which can be used for a LE controller trained in a simplified simulation environment without any practical delay. For the computation time and actuator processing delay compensation, we use the same design as plan A by shifting the initial state. For actuator dynamic delay compensation, we use a separate unit which takes the commands from the nominal controller as a reference to track. The computation time and actuator processing delays are estimated via Plan A to shift the initial state of the LE controller and conduct rollouts to obtain sequential commands $\hat{U}=\{\hat{u}_{1},\hat{u}_{2}...,\hat{u}_{N}\}$. The refined commands after compensating for actuator dynamic delay are $U=\{u_{1},u_{2}..,u_{N}\}$. $U$ is obtained by solving a quadratic optimization problem (Eq. 7) where $u_{start}$ is the current value of the steering angle, $r_{k}$ is the unit step response of the steering actuator at the $k^{th}$ time step, and $Q_{ac}$ and $R_{ac}$ are positive semidefinite weight matrices. The optimization is to track the desired actuator commands from the LE controller as closely as possible while minimizing the control effort. Though collision avoidance is not considered in the experiments for Plan B, safety constraints such as control barrier functions [14] can be easily incorporated into the optimization.

where $r_{i}=(1-e^{-Ki\Delta t})$.

For testing controller plan A, we perform a static obstacle avoidance experiment. The planner used is hybrid A^∗ [15]. We compare the paths followed by the controller with and without considering delays. In the case in which no compensation is considered, due to the delays, the vehicle overshoots during the first turn and when it tries to get back to the reference safely, the vehicle collides with the static obstacle as marked by pose B in Fig. 5. In the cases in which delay compensations are considered, the resulting path followed is closer to the reference line and smoother, while safely avoiding collision. We also compare the results between when 1) the delay time is taken as an upper bound equal to the horizon length [2] and 2) the local upper bound estimate is found using INFLUENCE. The path followed using our proposed method is clearly seen to be smoother in Fig. 5. This is because in the case of constant delay compensation, at pose A, the state constraints generated from IRIS force the vehicle to deviate from the reference path, which thus overshoots by a significant amount, but is still able to get back to the reference safely. On the other hand, if we approximate the delay time using INFLUENCE and adjust the expected local upper bound value accordingly (see Fig. 6), the controller responds faster. Hence, after passing pose A, the state constraints change and the controller reacts faster to get back to the reference path, giving less overshoot. It is important to note here that the minute computation time difference between the with and without delay compensation cases is due to the shifted state calculation and the extended Kalman filter, but it seems that having an efficient delay-aware algorithm outweighs any effect the increased computation time has on making the controller less responsive.

We further test the controller plan A in an overtaking scenario, in which the lead vehicle brakes suddenly at point A when $t=4s$. We use the Frenet planner [16] with the reference path as the lane center. Frenet frame-based planning has been successful in practice due to the significant advantage of its independence from complex road geometry. We perform the experiment with the same starting conditions and compare the results with (Fig. 7(a)) and without (Fig. 7(b)) delay compensation. The Frenet planner expects the ego vehicle to move at constant speed, but as the speed rapidly drops at point A, the reference path changes rapidly. The Frenet planner thus rapidly changes path after point A. Point B is the closest position between the ego vehicle and lead vehicle in all the cases. If delay time is not considered, the ego vehicle hits the other vehicle slightly at point B. Also, in this case if computation time is taken as a constant upper bound of $0.2s$, due to the slow reaction of the controller, the ego vehicle hits the lead vehicle at point B (Fig. 7(c)), which proves that taking the computation time as an upper bound is ineffective in a rapidly changing environment.

We test controller plan B for a LE controller trained in an ideal environment without delays. The LE lateral controller is a neural network trained on waypoint following with inputs $[\Delta x,\Delta y,\Delta\theta]$, where $(\Delta x,\Delta y)$ and $\Delta\theta$ are respectively the relative position and heading with a target waypoint. The output is the steering angle $\delta$. The network architecture is a simple feed-forward neural network with hidden layer sizes $(4,16,4)$. For longitudinal control, simple PID control is used to track a constant speed of $75\ m/s$ throughout. The simulation has been performed in the Ansys VRX simulator [17]. When deploying the LE controller in such a high-speed waypoint-following scenario in Gazebo, it performs worse due to the practical delays. The LE controller loses control at the time of turning, see Fig. 8. By using the proposed plan B, the vehicle retains control. The vehicle is operating at its friction limits, hence even a little bit of error caused due to delay leads to the vehicle losing control, even when the computation time is just around 0.02s.