Probabilistic Prediction of Interactive Driving Behavior via
Hierarchical Inverse Reinforcement Learning

Under discrete decision $d_{M}$, we assume that the cost of each trajectory can be linearly parametrized by a group of selected features $\{\mathbf{f}_{d_{M}}\}$, i.e., $C(\boldsymbol{\theta}_{d_{M}},\hat{\xi}_{M},\xi,\hat{\xi}_{H})=\mathbf{\boldsymbol{\theta}}^{T}_{d_{M}}\mathbf{f}_{d_{M}}(\hat{\xi}_{M},\xi,\hat{\xi}_{H})$, where $\theta_{d_{M}}$ is the parameter vector to determine the emphasis of each of the features. Then trajectories with higher cost are exponentially less likely based on the principle of maximum entropy :

$$P(\hat{\xi}_{M}|\boldsymbol{\theta}_{d_{M}},d^{i}_{M},\xi,\hat{\xi}_{H})\propto e^{-C(\boldsymbol{\theta}_{d_{M}},\hat{\xi}_{M},\xi,\hat{\xi}_{H})}$$

(4)

Hence, the log likelihood of the given demonstration subset $\{\Xi_{d_{M}}\}$ is given by

$\displaystyle\log P(\Xi_{d_{M}}|\boldsymbol{\theta}_{d_{M}}){=}\sum_{\hat{\xi}_{M}\in\Xi_{d_{M}}}\log\dfrac{e^{-C(\boldsymbol{\theta}_{d_{M}},\hat{\xi}_{M},\xi,\hat{\xi}_{H})}}{\int e^{-C(\boldsymbol{\theta}_{d_{M}},\tilde{\xi}_{M},\xi,\hat{\xi}_{H})}d\tilde{\xi}_{M}}.$

(5)

Our goal is to find the optimal $\boldsymbol{\theta}_{d_{M}}$ such that the given demonstration set is most likely to happen:

$$\boldsymbol{\theta}_{d_{M}}^{*}=\arg\max_{\boldsymbol{\theta}_{d_{M}}}P(\Xi_{d_{M}}|\boldsymbol{\theta}_{d_{M}})$$

(6)

To tackle the normalization factor in (5), we use Laplace approximation as in [15]. Namely, the cost along an arbitrary trajectory $\tilde{\xi}_{M}$ is approximated by its second-order Taylor expansion around the demonstrated trajectory $\hat{\xi}_{M}$:

	$\displaystyle C(\boldsymbol{\theta}_{d_{M}},\tilde{\xi}_{M},\xi,\hat{\xi}_{H}){\approx}C(\boldsymbol{\theta}_{d_{M}},\hat{\xi}_{M},\xi,\hat{\xi}_{H})+(\tilde{\xi}_{M}-\hat{\xi}_{M})^{T}\dfrac{\partial C}{\partial\xi_{M}}$
	$\displaystyle+(\tilde{\xi}_{M}-\hat{\xi}_{M})^{T}\dfrac{\partial^{2}C}{\partial\xi_{M}^{2}}(\tilde{\xi}_{M}-\hat{\xi}_{M})\qquad$

This enables the normalization factor become a Gaussian integral and can be solved analytically. Define $g_{\hat{\xi}_{M}}(\boldsymbol{\theta}_{d_{M}}){=}\frac{\partial C}{\partial\xi_{M}}|_{\hat{\xi}_{M}}$ and $H_{\hat{\xi}_{M}}(\boldsymbol{\theta}_{d_{M}}){=}\frac{\partial^{2}C}{\partial\xi_{M}^{2}}|_{\hat{\xi}_{M}}$ as, respectively, the gradient and Hessian of the cost along trajectory $\hat{\xi}_{M}$. Then (6) is translated to the optimization problem in (7), which intuitively means the optimal cost function should have small gradient and large positive Hessians along the demonstrated trajectories. For details, one can refer to [15].

$$\min_{\boldsymbol{\theta}_{d_{M}}}\sum_{\hat{\xi}_{M}\in\Xi_{d_{M}}}g_{\hat{\xi}_{M}}^{T}(\boldsymbol{\theta}_{d_{M}})H_{\hat{\xi}_{M}}^{-1}(\boldsymbol{\theta}_{d_{M}})g_{\hat{\xi}_{M}}(\boldsymbol{\theta}_{d_{M}})-\log|H_{\hat{\xi}_{M}}(\boldsymbol{\theta}_{d_{M}})|$$

