F. Duerr, J. Ziehn, R. Kohlhaas, M. Roschani, M. Ruf and J. Beyerer, "Realtime Global optimization of a Fail-Safe Emergency Stop Maneuver for Arbitrary Electrical / Electronical Failures in Automated Driving," 2020 IEEE 23rd International Conference on Intelligent Transportation Systems (ITSC), Rhodes, Greece, 2020

In the event of a critical system failures in automated vehicles, fail-operational or fail-safe measures provide minimum guarantees for the vehicle's performance, depending on which of its subsystems remain operational. Various such methods have been proposed which, upon failure, use different remaining sets of operational subsystems to execute maneuvers that bring the vehicle into a safe state under different environmental conditions. One particular such method proposes a fail-safe emergency stop system that requires no particular electric or electronic subsystem to be available after failure, and still provides a basic situation-dependent emergency stop maneuver. This is achieved by preemptively setting parameters to a hydraulic / mechanical system prior to failure, which after failure executes the preset maneuver “blindly”. The focus of this paper is the particular challenge of implementing a lightweight planning algorithm that can cope with the complex uncertainties of the given task while still providing a globally-optimal solution at regular intervals, based on the perceived and predicted environment of the automated vehicle.

§ INTRODUCTION

The optimization of emergency maneuvers has been the subject of comprehensive research, with a wide range of solutions addressing different notions of what constitutes an “emergency” in this context: From a fully operational vehicle encountering challenging environmental conditions (e.g. [1] for model-predictive pedestrian avoidance; [2] for reactions to unexpected traffic situations; [3] for collision avoidance for cooperative vehicles), to various stages of degraded capabilities of subsystems under various internal and external conditions (e.g. [4, 5]), including incapacitation of the responsible human driver for systems up to SAE level 3 (e.g. [6, 7]).

Degraded capabilities of the ego vehicle include single- or multiple-point faults, which can be addressed by redundant systems to provide a fail-operational behavior (cf. [8]), possibly including graceful degradation (cf. [9]) by reducing active vehicle functions depending on the occurring failure modes, or at least fail-safe behavior, which provides minimal functions to assure safety in case of a failure. In each case, the chosen fallback behavior depends on the assumed set of remaining operational systems; a single failed sensor is more easily compensated than a fusion or planning unit; approaches to address various kinds of failure modes are given in [10, 4].

Motivating example: The ego vehicle E is equipped with a hydraulic piston accumulator, whose pressure is released onto the brakes in case of a severe system failure. To provide a situation-dependent maneuver, the pressure is controlled by a valve, which is adjusted at periodical intervals $\Delta t\subs{plan}$ prior to failure, to prepare for a possible emergency. Since failure can occur any time within the upcoming $\Delta t\subs{plan}$ (or never at all), not one single braking trajectory can be planned, but instead a continuous range X of trajectories (and stopping distances) can occur, displaced over failure time. In the given scenario, the safest decision would be to decelerate gently enough to avoid a rear-end collision with car R, yet strongly enough to not enter the road ahead Y. The goal of the proposed algorithm is to minimize the risks $W(t,s)$ over time $t$ and arc length $s$ within the region X with very limited computational effort.

An approach to establish a lower bound of possible safety is proposed in [11], where a situation-adaptive emergency stopping maneuver is provided without requiring the use of any electric or electronic system after the moment of failure; the system can therefore be used as a fallback for any failure mode where no superior dedicated solution can efficiently be implemented. To achieve this behavior, motivated in Fig. <ref>, it uses a hydraulic / mechanical subsystem to brake the vehicle to a halt, and an electronic system, required only prior to failure, which periodically adjusts the hydraulic / mechanical system to an optimal, situation-dependent braking deceleration, preemptively for the case of a failure before the next optimization interval.

This paper focuses on the planning task of the described system, addressing the choice of an appropriate planning model, and especially its efficient computational solution, since the purpose of the system as a last-resort fallback demands a lightweight implementation. To this end, Sec. <ref> provides a brief overview of the system presented in [11]; Sec. <ref> establishes the basic mathematical model for the emergency maneuver planning; Sec. <ref> describes the proposed approach to render the problem tractable for realtime computation; and Sec. <ref> describes an efficient approach to solving this problem algorithmically. The performance of the resulting algorithm, which provides a globally-optimal decision for the current planning step, is discussed in Sec. <ref> both in terms of result quality and of computational efficiency. Section <ref> summarizes the main conclusions and provides an outlook to possible future extensions.

§ SYSTEM OVERVIEW

The system's goal is to assure that an “optimal”, situation-dependent emergency stop maneuver is executed without requiring any electric or electronic components after the time of failure, $t\subs{fail}$. This is achieved, as described in [11] and shown in Figs. <ref> and <ref>, by electronically presetting hydraulic / mechanical components prior to failure in an “optimal”, situation-dependent way, such that upon failure, only hydraulic / mechanical processes are required to execute the preset maneuver. If no failure occurs, the hydraulic / mechanical components remain inactive, and the planned emergency maneuver is not executed.

This section will briefly outline the system, as far as relevant to the maneuver optimizer (D in Figs. <ref> and <ref>), whose optimization algorithm is the subject of this paper and will be detailed in Secs. <ref> through <ref>.

The optimization algorithm D determines, at regular intervals $\Delta t\subs{plan}$, a target braking deceleration $a\subs{next}$, which is used to adjust a pressure regulation valve B. If the system fails within the current interval (i.e. before the next step of the optimizer), pressure is released immediately, regulated by the valve, to act on the brake master cylinder, executing an emergency stop using the preset $a\subs{next}$.

The optimizer has to determine some $a\subs{next}$ based on the vehicle's current situation at $t\subs{now}$ (e.g. traffic, vehicle dynamics, predictions with uncertainties), conditioned on the assumption that the vehicle fails before the next planning cycle at $t\subs{now}+\Delta t\subs{plan} =: t\subs{plan}$. Since the electric/electronic (E/E) system is still live upon optimization, E/E components can be used to determine $a\subs{next}$, such as processors and data from the vehicle sensors.

On the other hand, the computation must be extremely lightweight, since under typical conditions, the system should rarely ever be required at all; and it must be able to cope with additional complications, arising from uncertainty of the exact time of failure, and from the non-negligible time the pressure regulation valve B takes to reach the state $a\subs{next}$.

The final requirement is that the optimizer be consistent with a given regular maneuver planner; this allows to naturally specify its key parameters based on the parameters of the regular maneuver planner, and, more importantly, to reuse results to reduce computational effort.

To assure predictability of the lateral motion, moderate force is applied to maintain the current steering wheel angle upon failure (while still allowing a human driver to intervene), such that the lateral motion of the vehicle can be assumed to be an arc with known curvature. The optimizer can thus make use of path-velocity decomposition (PVD, [12]), without optimizing lateral motion, and accounting only for a limited added degree of positional uncertainty, which further includes uncertainties in perception and prediction, road friction and initial speed due to measurement uncertainties or accelerations at the moment of failure. An experimental evaluation of the predictability of vehicle motion for this use case is provided in [11], which describes in more detail how uncertainties are included in the planning process.

Reduced layout of the system originally presented in [11]. Upon failure of the E/E systems (right), detected by a watchdog mechanism E, the hydraulic / mechanical subsystem (left, originally in [13]) engages. Valve B opens and releases the pressure from piston accumulator A towards the pressure regulation valve C, whose state is adjusted by the maneuver optimizing unit D (the subject of this paper) at regular intervals to choose the optimal deceleration profile for the vehicle's current situation.

(0,31.5)2cmpart labels as in (fig:hydraulic-electronic)
(28,29)$\Delta t\subs{valve}$
(35,35.5)$\Delta t\subs{plan}$

Exemplary timing diagram of the developed system, as in [11]. Prior to failure, the emergency planning system D computes a new target deceleration $a\subs{next}$ at regular intervals (spaced by $\Delta t\subs{plan}$), used to set a pressure regulation valve C. The valve transitions for some time $t\subs{valve}$ (hatched areas) before reaching $a\subs{next}$. (For simplicty, we consistently denote the hydraulic valve state directly by its associated, calibrated deceleration, in the sense that the valve is preset to achieve this deceleration.) When the watchdog signal ceases, the lock valve B releases the pressure onto valve C, whose current state freezes upon failure and effects a constant braking deceleration.

Hydraulic prototype of the system in a VW Golf VII Variant, developed by the Institute of Vehicle System Technology (FAST) at KIT.

Overview of the system controlled by the optimization algorithm.

§ MATHEMATICAL MODEL

As previously stated, we aim to define the emergency planning problem such that it is consistent with regular maneuver planning of the automated vehicle. Therefore, we first describe the assumed problem statement of the regular maneuver planning, and then derive the statement of emergency maneuver planning from a special case thereof.

§.§ Regular Planning

