Fast Nonlinear Risk Assessment for Autonomous Vehicles Using Learned Conditional Probabilistic Models of Agent Futures

Predictions are usually given in a global frame, so this section provides a method for transforming the global frame distribution moments into the ego vehicle frame. More generally, we are concerned with computing moments of $\mathbf{x}_{t}$ in a new frame offset by $\mathbf{v}\in\mathbb{R}^{2}$ and rotated by $-\theta\in\mathbb{R}$. As shown in the appendix, if $\mathbf{x}_{t}$ is a mixture model, its moments can be computed in terms of moments of its components, so, in this section, let $\mathbf{x}_{t}$ be a component of a mixture model. We propose only translating the moments and then accounting for the rotation by using $Q^{*}=R(\theta)^{T}QR(\theta)$ instead of $Q$. The rotation can be accounted for by using $Q^{*}$ instead of $Q$ because:

The translated moments can be computed by applying the binomial theorem to $(\mathbf{x}_{t}-\mathbf{v})^{n}$ (here, the power is applied element-wise). Note that applying the binomial theorem to $(\mathbf{x}_{t}-\mathbf{v})^{n}$ requires moments of $\mathbf{x}_{t}$ up to order $n$.

In this section, we provide a method to solve the risk assessment problem when the uncertain prediction is represented as a sequence of GMMs, $\mathbf{x}_{t}$, of the agents position with discrete modes determined by the Multinoulli $Z_{t}$. Many works currently learn GMMs for vehicle position as they express both multi-modal and continuous uncertainty [1, 8, 6]. As shown in Figure 2, they provide an intuitive representation of uncertainty in both the drivers high level decisions and low level execution. With time independence, the risk is:

Note that the above expression can be easily modified for the case when there is a single Multinoulli random variable that is constant across all time, an assumption used in, for example, [1]. The probabilities $\mathbb{P}(Z_{t}=z)$ are learned parameters of GMMs, so the problem of risk assessment can be solved by computing $\mathbb{P}\left(Q(\mathbf{x}_{t})\leq 1:Z_{t}=z\right)$ for each agent, time step, and mode. Note that this is exactly the CDF of $Q(\mathbf{x}_{t})$ conditioned on $Z_{t}=z$ which is a quadratic form in a multivariate Gaussian (QFMVG). Unfortunately, there does not exist a known closed form solution to exactly evaluate the CDF of QFMVGs, but fast approximation methods with bounded errors have been studied within the statistics community [32, 33, 34, 35, 36]. Several of these methods have been implemented in the R package CompQuadForm [37]. Of particular interest is the method of Imhof, which produces results with bounded approximation error by numerical inversion of the characteristic function of the QFMVG [36]. A faster, but less accurate, alternative is the method of Liu-Tang-Zhang which involves approximating the CDF of the QFMVG with the CDF of a non-central chi square distribution with parameters chosen to minimize the difference in kurtosis and skew between the approximate and target distributions [32].

As a consequence of the one-tailed Chebyshev’s Inequality, for any measurable function $g$, whenever $\mathbb{E}[g(\mathbf{x}_{t})]>0$, we have that:

That is, the first two moments of $g(\mathbf{x}_{t})$ are sufficient to establish a bound on the risk that the constraint $g(\mathbf{x}_{t})\leq 0$ is violated. We note that the requirement $\mathbb{E}[g(\mathbf{x}_{t})]>0$ is not particularly restrictive because $\mathbb{E}[g(\mathbf{x}_{t})]\leq 0$ means the average case involves collision, thus corresponding to what is usually an unacceptable level of risk.

When tighter risk bounds are desired than those obtained via Chebyshev’s Inequality, for any measurable function $g$, an univariate SOS program can be used to upper-bound $\mathbb{P}(g(\mathbf{x}_{t})\leq 0)$ – the SOS program is univariate in the sense that it searches for a polynomial in a single indeterminant, not in the sense that there is only one decision variable [28]. The fact that the SOS program is univariate is significant because the key disadvantages of SOS, scalability and conservatism, are not as limiting for univariate SOS because: 1) the number of decision variables in the resulting SDP scales quadratically w.r.t. the order of the polynomial we are searching for and 2) the set of nonnegative univariate polynomials is equivalent to the set of univariate SOS polynomials, allowing univariate SOS to explore the full space of possible solutions.

