Intent-Aware Probabilistic Trajectory Estimation for Collision Prediction with Uncertainty Quantification

TL;DR

This paper introduces a probabilistic collision prediction method using Gaussian processes that accounts for uncertainty in obstacle motion, enabling more robust predictions in dynamic environments with limited information.

Contribution

It presents a novel Gaussian process-based approach for collision prediction that models obstacle intentions probabilistically, improving over deterministic methods.

Findings

Successfully predicts collisions with limited obstacle motion data.

Handles complex obstacle behaviors like overtaking and crossing paths.

Quantifies uncertainty in trajectory predictions.

Abstract

Collision prediction in a dynamic and unknown environment relies on knowledge of how the environment is changing. Many collision prediction methods rely on deterministic knowledge of how obstacles are moving in the environment. However, complete deterministic knowledge of the obstacles' motion is often unavailable. This work proposes a Gaussian process based prediction method that replaces the assumption of deterministic knowledge of each obstacle's future behavior with probabilistic knowledge, to allow a larger class of obstacles to be considered. The method solely relies on position and velocity measurements to predict collisions with dynamic obstacles. We show that the uncertainty region for obstacle positions can be expressed in terms of a combination of polynomials generated with Gaussian process regression. To control the growth of uncertainty over arbitrary time horizons, a probabilistic obstacle intention is assumed as a distribution over obstacle positions and velocities, which can be naturally included in the Gaussian process framework. Our approach is demonstrated in two case studies in which (i), an obstacle overtakes the agent and (ii), an obstacle crosses the agent's path perpendicularly. In these simulations we show that the collision can be predicted despite having limited knowledge of the obstacle's behavior.

Equations

$$F\sim \mathcal{G}\mathcal{P}(M,K),$$

$$F(T)\sim \mathcal{N}(M(T),K(T,T)).$$

$$p(F(t)|D,t)$$

$$\mu (t)$$

$${\sigma }^2(t)$$

$$-K(T,t{)}^{\top }(K(T,T)+{\Sigma }^2{)}^{-1}K(T,t),$$

$$k_f(t,t^{\top })$$

$$={\theta }_f^2\left[\frac{1}{3}{\text{ min}}^3(\tilde{t},{\tilde{t}}^{\top })+\frac{1}{2}\left|t-t^{\top }\right|{\text{ min}}^2(\tilde{t},{\tilde{t}}^{\top })\right],$$

$$F=\left[\begin{array}{c}f\\ f^{\prime }\end{array}\right]\sim \mathcal{G}\mathcal{P}\left(\left[\begin{array}{c}{\mu }_f\\ {\mu }_{f^{\prime }}\end{array}\right],\left[\begin{array}{cc}{k}_f& k_f^{\partial }\\ {}^{\partial }{k}_f& {}^{\partial }{k}_f^{\partial }\end{array}\right]\right),$$

$${}^{\partial (i)}{k}_f^{\partial (j)}=\frac{{\partial }^{i+j}{k}_f(t,t^{\top })}{\partial t^i\partial t^{\top j}}.$$

$$k_f^{\partial }(t,t^{\top })=$$

$${\theta }_f{\theta }_{f^{\prime }}\left[\mathbb{I}(t<t^{\top })t^2/2+\mathbb{I}(t\ge t^{\top })(t{t}^{\top }-t^{\top 2}/2)\right],$$

$${}^{\partial }{k}_f(t,t^{\top })=$$

$${\theta }_f{\theta }_{f^{\prime }}\left[\mathbb{I}(t^{\top }<t)t^{\top 2}/2+\mathbb{I}(t^{\top }\ge t)(t{t}^{\top }-t^2/2)\right],$$

$${}^{\partial }{k}_f^{\partial }(t,t^{\top })={\theta }_{f^{\prime }}^2\text{min}(t,t^{\top }),$$

$${\mathcal{B}}^{\delta }(t)=\{x\in \mathbb{R}:\beta (t)-x\le |\delta (t)|\}.$$

$$d_{\text{min}}(t,\alpha ,{\mathcal{B}}^{\delta })=\underset{b\in {\mathcal{B}}^{\delta }(t)}{\min }|\alpha (t)-b|,$$

$$I_x\sim \mathcal{N}({\mu }_{Ix},{\sigma }_{Ix}^2),I_y\sim \mathcal{N}({\mu }_{Iy},{\sigma }_{Iy}^2)$$