The regular trajectory planning we assume to be modeled as a variational problem, as used e.g. in [14, 15, 16].  In this we consider the trajectory of the ego vehicle to be sufficiently determined by its trajectory

\begin{equation}
\traj : [t\subs{now}, t\subs{hzn}] \rightarrow \mathbb R \times \mathbb R, \quad \traj(t) = \left[\traj\subs{x}(t),\; \traj\subs{y}(t)\right]\transp,
\end{equation}

describing the ground coordinates of its rear axle center up to the prediction horizon. With the assumption of negligible tire slip, which strictly aligns the vehicle body with the trajectory's tangent, most common parameters such as heading, yaw rate or individual wheel speeds and angles can be derived given the basic vehicle geometry[17].

As stated in Sec. <ref>, the maneuver is executed with a constant steering wheel angle, to allow for path-velocity decomposition (PVD): We may consider a parametrization of $\traj$ by arc length, its path $\bar{\traj}(s)$, together with an appropriate timing along this path $\sigma(t)$, such that $\traj(t) \equiv \bar{\traj}(\sigma(t))$.

In the context of PVD, we assign penalty costs to a timing by using a functional of the form

\begin{equation}
\mathcal P[\sigma(\,\cdot\,)] = \int_{t\subs{now}}^{t\subs{hzn}}\!\dee t \; L(t, \sigma(t), \tfrac{\dee}{\dee t}\sigma(t), \tfrac{\dee^2}{(\dee t)^2} \sigma(t), ...),
\label{eq:euler-lagrange}
\end{equation}

where we write $\sigma(\,\cdot\,)$ to denote that the penalty is accumulated over the single parameter $t$ of $s$. For regular trajectory planning, $L$ uses the local (at $t$) derivatives to assign penalty costs e.g. for risks, comfort, traffic rule compliance, efficiency and ecology. For the evaluation of emergency stop maneuvers, we simplify the problem by using $L(t, \sigma(t), \tfrac{\dee}{\dee t}\sigma(t), ...) =: W(t, \sigma(t))$, which is sufficiently expressive to assign risk penalties to time–space coordinates that the vehicle should not traverse (e.g. coordinates of other dynamic objects) or stop on (e.g. railway tracks).

Vehicle speed     $v_0$    $\leqslant$    $45\,\unit{m/s}$ ($\approx 160\,\unit{km/h}, 100\,\unit{mph}$)
Braking decelerations     $a$    $\in$    $[-9\,\unit{m/s^2}, -1\,\unit{m/s^2}]$
Planning horizon     $t\subs{hzn}$    $=$    $10\,\unit{s}$
Replanning interval     $\Delta t\subs{plan}$    $=$    $0.25\,\unit{s}$
Valve speed     $\kappa$    $=$    $100\,\unit{m\,s^{-3}}$
Valve transition time      $\Delta t\subs{valve}$    $\leqslant$    $0.08\,\unit{s} = (a_{\max} - a_{\min})/\kappa$

Numerical reference values of relevant parameters introduced throughout the paper, used only where explicitly indicated and exclusively to provide realistic orders of magnitude, without loss of generality.

§.§ A Single Stopping Trajectory

The basic element of the emergency stopping problem description is a timing $\sigma(t,v_0, t\subs{fail}, a)$ which drives at constant speed $v_0$ until $t\subs{fail}$, and then decelerates with some negative $a$ until it comes to a halt.[Note that we strictly use $t\subs{fail}$ as the instant when the emergency deceleration engages. Any deterministic delay between actual failure and deceleration onset, such as by hydraulics, brake pad motion or signal times that can be determined a-priori, is considered included.]
This timing is given by

\begin{equation}
\sigma(t,v_0, t\subs{fail},a) = \begin{cases}
v_0 \, t & \text{for $t \leqslant t\subs{fail}$}\\
s_\blacksquare & \text{for $t \geqslant t_\blacksquare$}\\
v_0 \, t\subs{fail} + \frac{a \, (t-t\subs{fail})^2}{2} & \text{else,}\\
\end{cases}
\end{equation}

where the stopping time and distance are given by

\begin{equation}
t_\blacksquare = \phantom{v_0\,}t\subs{fail} - v_0 / a\quad\text{and}\quad
s_\blacksquare = v_0\, t\subs{fail} - v_0^2 / 2 a.
\end{equation}

With the trajectory shape completely specified by parameters $v_0, t\subs{fail},a$, the Euler–Lagrange form (<ref>) of the penalty cost functional can be expressed as a penalty function:

\begin{equation}
P(v_0, t\subs{fail},a)
:= \mathcal P[\sigma(\,\cdot\,,\, v_0, t\subs{fail},a)] = \!\!\int_{t\subs{now}}^{t\subs{hzn}} \!\!\!\!\!\! \dee t\, W(t, \sigma(t, ...)).\label{eq:penaltyfunction}
\end{equation}

Thereby, the optimization simplifies to $a^* \in \argmin_{a} P(a)$.

§.§ Adaptation to the Actual Problem

The actual planning problem, however, is more complex than optimizing $P$ for $a$. At each planning instant of the vehicle (i.e. strictly before the emergency), $v_0$ is known, but $t\subs{fail}$ is not: The system may fail at any time within the planning interval $T\subs{plan} = [t\subs{now}, t\subs{plan})$.[Do note that it may (and typically should) not fail within $T\subs{plan}$ at all; however, since any planned action is only executed iff the system fails, the failure within $T\subs{plan}$ acts as a stochastic precondition in the planning. Thereby, all modeling is independent of $p(\text{fail})$, which would typically be difficult to determine.] With a relatively short $\Delta t\subs{plan}$ (cf. Tab. <ref>), it is considered unlikely that there is considerable prior knowledge about when $t\subs{fail}$ would occur within $T\subs{plan}$, so we assume a uniform $t\subs{fail} \sim \mathcal U(T\subs{plan})$.

Along with the unknown $t\subs{fail}$, even $a$ is unknown: Since $a$ is a mechanical parameter (namely the state of valve C in Fig. <ref>), it cannot be switched instantaneously. Instead, if the optimal valve state from the previous planning cycle was $a\subs{prev}$, and our (yet undefined) optimization process obtains $a\subs{next}$ as the next optimal solution, the valve will take some non-negligible interval $[t\subs{now}, t\subs{valve}]$ to transition, modeled linearly as $t\subs{valve} = t\subs{now} + \kappa\,(a\subs{next}-a\subs{prev})$ using a signed “valve speed” $\kappa$ with $\operatorname{sign}\kappa = \operatorname{sign}(a\subs{next}-a\subs{prev})$. For example, for values as in Tab. <ref>, the probability of failure during valve transition can be up to $32\,\%$. We note that $a \not\sim \mathcal U(T\subs{plan})$: If valve motion is approximately linear with $a$, but $t\subs{valve} < \Delta t\subs{plan}$, $a$ is uniformly distributed during $[t\subs{now}, t\subs{valve}]$, but constant thereafter, with $t\subs{valve}$ depending on $a\subs{prev} - a\subs{next}$. We therefore denote the unknown value as $\alpha(a\subs{prev}, a\subs{next}, t\subs{fail})$ and find the actual curve family to be

\begin{equation}
\sigma(t,v_0, t\subs{fail},\alpha(a\subs{prev}, a\subs{next}, t\subs{fail})).
\end{equation}

The complete planning problem thus presents itself as minimizing the expected penalty value for a choice of $a\subs{next}$

\begin{equation}
a^* \in \argmin_{a\subs{next}}\; \tfrac{1}{\Delta t\subs{plan}} \langle P(..., a\subs{next}, ...)\rangle,\label{eq:planningproblem}
\end{equation}

using $\langle\,\cdot\,\rangle$ as the non-normalized expected value over $t\subs{fail}$,

\begin{equation}
\langle P(..., a\subs{next}, ...) \rangle := \!\! \int_{t\subs{now}}^{t\subs{plan}}\!\!\!\!\!\!\!\! \dee t\subs{fail}\; P(v_0, t\subs{fail},\alpha(a\subs{prev}, a\subs{next}, t\subs{fail})).
\label{eq:expected-value}
\end{equation}

For its solution, we note that $P$ depends on $W(t,s)$ (the risk predictions), which is typically not analytic but an array, so analytic solving is not feasible. Iterative solvers have difficulty guaranteeing time or quality constraints (cf. [18]), and for realtime safety applications, we require both. This points to discretization and subsequent global optimization of the discretized set, yet a direct, exhaustive solution of (<ref>) over $a\subs{next}$ is prohibitive for lightweight realtime applications: To test any $a$, we must evaluate all $t\subs{fail}$, and each $t\subs{fail}$ yields a timing $\sigma(t)$ to be integrated over (as in (<ref>)) to evaluate its cumulative penalty costs $P$. Hence, we are looking for ways to simplify the computation by reusing computations, not between planning cycles (we consider each planning step a new problem), but within a single cycle.

