Prediction-aware and Reinforcement Learning based Altruistic Cooperative Driving

The HPN (Figure 3) is a prediction autoencoder network, it uses the sequence of $N$ observations at time $t$, i.e., $\tilde{\textbf{o}}_{t-N:t}$ and outputs a predicted observation at time $t+1$, i.e., $\tilde{\textbf{o}}_{t+1}^{\prime}$. The HPN consists of a symmetric encoder-decoder architecture. The encoder consists of 3 convolutional layers with 3x3 filters, with 32, 64, and 64 feature maps. The encoder takes as input the history of observations ($\tilde{\textbf{o}}_{t-N:t}$), where each observation consists of a velocity map image ($VM_{t}$) and the Kinematic matrix ($K_{t}$). The $VM$ from $t-N:t$ are passed through the 3-convolutional layers and the $K$ vectors from $t-N:t$ are passed through 2-FC (fully connected) layers with 128 hidden units, whose final layer contains the same number of hidden units as in the convolution network (CNN) output. The outputs ($VM$ features and $K$ features representations) are combined using element-wise addition operation. The decoder consists of a symmetric version of the encoder, i.e., a deconvolutional network with 3-convolutional layers and 2-FC layers. The convolutional layers produce a prediction for the next $VM_{t+1}^{\prime}$ and the FC layers produce the prediction for the next $K_{t+1}^{\prime}$. The CNN encoder is designed to extract important spatial information of the input $VM$ image. The predictive autoencoder is trained by minimizing the Mean Squared Error (MSE) between the prediction $\tilde{\textbf{o}}_{t+1}^{\prime}$ and the target $\tilde{\textbf{o}}_{t+1}$.

Although the AE provides kinematic predictions, we found that an indirect hybrid GP prediction approach to correct the kinematic predictions provides better results. Our findings are based on previous works that show how a GP-based prediction system is powerful for accurate kinematic predictions and often performs better than other models like AE and LSTM models [14][56][32].

Therefore, while we use the predictive AE to predict the next $VM_{t+1}^{\prime}$ image and $K_{t+1}^{\prime}$ state, we correct the kinematic state $K_{t+1}^{\prime}$ using a GP approach to improve kinematics prediction. We particularly find that accurate kinematics prediction are important for the safety prioritizer that uses the predictions to constrain the RL policies to a safer space. In this work, instead of directly using the position time series ($x_{t-N:t},y_{t-N:t}$) for each vehicle, our GP inference algorithm treats the vehicles’ heading ($\psi_{t-N:t}$) and speed ($v_{t-N:t}$) as two independent time series that are regressed using GPs, and then calculates the vehicles’ position ($x_{t+1:t+M}^{\prime},y_{t+1:t+M}^{\prime}$) using the predicted heading and speed. We call this approach GP-indirect prediction and present more details in the following sections.

For each vehicle $c$, the GP prediction algorithm takes as input the history of the 4-dimensional kinematic vector ($x_{t-N:t},y_{t-N:t},v_{t-N:t},\psi_{t-N:t}$, ), uses the heading ($\psi_{t-N:t}$) and speed ($v_{t-N:t}$) time series to predict their future values ($\psi_{t+1:t+M}^{\prime},v_{t+1:t+M}^{\prime}$). Then after modelling speed and heading, the future position of the vehicle, i.e., $x_{t+1:t+M}^{\prime},y_{t+1:t+M}^{\prime}$ is computed as follows,

From the output of the GP model, the 4-dimensional kinematic vector ($k_{t+1}^{GP}=x_{t+1},y_{t+1},v_{t+1},\psi_{t+1}$) for each vehicle is used to correct the AE kinematic prediction ($k_{t+1}^{AE}$), and the GP prediction is performed for each vehicle in the $K$ matrix (rows of the matrix) and a new matrix is formed with all the predictions at time $t+1$, i.e., $K_{t+1}^{\prime}=\big{[}k_{t+1}^{ego},k_{t+1}^{1},k_{t+1}^{2},...,k_{t+1}^{|C|}\big{]}$. The final predicted observation is a combination of the predicted velocity map ($VM_{t+1}^{\prime}$) and the corrected kinematic prediction ($K_{t+1}^{\prime}$) as shown in Figure 3, i.e.,