$$I_{x^{\prime }}\sim \mathcal{N}({\mu }_{I{x}^{\prime }},{\sigma }_{I{x}^{\prime }}^2),I_{y^{\prime }}\sim \mathcal{N}({\mu }_{I{y}^{\prime }},{\sigma }_{I{y}^{\prime }}^2),$$

$${\sigma }^2(t)=\left[\begin{array}{cc}{\sigma }_f^2(t)& {\sigma }_{f,f^{\prime }}^2(t)\\ {\sigma }_{f^{\prime },f}^2(t)& {\sigma }_{f^{\prime }}^2(t)\end{array}\right].$$

$${\Psi }_x^{2\sigma }(t)=\{x\in \mathbb{R}:{\mu }_x(t)-x\le |2{\sigma }_x(t)|\}\text{and}$$

$${\Psi }_y^{2\sigma }(t)=\{y\in \mathbb{R}:{\mu }_y(t)-y\le |2{\sigma }_y(t)|\},$$

$$\sqrt{d_{\text{min}}^2(t,{\psi }_x,{\Psi }_x^{2\sigma })+d_{\text{min}}^2(t,{\psi }_y,{\Psi }_y^{2\sigma })}>{\Delta }_{\text{safe}},$$

$$F=\left[\begin{array}{c}f\\ f^{\prime }\end{array}\right]\sim \mathcal{G}\mathcal{P}(\left[\begin{array}{c}{\mu }_f\\ {\mu }_{f^{\prime }}\end{array}\right],\left[\begin{array}{cc}{k}_f& k_f^{\partial }\\ {}^{\partial }{k}_f& {}^{\partial }{k}_f^{\partial }\end{array}\right]).$$

$$F_x$$

$$F_y$$

$$x\in {\mathbb{R}}^n,x^{\prime }\in {\mathbb{R}}^n,y\in {\mathbb{R}}^n\text{and}{y}^{\prime }\in {\mathbb{R}}^n.$$

$$\bar{x}$$

$$\overline{x^{\prime }}$$

$$\bar{y}$$

$$\text{and}\overline{y^{\prime }}$$

$$D_x$$

$$K=\left[\begin{array}{cc}{k}_f& k_f^{\partial }\\ {}^{\partial }{k}_f& {}^{\partial }{k}_f^{\partial }\end{array}\right].$$

$$k_f(\bar{T},t)$$

$$k_f(\bar{T},t)=$$

$${\theta }_f^2\left(\frac{1}{3}\left[\begin{array}{c}{t}_1^3\\ t_2^3\\ ⋮\\ t_n^3\\ t^3\end{array}\right]+\frac{1}{2}\left[\begin{array}{c}t-t_1\\ t-t_2\\ ⋮\\ t-t_n\\ t_I-t\end{array}\right]\left[\begin{array}{c}{t}_1^2\\ t_2^2\\ ⋮\\ t_n^2\\ t^2\end{array}\right]\right).$$

$${}^{\partial }{k}_f(\bar{T},t)$$

$${}^{\partial }{k}_f(\bar{T},t)=$$

Full text

I INTRODUCTION

When deploying autonomous systems that are safe, it is important to not only ensure the safety of the vehicle in a static environment, but also to define interactions with other agents. These agents can be cooperative or non-cooperative depending on each agent’s ability or desire to communicate and compromise. When agents are cooperative, we can take advantage of inter-agent communication to ensure that performance objectives are achieved. Methods that take advantage of agent cooperation are described by the authors of [1] and [2]. Even without cooperation, we expect an autonomous system to be able to execute a planned trajectory faithfully while avoiding collisions with other agents. A high level planner can re-plan around predicted collisions by adjusting the planned trajectory of the vehicle such that no collision is predicted. This planning is expected to occur after mission start and before a collision is imminent. Once collision is imminent, feedback-based methods can be used to guarantee collision avoidance, as found in [3] and [4].