§ PROBLEM SIMPLIFICATION

(93, 52)$-2\,\unit{\frac{m}{s^2}}$
(70, 48)$-3\,\unit{\frac{m}{s^2}}$
(54, 44) $-4\,\unit{\frac{m}{s^2}}$
(34, 40)$-5\,\unit{\frac{m}{s^2}}$
(28, 35)$-6\,\unit{\frac{m}{s^2}}$

(72, 9)1.4cm

\begin{align*}v_0 &= 15\,\unit{\tfrac{m}{s}}\\
a\subs{prev} &= -4\,\unit{\tfrac{m}{s^2}}
\end{align*}

(4, 42)1.6cm
the timing for $t\subs{fail}{=}t\subs{now}$, $\sigma(t, t\subs{now}, ...)$, always brakes with $a\subs{prev}$

Areas covered for different choices of $a\subs{next}$ overlap, starting with the timing $\sigma(t, t\subs{now}, ...)$. This motivates the attempt to use an antiderivative of $W(t,s)$ w.r.t $s$ to determine the expected value of penalties accumulated within the area for a particular choice of $a\subs{next}$.

First, we note that at the beginning of each planning cycle, the following parameters are known: The current vehicle speed $v_0$, the current valve state $a\subs{prev}$, as well as the parametric constants $t\subs{now}$, $t\subs{plan}$ and $t\subs{hzn}$. Optimization result is $a\subs{next}$, whereas $t\subs{fail} \in [t\subs{now}, t\subs{now}+\Delta t\subs{plan}]$ always remains unknown. We introduce the three-parametric shorthand

\begin{align}
\sigma(t, t\subs{fail}, a\subs{next}) &= \sigma(t, v_0, t\subs{fail}, \alpha(\alpha\subs{prev}, \alpha\subs{next}, t\subs{fail})),
\end{align}

and state the goal to establish some easily precomputed $\mathcal I$ (an antiderivative w.r.t. $s$), to achieve a form

\begin{equation}
\begin{aligned}
\langle P(..., a\subs{next}, ...) \rangle = \!\!\int_{t\subs{now}}^{t\subs{hzn}} \!\dee t \; \biggl(\;
&\mathcal I(t, \sigma(t, \underbrace{t\subs{plan}}_{\text{\clap{range over possible $t\subs{fail}$}}}, a\subs{next}))\\[-2pt]
\;&\mathcal I(t, \sigma(t, \overbrace{t\subs{now}}, a\subs{next}))\;\biggr)
\end{aligned}
\label{eq:integralbild}
\end{equation}

such that (a) time steps up to $t\subs{hzn}$ can be treated independently, and (b) to evaluate one candidate $a\subs{next}$, we must no longer integrate over all timings (= trajectories) for all possible $t\subs{fail}$, but instead look up in the precomputed $\mathcal I$.

The first condition (a) can be readily rearranged by