We begin by noting that the probability of constraint violation is equivalent to the expectation of the indicator function of the sub-level set of $g$:

where, $pr(\mathbf{x}_{t})$ is probability density function of $\mathbf{x}_{t}$ and $\mathbf{1}_{g(\mathbf{x}_{t})\leq 0}$ is the indicator function of the sub-level set of $g$ defined as $\mathbf{1}_{g(\mathbf{x}_{t})\leq 0}=1$ if $x\in\{\mathbf{x}_{t}:g(\mathbf{x}_{t})\leq 0\}$, and 0 otherwise.

The expectation of the indicator function, however, is not necessarily easily computable. To solve this problem, we find some polynomial with a more easily computable expectation that upper bounds the indicator function. If we can find some univariate polynomial, $p:\mathbb{R}\rightarrow\mathbb{R}$ of order $d$ in some indeterminant $x\in\mathbb{R}$ with coefficients $c_{k},k=0,...,d$ that upper bounds the indicator function, then clearly the following implication holds by substitution:

Given the coefficients $c_{k}$, if we apply the expectation w.r.t. the density function of $\mathbf{x}_{t}$ to both sides, then we can reduce the problem of finding an upper bound on $\mathbb{P}(g(\mathbf{x}_{t})\leq 0)$ to that of computing moments of the random variable $g(\mathbf{x}_{t})$:

where, $\mathbb{E}[g(\mathbf{x}_{t})^{k}]$ is the moment of order $k$ of random variable $g(\mathbf{x}_{t})$. The moments of $g(\mathbf{x}_{t})$, in turn, are computable in terms of the moments of $\mathbf{x}_{t}$, i.e., $\mathbb{E}[\mathbf{x}_{t}^{\alpha}],\alpha\in\mathbb{N}$, by expanding out the polynomial power and applying the linearity of expectation, [28]. For example, if $g(\mathbf{x}_{t})=x_{t}^{2}+y_{t}^{2}$, then:

The moments of $\mathbf{x}_{t}$ can be computed using the moment generating function as in (1). In this section, we assume that we know the necessary moments of $\mathbf{x}_{t}$ to compute the moments $\mathbb{E}[g(\mathbf{x}_{t})^{k}],\forall k\in[d]$. We also normalize the moments, as doing so improves the numerical conditioning of the problem ¹¹1normalization is valid because $\mathbb{P}(X\leq 0)=\mathbb{P}(cX\leq 0)$ for $c>0$.

Now consider the following univariate SOS program in the indeterminant $x$ which can search for upper bound polynomial indicator function, i.e., $p(x):=\sum_{k=0}^{d}c_{k}x^{k}\geq\mathbf{1}_{x\leq 0}$, which minimizes the upper bound on risk [28]:

If the order of the polynomial is chosen to be $d=2n$ for some $n\in\mathbb{N}$, then we should have that $\text{deg}(s_{1})=d$ and $\text{deg}(s_{2})=d-2$. If $d=2n+1$ for some $n\in\mathbb{N}$, then we should have that $\text{deg}(s_{1})=2n$ and $\text{deg}(s_{2})=2n$. Note that constraints (18b) and (18c) are the nonnegativity constraints of the indicator function $\mathbf{1}_{x\leq 0}$, i.e., $\mathbf{1}_{x\leq 0}=1$ if $x\leq 0$, and otherwise $0$. More precisely, the constraint (18b) enforces:

Also, according to the constraint (18c), $p(x)$ is constrained to be SOS; So it is globally nonnegative, i.e, $p(x)\geq 0$. Hence, polynomial $p(x)$ is an upper bound polynomial approximation of the indicator function, i.e., $p(x)\geq\mathbf{1}_{x\leq 0},\forall x\in\mathbb{R}$. Thus, according to (15) and (16), the optimal objective value of this SOS program yields an upper bound on $\mathbb{P}(g(\mathbf{x}_{t})\leq 0)$.