Re-planning is necessary in a changing environment; however, to effectively re-plan, an expected future state of the environment is necessary. Without knowledge of these trajectories, an estimate must be constructed. Given deterministic knowledge of a vehicle’s trajectory, collision can be avoided using the methods described in [5], [6] and [7]. In this work, we consider a single obstacle and a single agent attempting to avoid the obstacle. Predicting the future behavior of an obstacle is difficult with partial or imperfect knowledge of the obstacle’s dynamics and intentions. With knowledge of the obstacle dynamics and control inputs, we can predict the obstacle’s future behavior accurately. If the control inputs are not known but the dynamics are well understood, then the future behavior can be quantified in terms of reachability sets as proposed in [8]. Even with knowledge of the obstacle dynamics, as the the prediction time horizon increases, the reachability set of many systems will grow to cover the entire space. Methods designed to predict collision using only this information tend to be very conservative, suitable for trajectory estimation on a short time scale. When even less information is known about the obstacle dynamics, the problem becomes increasingly difficult to solve.

To estimate the obstacle’s future behavior, without being overly conservative, the estimation method can include some knowledge of the agent’s intention. Methods of predicting the agent’s intention with Markovian models are given in [9], which uses a mixed observable Markov decision process, and in [10], which uses growing hidden Markov models. The authors of [9] consider an urban autonomous driving context, where pedestrian intentions need to be identified in real time based on past observations. The intentions are a finite set of points in the environment, and the planner assigns probabilities to each point. The authors of [10] extend the Markovian model in a way that allows both the probability of each intention and the set of intentions to be updated at every time-step.

Once the intention is established, probabilistic trajectory estimates can be generated in different ways, varying in complexity and dependence on the knowledge of obstacle dynamics. Some of the simplest are constant extrapolation, such as the constant heading and velocity assumptions as in [11]. These methods are fast, easy to implement and perform well in situations where the obstacle is assumed to be in steady-state. The intention of these obstacles is implicitly modeled as a desire to continue previous behavior. Other methods use the knowledge of obstacle dynamics to propagate the current state into the future, rather than constant extrapolation. An example of this type of method can be found in [12], in which state propagation requires knowledge of agent dynamics. The author of [12] estimates the obstacle’s intention using knowledge of its optimization method and cost function. With the assumption that the obstacle is attempting to minimize a certain cost function, the inverse of the optimization procedure is computed to predict the optimal trajectory. This trajectory is then used as the intention. In addition to model-based methods, there are data based methods that predict obstacle trajectories by propagating the obstacle state through Monte-Carlo methods, where the state propagation is learned from a large data-set of previous behaviors. These methods typically incur a large, upfront training cost which can be prohibitively expensive without a representative data-set. In [13], a pre-trained Bayesian dynamic network is assumed, and the prediction is performed by considering 100 simulations of the Bayesian network and around 20 look-ahead time steps. The method presented in [14] uses a large data-set of known pedestrian trajectories to create a velocity field map of the area in question. Trajectory estimation is then performed by using 250 particles. In these methods, the intention is encoded naturally by the data. Clustering the final locations of the particles would return intentions in the form of a distribution over particles. Each of these methods can be used for collision prediction, but the collision prediction step becomes more expensive or has limited accuracy, depending on the number of points checked.

To avoid the trade-off between accuracy and sampling seen in these methods, the authors of [15] and [16] use Bézier and B-spline basis functions for fast collision prediction. Using these basis functions, collision prediction can be quickly and analytically checked and a new plan can be computed by modifying the trajectory parameters. The method used in this paper for collision prediction is detailed in [17] and can be implemented with any absolutely continuous curve.

To take advantage of this computationally efficient method, we use a data-based method that produces predictions of a known basis. In this paper, we present a Gaussian process regression method for predicting obstacle trajectories given a probabilistic intention estimate. By choosing an appropriate covariance function, we can fix the basis of the mean and variance functions that allows collisions to be quickly predicted. This paper has three main contributions:

a data-based method for extrapolating obstacle trajectories, 2. 2.

a method for incorporating a probabilistic intention into the estimation method, and 3. 3.

we show that for the cubic-spline kernel, we obtain an uncertainty region as a combination of polynomials, and the square-root of polynomials can be used for collision prediction.

In Section II, we present preliminary information on Gaussian processes, multi-output Gaussian processes and collision prediction. In Section III, the necessary definitions and assumptions are provided and we present the problem statement. In Section IV, we present the methods and analysis results for trajectory estimation with a probabilistic intent. Finally, in Section V we demonstrate the method in two collision avoidance scenarios.

II PRELIMINARIES

II-A Gaussian Process