\begin{align}
%& \int_{t\subs{now}}^{t\subs{plan}}\! \dee t\subs{fail} \; P(v_0, t\subs{fail},\alpha(a\subs{prev}, a\subs{next}, t\subs{fail}))\\
\langle P(..., a\subs{next}, ...) \rangle = & \int_{t\subs{now}}^{t\subs{plan}}\!\!\!\!\! \dee t\subs{fail}  \int_{t\subs{now}}^{t\subs{hzn}}\!\!\!\!\! \dee t\; W(t, \sigma(t, t\subs{fail}, a\subs{next}))\\
\intertext{where we may apply Fubini's theorem to obtain}
= & \int_{t\subs{now}}^{t\subs{hzn}}\!\!\!\!\! \dee t \int_{t\subs{now}}^{t\subs{plan}}\!\!\!\!\! \dee t\subs{fail} \; W(t, \sigma(t, t\subs{fail}, a\subs{next})),\label{eq:fubini}
\end{align}

since all physically possible timings are necessarily continuous and all intervals are closed.

For (b), we seek some
\mathcal I(t, \sigma(t, t\subs{fail}, a\subs{next}))
that is easily precomputed (“pre” in the sense of before picking any candidate $a\subs{next}$), so we must integrate over $s$ instead of $t\subs{fail}$, since evaluating $\sigma(t, t\subs{fail}, a\subs{next})$ requires $a\subs{next}$. Hence we mean to establish a substitution function $\tau\subs{fail}$ s.t. for any valid arc length $s$

\begin{equation}
\sigma(t, \tau\subs{fail}(t, s), a\subs{next}) = s\quad\text{and thus}
\end{equation}

\begin{equation}
\!\;\;\int_{t\subs{now}}^{t\subs{plan}}\!\!\!\!\! \dee t\subs{fail} \; W(t, \sigma(t, t\subs{fail}, a\subs{next}))
%= & \int_{\tau^{-1}\subs{fail}(t, t\subs{now})}^{\tau^{-1}\subs{fail}(t,t\subs{plan})}\!\!\! \dee s \; W(t, \sigma(
%t,\tau\subs{fail}(t,s), a\subs{next})) \left.\frac{\partial \tau\subs{fail}}{\partial s}\right|_{t,s}\!\!\!\!\\[5pt]
=\!\!  \int_{\sigma(t, t\subs{now}, a\subs{next})}^{\sigma(t, t\subs{plan}, a\subs{next})}\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\! \dee s \; W(t, s) \left.\frac{\partial \tau\subs{fail}}{\partial s}\right|_{t,s}
\end{equation}

\begin{equation}
=:  \;\; \mathcal I(t, \sigma(t, t\subs{plan}, a\subs{next}))
- \mathcal I(t, \sigma(t, t\subs{now}, a\subs{next}))
\end{equation}

which allows to specify $\mathcal I$ as required in (<ref>) as

\begin{equation}
\mathcal I(t,s) = \int_0^s\!\dee s\; W(t,s)  \left.\frac{\partial \tau\subs{fail}}{\partial s}\right|_{t,s}
\label{eq:integrabild-multiplikation}
\end{equation}

To compute (<ref>), we require $\partial \tau\subs{fail}/\partial s$, intuitively the density of trajectories passing through some space segment by change in failure times. We distinguish between the following sets of sub-trajectories, as shown in Fig. <ref>:

* Sub-trajectories A that have not failed yet and hence lie on the regularly planned trajectory. All trajectories start in this set at the instant $t=t\subs{now}$ with the common point $\sigma(t\subs{now}) = 0$, but branch off to a different set ($\mathbf{B}$ or $\mathbf{C}$) once they fail.	Since the entire planning process is conditioned upon the assumption that failure is certain within $[t\subs{now}, t\subs{plan}]$, the longest sub-trajectory in this set lasts until $t\subs{fail}$, when it is the last to fail and branch off. The set A is special in that all sub-trajectories therein overlap perfectly, but their density decreases with $t$.

* Sub-trajectories $\mathbf{B}$ that have failed, but the valve had not yet reached $a\subs{next}$. These sub-trajectories decelerate with varying decelerations $[a\subs{prev}, a\subs{next}]$. As seen in Fig. <ref>, these sub-trajectories can cover a wide interval over $s$ for large $|a\subs{next}-a\subs{prev}|$. If $a\subs{next} = a\subs{prev}$, B is empty.

* Sub-trajectories $\mathbf{C}$ that have failed, and the valve did reach $a\subs{next}$ before that. These sub-trajectories all decelerate with $a\subs{next}$, and are only spaced by the vehicle driving along its original path for a longer time.

The sub-trajectories within these sets are given by

\begin{align}
\sigma^{\mathbf{A}}(t, t\subs{fail}) &= v_0\, t\;,\label{eq:sigma-A}\\[5pt]
\sigma^{\mathbf{B}}(t, t\subs{fail}) &=
\begin{cases}
v_0 t + \frac{1}{2} (a\subs{prev} + \kappa t\subs{fail}) (t-t\subs{fail})^2 & t < t_\blacksquare\\
v_0   t\subs{fail} - v_0^2 / (2 (a\subs{prev} + \kappa t\subs{fail})) & \text{else, and}\!\!\!
\end{cases}\label{eq:sigma-B}\\[5pt]
\sigma^{\mathbf{C}}(t, t\subs{fail}) &=
\begin{cases}
v_0 t + \frac{1}{2} a\subs{next} (t-t\subs{fail})^2 \hspace{35pt}& t < t_\blacksquare\\
v_0 t\subs{fail} - v_0^2 / (2 a\subs{next}) & \text{else.}
\end{cases}\label{eq:sigma-C}
\end{align}

Using this distinction, we state for the expected penalty costs

\begin{equation}
\langle P(..., a\subs{next}, ...) \rangle =
\langle P\rangle^{\mathbf{A}} + \langle P \rangle^{\mathbf{B}} + \langle P \rangle^{\mathbf{C}}
\end{equation}

where $\langle P \rangle^{\mathbf{A}}$, ..., $\langle P \rangle^{\mathbf{C}}$ are the expected penalty costs for sub-trajectories within $\mathbf A$ through $\mathbf C$, namely, by the boundaries of integration shown in Fig. <ref>,

\begin{align}
\langle P\rangle^{\mathbf{A}} &\!\!\,=\!\!\!
\int_{t\subs{now}}^{t\subs{plan}}\hneg \dee t \int_{t}^{t\subs{plan}} \hneg\dee t\subs{fail} W(t, ...) \;=\! \int_{t\subs{now}}^{t\subs{plan}}\hneg\dee t\; (t\subs{plan}{-}t) \, W(t, v_0t)\label{eq:area-p-a}\\[5pt]
\langle P\rangle^{\mathbf{B}} &\!\!\,=\!\!\!
\underbrace{\int_{t\subs{now}}^{t\subs{valve}}\hneg \dee t \int_{t\subs{now}}^{t}\hneg\;\; \dee t\subs{fail} \; W(t, ...)}_{\mathbf B_1}
\underbrace{\int_{t\subs{valve}}^{t\subs{hzn}}\hneg\;\; \dee t \int_{t\subs{now}}^{t\subs{valve}} \hneg \dee t\subs{fail} W(t, ...)}_{\mathbf B_2}\label{eq:area-p-b}\\
\langle P\rangle^{\mathbf{C}} &\!\!\,=\!\!\!
\underbrace{\int_{t\subs{valve}}^{t\subs{plan}}\hneg \dee t \int_{t\subs{valve}}^{t}\hneg\;\; \dee t\subs{fail} \; W(t, ...)}_{\mathbf C_1}
\underbrace{\int_{t\subs{plan}}^{t\subs{hzn}}\hneg\;\; \dee t \int_{t\subs{valve}}^{t\subs{plan}} \hneg \dee t\subs{fail} W(t, ...)}_{\mathbf C_2}\label{eq:area-p-c}
\end{align}

where the ellipses (“...”) denote $\sigma^{\mathbf{A}}(t, t\subs{fail})$ through $\sigma^{\mathbf{C}}(t, t\subs{fail})$ from the previous (<ref>)–(<ref>)  respectively, such that

\begin{equation}
\langle P(..., a\subs{next}, ...) \rangle =  \langle P\rangle^{\mathbf{A}}  +  \langle P \rangle^{\mathbf{B}} +  \langle P\rangle^{\mathbf{C}}.
\end{equation}

We note that sub-trajectories contained in $\mathbf{A}$ lie on the regularly planned trajectory and range up to $t\subs{fail}$—thus, neither their shape nor their density is affected by the choice of $a\subs{next}$. Therefore, $\langle P\rangle^{\mathbf{A}}$ is a constant term in the optimization that does not affect the solution $a^*$. We thus need not specify a substitution function for $\langle P\rangle^{\mathbf{A}}$ to obtain $a^*$ in (<ref>).[Also note that, since we aimed to pose the emergency planning problem as consistent with the regular maneuver planning, which minimizes the penalty costs of the regular trajectory, $\langle P\rangle^{\mathbf{A}}$ should typically be very low.] For $\langle P\rangle^{\mathbf{B}}$ and $\langle P\rangle^{\mathbf{C}}$, we define the substituted integrals via limits, to avoid integrating over discontinuous boundaries. We define

\begin{align}
\langle P\rangle^{\mathbf{B}}_{\boldsymbol\varepsilon} &=
\underbrace
\int_{\varepsilon_1}^{t\subs{valve}}\hneg\;\;\,\dee t\quad
\int_{\sigma^{\mathbf{B}}(t, t\subs{now})}^{\sigma^{\mathbf{B}}(t, t-\varepsilon_2)}\hneg\dee s\;
W(t,s)\left.\frac{\partial \tau^{\mathbf{B}}}{\partial s}\right|_{t,s}
}_{\mathbf B_1}\notag\\
\underbrace
\int_{t\subs{valve}+\varepsilon_5}^{t\subs{hzn}}\hneg\,\dee t\quad
\int_{\sigma^{\mathbf{B}}(t, t\subs{now})}^{\sigma^{\mathbf{B}}(t, t\subs{valve})}\hneg\dee s\;
W(t,s)\left.\frac{\partial \tau^{\mathbf{B}}}{\partial s}\right|_{t,s}
}_{\mathbf B_2}\label{eq:area-p-b-tau}\\
\mathllap{\text{and}}\quad\langle P\rangle^{\mathbf{C}}_{\boldsymbol\varepsilon} &=
\underbrace
\int_{t\subs{valve}+\varepsilon_3}^{t\subs{plan}}\hneg\dee t\quad
\int_{\sigma^{\mathbf{C}}(t, t\subs{valve})}^{\sigma^{\mathbf{B}}(t, t-\varepsilon_4)}\hneg\dee s\;
W(t,s)\left.\frac{\partial \tau^{\mathbf{C}}}{\partial s}\right|_{t,s}
}_{\mathbf C_1}\notag\\
\underbrace
\int_{t\subs{plan}+\varepsilon_6}^{t\subs{hzn}}\hneg\;\,\dee t\quad
\int_{\sigma^{\mathbf{C}}(t, t\subs{valve})}^{\sigma^{\mathbf{B}}(t, t\subs{plan})}\hneg\dee s\;
W(t,s)\left.\frac{\partial \tau^{\mathbf{C}}}{\partial s}\right|_{t,s}
}_{\mathbf C_2}\label{eq:area-p-c-tau}\\
\mathllap{\text{such that}}\quad&\quad
\langle P \rangle -  \langle P\rangle^{\mathbf{A}}  =
\lim_{\boldsymbol\varepsilon \rightarrow \boldsymbol 0}\;\;
\langle P \rangle^{\mathbf{B}}_{\boldsymbol\varepsilon} \;+\;  \langle P\rangle^{\mathbf{C}}_{\boldsymbol\varepsilon}
\end{align}

under $\varepsilon_1 \geqslant \varepsilon_2$ and $\varepsilon_3 \geqslant \varepsilon_4$. The existence of this limit is shown in [19]. To derive the substitution functions $\tau^{\mathbf{B}}$ and $\tau^{\mathbf{C}}$, we distinguish the sets $\mathbf{B}$ and $\mathbf{C}$ further into regions while the vehicle still moves, and regions where it already stopped, namely ${\mathbf{B}} = {\mathbf{B}}_\blacktriangleright \;\dot\cup\; {\mathbf{B}}_\blacksquare$ and ${\mathbf{C}} = {\mathbf{C}}_\blacktriangleright \;\dot\cup\; {\mathbf{C}}_\blacksquare$, cf. Fig. <ref>.

(13,23.5)exaggerated detail

(67,33) ${\mathbf{B}}_\blacktriangleright$
(65,47) ${\mathbf{B}}_\blacksquare$

(32,52) ${\mathbf{B}}$

The set of all possible sub-trajectories for a given transition is composed as follows: The set A of sub-trajectories that have not failed yet (thin light blue line); the set B of sub-trajectories that ensue for failures at some $t\subs{fail} \in [t\subs{now}, t\subs{valve})$ which brake with intermediate accelerations $[a\subs{prev}, a\subs{now})$; and the set C of sub-trajectories for failures $t\subs{fail} \in [t\subs{valve}, t\subs{plan}]$ that brake with the target deceleration of $a\subs{next}$ (hatched green). The latter two we further distinguish into ${\mathbf{B}}_\blacktriangleright$, $\mathbf{B}_\blacksquare$ and  $\mathbf{C}_\blacktriangleright$, ${\mathbf{C}}_\blacksquare$ by whether the vehicle already has stopped (indicated by the dashed $[s_\blacksquare, t_\blacksquare]\transp$ line).

(12.5,6) ${\mathbf{B}}_1$
(56,9)    ${\mathbf{B}}_2$

(10,30)2cm$t \leqslant t\subs{fail}$

We distinguish between trajectories which have not failed/decelerated yet (A, solid light blue), trajectories where the valve was activated in an intermediate state ($\mathbf{B}$, solid dark blue), and trajectories where the valve was activated in its constant state $a\subs{next}$ ($\mathbf{C}$, hatched green). The subsets $\mathbf{B}_1$, $\mathbf{B}_2$ and $\mathbf{C}_1$, $\mathbf{C}_2$ defined to obtain simple boundaries for integration in (<ref>)–(<ref>).

§.§.§ $\mathbf{B}_\blacktriangleright$—Vehicle decelerating, valve stopped in transition

In this case, we find for the trajectories