The prediction chain, as presented in Figure 4 is a multi-step prediction process that uses the HPN (Figure 3) in a chain to produce a set of $M$ future hypotheses. It takes a history of observation at time $t$, i.e., $\tilde{\textbf{o}}_{t-N:t}$ and produces a set of $M$ predicted observations, i.e., $\tilde{\textbf{o}}_{t+1:t+M}^{\prime}$ , as described in algorithm 1, to compute the input state for the VFN, i.e., $s_{t}=[\tilde{\textbf{o}}_{t-N:t},\tilde{\textbf{o}}_{t+1:t+M}^{\prime}]$.

In order to improve safety, we propose a safety prioritizer within our VFN. The safety prioritizer penalizes high-risk actions, thereby reducing imminent crashes. If the AVs come into an unexpected situation and based on the output of the VFN, decide to perform a high-risk action, the safety prioritizer will mask the action. The safety prioritizer is comprised of two algorithms, i.e., Algorithm 2 that checks for safe actions and Algorithm 3 that performs action selection.

Algorithm 2 verifies if the selected action $a_{t}$ is safe based on a safety score for $M_{steps}$ of prediction. Algorithm 2 simulates $I_{i}$ taking the action $a_{t}$ and uses the kinematic predictions from HPN, i.e., $\tilde{\textbf{K}}_{t+1:t+M}^{\prime}$ = HPN ($\tilde{\textbf{K}}_{t-N:t}$), for all vehicles in the road $c\in\mathcal{C}$ to compute the time-to-collision ($ttc$) at time $t$, i.e., $ttc_{t}$ between $I_{i}$ and all $c\in(\widetilde{\mathcal{I}}\cup\widetilde{\mathcal{H}})\setminus\{I_{i}\}$ using $x,y,v,\psi$, at each prediction step is calculated and the minimum $ttc$ is saved, and using the predicted $ttc$ for all the $M_{steps}$ of prediction ($ttc_{t+1:t+M}$), the $safe_{score}$ is computed. The $safe_{score}$ is a weighted average of the $ttc_{t+1:t+M}$, with exponential decay to give more importance to the short-term predictions. Finally, if $safety_{score}<safe_{th}$ or any of the predicted $ttc$ is less than the critical threshold, i.e., $any(ttc_{t+1:t+M})<critical_{th}$, the action is considered unsafe. The $safe_{th}$ is the safe $ttc$ threshold for possible crash, and $critical_{th}$ is a critical $ttc$ threshold for imminent crash. If the current action is considered unsafe,  Algorithm 3 will select another action.

Algorithm 3 presents the selection of the action. It iteratively verifies the actions using Algorithm 2 and selects a safe action that follows the learned policy. The restricted actions will prevent the agent from engaging in risky behavior during training, resulting in a more balanced learning and efficient sampling.

The VFN estimates the state-action value function. The combination of prediction (HPN) and decision-making (VFN) allows prediction-aware planning and improves the AVs’ ability to learn to navigate complex scenarios, and the safety prioritizer further increases safety. The proposed approach utilizes deep reinforcement learning (DRL) to achieve a high-level policy for safe tactical decision-making. As presented, the input consists of a stack of $N$ past observations and $M$ future hypotheses, i.e., $s_{t}=[\tilde{\textbf{o}}_{t-N:t},\tilde{\textbf{o}}_{t+1:t+M}]$, and the 3D CNN operates as a feature extractor. The VFN is trained to learn the optimal Q-values that maximize our social reward function, optimizing social utility. During training, agents are trained in a semi-sequential manner, as in [17].

The VFN outputs the Q-values that are masked by a safety prioritizer, constraining the RL policy to a safe action space. Therefore, in our framework, when the agent policy chooses an unsafe action, the safety prioritizer masks the action and selects a safer action, saving the unsafe action ($a_{t}$) and the associated state in the $RM$ with a negative reward ($r_{unsafe}$). By reducing episode restarts due to potential collisions, the safety prioritizer increases sample efficiency and safety.

The proposed prediction-aware planning and the social-aware optimization algorithm is described in Algorithm 4. We first run a batch of sample simulations to pre-fill our replay buffer before starting the learning phase. To account for the unbalance in training data, the experience replay buffer is re-balanced [63].

We customize the OpenAI Gym Highway environment in [65]. We design five scenarios for our experiments, i.e, a straight highway, highway exiting, highway merging, intersection, and roundabout scenarios ($f_{h},f_{e},f_{m},f_{i},f_{r}\in\mathcal{F}$). The AVs are trained surrounded by HVs with various behaviors, i.e, conservative, moderate, and aggressive, ($b_{c},b_{m},b_{a}\in\mathcal{B}$). A scenario with mixed behavior is obtained by sampling from the behaviors in $\mathcal{B}$ for each HV. The VFN is trained for $N_{episodes}=15,000$ episodes and multiple iterations of the training procedure are carried out to guarantee the convergence of the policies. Table I lists our training and simulation parameters.