(7)

The features we selected to parametrize the continuous trajectories can be grouped as follows:

• •

  Speed - The incentive of the human driver to reach a certain speed limit $v_{\lim}$ is captured by the feature

  $$f_{v}(\hat{\xi}_{M})=\sum_{t=0}^{L}(v_{t}-v_{\lim})^{2}$$

  (8)

  $v_{t}$ is the speed at time $t$ along trajectory $\hat{\xi}_{M}$ and $L$ is the length of the trajectory.

• •

  Traffic - In dense traffic environment, human drivers tend to follow the traffic. Hence, we introduce a feature based on the intelligent driver model (IDM) [20]

  $$f_{\text{IDM}}(\hat{\xi}_{M})=\sum_{t=0}^{L}(s_{t}-s_{t}^{\text{IDM}})^{2}$$

  (9)

  where $s_{t}$ is the actual spatial headway between the front vehicle and predicted vehicle at time $t$ along trajectory $\hat{\xi}_{M}$, and $s_{t}^{\text{IDM}}$ is the spatial headway suggested by IDM.

• •

  Control effort and smoothness - Human drivers typically prefer to drive efficiently and smoothly, avoiding unnecessary accelerations and jerks. To address such preference, we introduce a set of kinematics-related features:

  $$f_{\text{acc}}(\hat{\xi}_{M})=\sum_{t=0}^{L}a_{t}^{2},\quad f_{\text{jerk}}(\hat{\xi}_{M})=\sum_{t=1}^{L}\left(\dfrac{a_{t}-a_{t-1}}{\triangle t}\right)^{2}$$

  (10)

  where $a_{t}$ represents the acceleration at time $t$ along the trajectory $\hat{\xi}_{M}$. $\triangle t$ is the sampling time.

• •

  Clearance to other road participants - Human drivers care about their distances to other road participants when they drive since distance is crucially related to safety. Hence, we introduce a distance-related feature

  $$f_{\text{dist}}(\hat{\xi}_{M})=\sum_{t=0}^{L}\sum_{k=1}^{N{+}1}e^{-\frac{(x_{t}-x^{k}_{t})^{2}}{l^{2}}-\frac{(y_{t}-y^{k}_{t})^{2}}{w^{2}}}$$

  (11)

  where $(x_{t},y_{t})$ and $(x^{k}_{t},y^{k}_{t})$ represent, respectively, the coordinates of the predicted vehicle along $\hat{\xi}_{M}$ and those of the $k$-th surrounding vehicle. Parameters $l$ and $w$ are the length and width of the predicted vehicle. We use coordinates in Frenet Frame to deal with curvy roads, i.e., $x$ denotes the travelled distance along the road and $y$ is the lateral deviation from the lane center.

• •

  Goal - This feature describes the short-term goals of human drivers. Typically, goals are determined by the discrete driving decisions. For instance, if a lane-changing vehicle decides to merge in front of a host vehicle on his target lane, he will set his short-term goals to be ahead of the host vehicle. The goal-related feature is given by

  $$f_{g}(\hat{\xi}_{M})=\sum_{t=0}^{L}\|(x_{t},y_{t})-(x_{t}^{g},y_{t}^{g})\|^{2}_{2}$$

  (12)

• •

  Courtesy - Most of human drivers view driving as a social behavior, meaning that they not only care about their own cost, but also care about others’ cost, particularly when they are merging into others’ lanes [21]. Suppose that the cost of the host vehicle is $C_{H}(\hat{\xi}_{M},\xi,\hat{\xi}_{H})$, then to address the influence of courtesy to the interaction, we introduce the feature

  $$f_{\text{court}}(\hat{\xi}_{M})=\max\left\{C_{H}\left(\hat{\xi}_{M},\xi,\hat{\xi}_{H}\right){-}C_{H}^{\text{default}},0\right\}.$$

  (13)

  This feature describes the possible extra cost brought by the trajectory $\hat{\xi}_{M}$ of the predicted vehicle to the host vehicle, compared to the default host vehicle’s cost $C_{H}^{\text{default}}$. We can learn about $C_{H}(\cdot)$ also from demonstrations. Details about this will be covered in the case study.