Also, note that SOS optimization $(18)$ is a convex optimization. More precisely, it has a linear cost function in terms of the coefficients of the polynomial $p(x)$ and convex constraints in the form of linear matrix inequalities in terms of the coefficients of the polynomials $p(x),s_{1}(x)$, and $s_{2}(x)$. Hence, we can solve the SOS optimization efficiently using the off-the-shelf LMI optimization solvers.

Remark 1: We can solve the optimization in $(18)$ to obtain the polynomial indicator function i.e., $p(x)=\sum_{k=0}^{d}c_{k}x^{k}$, in the offline step. Hence, we can compute the upper bound of the risk in terms of the obtained polynomial indicator function and moments of the uncertain states of the agent vehicle, in real-time, i.e., $\mathbb{P}(g(\mathbf{x}_{t})\leq 0)\leq\mathbb{E}[p(g(\mathbf{x}_{t}))]=\sum_{k=0}^{d}c_{k}\mathbb{E}[g(\mathbf{x}_{t})^{k}].$ For more information see [28] and https://github.com/jasour/Nonlinear-Risk-Assessment

Remark 2: The provided non-Gaussian risk assessment approaches in Sections IV.C and IV.D are less conservative than the existing non-sampling based methods. This is because, we are using higher order moments of uncertainties and also tight polynomial approximation of the indicator functions of safety constraints. One can improve the risk assessment results by increasing the number of the moments in SOS programming based approach.

To show how our moment propagation algorithm works, we begin by showing how the dynamics of the first order moment for the state $x_{t}$ in system $(\ref{eq:dynamics})$ can be found manually, i.e., mapping between $\mathbb{E}[x_{t+1}]$ and $\mathbb{E}[x_{t}]$. Our proposed algorithm is essentially an automated version of this process. By substituting the equations $(\ref{eq:dynamics})$ in and applying the linearity of expectation, we arrive at the dynamics of the moment:

To complete the obtained mapping between $\mathbb{E}[x_{t+1}]$ and $\mathbb{E}[x_{t}]$, we need to compute the update rule for the term $\mathbb{E}[v_{t}cos(\theta_{t})]$. By substituting the equations $(\ref{eq:dynamics})$ in and applying the linearity of expectation, we arrive at the update rule of the moment as:

Similarly, we can obtain the dynamics of the higher order moments of the uncertain position, i.e., $\mathbb{E}[x^{\alpha}_{t}y^{\beta}_{t}]$. This process, however, is tedious and is easily subject to human error, especially for larger moment orders $\alpha,\beta$. To address these issues, in [43, 44], we developed TreeRing algorithm that uses a dependency graph to identify all the slack moment states needed to construct the moment dynamical systems.

In this paper, we use a different approach and provide a general framework to construct the moment dynamical systems. The main idea is to transform the nonlinear stochastic Dubbin’s model in (20) into a equivalent new augmented linear-state system. In this case, due to the linear relation of the states of the augmented linear-state system at time $t$ and $t+1$, moments of order $\alpha$ of the states at time $t+1$ can be described only in terms of the moments of order $\alpha$ of the states at time $t$. Hence, we do not need to look for a set of slack moment states as described in (21).

In [42], we show that nonlinear stochastic motion dynamics can be transformed in to equivalent linear-state dynamical systems by introducing suitable new state variables. In this section, we define such equivalent augmented linear-state system for stochastic nonlinear Dubin’s model (20) as follows:

where $\mathbf{x}_{aug}=[x,y,vcos(\theta),vsin(\theta),cos(\theta),sin(\theta)]^{T}$ is the augmented state vector in terms of the position $(x,y)$ and a set of nonlinear functions of the states of the original nonlinear model in (20). Also, matrix $A_{t}(\omega_{\theta_{t}},\omega_{v_{t}})$ is defined only in terms of the nonlinear functions of the uncertain control inputs $\omega_{\theta_{t}}$ and $\omega_{v_{t}}$ as follows:

Note that the obtained augmented system in (22) is linear in terms of the states $\mathbf{x}_{aug}$ and also equivalent to the original stochastic nonlinear Dubin’s model in (20). We use the obtained augmented linear-state system in (22) to obtain moment-state linear dynamical systems that govern the exact time evolution of the moments of the uncertain position states $(x,y)$ in nonlinear stochastic system (20).

We define the moment-state linear systems for the obtained augmented linear-state system in (22) as follows [42]:

where, $\mathbb{E}[\mathbf{x}_{aug_{t}}^{\alpha}]$ is the vector of all moments of order $\alpha$ of the augmented state vector $\mathbf{x}_{aug}$ and $A_{mom_{\alpha}}$ is a matrix in terms of the moments of the uncertain control inputs. To construct the moment-state linear system in (23), we need the expected values of the monomials of order $\alpha$ of the vector $\mathbf{x}_{aug_{t+1}}$, e.g., $\mathbb{E}[x_{aug_{t+1}}^{\alpha}]$. Since $\mathbf{x}_{aug_{t+1}}$ is a linear function of $\mathbf{x}_{aug_{t}}$ as in (22), we can describe the moments of order $\alpha$ of $\mathbf{x}_{aug_{t+1}}$ completely in terms of the moments of order $\alpha$ of $\mathbf{x}_{aug_{t}}$. Hence, we do not need to look for a set of slack moment states as described in the motivating example of Section V-A.

For example, we obtain the moment-state linear system of order $\alpha=1$ of the form $\mathbb{E}[\mathbf{x}_{aug_{t+1}}]=A_{{mom_{1}}_{t}}\mathbb{E}[\mathbf{x}_{aug_{t}}]$ where

is the vector of all moments of order $\alpha=1$ of $\mathbf{x}_{aug_{t}}$. Also, matrix $A_{{mom_{1}}_{t}}$ is described in terms the first order moments of uncertain control input $\omega_{v_{t}}$ and first order trigonometric moments of uncertain control input $\omega_{\theta_{t}}$ as follows:

We can compute the moments of uncertain control inputs in terms of the characteristic functions as shown in (1) and Appendix.B. The obtained first order moment-state linear dynamical system describes the exact time evolution of the first order moments of the uncertain position $(x,y)$ of the original stochastic nonlinear system in (20). Similarly, we can obtain the moment-state linear system of the form (23) for higher moment order $\alpha$ to describe the exact time evolution of the higher order moments of the uncertain position.

Note that we can construct the deterministic linear moment-state systems of the form (23) for different moment order $\alpha$ in the offline step. Hence, we can use the obtained moment systems to propagate the moments of the uncertain initial states over the planning horizon in real-time. More precisely, given initial moments $\mathbb{E}[\mathbf{x}_{aug_{0}}^{\alpha}]$ and $A_{{mom_{\alpha}}_{t}}|_{t=0}^{N-1}$, the moments at time step $N$ can be obtained by recursion of $\mathbb{E}[\mathbf{x}_{aug_{t+1}}^{\alpha}]=A_{{mom_{\alpha}}_{t}}\mathbb{E}[\mathbf{x}_{aug_{t}}^{\alpha}],t=0,...,N-1$. Similarly, we can describe the moments by the solution of the linear moment system as $\mathbb{E}[\mathbf{x}_{aug_{N}}^{\alpha}]=\Pi_{t=0}^{N-1}A_{{mom_{\alpha}}_{t}}\mathbb{E}[\mathbf{x}_{aug_{0}}^{\alpha}]$.

Remark 3: The provided approach is not limited to the motion dynamics in $(20)$. We can use different motion dynamics with different sources of uncertainties to model the uncertain behaviour of the agent vehicles. For more information see [42] and https://github.com/jasour/Uncertainty-Propagation