We model the HV’s lateral behavior using the MOBIL model [66] and the longitudinal behavior of HVs is based on the Intelligent Driver Model (IDM) [67]. MOBIL is based on the safety and incentive criteria. For safety, it verifies $\mathrm{a}_{n}>-b_{\mathrm{safe}}$, where $\mathrm{a}_{n}$ is the deceleration of the following vehicle in the new lane, and $-b_{\mathrm{safe}}$ is a safe threshold. Then MOBIL verifies the incentive to change lane measured by $\mathrm{a}^{\prime}_{ego}-\mathrm{a}_{ego}+\mathrm{p}\Big{(}(\mathrm{a}^{\prime}_{n}-\mathrm{a}_{n})+(\mathrm{a}^{\prime}_{o}-\mathrm{a}_{o})\Big{)}>\Delta a_{th}$, where $\mathrm{p}$ is the politeness term and $\mathrm{a}_{ego}$ , $\mathrm{a}_{n}$ and $\mathrm{a}_{o}$ are the accelerations of the ego-HV, the new following vehicle, and the old following vehicle, respectively. Finally, based on the MOBIL model, if both criteria are verified, then the HV performs a lane change.

The longitudinal behavior of HV $k$ is modeled by using the IDM model which computes the acceleration $\dot{v}_{\mathrm{k}}$ as $\dot{v}_{\mathrm{k}}=\mathrm{a}_{\mathrm{max}}\Big{[}1-\Big{(}\frac{v_{k}}{v_{\mathrm{k}}^{0}}\Big{)}^{\delta}-\Big{(}\frac{d^{*}(v_{k},\Delta v_{k})}{d_{k}}\Big{)}^{2}\Big{]}$; in which $\delta$ is the exponent of acceleration, $d_{k}$ is the gap, $v_{\mathrm{0}}^{k}$ is the preferred speed, and $v_{k}$ is the current speed. Following, the preferred minimum gap is computed as $d^{*}(v_{k},\Delta v_{k})=d_{k}^{0}+v_{k}T_{\mathrm{k}}^{0}+\frac{v_{k}\Delta v_{k}}{(2\sqrt{\mathrm{a}_{\mathrm{max}}.\mathrm{a}_{\mathrm{des}}})}$, where $d_{k}^{0}$ is the minimum distance, $T_{k}^{0}$ is the safe time gap, $\mathrm{a}_{\mathrm{max}}$ is the acceleration limit, and $\mathrm{a}_{\mathrm{des}}$ is the deceleration limit.

We use the centrality metrics as in [64, 45] to obtain the parameters $\mathcal{P}$ that simulate the driver behaviors for the MOBIL and IDM models. In our scenarios, the computed parameters $\mathcal{P}$ that represent aggressive, conservative, and moderate behavior are shown in Table II.

Predicting the actions of HVs is a crucial component of AVs’ decision-making. We take advantage of this feature and investigate how incorporating prediction into our framework improves safety and efficiency. We look into this insight while investigating H1. Particularly, we show that prediction in the image domain allows learning powerful representations, and we present how the HPN learns to predict the $VM$ image and the advantages of using the GP approach to improve Kinematic prediction.

Kinematic prediction. We first investigate our H1 and show how using the GP for kinematic prediction improves the prediction performance. We compare different kinematic prediction baselines and measure their performance using the position error $PE$ in meters. We compare five prediction approaches, i.e, the GP prediction approach, an LSTM network, Constant Speed (CS), Constant Acceleration (CA) based prediction, and the predictive AE kinematic prediction. GP, LSTM, and AE can be used to predict any time series and leveraging that, we consider two approaches for each method: direct and indirect prediction. Therefore, we compare eight baselines the GP direct (GP D), the GP indirect (GP I), LSTM Direct (LSTM D), LSTM Indirect (LSTM I), CS, CA, AE direct (AE D) and AE indirect (AE I).