For vehicles in different driving scenarios, or under different discrete driving decisions, the features we used to parametrize their costs are different subsets of the above listed ones. For instance, drivers decided to merge behind or front would set different goals, and drivers with right of way would most likely care less about courtesy than those without.

Remark I: Note that all variables $v_{t},a_{t},\delta_{t},x_{t}$ and $y_{t}$ in (8)-(13) can be expressed as functions of trajectory $\hat{\xi}_{M}$. For instance, if we define $\hat{\xi}_{M}=[x_{0},y_{0},\cdots,x_{L},y_{L}]^{T}$ where $x$ and $y$ are the coordinates in Frenet Frame, then we can obtain all variables via numerical differentiation. Details are omitted.

Different from the continuous driving decisions that influence the higher-order dynamics of the trajectories, the discrete decisions determine the homotopy of the trajectories. To capture this, we selected the following two features to parametrize the cost function that induces the discrete decisions:

• •

  Rotation angle - To describe the interactive driving behavior such as overtaking from left or right sides, merging in from front or back, we compute the rotation angle from $(x_{t},y_{t})$ to $(x_{t,H},y_{t,H})$ along trajectory $\hat{\xi}_{M}{\in}\Xi_{d_{M}}$. Define the angle as $\omega_{t}$, and then the rotation angle feature is given by

  $$f_{\angle}(d_{M})=\sum_{t=0}^{L}\omega_{t}$$

  (14)

  where $d_{M}$ is a discrete decision and $L$ is the length of the trajectory $\hat{\xi}_{M}$.

• •

  Minimum cost - It is also possible that human drivers make discrete decisions by evaluating the cost of trajectories under each decision, and select the one leading to the minimum-cost trajectory. To address this factor, we consider the feature

  $$f_{\text{cost}}(d_{M})=\min_{\tilde{\xi}_{M}}\boldsymbol{\theta}_{{d}_{M}}^{T}\mathbf{f}_{d_{M}}(\tilde{\xi}_{M},\xi,\hat{\xi}_{H}).$$

  (15)

  where $\boldsymbol{\theta}_{{d}_{M}}$ and $\mathbf{f}_{d_{M}}$, respectively, represent the learned parameters and selected features for the continuous trajectory distribution under discrete decision $d_{M}$.

Similarly, we assume that the decisions with lower cost are exponentially more probable. We also assume that the cost function is linearly parametrized by $\boldsymbol{\psi}$ and feature vector $\mathbf{f}^{\text{d}}=[f_{\angle},f_{\text{cost}}]^{T}$, i.e., $C^{\text{d}}(d_{M},\psi)=\boldsymbol{\psi}^{T}\mathbf{f}^{\text{d}}(d_{M})$. Again, our goal is to find the optimal $\boldsymbol{\psi}^{*}$ such that the likelihood of the demonstration set $\Xi$ is maximized:

$$\max_{\boldsymbol{\psi}}P(\Xi|\boldsymbol{\psi})=\max_{\boldsymbol{\psi}}\prod_{d_{M}\in\Xi}\dfrac{e^{-\boldsymbol{\psi}^{T}\mathbf{f}^{\text{d}}(d_{M})}}{\sum_{\tilde{d}_{M}\in\mathcal{D}}e^{-\boldsymbol{\psi}^{T}\mathbf{f}^{\text{d}}(\tilde{d}_{M})}}$$

(16)

By taking the log probability and gradient-descent approach, the parameter $\boldsymbol{\psi}$ is updated via

$$\boldsymbol{\psi}_{s+1}{=}\boldsymbol{\psi}_{s}{-}\alpha\left(\frac{1}{N}\sum_{\Xi}\mathbf{f}^{\text{d}}(d_{M}){-}{\sum}_{\tilde{d}_{M}}P(\tilde{d}_{M}|\boldsymbol{\psi}_{s})f^{\text{d}}(\tilde{d}_{M})\right)$$

(17)

$$P(\tilde{d}_{M}|\boldsymbol{\psi}_{s})=\dfrac{e^{-\boldsymbol{\psi}_{s}^{T}\mathbf{f}^{\text{d}}(\tilde{d}_{M})}}{\sum_{\tilde{d}_{M}\in\mathcal{D}}e^{-\boldsymbol{\psi}_{s}^{T}\mathbf{f}^{\text{d}}(\tilde{d}_{M})}}\qquad\qquad\quad$$