\begin{equation}
\sigma^{\mathbf{B}}(t,v_0, t\subs{fail},a\subs{prev}) = v_0\, t + \tfrac{1}{2} (a\subs{prev} + \kappa\,t\subs{fail}) (t-t\subs{fail})^2
\end{equation}

\begin{equation}
= \frac{\kappa t\subs{fail}^3}{2}
\left(\frac{a\subs{prev}}{2}-\kappa t\right) t\subs{fail}^2
\left(\frac{\kappa t\mathrlap{{}^2}}{2}-a\subs{prev}t\right) t\subs{fail}
v_0t + \frac{a\subs{prev} t^2}{2},
\end{equation}

where the absolute value of the third-order term is, in the exemplary quantities of Tab. <ref>, bounded by $\left|(\kappa/2) \,t\subs{valve}^3\right| < 2.6\,\unit{cm}$.[Since in the case of the valve stopping in transition we have $t\subs{fail} \leqslant t\subs{valve}$.] As this will typically be considerably lower than positional uncertainty of the predictions, we approximate $s$ by omitting this term to proceed with

\begin{equation}
%\tilde \sigma^{\mathbf{B}}(t,v_0, t\subs{fail},a\subs{prev}) = \tfrac{a\subs{prev} {-} \kappa t}{2} t\subs{fail}^2 + \tfrac{\kappa t^2{-}a\subs{prev} t}{2} t\subs{fail}
%+ v_0 t + \tfrac{a\subs{prev} t^2}{2},
\tilde \sigma^{\mathbf{B}}(...) =
\left(\frac{a\subs{prev}}{2}-\kappa t\right) t\subs{fail}^2
\left(\frac{\kappa t\mathrlap{{}^2}}{2}-a\subs{prev}t\right) t\subs{fail}
v_0t + \frac{a\subs{prev} t^2}{2}
\end{equation}

\begin{equation}
\tau\subs{fail}^{\mathbf{B}}(t,s) = -\frac{\beta_{\blacktriangleright} \pm \operatorname{sign}(\kappa)}{2\,\alpha_{\blacktriangleright}} \, \sqrt{\beta_{\blacktriangleright}^2 - 4\,\alpha_{\blacktriangleright}\,\gamma_{\blacktriangleright} + 4\,\alpha_{\blacktriangleright}\,s}
\end{equation}

with $\alpha_{\blacktriangleright} = \tfrac{1}{2} a\subs{prev} - \kappa t$, $\beta_{\blacktriangleright} = \tfrac{1}{2} \kappa t^2 - a\subs{prev}t$ and $\gamma_{\blacktriangleright} = v_0t + \tfrac{1}{2}a\subs{prev}t^2$. It can be seen that $\tau\subs{fail}^{\mathbf{B}}(t,s)$ is not uniquely determined at $s$. Particularly we find that

\begin{equation}
\biggl. \frac{\partial \tilde{\sigma}^\mathbf{B}}{\partial t\subs{fail}} \biggr|_{t\subs{fail}, t} = 0 \Leftrightarrow t\subs{fail} = t + \frac{3 \, \kappa\, t^2}{2\,a\subs{prev}-4 \, \kappa\, t}
\end{equation}

where arc length $s$ reaches an extremum over $t\subs{fail}$ and several arc lengths may occur twice along $t\subs{fail}$ (details in [19]).

§.§.§ $\mathbf{B}_\blacksquare$—Vehicle at rest, valve stopped in transition

In this case we have the trajectories

\begin{equation}
\sigma^{\mathbf{B}}(t,t\subs{fail}) = v_0 \, t\subs{fail} - v_0^2/(2\, a\subs{prev} + 2\,\kappa\,t\subs{fail})
\end{equation}

which gives

\begin{equation}
\tau\subs{fail}^{\mathbf{B}, \pm}(t,s) = \frac{-\beta_{\blacksquare} \pm \operatorname{sign}(\kappa)}{2\,\alpha_{\blacksquare}} \, \sqrt{\beta_{\blacksquare}^2 - 4\,\alpha_{\blacksquare} \, \gamma_{\blacksquare}}
\end{equation}

with $\alpha_{\blacksquare} = 2v_0\kappa$, $\beta_{\blacksquare} = 2v_0a\subs{prev}-2\kappa s$ and $\gamma_{\blacksquare} = -v_0^2-2\alpha_0s$. Again looking for singular points gives the solutions

\begin{equation}
\biggl. \frac{\partial \sigma^{\mathbf{B}}}{\partial t\subs{fail}} \biggr|_{t\subs{fail}, t} = 0 \Leftrightarrow t\subs{fail}^\pm = \frac{-a\subs{prev} \pm \sqrt{-\tfrac{1}{2}\,\kappa\,v_0}}{\kappa},
\end{equation}

for $\kappa < 0$, of which only $t\subs{fail}^+ = (-a\subs{prev} + \sqrt{...})/\kappa$ can lie within $[t\subs{now}, t\subs{valve}]$, and only for specific parameters:

\begin{equation}
t\subs{fail}^+ \in [t\subs{now}, t\subs{valve}] \Leftrightarrow v_0 < -2\,a^2\subs{next} / \kappa
\end{equation}

By Tab. <ref>, this condition is satisfied only for $v_0 \leqslant 1.62\,\unit{m/s}$; under relevant conditions, $\sigma^{\mathbf{B}}$ is non-singular w.r.t. $t\subs{fail}$.

§.§.§ $\mathbf{C}_\blacktriangleright$—Vehicle decelerating, valve reached $a\subs{next}$

In this case we have the trajectories

\begin{equation}
\sigma^{\mathbf{C}}(t, t\subs{fail}) = v_0 \, t + \tfrac{1}{2} \, a\subs{next} \, (t-t\subs{fail})^2,
\end{equation}

\begin{equation}
\text{and}\quad \tau\subs{fail}^{\mathbf{C},\pm} (t,s) = \frac{a\subs{next}\,t \pm \sqrt{-2\,a\subs{next}\,v_0\,t+2\,a\subs{next}\,s}}{a\subs{next}}
\end{equation}

which is strictly $\tau\subs{fail}^{\mathbf{C}} = \tau\subs{fail}^{\mathbf{C},+}$ for $t > t\subs{fail}$.

§.§.§ $\mathbf{C}_\blacksquare$—Vehicle at rest, valve reached $a\subs{next}$

This final case contains the trajectories

\begin{equation}
\sigma^{\mathbf{C}}(t,t\subs{fail}) = v_0 \, t\subs{fail} - v_0^2/(2\,a\subs{next})
\end{equation}

\begin{equation}
\text{with}\quad\tau\subs{fail}^{\mathbf{C}}(t,s) = s/v_0 + v_0/(2\,a\subs{next}).
\end{equation}

§.§.§ Partial Derivatives of $\tau\subs{fail}(t,s)$ over $\mathbf{B}\, \dot\cup\,\mathbf{C}$

The results of the previous sections give the partial derivatives of $\tau\subs{fail}$ (as required in (<ref>) and (<ref>)) as

\begin{align}
%\hspace{0pt}\tau\subs{fail}^{\mathbf{B}}(t,s) &= \begin{cases}
%\frac{-\beta_{\blacktriangleright} + \operatorname{sign}(\kappa)  \sqrt{\beta_{\blacktriangleright}^2 - 4\alpha_{\blacktriangleright}\gamma_{\blacktriangleright}+4\alpha_{\blacktriangleright}s}}{2\,\alpha_{\blacktriangleright}}
%& s > s_{\blacksquare}\\[20pt]
%\frac{-\beta_{\blacksquare} - \operatorname{sign}(\kappa)\, \sqrt{\beta_{\blacksquare}^2 - 4 \alpha_{\blacksquare} \gamma_{\blacksquare}}}{2\, \alpha_{\blacksquare}}
%& \text{else}
\left.\frac{\partial \tau\subs{fail}^{\mathbf{B}}}{\partial s}\right|_{t,s} &= \begin{cases}
\displaystyle
\frac{\operatorname{sign}(\kappa)}{\sqrt{\beta_{\blacktriangleright} - 4\alpha_{\blacktriangleright}\gamma_{\blacktriangleright}+4\alpha_{\blacktriangleright} s}}
& s > s_{\blacksquare}\\[20pt]
\displaystyle
\frac{1}{2\,v_0} - \frac{\operatorname{sign}(\kappa) (\beta_{\blacksquare}+4\,\kappa\,s)}{2v_0\sqrt{\beta_{\blacksquare}^2-4\alpha_{\blacksquare}\gamma_{\blacksquare}}} & \text{else, and}
\end{cases}\label{eq:dtau-b-ds}\\[3pt]%\hspace{-10pt}
%\tau\subs{fail}^{\mathbf{C}}(t,s) &= \begin{cases}
%\frac{a\subs{next}t + \sqrt{-2a\subs{next} v_0 t + 2 a\subs{next} s}}{a\subs{next}} & s > s_{\blacksquare}\\[10pt]
%\frac{s}{v_0} + \frac{v_0}{2\,a\subs{next}} & \text{else}
\left.\frac{\partial \tau\subs{fail}^{\mathbf{C}}}{\partial s}\right|_{t,s} &= \begin{cases}
\displaystyle
\frac{1}{\sqrt{-a\subs{next}} \sqrt{2 v_0 t + 2 s}}
\hspace{14pt}
& s > s_{\blacksquare}\\[10pt]
\displaystyle
& \text{else.}
\end{cases}
\label{eq:dtau-c-ds}
\end{align}