In a direct prediction approach, $(x,y)$ are regressed by two distinct models learned from the history ($x_{t-N:t},y_{t-N:t}$), producing direct predictions of futures $(x,y)$. Differently, in an indirect prediction approach, the vehicle’s heading ($\phi_{t-N:t}$) and speed ($v_{t-N:t}$) histories are considered and the models for heading and speed $(\phi,v)$ are learned using the predictive approach. Using the learned models, the predictions for future heading and speed $(\phi,v)$ are computed and utilized to calculate future position ($x_{t+1:t+M},y_{t+1:t+M}$). In our experiments, a compound kernel of linear and RBF is used following previous work [14].

As illustrated in Figure 5 the indirect GP (GP I, in orange) approach outperforms the other baselines in terms of $PE$, showing how this non-parametric Bayesian scheme allows the incorporation of complex model structures and is a suitable option for our kinematic prediction method, verifying our H1.

Observation history. Together with the Kinematic prediction, the HPN outputs the velocity map image prediction ($VM$). A prediction reconstruction error ($PRE$) loss is utilized to calculate the error between the predicted observation $o^{pred}$ and the corresponding $o$, i.e., $L_{2}(\boldsymbol{o_{t+1}},\boldsymbol{o_{t+1}^{\prime}})=\sum_{i}(o_{i}-o_{i}^{\prime})^{2}$. We evaluate the HPN’s performance using only the current observation as input and history of $N$ observations, demonstrating the importance of temporal information for accurate prediction measured by the $PRE$. Figure 6 depicts the training loss results, where the left image is for the HPN that uses the history of $N$ observation and the right image uses just the current observation. As shown in the figure, when using the temporal information the $PRE$ loss is approximately $20\%$ smaller when using history (left) than without history (right). Similarly, Figure 8 shows the qualitative output of the HPN prediction chain using the current observation or a history of observations as input. The results with history (top) show clearer and more accurate visual predictions and confirm why the $PRE$ is lower when using the history.

Figure 7 presents some qualitative results of the internal representations at different layers of the HPN for a merging scenario. In Figure 7 (bottom), a zoomed-in version of some internal representations are illustrated for visualization. As observed, the HPN learns to extract and highlight important information from the input observation, such as (a) lanes, (b) road agents, (c) road segments, and (d) possible hypotheses on how the environment evolves. Despite the fact that the HPN has not been trained for a segmentation task, it learns to segment the road, agents, and lanes, which could be useful information for the prediction and driving tasks.

Using the HPN and prediction chain, the VFN is trained to optimize for a social utility. We evaluate the performance of the VFN when using just the history as input, i.e, $s_{t}=[\tilde{\textbf{o}}_{t-N:t}]$ and when additionally utilizing the prediction output of the prediction chain, i.e. $s_{t}=[\tilde{\textbf{o}}_{t-N:t},\tilde{\textbf{o}}_{t+1:t+M}]$. Figure 9 shows performance improvement by using prediction quantified by crash percentage (Top, $C(\%)$) and average distance traveled (Bottom, $DT(m)$). The results present the AV’s performance in the highway merging scenario $f_{m}$, in the presence of HVs with conservative, aggressive or mixed behavior. We observe that when train and test are performed in a conservative environment, or in other words, when HV yields and takes safer actions, the gains from prediction capabilities are not as noticeable, whereas, in an aggressive and mixed environment in which the behavior changes, the performance increases are significant. We believe that anticipating the future is especially useful in those scenarios, which is why performance has improved.

We evaluate the effectiveness of our architecture in diverse scenarios, as well as the performance enhancement of leveraging prediction. We argue that by using prediction, we provide the VFN a prior on how the world will evolve which is helpful for decision-making. Table III presents the results in different traffic scenarios, i.e, Exiting ($f_{e}$), Merging ($f_{m}$), Roundabout ($f_{r}$), intersection ($f_{i}$), and Highway ($f_{h}$) under mixed HV behaviors ($b\in\mathcal{B}$). We compare our architecture when using prediction (VFN+P), and without prediction (VFN) with other related architectures [68, 17, 18]. Our architectures as shown in Table III outperform the alternative methods, and the improvements are particularly pronounced in the more complex scenarios.

The combination of prediction (HPN) and decision-making (VFN) allows for prediction-aware planning and improves the AVs’ ability to learn to navigate complex scenarios, and the safety prioritizer is further improved by leveraging the information provided by the prediction chain, increasing safety and efficiency. The results presented in Figure 9 and Table III verify our H2. Additionally, Figure 10 provides the output of the prediction chain for different traffic scenarios, showing qualitative results that further illustrate the capabilities of the prediction network to forecast the future.