(18)

where $\alpha$ is the step size and $N$ is the number of demonstrated trajectories in set $\Xi$. To estimate the probability $P(\tilde{d}_{M}|\boldsymbol{\psi}_{s})$, we use sampling-based method. Detailed implementation will be covered later in the case study.

In the ramp-merging scenario, the driving decisions are listed as in Table I. As mentioned above, $\mathbf{x}{=}[x_{1},\cdots,x_{L}]^{T}$ and $\mathbf{y}{=}[y_{1},\cdots,y_{L}]^{T}$ are, respectively, the coordinate vectors in Frenet Frame along the longitudinal and lateral directions. $L$ is set to be $50$, i.e., in each demonstration, $5$s trajectories are collected.

Since the right of way for the merging vehicle and the lane-keeping vehicles are different, we define different features for them.

For the lane-keeping vehicle, the feature vectors related to the continuous driving decisions are as follows:

• •

  Yield: $\mathbf{f}_{\text{yield}}{=}[f_{v},f_{\text{acc}},f_{\text{jerk}},f_{\text{dist}},f_{g}]^{T}$. We exclude the feature $f_{\text{IDM}}$ because once the lane-keeping driver decides to yield to the merging vehicle, it is very likely that he cares more about the relative positions to the merging vehicle instead of the heading space to the front vehicle when he plans his continuous trajectories. The goal position in $f_{g}$ is set to be $[x_{\text{current lane center}},y_{t,\text{merging vehicle}}{-}s_{0}]$, i.e., $s_{0}$ behind the merging vehicle along longitudinal direction.

• •

  Pass: $\mathbf{f}_{\text{pass}}{=}[f_{v},f_{\text{IDM}},f_{\text{acc}},f_{\text{jerk}},f_{\text{dist}},f_{g}]^{T}$. In this case, the goal position in $f_{g}$ is set to be ahead of the merging vehicle along longitudinal direction, i.e., $[x_{\text{current lane center}},y_{t,\text{merging vehicle}}{+}s_{0}]$. Also, if the driver decides to pass, it is more probable that the heading space to the front vehicle will influence the distribution of his continuous trajectories.

For the merging vehicle, the feature vectors for the continuous driving models are:

• •

  Merge back: $\mathbf{f}_{\text{back}}{=}[f_{v},f_{\text{acc}},f_{\text{jerk}},f_{\text{dist}},f_{g}]^{T}$. The goal position is set to be $[x_{\text{target lane center}},y_{t,\text{on-lane vehicle}}{-}s_{0}]$.

• •

  Merge front: $\mathbf{f}_{\text{front}}{=}[f_{v},f_{\text{IDM}},f_{\text{acc}},f_{\text{jerk}},f_{\text{dist}},f_{\text{court}},f_{g}]^{T}$. Once the merging driver decides to merge in front of the lane-keeping vehicle, his heading space to the front lane-keeping vehicle is crucial to the distribution of his possible trajectories. Hence, we include feature $f_{\text{IDM}}$. Moreover, to respect the right of way of the lane-keeping vehicle, merging drivers might care about the extra cost they bring to the lane-keeping vehicle and prefer trajectories that induce less extra cost. To capture such effect, we add $f_{\text{court}}$. Given demonstrated trajectory group $\boldsymbol{\xi}{=}[\xi_{\text{merging}},\xi_{\text{surroudings}},\xi_{\text{lane-keeping}}]$, $f_{\text{court}}$ is computed via (13), i.e.,

  $\displaystyle f_{\text{court}}(\xi_{\text{merging}})=C_{\text{lane-keeping}}(\boldsymbol{\theta}_{\text{yield}},\boldsymbol{\xi})-C_{\text{lane-keeping}}^{\text{default}}$

  (19)

  The cost function $C_{\text{lane-keeping}}(\boldsymbol{\theta}_{\text{yield}}){=}\boldsymbol{\theta}_{\text{yield}}^{T}\mathbf{f}_{\text{yield}}$ is learned with the features selected above. Regarding to $C_{H}^{\text{default}}$, we assume that the lane-keeping vehicle is by default following IDM.