§ PROBLEM SOLUTION

Having established $\partial \tau/\partial s$ now allows us to compute $\mathcal I(t,s)$ by (<ref>), to approximately optimize $ \langle P(..., a\subs{next}, ...) \rangle$ in (<ref>).

§.§ Discretization

We use the discretizations $\hat{\Delta}t$ for prediction time and $\hat{\Delta}s$ for stopping distance to define the following sets:

\begin{align}
\hat T &= \Set{ \hat{t} | \hat{t} = t\subs{now} + m \; \hat{\Delta}t, \;  \hat{t} \leqslant t\subs{hzn},\; m \in \{0, 1, 2, ...\}  }\\
\hat S &= \Set{ \hat{s} | \hat{s} = 0\,\unit{m} + n \; \hat{\Delta}s, \; n \in \{0, 1, 2, ...\}  }\\
\hat S(t) &= \Set{ s | s \in \hat S, \; \sigma\subs{min}(t) \leqslant s \leqslant \sigma\subs{max}(t)}\\
\hat A &= \Set{ a\subs{min},\; a\subs{min}+\Delta a,\; ...,\; a\subs{max}-\Delta a,\; a\subs{max} }
\end{align}

where $\sigma\subs{min}(t)$ and $\sigma\subs{max}(t)$ are the shortest and longest possible stopping trajectories respectively,

\begin{align}
\sigma\subs{min}(t) &= \sigma(t,\; t\subs{fail} = (a\subs{max}-a\subs{prev})/\kappa,\; a\subs{max}) \\
\text{and}\quad \sigma\subs{max}(t) &= \sigma(t,\; t\subs{fail} = (a\subs{min}-a\subs{prev})/\kappa,\; a\subs{min}).
\end{align}

We introduce the additional simplification that variations in penalty costs at different $a\subs{next}$ for sub-trajectories within $t \in [t\subs{now}, t\subs{now} + \hat{\Delta} t]$ are negligible. Their positional difference is (based on the values in Tab. <ref>) bounded by

\begin{equation}
v_0\,t - \left( v_0\, t + \tfrac{1}{2}\, a\subs{max} \, \hat{\Delta} t \right) \leqslant \tfrac{1}{2} \, a\subs{max} \, \hat{\Delta} t^2 = 4.5\,\unit{cm},
\end{equation}

which, again, is likely far lower than accuracies in environment modeling. In turn, if $\hat{\Delta} t > t\subs{valve}$ (as applies here), we may simplify the statements in (<ref>) and (<ref>) to

\begin{equation}
{\langle P(..., a\subs{next}, ...)\rangle^{\mathbf B}}' =   \underbrace{\int_{t\subs{now} + \hat{\Delta} t}^{t\subs{hzn}} \hspace{-0.5cm} \dee t
\;\;
\int_{\sigma^{\mathbf{B}}(t, t\subs{now})}^{\sigma^{\mathbf{B}}(t, t\subs{valve})} \hneg\hneg \dee s
\;  W(t,s)
\hspace{0.5pt}\left.\frac{\partial \tau\subs{fail}^{\mathbf{B}}}{\partial s}\right|_{t,s}}_{\mathbf{B}_2}
\end{equation}

\begin{align}
{\langle P\rangle^{\mathbf{C}}_{\boldsymbol\varepsilon}}' &=
\underbrace
\int_{t\subs{now}+\hat{\Delta}t}^{t\subs{plan}}\hneg\dee t\quad
\int_{\sigma^{\mathbf{C}}(t, t\subs{valve})}^{\sigma^{\mathbf{B}}(t, t-\varepsilon_4)}\hneg\dee s\;
W(t,s)\left.\frac{\partial \tau^{\mathbf{C}}}{\partial s}\right|_{t,s}
}_{\mathbf C_1}\notag\\
\underbrace
\int_{t\subs{plan}+\varepsilon_6}^{t\subs{hzn}}\hneg\,\dee t\quad
\int_{\sigma^{\mathbf{C}}(t, t\subs{valve})}^{\sigma^{\mathbf{B}}(t, t\subs{plan})}\hneg\dee s\;
W(t,s)\left.\frac{\partial \tau^{\mathbf{C}}}{\partial s}\right|_{t,s}
}_{\mathbf C_2}
\end{align}

which eliminates the term over $\mathbf{B}_1$, as well as the limits for $\varepsilon_1$, $\varepsilon_2$, $\varepsilon_3$ and $\varepsilon_5$.

§.§ Area under $\mathbf{B}$

We precompute the antiderivative relative to the initial trajectory $\sigma(t, t\subs{fail}=t\subs{now})$ (which decelerates with $a\subs{prev}$ and is hence invariant to $a\subs{next}$), by using

\begin{align}
W^{\mathbf{B}}(\hat{t},\hat{s}) &= W(\hat{t},\hat{s}) \left.\frac{\partial \tau\subs{fail}^{\mathbf{B}}}{\partial s}\right|_{\hat{t},\hat{s}}\\
\text{as}\quad \mathcal I^{\mathbf{B}}(\hat{t},\hat{s}) &= - W^{\mathbf{B}}(\hat{t},\sigma(\hat{t}, t\subs{now})) + \sum_{\mathclap{ \set{ s \in \hat S(t) \,|\, s \leqslant \hat{s} } }} W^{\mathbf{B}}(\hat{t},s).
\end{align}

Due to this, we then can evaluate a given $a\subs{next}$ via

\begin{equation}
P^{\mathbf{B}}(\hat{t}, a\subs{next}) = |\; \mathcal I^{\mathbf{B}}(\hat{t},\sigma(\hat{t}, \hat{t}\subs{valve}, a\subs{next}))\; | .
\label{eq:p-B}
\end{equation}

§.§ Area under $\mathbf{C}$

For $\partial \tau\subs{fail}^{\mathbf{C}}/\partial s$ as in (<ref>) we distinguish between its domain $\mathbf{C}_\blacktriangleright$ (where it depends on $a\subs{next}$) and $\mathbf{C}_\blacksquare$ (where it does not). We hence define the following arrays which are both invariant with $a\subs{next}$:

\begin{align}
W^{\mathbf{C}_\blacktriangleright}(\hat{t},\hat{s}) &= \sqrt{-a\subs{next}}\,W(\hat{t}, \hat{s}) \left.\frac{\partial \tau\subs{fail}^{\mathbf{C}}}{\partial s}\right|_{\hat{t},\hat{s}} \!\!\!\!\!= \frac{W(\hat{t}, \hat{s})}{\sqrt{2\,v_0\,\hat{t}-2\,\hat{s}}}
\\
\text{and } W^{\mathbf{C}_\blacksquare}(\hat{t},\hat{s}) &= W(\hat{t},\hat{s}),
\end{align}

which can be accumulated to give the antiderivatives

\begin{equation}
\mathcal I^{\mathbf{C}_\blacktriangleright}(\hat{t},\hat{s}) = \!\sum_{\mathclap{ \set{ s \in \hat S(t) \,|\, s \leqslant \hat{s} } }} W^{\mathbf{C}_\blacktriangleright}(\hat{t},s)
\;\,\text{and}\;\,
\mathcal I^{\mathbf{C}_\blacksquare}(\hat{t},\hat{s}) = \!\sum_{\mathclap{ \set{ s \in \hat S(t) \,|\, s \leqslant \hat{s} } }} W^{\mathbf{C}_\blacksquare}(\hat{t},s).
\end{equation}

These can then be used to evaluate a given $a\subs{next}$: For the case of $\sigma(t, t\subs{fail}, a\subs{next})$ lying entirely within $\mathbf{C}_\blacktriangleright$ for all $t\subs{fail} \in [t\subs{valve}, t\subs{plan}]$, we have

\begin{equation}
\begin{aligned}
P^{\mathbf{C}_\blacktriangleright}(\hat{t}, a\subs{next}) = \frac{1}{\sqrt{-a\subs{next}}}
\,
\bigg(\;\;\;&\mathcal I^{\mathbf{C}_\blacktriangleright}(\hat{t}, \sigma(\hat{t}, \hat{t}\subs{plan}, a\subs{next}))\\[-7pt]
- &\mathcal I^{\mathbf{C}_\blacktriangleright}(\hat{t}, \sigma(\hat{t}, \hat{t}\subs{valve}, a\subs{next})) \;\;\;\bigg).
\end{aligned}
\label{eq:p-C-play}
\end{equation}