Gaussian process regression is a data based regression method that quantifies the uncertainty of predictions and allows the basis of the trajectory estimate to be chosen. An overview of Gaussian process regression can be found in [18], which provides general information on the regression method, and [19], which focuses on timeseries modelling. In [19], the authors discuss the model design choices that incorporate domain knowledge, such as the choice of covariance functions, updating methods and hyperparameter optimization.

A Gaussian process is a collection of random variables, where any finite numbers are jointly Gaussian. We can define these processes as a distribution over functions on $\mathbb{R}^{d}$ as

[TABLE]

where $M:\mathbb{R}^{d}\rightarrow\mathbb{R}$ is the mean function, and $K:\mathbb{R}^{d}\times\mathbb{R}^{d}\rightarrow\mathbb{R}$ is a symmetric positive definite covariance kernel function. The Gaussian process condition is then satisfied if for any finite set of sample times $T=\{t_{1},\dots t_{n}\}$, the function evaluated at the times in $T$ are samples from a multi-variate Gaussian distribution, that is,

[TABLE]

For $d=1$, the mean and covariance functions, evaluated at $T$, are a vector and a matrix respectively. The properties of the Gaussian process are completely determined by the mean function and covariance function. The mean function is often taken to be identically zero; in this case the predictive distribution for a test time, $t$, is given by

[TABLE]

where $D$ is a collection of times and corresponding outputs $D=\{T,\,Y\}$. The individual measurements, elements of $Y$, are given for every sample time in $T$, so we have $Y=[y_{1},\dots,y_{n}]^{\top}$. Each element is normally distributed. The measurement covariance matrix is $\Sigma^{2}$. This matrix is a diagonal for independent measurements. The matrix $P=(K(T,T)+\Sigma^{2})^{-1}$ is called the precision matrix.

The covariance function used in this paper is the cubic spline covariance function,

[TABLE]

where $t$ is any input time, $\tilde{t}\coloneqq t+\tau$, is the time shifted input, shifted by a constant $\tau>0$, such that the covariance function is positive semi-definite (in this paper $\tau=11$). The scaling hyperparameter is denoted $\theta_{f}$. The use of the cubic-spline covariance function is a natural choice in the domain of dynamically defined trajectories since they correspond to the solution of double integrator systems with piece-wise constant input commands. Even if the system is not a double integrator, a double integrator is a common simplified model used in systems with only a force input. This kernel is discussed in depth in [20], [21] and [22].

II-B Multi-Output Gaussian Process

Generally, a Gaussian process may be defined on any $d$-dimensional space. In this paper, we will consider multi-output Gaussian processes. The first output is the predicted mean given a data-set, and the second is time-derivative of the data-set. Since differentiation is a linear operator and our covariance function is differentiable, we can perform regression with a single Gaussian process:

[TABLE]

where the derivative of a covariance function is given by

[TABLE]

In this multi-output model, the mean function is a vector of two functions, and the covariance is a matrix of functions. Note that the subscripts $f$ and $f^{\prime}$ can be replaced with either $x$ or $y$ to indicate spatial dimensions. For the cubic spline covariance function in Equation (2), these derivatives are given by the authors of [20] as

[TABLE]

where $\mathbb{I}$ is the indicator function and $\theta_{f^{\prime}}$ is the function derivative scaling hyperparameter. Additional information on this joint estimation scheme is presented in [18], [19] and [23].

II-C Minimum Distance

Minimum distance calculation is central to collision prediction. To avoid collision, we wish to keep the minimum distance between the agent trajectory and a specified obstacle uncertainty region greater than a safety distance. In general, given parametric equations $\beta:\mathcal{T}\to\mathbb{R}$ and $\delta:\mathcal{T}\to\mathbb{R}$ defined over a closed interval $\mathcal{T}\subset\mathbb{R}$, we define the $\delta$-region around the parametric equation $\beta$ as

[TABLE]

The minimum distance between a parametric equation $\alpha:\mathcal{T}\to\mathbb{R}$ and $\mathcal{B}_{\delta}$ is given by

[TABLE]

for all $t\in\mathcal{T}$.

III PROBLEM FORMULATION

Consider a mission in which an agent must navigate a planar environment in the presence of a dynamic obstacle. The obstacle is non-cooperative and moves through the $x$-$y$ plane without providing information about its future trajectory to the agent. Based on the obstacle’s motion, the agent must predict if its planned trajectory will cause a collision. We formalize the scenario with the following definitions.