The input consists of observed vehicle positions $\mathbf{x}_{-19:0}$ for the past 20 timesteps, as well as scene context $\mathbf{c}$ representing driving context such as lanes coordinates $\mathbf{c}_{M}$ and the future trajectory of the ego car $\mathbf{c}_{E}$. Since the observed trajectory is usually collected in global coordinate, which can lead to bias in prediction, we normalize the past trajectory of the target vehicle, so that the last and first observed positions are at origin and the $x$-axis, respectively, as shown in Figure 3.

The conditioning scene context is represented by a set of coordinates of the lanes that are close to the target vehicle (e.g., within 50 meters) and also the future planned trajectory of the ego car. The coordinates of the lanes and the ego car trajectory are normalized into the local coordinate of the target car. Such scene context provides additional cues on the possible future location of the target vehicle. Note that these contexts are usually not available across all driving platforms. Thus, we design a prediction model that is flexible in terms of the inputs, as we show in Section VI-C.

The output $Y$ consists of a sequence of GMM parameters over the prediction horizon $T$ as follows:

The encoder-decoder predictor is shown in Figure 4. The encoder is a sequence of LSTM units taking observed trajectories of the target agent and scene context as input and outputs a latent vector encoding agent hidden state. The input modalities are processed with separate multilayer perceptrions (MLPs) before being fed into the LSTM unit. This allows our model to handle different input options given their availability. The decoder is also a sequence of LSTM units that takes the latent vector and generates a set of GMM parameters from each LSTM unit with MLPs. For simplicity, we use three component mixture models. For each component, we generate a weight value, a mean vector, and a covariance matrix representing uncertainties of predictions.

Given the prediction $Y$ and the groundtruth position or control values $\hat{Y}$, the loss function is composed of 2 terms as follows:

where $\mathcal{L}_{L2}$ measures the $L_{2}$ loss between the predicted mean values and ground truth observations over the future trajectories, $\mathcal{L}_{NLL}$ measures the negative log-likelihood loss between the predicted distributions and groundtruth observations, and $\alpha$ is the weight coefficient for the $L_{2}$ loss.

Let $f_{X}$ denote the pdf of a $K$-component mixture model $X$, with pdf components $f_{X_{i}},\forall i\in[K]$ and let $f_{Z}$ denote the pdf of the $K$ category Multinoulli. Then, by definition $f_{X}(x)=\sum_{i=1}^{m}f_{X_{i}}(x)f_{Z}(i)$. For any measurable function $g$, by interchanging the order of integration and summation, the following holds true

By letting $g(X)=X^{n}$ or $g(X)=e^{itX}$, the moments and characteristic function of $X$ can both be computed as the weighted sum of those of their components.

In this section, we show how trigonometric moments of the form $\mathbb{E}[\cos^{n}(X)]$, $\mathbb{E}[\sin^{n}(X)]$, and $\mathbb{E}[\cos^{m}(X)\sin^{n}(X)]$ can be computed in terms of the characteristic function of the random variable $X$, denoted by $\Phi_{X}$ [42, 43]. We begin by applying Euler’s Identity to the definition of the characteristic function as follows:

Thus, we have that $\mathbb{E}[\cos(tX)]=\text{Re}(\Phi_{X}(t))$ and $\mathbb{E}[\sin(tX)]=\text{Im}(\Phi_{X}(t))$. This immediately gives us the ability to compute the first moments of our trigonometric random variables. For higher moments, the trigonometric power formulas can be used to express quantities of the form $\cos^{n}(X)$ as the sum of quantities of the form $\cos(mX)$ where $m\in\mathbb{N}$ and similarly for $\sin^{n}(X)$ [48]. Thus, higher moments of $\sin(X)$ and $\cos(X)$ can be computed using $\Phi_{X}(t)$. More precisely, given $n\in\mathbb{N},$ trigonometric moments of order $n$ of the forms $\mathbb{E}[{cos}^{n}(X)]$ and $\mathbb{E}[{sin}^{n}(X)]$ reads as [42]:

where, $\binom{n}{k}=\frac{n!}{k!(n-k)!}$.