For the case of $\sigma(t, t\subs{fail}, a\subs{next})$ lying entirely within $\mathbf{C}_\blacksquare$ for all $t\subs{fail} \in [t\subs{valve}, t\subs{plan}]$, we have

\begin{equation}
\begin{aligned}
P^{\mathbf{C}_\blacksquare}(\hat{t}, a\subs{next}) = \frac{1}{v_0}
\,
\bigg(\;\;\;&\mathcal I^{\mathbf{C}_\blacksquare}(\hat{t}, \sigma(\hat{t}, \hat{t}\subs{plan}, a\subs{next}))\\[-7pt]
- &\mathcal I^{\mathbf{C}_\blacksquare}(\hat{t}, \sigma(\hat{t}, \hat{t}\subs{valve}, a\subs{next})) \;\;\;\bigg)\,.
\end{aligned}
\label{eq:p-C-stop}
\end{equation}

For cases which transition between $\mathbf{C}_\blacktriangleright$ and $\mathbf{C}_\blacksquare$ by crossing $[s_\blacksquare, t_\blacksquare]$ (cf. Fig. <ref>), we evaluate each side separately.

§ PRACTICAL RESULTS

The algorithm was evaluated on different platforms using different parameters. For the risk predictions in $W(t,s)$, both purely synthetic noise fields were used, as well as data from simulated traffic scenarios.

(-13,9.5)2cm4.8 ms
(18,21)2cm41.2 ms
(18,14)2cm10.9 ms
(50,25)2cm87.7 ms
(50.00, 20)2cm30.6 ms
(82.00, 31)2cm272.1 ms

(34.5, -5)2cmi5-470UM
(66.00, -5)2cmRaspberry Pi 3

(-17.00, 11)1cm$10\,\unit{ms}$
(-17.00, 23)1cm$100\,\unit{ms}$
(-17.00, 35)1cm$1000\,\unit{ms}$

Worst-case execution times on different systems on a logarithmic scale. The proposed algorithm reduces the computation time consistently to about 11 % of that of the direct computation.

(40.00, -5.00)1cm$20\,\unit{m/s}$
(66.00, -5.00)1cm$30\,\unit{m/s}$

(-17.00, 7.60)1cm$10\,\unit{ms}$
(-17.00, 15.20)1cm$20\,\unit{ms}$
(-17.00, 22.80)1cm$30\,\unit{ms}$
(-17.00, 30.40)1cm$40\,\unit{ms}$

Execution times on an i7-2600 over different $v_0$, for a maximum allowed braking distance of 100 m (dashed) and 200 m (solid). A shorter maximum distance reduces computation times at high $v_0$ because gentler decelerations (causing longer braking distances) can be ruled out a-priori.

Comparison of execution times between the proposed algorithm (green) and the direct computation by various relevant factors.

(14,25.5)1cm $0.0$
(14,34.5)1cm $0.5$
(14,43.5)1cm $1.0$
(23,43.5)1cm $W(t,s)$

Result at $v_0 = 30\,\unit{\frac{m}{s}}$: To avoid high values of $W(t,s)$, the optimal choice is to maintain the valve setting at $a\subs{prev} = a\subs{next} = -5.5\,\unit{m/s^2}$. Since the valve does not transition, B is empty; the width of the enclosed area only results from the vehicle motion within $[t\subs{now},t\subs{plan}]$, corresponding to set C.

(14,25.5)1cm $0.0$
(14,34.5)1cm $0.5$
(14,43.5)1cm $1.0$

(23,43.5)1cm $W(t,s)$

Result at $v_0 = 15\,\unit{\frac{m}{s}}$: The algorithm decides to transition from $a\subs{prev} = -2.2\,\unit{m/s^2}$ to $a\subs{next} = -3.0,\unit{m/s^2}$ to avoid the high penalties in areas (dashed ellipses). The largest area of $W(t,s)$ is swept by the transition; in contrast to (a), later $t\subs{fail}$ produce shorter stopping distances as the stronger deceleration outweighs the vehicle's motion at $v_0$.

(-5.00, 5.00)1cm$0\,\unit{m}$
(7.50, 3.50)1cm$10\,\unit{m}$
(20.00, 2.00)1cm$20\,\unit{m}$
(32.50, 0.50)1cm$30\,\unit{m}$

(30.50, -5)1cm$s$

(45.00, -1.00)1cm$40\,\unit{m}$
(57.50, -2.50)1cm$50\,\unit{m}$
(70.00, -4.00)1cm$60\,\unit{m}$

(84.00, 1.00)1cm$0\,\unit{s}$

(87.40, 9.40)1cm$2\,\unit{s}$
(90.80, 17.80)1cm$4\,\unit{s}$

(98, 21)1cm$t$

(94.20, 26.20)1cm$6\,\unit{s}$
(97.60, 34.60)1cm$8\,\unit{s}$

(101.00, 43.00)1cm$10\,\unit{s}$

Result of (a), with $t\subs{fail}$ on a separate axis: The transition area B is wide, but swept only for $0.008\,\unit{s}$ or $3.2\,\%$ of the time, due to the relatively minor change in valve state. It overlaps with the area C of constant motion with $v_0$, such that along $t\subs{fail}$, several stopping distances $s$ are attained twice, once if the valve fails in transition, and once after it has reached $a\subs{next}$.

Examples of optimization results on Brownian noise fields, as used in [19], illustrating the effects of constant vehicle motion (a) if the valve state is not changed; and of valve transition (b, c) leading to reoccurring stopping distances at increasing $t\subs{fail}$. Results of realistic traffic scenarios can be found in [11].

To verify the results and to relate computation speeds, the proposed implementation described here was compared to a direct solution of (<ref>): To achieve comparable result quality, $t\subs{fail}$ was discretized into $\hat{T}\subs{fail}$ such that, during a transition, any intermediate arc length $\hat{s} \in \hat S$ is evaluated at least once, as is the case with the proposed algorithm. For each $a\subs{next} \in \hat A$, the direct solver computes and minimizes $\langle P \rangle$ by (<ref>), and therein computes $P(..., t\subs{fail}, ...)$ by (<ref>); the proposed solver precomputes $\mathcal I^{\mathbf{B}}$, $\mathcal I^{\mathbf{C_\blacktriangleright}}$,  $\mathcal I^{\mathbf{C_\blacksquare}}$, and then for each $a\subs{next} \in \hat A$, computes and minimizes $P^{\mathbf{B}}+P^{\mathbf{C}_\blacktriangleright}+P^{\mathbf{C}_\blacksquare}$, accumulated over $\hat{t}$, via (<ref>), (<ref>), (<ref>).

Experimental results using random Brownian noise fields for $W(t,s)$ (cf. Fig. <ref>) and simulated traffic scenarios (in [11]), show that the numerical results of both approaches agree within the numerical tolerance, such that both can, in particular, be used equivalently to obtain the optimization result $a^*$. The key goal of the proposed optimization method, however, is to achieve a significant decrease in computational effort with respect to the direct solution.

Formally we note that the algorithms have fundamentally different worst-case complexities: If memory for $W(t,s)$ is not considered, the direct algorithm can work with negligible space, opposed to the proposed algorithm that stores several intermediate results of size $\mathcal O(|\hat S|\,|\hat T|)$. However, the computation time is considerably higher to achieve accurate results: The effort of $\mathcal O(|\hat A| \, |\hat T| \, |\hat{T}\subs{fail}|)$ corresponds to $\mathcal O(|\hat A|\, |\hat T| \, |\hat S|)$, if $|T\subs{fail}|$ is chosen to provide accurate results as described above; in contrast, the proposed algorithm provides the accurate global solution at $\mathcal O(|\hat S|\,|\hat T|+|\hat A|\,|\hat T|)$.

In practical scenarios, this corresponds to an approximate average factor of $8$ in computation time between the proposed and the direct solver (or a reduction of about 89 %, Fig. <ref>) when tested on an Intel i7-2600 processor (at 3.4 GHz base clock speed and 3.8 GHz turbo clock speed), an Intel i5-470UM processor (1.33 GHz base, 1.86 GHz turbo) and an ARM Cortex A53 (1.2 GHz in a Raspberry Pi 3). Besides the stated complexity parameters, effort also depends on vehicle speed $v_0$: At a given maximum braking distance $s\subs{max}$, gentler decelerations can be ruled out at higher speeds, since their trajectories would exceed $s\subs{max}$. Either solver can considerably reduce effort by truncating the search space this way, as shown in Fig. <ref>.

§ CONCLUSION AND OUTLOOK