III-A Definitions and Assumptions

Definition 1** (Agent)**

The agent, $A$, is represented by a coordinate in the $x$-$y$ plane and its safety distance $\Delta_{\textrm{safe}}>0$.

Assumption 1** (Agent Trajectory)**

We assume that the agent has a known trajectory, $\psi:\mathcal{T}\rightarrow\mathbb{R}^{2}$, for any time in $\mathcal{T}\subset\mathbb{R}$. The $x$ component of this trajectory is denoted by $\psi_{x}$, and the $y$ component is referred to as $\psi_{y}$.

Definition 2** (Obstacle)**

The obstacle, $O$, is represented by a coordinate in the $x$-$y$ plane.

Assumption 2** (Initial Separation)**

Assume that the agent trajectory is initially separated from the obstacle location. This separation must be larger than $\Delta_{\textrm{safe}}$.

Definition 3** (Intention)**

We define the probabilistic intention, $I$, as a distribution over positions and velocities at a future time, the intention time, $t_{I}$. Note that $x$ and $y$ indicate positions, and $x^{\prime}$ and $y^{\prime}$ indicate velocities.

Assumption 3** (Intention Distribution)**

For each dimension in the plane, indicated by subscript $x$ or $y$, we assume that the intention follows a normal distribution:

[TABLE]

and that these values are known.

Assumption 4** (Constant Intention)**

For the duration of the collision avoidance task, it is assumed that the intention is constant, i.e. the means, variances and intention time are fixed.

Assumption 5** (Dynamic Behavior)**

We assume that the motion of the obstacle is governed by a differential equation.

Definition 4** (Expected Position)**

In Equation 3, we see that the mean function is a vector of two functions. The expected position of the obstacle is, $\mu_{f}$, the first element in this vector of functions. Note that $f$ can be replaced with either $x$ or $y$ to indicate spatial dimension.

Definition 5** (Position Variance)**

In Equation 3, we see that the covariance function is a matrix of four functions. Applying the update equations in Equation 1 to this matrix of functions yields $\sigma^{2}(t)$, another matrix of functions:

[TABLE]

The position variance of the obstacle is, $\sigma^{2}_{f}$, the first element in this matrix of functions. Note that $f$ can be replaced with either $x$ or $y$ to indicate spatial dimension.

Definition 6** (Uncertainty Region)**

We define the uncertainty region for each dimension as

[TABLE]

where $\mu(t)$ and $\sigma(t)$ are the posterior mean and standard deviation for each dimension, as defined in Equation (1).

Assumption 6** (Sequential Data)**

The data are collected sequentially, that is the elements of the time vector, $T=[t_{1},\dots,t_{n}]$ and the corresponding measurements are ordered such that $t_{k}<t_{k+1}$ for a positive integer $k<n$.

III-B Time Intervals

While the trajectory of the obstacle is unknown for future times, we assume that the trajectory can be measured at discrete time instances between time $t_{a}$ and $t_{b}$, called the observation time interval, $\mathcal{T}_{O}=[t_{a},t_{b}]$. During this interval, it is assumed that we have a set of noisy measurements of the vehicle’s position and velocity. The prediction time interval is the time interval between the last measurement and the intention time: $\mathcal{T}_{P}=(t_{b},t_{I}]$. The union of these intervals is $\mathcal{T}$, the time interval of interest.

III-C Problem Statement

Given full knowledge of the agent’s planned trajectory on the time interval of interest $\mathcal{T}$, a data-set ${D}$ of measured positions and velocities of the obstacle on the observation time interval $\mathcal{T}_{O}$, and a probabilistic intention $\mathcal{I}$, we wish to determine whether or not the agent will collide with the uncertainty region, that is:

[TABLE]

for any $t\in\mathcal{T}_{O}$. If this inequality is violated, we say collision has occurred.

IV METHOD

IV-A Covariance Choice

Recall that our Gaussian process is defined in terms of a mean and joint position-velocity covariance function:

[TABLE]

This Gaussian process can be updated through Gaussian process regression using Equation (1). While the position and velocity information of the vehicle are assumed to be correlated by the choice of covariance function, the spatial coordinates are assumed to be independent. This assumption allows the trajectory to be estimated in each dimension separately and does not require additional assumptions on dynamic correspondences between the dimensions. These independent Gaussian processes are given by the equations:

[TABLE]

where $F_{x}$ and $F_{y}$ are the Gaussian process estimates for the positions and velocities of the agent in the $x$ and $y$ coordinates respectively. Their corresponding mean vectors are $M_{x}$ and $M_{y}$, while the covariance matrices are denoted by $K_{x}$ and $K_{y}$.

In the following theorem, we show that the future location of the obstacle can be estimated, and the uncertainty is quantified in terms of the standard deviation, as a function of mean and variance, both of which are in polynomial basis.

IV-B Trajectory Estimation

Consider an ordered set of $n$ measurement times $T=\{t_{1},\dots,t_{n}\}\in\mathcal{T}_{O}$. At each time, a noisy measurement of position and velocity is generated for each dimension:

[TABLE]

The measurement vectors are augmented with the mean of the intention at the time $t_{I}$. The augmented time vector then becomes $\bar{T}=\left[t_{1},\dots,t_{n},t_{I}\right]^{\top}$, and the augmented measurement vectors become

[TABLE]

Next, this information is composed into two datasets of the form given in Equation (1):

[TABLE]

The posterior mean and covariance are then calculated using Equation (1). Numerical methods for calculating the matrix inverse can be found in [18].

IV-C Estimation Basis

Lemma IV.1

Consider the multi-output covariance function given in Equation (5):

[TABLE]

Given a data-set $D$, the elements of $k_{f}(\bar{T},t)$ and ${}^{\partial}k_{f}(\bar{T},t)$, for any $t\in\mathcal{T}_{P}$, are polynomials of order less than or equal to three.

Proof:

Without loss of generality, let $\tau\equiv 0$, then

[TABLE]

To evaluate $\mathrm{min}(\tilde{T},t)$, recall that $T=[t_{1},\dots,t_{n}]\subset\mathcal{T}_{O}=[t_{a},t_{b}]$ and that $t\in\mathcal{T}_{P}=(t_{b},t_{I}]$. Then we can simplify the expression element-wise for the vector $\bar{T}$:

[TABLE]

Note that vector multiplication here is element-wise. Similarly,

[TABLE]

and we can simplify this equality under the same assumptions to be

[TABLE]

Clearly each element of these arrays is a polynomial of order less than or equal to three. ∎

Lemma IV.2** (Mean Basis for Position)**

Consider the posterior mean computed in Equation (1) given a data-set D:

[TABLE]

and the chosen covariance function as defined in Equation (5):

[TABLE]

Then the expected position trajectory is a cubic polynomial for any $t\in\mathcal{T}_{P}$.

Proof:

Let $P$ denote the precision matrix, partitioned as follows:

[TABLE]

Note that this matrix has no dependence on $t$. Combining the precision matrix with the vector of measurements yields

[TABLE]

Considering only the position mean, the matrix multiplication becomes

[TABLE]

Notice that the mean is a linear combination of $k_{f}(\bar{T},t)$ and ${}^{\partial}k_{f}(\bar{T},t)$. Since a linear combination of polynomials of order less than and equal to order three is a cubic polynomial, we have that $\mu_{f}$ is a cubic polynomial. ∎

Lemma IV.3** (Variance Basis for Position)**

Consider the posterior variance computed in Equation (1), given a data-set D:

[TABLE]

and the chosen covariance function, $K$, as defined in Equation (5). Then for any $t\in\mathcal{T}_{P}$, the position variance, $\sigma^{2}_{f}(t)$, is a sixth-order polynomial.

Proof:

The variance function for position is given by the first block in the posterior covariance function:

[TABLE]

Clearly, $k_{f}(t,t)$ is cubic by substitution of $t$ into Equation (2). By Lemma IV.1, the elements of both $k_{f}(\bar{T},t)$ and ${}^{\partial}k_{f}(\bar{T},t)$ are polynomials. Since the posterior variance function is a product of these polynomial elements with a linear combination of these elements, the resulting function will be a sixth-order polynomial. ∎

Theorem IV.4** (Uncertainty Region Boundary Basis)**

The boundary of the obstacle uncertainty region over the prediction time interval, $\mathcal{T}_{P}$, is given by the sum of a third-order polynomial and the square root of a sixth-order polynomial:

[TABLE]

Proof:

By Lemma IV.2 we have that the posterior mean $\mu_{f}$ over the interval $\mathcal{T}_{P}$ is a cubic polynomial. By Lemma IV.3 we have that the variance, $\sigma^{2}_{f}$, is a sixth-order polynomial. We construct the confidence interval in each dimension by adding the square root of the variance and mean functions as shown in Equation (6). Therefore we have a confidence interval described by a known basis. ∎

IV-D Parameterization

To predict the future position of the vehicle, the matrix $K(T,t)$ would need to be sampled or converted into a parametric form. Since a third-order polynomial can be fit with four samples and a sixth-order polynomial with seven samples, we sample the mean and covariance functions at uniformly spaced times $\mathbf{T}_{\mu}\subset\mathcal{T}_{P}$ and $\mathbf{T}_{\sigma}\subset\mathcal{T}_{P}$. These polynomials are exactly equivalent to the mean and variance functions, for $t\in\mathcal{T}_{P}$, but are renamed to indicate their form:

[TABLE]

IV-E Collision Prediction

We employ the collision prediction methods described in [17] to find the intersection between the boundary of the uncertainty region, given in Equation 6, and the agent’s parametric trajectory, Section IV-D. This method is computationally efficient and relies on interval optimization techniques to find the point intersection without sacrificing accuracy. Note that the nature of global optimization methods only allows for detecting one intersection even if the parametric curves intersect several times. However, since trajectory estimation is only performed for short time horizons, we make a reasonable assumption that the agent’s trajectory only intersects the uncertainty region once. Using this framework, we obtain the interval $\mathcal{T}_{x}^{C}\subset\mathcal{T}_{P}$, which indicates the time interval of collision in the $x$ dimension, so that

[TABLE]

holds true for all $t\in\mathcal{T}_{x}^{C}$, and $\mathcal{T}_{y}^{C}\subset\mathcal{T}_{P}$ indicates the time interval of collision in the $y$ dimension, so that

[TABLE]

holds for all $t\in\mathcal{T}_{y}^{C}$. When $\mathcal{T}_{x}^{C}\cap\mathcal{T}_{y}^{C}=\{\varnothing\}$, then it is easy to see that either $d_{\textrm{min}}(t,\psi_{x},\Psi_{x}^{2\sigma})\geq\Delta_{\textrm{safe}}$ or $d_{\textrm{min}}(t,\psi_{y},\Psi_{y}^{2\sigma})\geq\Delta_{\textrm{safe}}$ for all $t\in\mathcal{T}_{P}$, which implies that Equation 4 is not violated.

V RESULTS

Two simulations are considered to demonstrate the application of the proposed Gaussian process prediction method. The first is a merging scenario, where an obstacle moves from a parallel lane into the agent’s lane in front of the agent. The second scenario is a perpendicular cross, where the obstacle crosses in front of the agent. In each case, the agent considers two planned trajectories and checks to see which maintains the safety distance away from the predicted obstacle uncertainty region.

In both situations, the observed data is shown as gray points, and the intention as a red triangle. Based on this information, the confidence region is constructed and shown in red, with the solid red line being the expected future obstacle trajectory. The blue and green lines show the agent’s past trajectory, with dotted lines showing possible future trajectories.

The following parameters are fixed in these simulations. The time interval of interest is fixed, with $t_{a}=0$, $t_{b}=1$ and $t_{I}=3$. We assume that we have $n=10$ samples on the observation time interval. The kernel parameters for both $x$ and $y$ dimensions are $\theta_{f}=10$, $\theta_{f^{\prime}}=30$ and $\tau=11$.

V-A Merging with Speed Adjustment

In the first scenario, shown in Figure 2(a), the obstacle begins to move in the $x$-$y$ plane towards the agent. The assumed intention is that the vehicle will overtake and merge in front of the agent at an appropriate distance ahead, shown as the red triangle. The agent considers two possible trajectories, one that reduces its $x$ velocity and one that does not. In Figure 2(b), we see that the green trajectory, which alters the vehicle’s speed, is outside the red, shaded obstacle uncertainty region for the full prediction horizon. The blue trajectory shows that a constant $x$ velocity would lead to the agent entering the uncertainty region. The safety distance is calculated over time in Figure 2(d). The green trajectory, with altered $x$ velocity remains greater than $\Delta_{\textrm{safe}}$, the blue trajectory violates the safety distance constraint, leading to a predicted collision.