To measure the accuracy of the probabilistic prediction of discrete decisions, we extract $N{=}2000$ short trajectories in the test set with a horizon length of $10$ from the 54 long trajectories. For each short trajectory, starting from the same initial condition, we sample $M{=}4$ trajectories with different motion patterns (discrete decisions) in the spatiotemporal domain, and one of them is set the same as the ground truth. Hence, the ground truth probabilities for all trajectories are either 0 or 1.

Metric: We adopt a fatality-aware Brier metric [23] to evaluate the prediction accuracy. The fatality-aware metric refines the Brier score [24] by formulating the prediction errors in three different aspects: ground-truth accuracy ($\mathcal{G}$) measuring the prediction error of the ground-truth motion pattern ($\Xi_{g}$), conservatism accuracy ($\mathcal{C}$) measuring the false alarm of aggressive motion patterns ($\Xi_{a}$), and non-defensiveness accuracy ($\mathcal{D}$) measuring the miss detection of dangerous motion patterns ($\Xi_{d}$). Let $P_{i,j}$ be the predicted probability of motion pattern $j$ for $i$-th test example ($j{=}1,2,\cdots,M$ and $i{=}1,2,\cdots,N$) , then the three scores and the overall score are given by

	$\displaystyle\mathcal{G}$	$\displaystyle=$	$\displaystyle\dfrac{1}{|\Xi_{g}|}\sum_{(i,j)\in\Xi_{d}}\left(P_{i,j}-1\right)^{2},$		(20)
	$\displaystyle\mathcal{C}$	$\displaystyle=$	$\displaystyle\dfrac{1}{|\Xi_{a}|}\sum_{(i,j)\in\Xi_{a}}W_{c}(i,j)\left(P_{i,j}-0\right)^{2},$		(21)
	$\displaystyle\mathcal{D}$	$\displaystyle=$	$\displaystyle\dfrac{1}{|\Xi_{d}|}\sum_{(i,j)\in\Xi_{d}}W_{d}(i,j)\left(P_{i,j}-0\right)^{2},$		(22)
	$\displaystyle\mathcal{B}_{c}$	$\displaystyle=$	$\displaystyle\mathcal{G}+\mathcal{C}+\mathcal{D}.$		(23)

$W_{c}(i,j)$ and $W_{d}(i,j)$ are, respectively, the weights penalizing the conservatism and non-defensiveness of the motion pattern $(i,j)$. For more details, one can refer to [23].

Comparison: We compare the prediction among three different approaches: the proposed hierarchical IRL method, a neural-network (NN) based method [25] and a hidden markov models (HMM) based method [26].

Results: The scores for all three methods are shown in Table II. We can see that the proposed method can yield better overall prediction performance than HMM and NN based methods, particularly in terms of conservatism and non-defensiveness. This means that the prediction generated by the proposed method has similar criticality as the ground truth in the interaction process.

In this test, we generate the most probable trajectories under different discrete driving decisions by solving a finite horizon Model Predictive Control (MPC) problem using the above learned continuous cost functions. We show three illustrative examples in Fig. 4. The red dotted lines and blue solid lines represent, respectively, the predicted most-likely trajectories and the ground truth trajectories. The thick black dash-dot lines are trajectories of other vehicles. We can see that the predicted trajectories are very close to the ground truth ones.

Metric: We also adopt Mean Euclidean Distance (MED) [27] to quantitatively evaluate the accuracy of the prediction for continuous trajectories. Given the ground truth trajectory $\xi_{\text{ground}}{=}[x_{g,1},y_{g,1},{\cdots},x_{g,L},y_{g,L}]^{T}$ and predicted trajectory $\xi_{\text{prediction}}{=}[x_{p,1},y_{p,1},{\cdots},x_{p,L},y_{p,L}]^{T}$ of same length $L$ and same sampling time $\triangle T$, the trajectory similarity is calculated as follows:

$$\mathcal{S}_{\text{MED}}=\dfrac{1}{L}\sum_{i{=}1}^{L}\|\left[x_{p,i},y_{p,i}\right]^{T}-\left[x_{g,i},y_{g,i}\right]^{T}\|_{2}$$

(24)

Results: We test on $20$ long trajectories in the test set, and results are summarized in Table III: the proposed hierarchical IRL method can achieve trajectory prediction with a mean MED of 0.6172m with standard deviation of 0.2473m.