We have presented the problem of planning an optimal decision for a fail-safe emergency stop system, which can adjust a single hydraulic parameter $a\subs{next}$ that governs the braking deceleration in the event of a failure. This predictive approach allows to adapt the deceleration to the environment of the automated vehicle, and yet does not require any E/E components after failure. Optimization of $a\subs{next}$ has to take into account that the exact time of failure is unknown, leading to uncertainty even about the resulting deceleration due to transition times of hydraulic valve, and thereby to a complex planning task; at the same time, planning must be lightweight since it only serves a system of last resort. Based on these considerations, we have posed a suitable problem model, and restated it to enable efficient precomputation. The implementation provides accurate, globally optimal solutions, yet at only about $\nicefrac{1}{8}$ of the computational effort of a direct solver.

§.§ Outlook

So far, the algorithm's performance was evaluated only on a limited set of scenarios; a more exhaustive evaluation with different vehicle models, traffic scenarios and regular planning / prediction systems is required.

The algorithm yields a globally optimal $a^*$ conditioned on the failure within $[t\subs{now}, t\subs{next}]$ the current situation; this can, however, still lead to non-optimal results outside of the given model: Accelerations applied upon $t\subs{fail}$ in the present model only are included as added longitudinal uncertainties, like sensor and road friction uncertainties. An explicit acceleration model is left for future work. Also, a vehicle approaching a railway crossing or intersection would, initially, pick increasing decelerations $a^*$, until a safe stop before the crossing cannot be assured; then it would switch to gentler $a^*$, to clear the crossing before stopping. This leads to a systematic, artifactual selection of marginal decelerations, in between which possible failure trajectories necessarily lie on the crossing; it can be resolved by extending the proposed solver to anticipate future planning intervals.

The present algorithm handles the unknown interval of $t\subs{fail}$ by precomputing antiderivatives; a different approach to reduce the computational effort is to modify the underlying system such that the braking pressure can only be released at several discrete (instead of continuous) $t\subs{fail}$; thereby, no contiguous areas must be evaluated but discrete trajectories.

With the increasing use of GPUs for perception, prediction and planning tasks, it may be desirable to locate the emergency maneuver planner there as well. The highly parallel structure of the proposed solution suggests that an even more lightweight (with respect to other tasks) GPU implementation is possible, and should be evaluated.

[1]
M. Werling and D. Liccardo, “Automatic collision avoidance using
model-predictive online optimization,” in 2012 IEEE 51st IEEE
Conference on Decision and Control (CDC).1em plus 0.5em minus
0.4emIEEE, 2012. doi: 10.1109/CDC.2012.6426612 pp. 6309–6314.

[2]
S. Magdici and M. Althoff, “Fail-safe motion planning of autonomous
vehicles,” in 2016 IEEE 19th International Conference on Intelligent
Transportation Systems (ITSC).1em plus 0.5em minus 0.4emIEEE,
2016. doi: 10.1109/ITSC.2016.7795594 pp. 452–458.

[3]
C. Frese and J. Beyerer, “A comparison of motion planning algorithms for
cooperative collision avoidance of multiple cognitive automobiles,” in
2011 IEEE Intelligent Vehicles Symposium (IV).1em plus 0.5em
minus 0.4emIEEE, 2011. doi: 10.1109/IVS.2011.5940489 pp. 1156–1162.

[4]
A. Reschka, “Safety concept for autonomous vehicles,” in Autonomous
Driving.1em plus 0.5em minus 0.4emSpringer, 2016, pp.

[5]
M. Ruf, J. Ziehn, D. Willersinn, B. Rosenhahn, J. Beyerer, and H. Gotzig,
“Global Trajectory Optimization on Multilane Roads,” in
18$^{\text{th}}$ IEEE International Conference on Intelligent
Transportation Systems (ITSC), Sep. 2015. doi: 10.1109/ITSC.2015.309

[6]
I. Takahashi, T. Nguyen, H. Kanamori, T. Tanaka, S. Kato, Y. Ninomiya,
E. Takeuchi, T. Nakagawa, M. Makiguchi, and H. Aoki, “Automated safety
vehicle stop system for cardiac emergencies,” in 2016 IEEE
International Conference on Emerging Technologies and Innovative Business
Practices for the Transformation of Societies (EmergiTech).1em plus
0.5em minus 0.4emIEEE, 2016. doi: 10.1109/EmergiTech.2016.7737302 pp.

[7]
J. Becker, M.-B. A. Colas, S. Nordbruch, and M. Fausten, “Bosch’s vision and
roadmap toward fully autonomous driving,” in Road vehicle
automation.1em plus 0.5em minus 0.4emSpringer, 2014, pp.

[8]
T. Ishigooka, S. Honda, and H. Takada, “Cost-Effective Redundancy
Approach for Fail-Operational Autonomous Driving System,” in 2018
IEEE 21st International Symposium on Real-Time Distributed Computing
(ISORC), May 2018. doi: 10.1109/ISORC.2018.00023. ISSN 2375-5261 pp.

[9]
J. Kim, R. Rajkumar, and M. Jochim, “Towards dependable autonomous driving
vehicles: a system-level approach,” ACM SIGBED Review, vol. 10,
no. 1, pp. 29–32, 2013. doi: 10.1145/2492385.2492390

[10]
A. Reschka, J. R. Böhmer, T. Nothdurft, P. Hecker, B. Lichte, and
M. Maurer, “A surveillance and safety system based on performance criteria
and functional degradation for an autonomous vehicle,” in 2012 15th
International IEEE Conference on Intelligent Transportation Systems, Sep.
2012. doi: 10.1109/ITSC.2012.6338682. ISSN 2153-0009 pp. 237–242.

[11]
J. Ziehn, J. Doll, F. Duerr, M. Frey, F. Gauterin, S. Hohmann, E. Knoch,
R. Kohlhaas, A. Lauber, F. Pistorius, M. Roschani, M. Ruf, E. Sax, and
S. Strasser, “General Fail-Safe Emergency Stopping for Highly Automated
Vehicles,” in 9. Tagung Automatisiertes Fahren.1em plus 0.5em
minus 0.4emMunich: Lehrstuhl für Fahrzeugtechnik mit TÜV
SÜD Akademie, 2019.

[12]
K. Kant and S. W. Zucker, “Toward efficient trajectory planning: The
path-velocity decomposition,” The International Journal of Robotics
Research, vol. 5, no. 3, pp. 72–89, 1986. doi: 10.1177/027836498600500304

[13]
S. Weissenbach, “Konzeption und Aufbau eines Funknothaltesystems für
autonom fahrende Elektrofahrzeuge,” Master's thesis, Karlsruhe Institute of
Technology KIT, Karlsruhe, Germany, Sep. 2017.

[14]
J. Ziegler, P. Bender, T. Dang, and C. Stiller, “Trajectory planning for
Bertha – A local, continuous method,” in Proceedings of the 2014
IEEE Intelligent Vehicles Symposium (IV), Dearborn, June 2014. doi:
10.1109/IVS.2014.6856581 pp. 450–457.

[15]
M. Ruf, J. Ziehn, B. Rosenhahn, J. Beyerer, D. Willersinn, and H. Gotzig,
“Situation Prediction And Reaction Control (SPARC),” in 9.
Workshop Fahrerassistenzsysteme (FAS 2014), B. Färber, K. Dietmayer,
K. Bengler, M. Maurer, C. Stiller, and H. Winner, Eds., Walting im
Altmühltal, Germany, March 2014. ISBN 978-3-00-044955-0 pp. 55–66.

[16]
M. Ruf, “germanGeometrie und topologie von
trajektorienoptimierung für vollautomatisches fahren,” Ph.D. dissertation,
Karlsruher Institut für Technologie (KIT). ISBN 978-3-7315-0832-8 2018.

[17]
M. Ruf, J. Ziehn, B. Rosenhahn, J. Beyerer, D. Willersinn, and H. Gotzig,
“Evaluation of an Analytic Model for Car Dynamics,” in
International Conference on Mechatronics and Control (ICMC), Jinzhou,
China, Jul. 2014. doi: 10.1109/ICMC.2014.7232008 pp. 2446–2451.

[18]
M. Ruf, J. Ziehn, D. Willersinn, B. Rosenhahn, J. Beyerer, and H. Gotzig,
“Comparison of Local vs. Global Optimization for Trajectory Planning in
Automated Driving,” in 10. Workshop Fahrerassistenzsysteme (FAS
2015), C. Stiller, K. Bengler, K. Dietmayer, L. Eckstein, B. Färber,
M. Maurer, and H. Winner, Eds., Walting im Altmühltal, Germany, Sep.
2015. ISBN 978-3-00-050746-5 pp. 71–83.

[19]
F. Duerr, “Notbremssystem für Systemausfälle im vollautomatischen
Fahren,” Master's thesis, Karlsruhe Institute of Technology KIT, Fraunhofer
IOSB, Karlsruhe, Germany, Apr. 2018.