In Figure 1, the uncertainty region of the obstacle is shown at four times during the prediction interval in the $x$-$y$ plane. The red cross indicates the expected obstacle location, and the red ellipse is the uncertainty region. The light gray shadow of the uncertainty region shows the past $0.5s$ of uncertainty regions. This interval is colored gray to indicate that it is past data and not actively avoided. The vehicle locations are given as blue and green points, with trails showing the past $0.5s$ interval of the trajectory. Clearly the green points do not intersect with the uncertainty region. The blue points, representing the vehicle location if it were to hold a constant velocity, are inside the uncertainty region in Figures 1(b) and 1(c).

In this simulation the safety distance is $2$ m. The measurement noise is zero mean with a variance of $0.25$ in both position and velocity in the $x$ axis and a variance of $0.01$ in both position and velocity in the $y$ axis. The intention uncertainty is set with a variance of $1$ for both position and velocity in the $x$ axis, and $0.0625$ for both position and velocity in the $y$ axis.

V-B Perpendicular Cross with Path Re-planning

In the second scenario, shown in Figure 4(a), the obstacle moves in the $x$-$y$ plane towards the agent. The assumed intention is that the vehicle will continue its current path, shown as the red triangle. The agent considers two possible trajectories, one that responds to the obstacle and one that does not. In Figures 4(b) and 4(c), we see that the green trajectory, that avoids the obstacle, is adjusted so that the $x$ and $y$ components of the trajectory are not simultaneously in the uncertainty region. The blue trajectory shows that a constant $x$ velocity would lead to the agent entering the uncertainty region. Since this blue trajectory is inside the $x$ uncertainty region for all of $\mathcal{T}_{P}$, there is no action that could be taken in the $y$ dimension that would avoid predicted collision. The safety distance is calculated over time in Figure 4(d). The green trajectory, with altered $x$ and $y$ trajectories, remains greater than $\Delta_{\textrm{safe}}$, while the blue trajectory violates the safety distance constraint, leading to a predicted collision.

In Figure 3, the uncertainty region of the obstacle is shown at four times during the prediction interval in the $x$-$y$ plane. The red cross indicates the expected obstacle location, and the red ellipse is the uncertainty region. The light gray shadow of the uncertainty region shows the past $0.5s$ of uncertainty regions. This interval is colored gray to indicate that it is past data and not actively avoided. The vehicle locations are given as blue and green points, with trails showing the past $0.5s$ interval of the trajectory. Clearly the green points do not intersect with the uncertainty region. The blue point, representing the vehicle location if it were to continue its previous behavior, is inside the uncertainty region in Figure 1(c).

In this simulation, the safety distance is $5$ m and the measurement noise is zero mean with a variance of $1$ in position and $4$ in velocity. The intention uncertainty is set with a variance of $4$ for position and $16$ for velocity.

VI CONCLUSION

In this paper, we have presented a Gaussian process based method to predict collision with uncertainty quantification. We show that with this approach the uncertainty regions for the future location of the obstacle can be modeled and parameterized, which allows collisions to be predicted efficiently. With the assumption of a known yet probabilistic intention, the conservatism of the method is reduced. The presented simulations show that the method can be used to construct an estimate for an unknown obstacle’s future position based solely on measurements of past position and velocity. The data-based approach reduces the knowledge of the obstacle required for many trajectory estimation methods.

The current method is formulated as a prediction at a single time instance rather than a series of times. We wish to extend this work by considering predictions over a series of times with two components: (1) a method of updating hyperparameters and (2) a method of handling dynamic intentions. The prediction at each step relies on the choice of the scaling hyperparameters for both position and velocity in two dimensions. In future work, we wish to compare standard hyperparameter optimization to hyperparameter choice based on the physical limitations of the obstacle. Additionally, the assumption of an a priori known intention at a fixed time is restrictive in a dynamic environment. Future work will investigate propagating the intention as the output of an intent estimation procedure.

VII ACKNOWLEDGMENTS

This work is supported by the National Aeronautics and Space Administration (NASA), National Science Foundation (NSF) National Robotics Initiative (NRI) award #1820639 and NSF NRI award #1528036.