# End-to-end Interpretable Neural Motion Planner

Wenyuan Zeng1,2 Wenjie Luo1,2∗
Simon Suo1,2 Abbas Sadat1 Bin Yang1,2 Sergio Casas1,2 Raquel Urtasun1,2
1Uber Advanced Technologies Group 2University of Toronto
<EMAIL_ADDRESS><EMAIL_ADDRESS>denotes equal contribution.

###### Abstract

In this paper, we propose a neural motion planner (NMP) for learning to drive
autonomously in complex urban scenarios that include traffic-light handling,
yielding, and interactions with multiple road-users. Towards this goal, we
design a holistic model that takes as input raw LIDAR data and a HD map and
produces interpretable intermediate representations in the form of 3D
detections and their future trajectories, as well as a cost volume defining
the goodness of each position that the self-driving car can take within the
planning horizon. We then sample a set of diverse physically possible
trajectories and choose the one with the minimum learned cost. Importantly,
our cost volume is able to naturally capture multi-modality. We demonstrate
the effectiveness of our approach in real-world driving data captured in
several cities in North America. Our experiments show that the learned cost
volume can generate safer planning than all the baselines.

##  1 Introduction

Self-driving vehicles (SDVs) are going to revolutionize the way we live.
Building reliable SDVs at scale is, however, not a solved problem. As is the
case in many application domains, the field of autonomous driving has been
transformed in the past few years by the success of deep learning. Existing
approaches that leverage this technology can be characterized into two main
frameworks: end-to-end driving and traditional engineering stacks.

Figure 1: Our end-to-end interpretable neural motion planner (NMP). Backbone
network takes LiDAR data and maps as inputs, and outputs bounding boxes of
other actors for future timesteps (perception), as well as a cost volume for
planning with $T$ filters. Next, for each trajectory proposal from the
sampler, its cost is indexed from different filters of the cost volume and
summed together. The trajectory with the minimal cost will be our final
planning.

End-to-end driving approaches [3, 24] take the output of the sensors (e.g.,
LiDAR, images) and use it as input to a neural net that outputs control
signals, e.g., steering command and acceleration. The main benefit of this
framework is its simplicity as only a few lines of code can build a model and
labeled training data can be easily obtained automatically by recording human
driving under a SDV platform. In practice, this approach suffers from the
compounding error due to the nature of self-driving control being a sequential
decision problem, and requires massive amounts of data to generalize.
Furthermore, interpretability is difficult to obtain for analyzing the
mistakes of the network. It is also hard to incorporate sophisticated prior
knowledge about the scene, e.g. that vehicles should not collide.

In contrast, most self-driving car companies, utilize a traditional
engineering stack, where the problem is divided into subtasks: perception,
prediction, motion planning and control. Perception is in charge of estimating
all actors’ positions and motions, given the current and past evidences. This
involves solving tasks such as 3D object detection and tracking.
Prediction111We’ll use prediction and motion forecasting interchangeably., on
the other hand, tackles the problem of estimating the future positions of all
actors as well as their intentions (e.g., changing lanes, parking). Finally,
motion planning takes the output from previous stacks and generates a safe
trajectory for the SDV to execute via a control system. This framework has
interpretable intermediate representations by construction, and prior
knowledge can be easily exploited, for example in the form of high definition
maps (HD maps).

However, solving each of these sub-tasks is not only hard, but also may lead
to a sub-optimal overall system performance. Most self-driving companies have
large engineering teams working on each sub-problem in isolation, and they
train each sub-system with a task specific objective. As a consequence, an
advance in one sub-system does not easily translate to an overall system
performance improvement. For instance, 3D detection tries to maximize AP,
where each actor has the same weight. However, in a driving scenario, high-
precision detections of near-range actors who may influence the SDV motion,
e.g. through interactions (cutting in, sudden stopping), is more critical. In
addition, uncertainty estimations are difficult to propagate and computation
is not shared among different sub-systems. This leads to longer reaction times
of the SDV and make the overall system less reliable.

In this paper we bridge the gap between these two frameworks. Towards this
goal, we propose the first end-to-end learnable and interpretable motion
planner. Our model takes as input LiDAR point clouds and a HD map, and
produces interpretable intermediate representations in the form of 3D
detections and their future trajectories. Our final output representation is a
space-time cost volume that represents the “goodness” of each location that
the SDV can take within a planning horizon. Our planner then samples a set of
diverse and feasible trajectories, and selects the one with the minimum
learned cost for execution. Importantly, the non-parametric cost volume is
able to capture the uncertainty and multi-modality in possible SDV
trajectories, e.g changing lane v.s keeping lane.

We demonstrate the effectiveness of our approach in real world driving data
captured in several cities in North America. Our experiments show that our
model provides good interpretable representations, and shows better
performance. Specifically for detection and motion forecasting, our model
outperforms recent neural architectures specifically designed on these tasks.
For motion planning, our model generates safer planning compared to the
baselines.

##  2 Related Work

Imitation Learning: Imitation learning (IL) uses expert demonstrations to
directly learn a policy that maps states to actions. IL for self-driving
vehicles was introduced in the pioneering work of [24] where a direct mapping
from the sensor data to steering angle and acceleration is learned. [3]
follows the similar philosophy. In contrast, with the help of a high-end
driving simulator [9], Codevilla _et al_. [8] exploit conditional models with
additional high-level commands such as continue, turn-left, turn-right. Muller
_et al_. [21] incorporate road segmentation as intermediate representations,
which are then converted into steering commands. In practice, IL approaches
suffer from the compounding error due to the nature of self-driving control
being a sequential decision problem. Furthermore, these approaches require
massive amount of data, and generalize poorly, e.g., to situations drifting
out of lane.

RL & IRL: Reinforcement learning (RL) is a natural fit for sequential decision
problems as it considers the interactions between the environment and the
agent (a self-driving car in this case). Following the success of Alpha GO
[29], RL has been applied to self-driving in [15, 23]. On the other hand, the
inverse reinforcement learning (IRL) looks at learning the reward function for
a given task. [31, 35] develop IRL algorithms to learn drivable region for
self-driving cars. [25] further infers possible trajectories with a
symmetrical cross-entropy loss. However, all these approaches have only been
tested on simulated datasets or small real-world datasets, and it is unclear
if RL and IRL can scale to more realistic settings. Furthermore, these methods
do not produce interpretable representations, which are desirable in safety
critical applications.

Optimization Based Planners: Motion planning has long been treated as an
independent task that uses the outputs of perception and prediction modules to
formulate an optimization problem, usually by manually engineering a cost
function [4, 10, 20, 36]. The preferred trajectory is then generated by
minimizing this cost function. In practice, to simplify the optimization
problem, many approaches assume the objective to be quadratic [7], decompose
lateral and longitudinal planning as two tasks [1, 10] or represent the search
space into speed and path [11, 14]. In [1] A* is used to search the space of
possible motion. Similarly, the Baidu motion planner [10] uses dynamic
programming to find an approximate path and speed profile. In [36], the
trajectory planning problem is formulated as continuous optimization and used
in practice to demonstrate 100km of autonomous driving. In sampling-based
approaches, a set of trajectories is generated and evaluated against a
predefined cost, among which, the one with minimum cost is chosen [27, 30].
Such approaches are attractive since they are highly parallelizable [19]. The
drawback of all these hand-engineered approaches is that they are not robust
to real-world driving scenarios, thus requires tremendous engineering efforts
to fine-tune it.

Planning under uncertainty: Planning methods for robust and safe driving in
the presence of uncertainty have also been explored [2, 12, 33]. Uncertainty
in the intention of other actors is the main focus of [2, 33]. In [12],
possible future actions of other vehicles and collision probability are used
to account for the uncertainty in obstacles positions. Compared to these
approaches, our planner naturally handles uncertainty by learning a non-
parametric cost function.

Holistic Models: These models provide interpretability. Chen _et al_. [6]
propose to learn a mapping from the sensor data to affordances, such as
distance to left boundary/leading vehicle. This is then fed into a controller
that generates steering command and acceleration. Sauer _et al_. [26] further
propose a variant conditioned on direction command. On the other hand, Luo _et
al_. [18] propose a joint model for perception and prediction from raw LiDAR
data and [5] extends it to predict each vehicle’s intention. All the methods
above are trained for tasks that provide interpretable perception/prediction
outputs to be used in motion planning. However, no feed-back is back-
propagated from the motion planning module.

Figure 2: Trajectory Representation. We first sample a set of parameters of a
Clothoid to determine the shape of a trajectory. We then sample a velocity
profile to determine how fast the SDV go along this trajectory. Combining
these two, we can get a space-time trajectory.

In this work, we take a holistic model approach and take it one step further
by designing a single neural network that takes raw sensors and dynamic map
data as input and predicts the cost map for planning. Compared with imitation
learning approaches [3, 8, 24] that directly regress a steer angle (from the
raw data), our approach provides interpretability and handles multimodality
naturally. When compared with traditional planners which use manually designed
cost functions built on top of perception and prediction systems, our model
has the advantage of being jointly trained and thus learns representations
that are optimal for the end-task. Furthermore, our model can handle
uncertainty naturally (as this is represented in the cost) and does not
require costly parameter tuning.

##  3 Deep Structured Interpretable Planner

We propose an end-to-end learnable motion planner that generates accurate
space-time trajectories over a planning horizon of a few seconds. Importantly,
our model takes as input LiDAR point clouds and a high definition map and
produces interpretable intermediate representations in the form of 3D
detections and their future motion forecasted over the planning horizon. Our
final output representation is a space-time cost volume that represents the
“goodness” of each possible location that the SDV can take within the planning
horizon. Our planner then scores a series of trajectory proposals using the
learned cost volume and chooses the one with the minimum cost.

We train our model end-to-end with a multi-task objective. Our planning loss
encourages the minimum cost plan to be similar to the trajectory performed by
human demonstrators. Note that this loss is sparse as a ground-truth
trajectory only occupies small portion of the space. As a consequence,
learning with this loss alone is slow and difficult. To mitigate this problem,
we introduce an another perception loss that encourages the intermediate
representations to produce accurate 3D detections and motion forecasting. This
ensures the interpretability of the intermediate representations and enables
much faster learning.

###  3.1 Deep Structured Planning

More formally, let
$\mathbf{s}=\\{\mathbf{s}^{0},\mathbf{s}^{1},\cdots,\mathbf{s}^{T-1}\\}$ be a
trajectory spanning over $T$ timesteps into the future, with $\mathbf{s}^{t}$
the location in bird’s eye view (BEV) at the timestep $t$. We formulate the
planning problem as a deep structured minimization problem as follows

$\mathbf{s}^{*}=\arg\min_{\mathbf{s}}\sum_{t}c^{t}(\mathbf{s}^{t})$ (1)

where $c^{t}$ is our learned cost volume indexed at the timestep $t$, which is
a 2D tensor with the same size as our region of interest. This minimization is
approximated by sampling a set of physically valid trajectories s, and picking
the one with minimum cost. Our model employs a convolutional network backbone
to compute this cost volume. It first extracts features from both LiDAR and
maps, and then feeds this feature map into two branches of convolution layers
that output 3D detection and motion forecasting as well as the planning cost
volume respectively. In this section we describe our input representation and
network in details.

#### Input representation:

Our approach takes raw point clouds as inputs, captured by a LiDAR mounted on
top of the SDV. We employ $T^{\prime}=10$ consecutive sweeps as observations,
in order to infer the motion of all actors. For those sweeps, we correct for
ego-motion and bring the point clouds from the past 10 frames to the same
coordinate system centered at SDV’s current location. To make the input data
amenable to standard convolutions, we follow [5] and rasterize the space into
a 3D occupancy grid, where each voxel has a binary value indicating whether it
contains a LiDAR point. This results in a 3D tensor of size
$H$x$W$x$(ZT^{\prime})$, where $Z,H,W$ represents the height and x-y spatial
dimensions respectively. Note that we have concatenated timesteps along the
$Z$ dimension, thus avoiding 3D convolutions which are memory and computation
intensive.

Access to a map is also a key for accurate motion planning, as we need to
drive according to traffic rules (e.g., stop at a red light, follow the lane,
change lanes only when allowed). Towards this goal, we exploit HD maps that
contain information about the semantics of the scene such as the location of
lanes, their boundary type (e.g., solid, dashed) and the location of stop
signs. Similar to [5], we rasterize the map to form an $M$ channels tensor,
where each channel represents a different map element, including road,
intersections, lanes, lane boundaries, traffic lights, etc. Our final input
tensor is thus of size $H$x$W$x$(ZT^{\prime}+M)$.

#### Backbone:

Our backbone is adapted from the detection network of [32] and consists of
five blocks. Each block has {2, 2, 3, 6, 5} Conv2D layers with filter number
{32, 64, 128. 256, 256}, filter size 3x3 and stride 1. There are MaxPool
layers after each of the first 3 blocks. A multi-scale feature map is
generated after the first 4 blocks as follows. We resize the feature maps from
each of the first 4 blocks to 1/4 of the input size and concatenate them
together similar to [34], in order to increase the effective receptive field
[17]. These multi-scale features are then fed into the $5$-th block. The whole
backbone has a downsampling rate of 4.

#### Perception Header:

The perception header has two components formed of convolution layers, one for
classification and one for regression. To reduce the variance of regression
targets, we follow SSD [16] and employ multiple predefined anchor boxes
$a_{i,j}^{k}$ at each feature map location, where subscript $i,j$ denotes the
location on the feature map and $k$ indexes over the anchors. In total, there
are 12 anchors at each location, with different sizes, aspect ratios and
orientations. The classification branch outputs a score $p_{i,j}^{k}$ for each
anchor indicating the probability of a vehicle at each anchor’s location. The
regression branch also outputs regression targets for each anchor
$a_{i,j}^{k}$ at different time-steps. This includes localization offset
$l_{x}^{t},l_{y}^{t}$, size $s_{w}^{t},s_{h}^{t}$ and heading angle
$a_{sin}^{t},a_{cos}^{t}$. The superscript $t$ stands for time frame, ranging
from $0$ (present) to $T-1$ into the future. Regression is performed at every
timesteps, thus producing motion forecasting for each vehicle.

#### Cost Volume Head:

The cost volume head consists of several convolution and deconvolution layers.
To produce a cost volume $c$ at the same resolution as our bird-eye-view (BEV)
input, we apply two deconvolution layers on the backbone’s output with filter
number {128, 64}, filter size 3x3 and stride 2. Each deconvolution layer is
also followed by a convolution layer with filter number {128, 64}, filter size
3x3 and stride 1. We then apply a final convolution layer with filter number
$T$, which is our planning horizon. Each filter generates a cost volume
$c^{t}$ for a future timestep $t$. This allows us to evaluate the cost of any
trajectory $\mathbf{s}$ by simply indexing in the cost volume $c$. In our
experiments, we also clip the cost volume value between -1000 to +1000 after
the network. Applying such bounds prevents the cost value shifting
arbitrarily, and makes tuning hyper-parameters easier. We next describe our
output trajectory parameterization.

###  3.2 Efficient Inference

Given the input LiDAR sweeps and the HD map, we can compute the corresponding
cost volume $c$ by feedforward convolutional operations as describe above. The
final trajectory can then be computed by minimizing Eq. (1). Note, however,
that this optimization is NP hard222We expect the output trajectory of our
planner is physically feasible. This introduces constraints on the solution
set. Under these physical constraints, the optimization is NP hard.. We thus
rely on sampling to obtain a low cost trajectory. Towards this goal, we sample
a wide variety of trajectories that can be executed by the SDV and produce as
final output the one with minimal cost according to our learned cost volume.
In this section we describe how we efficiently sample physically possible
trajectories during inference. Since the cost of a trajectory is computed by
indexing from the cost volume, our planner is fast enough for real-time
inference.

#### Output Parameterization:

A trajectory can be defined by the combination of the spatial path (a curve in
the 2D plane) and the velocity profile (how fast we go along this path).
Sampling a trajectory as a set of points in $(x,y)\in\Re^{2}$ space is not a
good idea, as a vehicle cannot execute all possible set of points in the
cartesian space. This is due for example to the physical limits in speed,
acceleration and turning angle. To consider these real-world constraints, we
impose that the vehicle should follow a dynamical model. In this paper, we
employ the bicycle model [22], which is widely used for planning in self-
driving cars. This model implies that the curvature $\kappa$ of the vehicle’s
path is approximately proportional to the steering angle $\phi$ (angle between
the front wheel and the vehicle): $\kappa=2tan(\phi)/L\approx 2\phi/L,$ where
$L$ is the distance between the front and rear axles of the SDV. This is a
good approximation as $\phi$ is usually small.

We then utilize a Clothoid curve, also known as Euler spiral or Cornu spiral,
to represent the 2D path of the SDV [28]. We refer the reader to Fig. 2 for an
illustration. The curvature $\kappa$ of a point on this curve is proportional
to its distance $\xi$ alone the curve from the reference point, i.e.,
$\kappa(\xi)=\pi\xi$. Considering the bicycle model, this linear curvature
characteristic corresponds to steering the front wheel angle with constant
angular velocity. The canonical form of a Clothoid can be defined as

$\mathbf{s}(\xi)=\mathbf{s_{0}}+a\left[C\left(\frac{\xi}{a}\right)\mathbf{T_{0}}+S\left(\frac{\xi}{a}\right)\mathbf{N_{0}}\right]$
(2) $S(\xi)=\int_{0}^{\xi}sin\left(\frac{\pi u^{2}}{2}\right)du$ (3)
$C(\xi)=\int_{0}^{\xi}cos\left(\frac{\pi u^{2}}{2}\right)du$ (4)

Here, $\mathbf{s}(\xi)$ defines a Clothoid curve on a 2D plane, indexed by the
distance $\xi$ to reference point $\mathbf{s_{0}}$, $a$ is a scaling factor,
$\mathbf{T_{0}}$ and $\mathbf{N_{0}}$ are the tangent and normal vector of
this curve at point $\mathbf{s_{0}}$. $S(\xi)$ and $C(\xi)$ are called the
Fresnel integral, and can be efficiently computed. In order to fully define a
trajectory, we also need a longitudinal velocity $\dot{\xi}$ (velocity
profile) that specifies the SDV motion along the path $\mathbf{s(\xi)}$:
$\dot{\xi}(t)=\ddot{\xi}t+\dot{\xi}_{0},$ where $\dot{\xi}_{0}$ is the initial
velocity of the SDV and $\ddot{\xi}$ is a constant forward acceleration.
Combining this and (2), we can obtain the trajectory points $\mathbf{s}$ in
Eq. (1).

#### Sampling:

Since we utilize Clothoid curves, sampling a path corresponds to sampling the
scaling factor $a$ in Eq. (2). Considering the city driving speed limit of
15m/s, we sample $a$ uniformly from the range of 6 to 80m. Once $a$ is
sampled, the shape of the curve is fixed.333We also sample a binary random
variable indicating it’s a canonical Clothoid or a vertically flipped mirror.
They correspond with turning left or right respectively.  We then use the
initial SDV’s steering angle (curvature) to find the corresponding position on
the curve. Note that Clothoid curves cannot handle circle and straight line
trajectories well, thus we sample them separately. The probability of using
straight-line, circle and Clothoid curves are 0.5, 0.25, 0.25 respectively.
Also, we only use a single Clothoid segment to specify the path of SDV which
we think is enough for the short planning horizon. In addition, we sample
constant accelerations $\ddot{\xi}$ ranging from $-5m/s^{2}$ to $5m/s^{2}$
which specifies the SDV’s velocity profile. Combining sampled curves and
velocity profiles, we can project the trajectories to discrete timesteps and
obtain the corresponding waypoints (See Fig 2) for which to evaluate the
learned cost.

Method | L2 (m) | Collision Rate (%) | Lane Violation (%)
---|---|---|---
| 1.0s | 2.0s | 3.0s | 0.5s | 1.0s | 1.5s | 2.0s | 2.5s | 3.0s | 1.0s | 2.0s | 3.0s
Ego-motin | 0.281 | 0.900 | 2.025 | 0.00 | 0.01 | 0.20 | 0.54 | 1.04 | 1.81 | 0.51 | 2.72 | 6.73
IL | 0.231 | 0.839 | 1.923 | 0.00 | 0.01 | 0.19 | 0.55 | 1.04 | 1.72 | 0.44 | 2.63 | 5.38
Acc | 0.403 | 1.335 | 2.797 | 0.05 | 0.12 | 0.27 | 0.53 | 1.18 | 2.39 | 0.24 | 0.46 | 0.64
Manual Cost | 0.402 | 1.432 | 2.990 | 0.00 | 0.02 | 0.09 | 0.22 | 0.79 | 2.21 | 0.39 | 2.73 | 5.02
Ours-NMP | 0.314 | 1.087 | 2.353 | 0.00 | 0.01 | 0.04 | 0.09 | 0.33 | 0.78 | 0.35 | 0.77 | 2.99
Table 1: Planning Metrics

###  3.3 End-to-End Learning

Our ultimate goal is to plan a safe trajectory while following the rules of
traffic. We want the model to understand where obstacles are and where they
will be in the future in order to avoid collisions. Therefore, we use a multi-
task training with supervision from detection, motion forecasting as well as
human driven trajectories for the ego-car. Note that we do not have
supervision for cost volume. We thus adopt max-margin loss to push the network
to learn to discriminate between good and bad trajectories. The overall loss
function is then:

$\mathcal{L}=\mathcal{L}_{\text{perception}}+\beta\mathcal{L}_{\text{planning}}.$
(5)

This multi-task loss not only directs the network to extract useful features,
but also make the network output interpretable results. This is crucial for
self-driving as it helps understand failure cases and improves the system. In
the following, we describe each loss in more details.

#### Perception Loss:

Our perception loss includes classification loss, for distinguishing a vehicle
from the background, and regression loss, for generating precise object
bounding boxes. For each predefined anchor box, the network outputs a
classification score as well as several regression targets. This
classification score $p_{i,j}^{k}$ indicates the probability of existence of a
vehicle at this anchor. We employ a cross-entropy loss for the classification
defined as

$\mathcal{L}_{cla}=\sum_{i,j,k}\left(q_{i,j}^{k}\log
p_{i,j}^{k}+(1-q_{i,j}^{k})\log(1-p_{i,j}^{k})\right),$ (6)

where $q_{i,j}^{k}$ is the class label for this anchor (i.e., $q_{i,j}^{k}=1$
for vehicle and $0$ for background). The regression outputs include
information of position, shape and heading angle at each time frame $t$,
namely

$l_{x}=\frac{x^{a}-x^{l}}{w^{a}}\quad l_{y}=\frac{y^{a}-y^{l}}{h^{a}},$
$s_{w}=\log\frac{w^{l}}{w^{a}}\quad s_{h}=\log\frac{h^{l}}{h^{a}},$
$a_{sin}=sin(\theta^{a}-\theta^{l})\quad a_{cos}=cos(\theta^{a}-\theta^{l}),$

where superscript $a$ means anchor and $l$ means label. We use a weighted
smooth L1 loss over all these outputs. The overall perception loss is

$\mathcal{L}_{perception}=\sum\left(\mathcal{L}_{cla}+\alpha\sum_{t=0}^{T}\mathcal{L}^{t}_{reg}\right).$
(7)

Note that the regression loss is summed over all vehicle correlated anchors,
from the current time frame to our prediction horizon $T$. Thus it teaches the
model to predict the position of vehicles at every time frame.

To find the training label for each anchor, we associate it to its neighboring
ground-truth bounding box, similar to [16, 18]. In particular, for each
anchor, we find all the ground-truth boxes with intersection over union (IoU)
higher than $0.4$. We associate the highest one among them to this anchor, and
compute the class label and regression targets accordingly. We also associate
any non-assigned ground-truth boxes with their nearest neighbor. The remaining
anchors are treated as background, and are not considered in the regression
loss. Note that one ground-truth box may associate to multiple anchors, but
one anchor can at most be associated with one ground-truth box. During
training, we also apply hard negative mining to overcome imbalance between
positive and negative samples.

#### Planning Loss:

Learning a reasonable cost volume is challenging as we do not have ground-
truth. To overcome this difficulty, we minimize the max-margin loss where we
use the ground-truth trajectory as a positive example, and randomly sampled
trajectories as negative examples. The intuition behind is to encourage the
ground-truth trajectory to have the minimal cost, and others to have higher
costs. More specifically, assume we have a ground-truth trajectory
$\\{(x^{t},y^{t})\\}$ for the next $T$ time steps, where $(x^{t},y^{t})$ is
the position of our vehicle at the $t$ time step. Define the cost volume value
at this point $(x^{t},y^{t})$ as $\hat{c}^{t}$. Then, we sample $N$ negative
trajectories, the $i^{th}$ among which is $\\{(x_{i}^{t},y_{i}^{t})\\}$ and
the cost volume value at these points are $c_{i}^{t}$. The sampling procedure
for negative trajectories is similar as we described in Section. 3.2, except
there is 0.8 probability that the negative sample doesn’t obey SDV’s initial
states, e.g. we randomly sample a velocity to replace SDV’s initial velocity.
This will provide easier negative examples for the model to start with. The
overall max-margin loss is defined as

$\mathcal{L}_{\text{planning}}=\sum_{\tiny{\\{(x^{t},y^{t})\\}}}\left(\max_{1\leq
i\leq
N}\left(\sum_{t=1}^{T}\left[\hat{c}^{t}-c_{i}^{t}+d_{i}^{t}+\gamma_{i}^{t}\right]_{+}\right)\right)$
(8)

The inner-most summation denotes the discrepancy between the ground-truth
trajectory and one negative trajectory sample, which is a sum of per-timestep
loss. $[]_{+}$ represents a ReLU function. This is designed to be inside the
summation rather than outside, as it can prevent the cost volume at one time-
step from dominating the whole loss. $d_{i}^{t}$ is the distance between
negative trajectory and ground-truth trajectory
$||(x^{t},y^{t})-(x_{i}^{t},y_{i}^{t})||_{2}$, which is used to encourage
negative trajectories far from the ground-truth trajectory to have much higher
cost. $\gamma_{i}^{t}$ is the traffic rule violation cost, which is a constant
if and only if the negative trajectory $t$ violates traffic rules at time $t$,
e.g. moving before red-lights, colliding with other vehicles etc. This is used
to determined how ‘bad’ the negative samples are, as a result, it will
penalize those rule violated trajectories more severely and thus avoid
dangerous behaviors. After computing the discrepancy between the ground-truth
trajectory and each negative sample, we only optimize the worst case by the
$\max$ operation. This encourages the model to learn a cost volume that
discriminates good trajectories from bad ones.

Method | L2 along trajectory (m) | L2 across trajectory (m) | L1 (m) | L2 (m)
---|---|---|---|---
| 0s | 1s | 2s | 3s | 0s | 1s | 2s | 3s | 0s | 1s | 2s | 3s | 0s | 1s | 2s | 3s
FaF[18] | 0.29 | 0.49 | 0.87 | 1.52 | 0.16 | 0.23 | 0.39 | 0.58 | 0.45 | 0.72 | 1.31 | 2.14 | 0.37 | 0.60 | 1.11 | 1.82
IntentNet[5] | 0.23 | 0.42 | 0.79 | 1.27 | 0.16 | 0.21 | 0.32 | 0.48 | 0.39 | 0.61 | 1.09 | 1.79 | 0.32 | 0.51 | 0.93 | 1.52
Ours-NMP | 0.21 | 0.37 | 0.69 | 1.15 | 0.12 | 0.16 | 0.25 | 0.37 | 0.34 | 0.54 | 0.94 | 1.52 | 0.28 | 0.45 | 0.80 | 1.31
Table 2: Motion Forecasting Metric

##  4 Experiments

ID | Loss | Input | Penalty | mAP@IoU | Prediction L2 (m) | Collision Rate (%) | Traffic Violation (%)
---|---|---|---|---|---|---|---
| Det | Plan | 5 | 10 |  | 0.5 | 0.7 | 1s | 2s | 3s | 1s | 2s | 3s | 1s | 2s | 3s
1 | ✓ |  |  | ✓ |  | 94.1 | 81.3 | 0.48 | 0.84 | 1.34 | - | - | - | - | - | -
2 |  | ✓ |  | ✓ |  | - | - | - | - | - | 0.01 | 0.23 | 1.42 | 0.37 | 1.06 | 3.85
3 | ✓ | ✓ | ✓ |  | ✓ | 93.6 | 80.1 | 0.46 | 0.83 | 1.35 | 0.01 | 0.15 | 0.93 | 0.36 | 0.86 | 3.09
4 | ✓ | ✓ |  | ✓ |  | 94.2 | 81.1 | 0.45 | 0.80 | 1.30 | 0.01 | 0.29 | 1.40 | 0.36 | 1.02 | 3.26
5 | ✓ | ✓ |  | ✓ | ✓ | 94.2 | 81.1 | 0.45 | 0.80 | 1.31 | 0.01 | 0.09 | 0.78 | 0.35 | 0.77 | 2.99
Table 3: Ablation Study. We compare effects of different supervisions,
different input horizons and different training losses. ID denotes model id
which we use for clarity and brevity.

In this section, we evaluate our approach on a large scale real-world driving
dataset. The dataset was collected over multiple cities across North America.
It consists of 6,500 scenarios with about 1.4 million frames, the training set
consists of 5,000 scenarios, while validation and test have 500 and 1,000
scenarios respectively. Our dataset has annotated 3D bounding boxes of
vehicles for every 100ms. For all experiments, we utilize the same spatial
region, which is centered at the SDV, with 70.4 meters both in front and back,
40 meters to the left and right, and height from -2 meters to 3.4 meters. This
corresponds to a 704x400x27 tensor. Our input sequence is 10 frames at 10Hz,
while the output is 7 frames at 2Hz, thus resulting in a planning horizon of 3
seconds.

In the following, we first show quantitative analysis on planning on a wide
variety of metrics measuring collision, similarity to human trajectory and
traffic rule violation. Next we demonstrate the interpretability of our
approach, through quantitative analysis of detection and motion forecasting,
as well as visualization of the learned cost volume. Last, we provide an
ablation study to show the effects of different loss functions and different
temporal history lengths.

###  4.1 Planning Results

We evaluate a wide variety of planning metrics. L2 Distance to Real
Trajectory: This evaluates how far away the planned trajectory is from the
real executed trajectory. Note that the real trajectory is just one of the
many possible trajectories that a human could do, and thus this metric is not
perfect. Future Potential Collision Rate: This is used to see if the planned
trajectory will overlap with other vehicles in the future. For a given
timestep t, we compute the percentage of occurrence of collisions up to time
t, thus lower number is preferred. Lane Violation: this metric counts the
percentage of planned trajectories crossing a solid yellow line. Note that
lower is better, and here crossing is defined if the SDV touches the line.

---
Figure 3: Cost Volume across Time We shown planned trajectory in red and
ground-truth in blue. We overlay lower cost region for different timesteps in
the same figure, using different colors (indicated by legend). Detection and
corresponding prediction results are in cyan. (best view in color)

We implement many baselines for comparison including: Ego-motion forecasting
(Ego-motion): Ego-motion provides a strong cue of how the SDV would move in
the future. This baselines takes only SDV’s past position as input and uses a
4-layer MLP to predict the future locations. Imitation Learning (IL): We
follow the imitation learning framework [3, 8, 24], and utilize a deep network
to extract features from raw LiDAR data and rasterized map. For fair
comparison, we use the same backbone described (Sec. 3.1) and same input
parameterization (Sec. 3.1) than our approach. In addition, the same MLP from
Ego-motion forecasting baseline is used to extract features from ego-motion.
These two features are then concatenated and fed into a 3 layer MLP to compute
the final prediction. Adaptive Cruise Control (ACC): This baseline implements
the simple behavior of following the leading vehicle. The vehicle follows the
lane center-line, while adaptively adjusting its speed to maintain a safe
distance from the vehicle ahead. When there is no lead vehicle, a safe speed
limit is followed. Traffic controls (traffic lights, stop signs) are observed
as a stationary obstacle, similar to a stopped lead vehicle. Plan w/ Manual
Cost (Manual): This baselines uses the same trajectory parameterization and
sampling procedure as our approach. However it utilizes a manually designed
cost using perception and motion forecasting outputs. In detail, we rasterize
all possible roads the SDV can take going forward and set it to a low cost of
0; all detected objects’s bounding box defines area of a high cost set to 255;
cost of any other area is set to a default value 100. This baseline is
designed to show the effectiveness of our learned cost volume as it utilize
the same sampling procedure as our approach but just a different cost volume.

As shown in Tab. 1, our approach has lower future collision rate at all
timesteps by a large margin. Note that Ego-motion and IL baselines give lower
L2 numbers as they optimize directly for this metric, however they are not
good from planning perspective as they have difficulty reasoning about other
actors and collide frequently with them. Comparing to the manual cost baseline
and ACC, we achieve both better regression numbers and better collision rates,
showing the advantage of our learned cost volume over manual a designed cost.
For lane violation, ACC is designed to follow the lane, thus it has about 0
violation by definition. Comparing to other baselines, we achieve much smaller
violation number, showing our model is able to reason and learn from the map.

###  4.2 Interpretability

Interpretability is crucial for self-driving as it can help understand failure
cases. We showcase the interpretability of our approach by showing
quantitative results on 3D detection and motion forecasting and visualization
our learned cost-map for all timesteps into the future.

Method |  Detection mAP @ IoU (pts $\geq$ 1)
---|---
| 0.5 | 0.6 | 0.7 | 0.8 | 0.9
MobileNet[13] | 86.1 | 78.3 | 60.4 | 27.5 | 1.1
FaF[18] | 89.8 | 82.5 | 68.1 | 35.8 | 2.5
IntentNet[5] | 94.4 | 89.4 | 75.4 | 43.5 | 3.9
Pixor[32] | 93.4 | 89.4 | 78.8 | 52.2 | 7.6
Ours-NMP | 94.2 | 90.8 | 81.1 | 53.7 | 7.1
Table 4: Detection mAP Result

#### Detection:

We compare against several state-of-the-art real-time detectors, validating
that our holistic model understand the environment. Our baselines include a
MobileNet adapted from [13], FaF[18], IntentNet[5] and Pixor[32], which are
specifically designed for LiDAR-based 3D object detection. The metric is mAP
with different IoU thresholds, and vehicles without LiDAR points are not
considered. As shown in Tab. 4, our model archives best results on 0.7 IoU
threshold, which is the metric of choice for self-driving. Qualitative results
can also be found in Fig. 3.

#### Motion Forecasting:

Tab. 2 shows quantitative motion forecasting results, including L1 and L2
distance to ground-truth locations. We also provides the L2 distance from our
predictions to the ground-truth position along and perpendicular to the
ground-truth trajectory. These help explain if the error is due to wrong
velocity or direction estimation. We use baselines from [5, 18], which are
designed for motion forecasting with raw LiDAR data. Our model performs better
in all metric and all time steps. Note that IntentNet uses high-level
intentions as additional information for training. Qualitative results are
shown in Fig.3.

#### Cost Map Visualization:

In Fig. 3, we visualize a few different driving scenarios. Each figure gives a
top-down view of the scene, showing the map, LiDAR point clouds, detection,
motion forecasting and planning results including learned cost map. Each
figure represents one example, where we overlay the cost map from different
timesteps. We use different color to represent the lower cost region for
different timesteps (indicated by color legend). As we can see, our model
learns to produce a time-dependent cost map. In particular, the first column
demonstrates multi-modality, second column shows lane-following in heavy
traffic and the last column shows collision avoidance.

###  4.3 Ablation Study

We conduct ablation studies and report the results in Table 3. Our best model
is Model 5, comparing to Model 1 which is optimized only for detection and
motion forecasting, it achieves similar performance in terms of detection and
motion forecasting. Model 2 trains directly with planning loss only, without
the supervision of object bounding boxes and performs worse. Model 3 exploits
different input length, where longer input sequence gives better results.
Model 4 is trained without the traffic rule penalty $\gamma$ in Eq. 8. It
performs worse on planning, as it has no prior knowledge to avoid collision.

##  5 Conclusion

We have proposed a neural motion planner that learns to drive safely while
following traffic rules. We have designed a holistic model that takes LiDAR
data and an HD map and produces interpretable intermediate representations in
the form of 3D detections and their future trajectories, as well as a cost map
defining the goodness of each position that the self-driving car can take
within the planning horizon. Our planer then sample a set of physically
possible trajectories and chooses the one with the minimum learned cost. We
have demonstrated the effectiveness of our approach in very complex real-world
scenarios in several cities of North America and show how we can learn to
drive accurately.

## References

  * [1] Zlatan Ajanovic, Bakir Lacevic, Barys Shyrokau, Michael Stolz, and Martin Horn.  Search-based optimal motion planning for automated driving.  arXiv, 2018.
  * [2] Tirthankar Bandyopadhyay, Kok Sung Won, Emilio Frazzoli, David Hsu, Wee Sun Lee, and Daniela Rus.  Intention-aware motion planning.  In Algorithmic foundations of robotics X. 2013.
  * [3] Mariusz Bojarski, Davide Del Testa, Daniel Dworakowski, Bernhard Firner, Beat Flepp, Prasoon Goyal, Lawrence D Jackel, Mathew Monfort, Urs Muller, Jiakai Zhang, et al.  End to end learning for self-driving cars.  arXiv, 2016.
  * [4] Martin Buehler, Karl Iagnemma, and Sanjiv Singh.  The DARPA urban challenge: autonomous vehicles in city traffic.  2009\.
  * [5] Sergio Casas, Wenjie Luo, and Raquel Urtasun.  Intentnet: Learning to predict intention from raw sensor data.  In Proceedings of The 2nd Conference on Robot Learning, 2018.
  * [6] Chenyi Chen, Ari Seff, Alain Kornhauser, and Jianxiong Xiao.  Deepdriving: Learning affordance for direct perception in autonomous driving.  In ICCV, 2015.
  * [7] J. Chen, W. Zhan, and M. Tomizuka.  Constrained iterative lqr for on-road autonomous driving motion planning.  In 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC), 2017.
  * [8] Felipe Codevilla, Matthias Miiller, Antonio López, Vladlen Koltun, and Alexey Dosovitskiy.  End-to-end driving via conditional imitation learning.  In ICRA, 2018.
  * [9] Alexey Dosovitskiy, German Ros, Felipe Codevilla, Antonio Lopez, and Vladlen Koltun.  Carla: An open urban driving simulator.  arXiv, 2017.
  * [10] Haoyang Fan, Fan Zhu, Changchun Liu, Liangliang Zhang, Li Zhuang, Dong Li, Weicheng Zhu, Jiangtao Hu, Hongye Li, and Qi Kong.  Baidu apollo em motion planner.  arXiv, 2018.
  * [11] Thierry Fraichard and Christian Laugier.  Path-velocity decomposition revisited and applied to dynamic trajectory planning.  In ICRA, 1993.
  * [12] Jason Hardy and Mark Campbell.  Contingency planning over probabilistic obstacle predictions for autonomous road vehicles.  IEEE Transactions on Robotics, 2013.
  * [13] Andrew G Howard, Menglong Zhu, Bo Chen, Dmitry Kalenichenko, Weijun Wang, Tobias Weyand, Marco Andreetto, and Hartwig Adam.  Mobilenets: Efficient convolutional neural networks for mobile vision applications.  arXiv, 2017.
  * [14] Kamal Kant and Steven W Zucker.  Toward efficient trajectory planning: The path-velocity decomposition.  The international journal of robotics research, 1986.
  * [15] Alex Kendall, Jeffrey Hawke, David Janz, Przemyslaw Mazur, Daniele Reda, John-Mark Allen, Vinh-Dieu Lam, Alex Bewley, and Amar Shah.  Learning to drive in a day.  arXiv, 2018.
  * [16] Wei Liu, Dragomir Anguelov, Dumitru Erhan, Christian Szegedy, Scott Reed, Cheng-Yang Fu, and Alexander C Berg.  Ssd: Single shot multibox detector.  In ECCV, 2016.
  * [17] Wenjie Luo, Yujia Li, Raquel Urtasun, and Richard Zemel.  Understanding the effective receptive field in deep convolutional neural networks.  In NIPS, 2016.
  * [18] Wenjie Luo, Bin Yang, and Raquel Urtasun.  Fast and furious: Real time end-to-end 3d detection, tracking and motion forecasting with a single convolutional net.
  * [19] Matthew McNaughton.  Parallel algorithms for real-time motion planning.  2011\.
  * [20] Michael Montemerlo, Jan Becker, Suhrid Bhat, Hendrik Dahlkamp, Dmitri Dolgov, Scott Ettinger, Dirk Haehnel, Tim Hilden, Gabe Hoffmann, Burkhard Huhnke, et al.  Junior: The stanford entry in the urban challenge.  Journal of field Robotics, 2008.
  * [21] Matthias Müller, Alexey Dosovitskiy, Bernard Ghanem, and Vladen Koltun.  Driving policy transfer via modularity and abstraction.  arXiv, 2018.
  * [22] Brian Paden, Michal Čáp, Sze Zheng Yong, Dmitry Yershov, and Emilio Frazzoli.  A survey of motion planning and control techniques for self-driving urban vehicles.  IEEE Transactions on intelligent vehicles, 2016.
  * [23] Xinlei Pan, Yurong You, Ziyan Wang, and Cewu Lu.  Virtual to real reinforcement learning for autonomous driving.  arXiv, 2017.
  * [24] Dean A Pomerleau.  Alvinn: An autonomous land vehicle in a neural network.  In NIPS, 1989.
  * [25] Nicholas Rhinehart, Kris M Kitani, and Paul Vernaza.  R2p2: A reparameterized pushforward policy for diverse, precise generative path forecasting.  In ECCV, 2018.
  * [26] Axel Sauer, Nikolay Savinov, and Andreas Geiger.  Conditional affordance learning for driving in urban environments.  arXiv, 2018.
  * [27] J. Schlechtriemen, K. P. Wabersich, and K. Kuhnert.  Wiggling through complex traffic: Planning trajectories constrained by predictions.  In 2016 IEEE Intelligent Vehicles Symposium (IV), 2016.
  * [28] Dong Hun Shin, Sanjiv Singh, and W Whittaker.  Path generation for a robot vehicle using composite clothoid segments.  IFAC Proceedings Volumes, 1992.
  * [29] David Silver, Julian Schrittwieser, Karen Simonyan, Ioannis Antonoglou, Aja Huang, Arthur Guez, Thomas Hubert, Lucas Baker, Matthew Lai, Adrian Bolton, et al.  Mastering the game of go without human knowledge.  Nature, 2017.
  * [30] Moritz Werling, Julius Ziegler, Sören Kammel, and Sebastian Thrun.  Optimal trajectory generation for dynamic street scenarios in a frenet frame.  In ICRA, 2010.
  * [31] Markus Wulfmeier, Peter Ondruska, and Ingmar Posner.  Maximum entropy deep inverse reinforcement learning.  arXiv, 2015.
  * [32] Bin Yang, Wenjie Luo, and Raquel Urtasun.  Pixor: Real-time 3d object detection from point clouds.
  * [33] Wei Zhan, Changliu Liu, Ching-Yao Chan, and Masayoshi Tomizuka.  A non-conservatively defensive strategy for urban autonomous driving.  In Intelligent Transportation Systems (ITSC), 2016 IEEE 19th International Conference on, 2016.
  * [34] H Zhao, J Shi, X Qi, X Wang, and J Jia.  Pyramid scene parsing network. corrabs/1612.01105, 2016.
  * [35] Brian D Ziebart, Andrew L Maas, J Andrew Bagnell, and Anind K Dey.  Maximum entropy inverse reinforcement learning.  In AAAI, 2008.
  * [36] Julius Ziegler, Philipp Bender, Thao Dang, and Christoph Stiller.  Trajectory planning for bertha—a local, continuous method.  In Intelligent Vehicles Symposium Proceedings, 2014 IEEE, 2014.


# Control-Aware Prediction Objectives for Autonomous Driving

Rowan McAllister1, Blake Wulfe1, Jean Mercat1, Logan Ellis1, Sergey Levine2,
Adrien Gaidon1 1Toyota Research Institute
<EMAIL_ADDRESS>of California, Berkeley
<EMAIL_ADDRESS>
###### Abstract

Autonomous vehicle software is typically structured as a modular pipeline of
individual components (e.g., perception, prediction, and planning) to help
separate concerns into interpretable sub-tasks. Even when end-to-end training
is possible, each module has its own set of objectives used for safety
assurance, sample efficiency, regularization, or interpretability. However,
intermediate objectives do not always align with overall system performance.
For example, optimizing the likelihood of a trajectory prediction module might
focus more on easy-to-predict agents than safety-critical or rare behaviors
(e.g., jaywalking). In this paper, we present control-aware prediction
objectives (CAPOs), to evaluate the downstream effect of predictions on
control without requiring the planner be differentiable. We propose two types
of importance weights that weight the predictive likelihood: one using an
attention model between agents, and another based on control variation when
exchanging predicted trajectories for ground truth trajectories.
Experimentally, we show our objectives improve overall system performance in
suburban driving scenarios using the CARLA simulator.

##  I Introduction

Autonomous vehicles (AVs) must navigate busy roads using predictive models to
anticipate what surrounding pedestrians and vehicles might do in order to plan
safe trajectories around them. Safe operation requires such components be well
calibrated, typically by minimizing some regression error on training data.
However, not all errors made by prediction modules are equally important: some
errors have minimal effect on downstream decisions, while some perceptual
errors [24] and predictive errors [25] can have fatal outcomes. As no model is
perfect, it is crucial to identify which prediction errors are safety-critical
to ensure safety [19].

Whether trained independently or as part of multi-task end-to-end
architectures [30], multi-agent trajectory forecasting models typically
optimize prediction-specific objectives based on regressing recorded future
trajectories by considering all agents equally important _a priori_. However,
when considering the target control task of autonomous navigation, some
predictions warrant more attention than others when deciding safe controls.
Consequently, control-agnostic optimizing of prediction models may not result
in improved downstream navigation performance due to limited data, model
capacity, rare events, or computational constraints. Even with end-to-end
training, multi-task objectives might not be aligned, thus resulting in
performance degradation due to task interference [36].

Figure 1: A vehicle drives to the right while reacting to pedestrians with
sample predicted trajectories shown in purple or pink. Our Control-Aware
Prediction Objectives (CAPO) can learn to capture which predictions should
have more influence on the vehicle’s controls (cyan lines proportional to
attention). Videos available at https://sites.google.com/view/control-aware-
prediction

In this work, we propose _Control-Aware Prediction Objectives_ (CAPOs) to
train prediction models that more accurately reflect the relative effects of
predictive errors on downstream control. Computing these downstream effects
requires only forward passes without backpropagation between modules. This
improves applicability with real-world AV planning and control systems, which
might not be fully differentiable due to complex design constraints (e.g.,
verifiability, interpretability, comfort and safety constraints). Our method
introduces importance-weighted prediction likelihood objectives using forward
passes of the prediction model and planner. We investigate two weighting
methods that can be trained with backpropagation. The first assigns weights
based on control variations due to prediction changes. The second uses learned
attention weights between agent predictions and AV controls.

Using the CARLA simulator, we experimentally show that training prediction
models with control-aware objectives leads to improved controller performance
in complex multi-agent urban driving scenarios. Compared with existing
prediction models, including prediction algorithms that treat everything as
equal, we show that our new objective helps to avoid precisely those errors
that would maximally influence downstream decisions.

##  II Related Work

Several related fields of study investigate objective-aware prediction metrics
as we discuss here.

###  II-A Objective-Aware Prediction in Reinforcement learning

Model-based reinforcement learning (MBRL) methods learn a dynamics model of an
autonomous agent to predict which control decisions lead to states with higher
objective rewards [7, 22, 21]. MBRL prediction is related to AV prediction,
with the main difference being that AVs predict the trajectories of other
human agents and not those of the autonomous agent. Nevertheless, several MBRL
works have recently challenged the common assumption that the better a
dynamics model’s predictive accuracy, the better the downstream policy will
maximize reward. For example, [*]lambert2020objective [17] show task-agnostic
loss functions used to train dynamics models are often uncorrelated with
episode rewards, an issue termed “objective mismatch”.

Indeed, learned dynamics models need not be accurate everywhere in the state
space, only in the areas that help maximize rewards [3]. Some RL works
investigate training models insofar as they improve estimating the value
function [11, 2], policy gradient [14, 1], or ability to reach a goal state
[23]. Others optimize downstream policies directly using Bayesian optimization
to search model parameters [3]. Work by [*]donti2017task [8] points out that
in practice we often want a combination of training a model to optimize its
likelihood as well as a downstream task term, although in the context of
constrained optimisation. Similarly, [*]lambert2020objective [17] correlate
both metrics by increasing the weight in the loss of data points closer to
data the optimal controller generates.

Unfortunately for AV applications, such objective-aware prediction methods are
often inapplicable for several reasons. First, MBRL assumes access to an
objective reward function, reward samples, or goal state, but objective
measures or goals of desirable driving are often difficult to define. By
contrast, human-designed AV control systems are often preferable for
verification and interpretability reasons. Second, a common assumption in RL
is that the policy is either differentiable or stochastic (in the case of
policy gradients), whereas real-world AV control systems often contain complex
logic that is neither differentiable nor stochastic. Our work focuses instead
on how to learn prediction given access to a safe, potentially non-
differentiable controller.

###  II-B Map-Aware Prediction Metrics

Map information can help incorporate prior knowledge into prediction metric
design. For example, since the ego vehicle drives on the road, pedestrian
forecasting errors could be given more weight on road surfaces than otherwise.
Work by [*]shridhar2020beelines [32] use maps to help focus on potential
collisions with other agents by generating a set of candidate ego trajectories
along known lane tracks that any controller might follow. This method does not
assume a particular downstream controller, but makes an educated guess as to
what a reasonable controller might do. Another map-based prediction metric is
Drivable Area Compliance (DAC) [5], which counts the proportion of model
samples that exit the drivable area. A conceptual difference with our method
is that our prediction metrics assume access to the specific downstream
controller that will be used at test-time, improving test-time performance.
Since controllers already consider road information, we circumvent the need to
explicitly design prediction metrics around mapping information, which can
incur additional hyperparameters, such as the relative costs of predicting if
an agent will traverse either road / sidewalk / building.

###  II-C Control-Aware Perception

[*]philion2020learning [26] propose a control-aware 3D object perception
metric called Planning KL-divergence (PKL) based on how perceptual errors
cause distributional divergence in the ego’s distribution of planned paths,
compared to a planner with perfect observations, measured by the KL
divergence. In contrast, our work focuses on prediction objectives, and
additionally demonstrates how the new objective empirically affects online-
control using a driving simulator. We also avoid KL distance losses in our
work since this assumes non-trivial stochasticity in the controller or data,
which is not always the case. Other works have also used KL policy distances
to investigate how observations can be compressed while preserving human-like
actions had they remained uncompressed [28]. Work by [*]piazzoni2020modeling
[27] also investigates how perceptual metrics affect downstream planning but
are specific to perception.

##  III Preliminaries

Here we formalize our notation and discuss some existing prediction metrics
before presenting our own.

###  III-A Notation and Assumptions

Let $\mathbf{x}\in\mathcal{X}$ denote past trajectory information about all
agents, used to make probabilistic predictions
$\hat{\mathbf{y}}\in\mathcal{P}_{\mathcal{Y}}$ about the future multi-agent
trajectories $\mathbf{y}\in\mathcal{Y}$. Trajectories are predicted up to time
horizon $T$, and $\mathbf{y}_{T}$ denotes the future state at time $T$. As the
intents of other agents are usually uncertain, we use a probabilistic
prediction model $q_{\theta}$ with trainable parameters $\theta$ to sample the
motion of others: $\hat{\mathbf{y}}\sim q_{\theta}(\mathbf{Y}|\mathbf{x})$,
denoting likelihoods as $q_{\theta}(\mathbf{y}|\mathbf{x})\doteq
q_{\theta}(\mathbf{Y}=\mathbf{y}|\mathbf{x})$. If multiple samples are taken,
$\hat{\mathbf{y}}^{k}$ refers to the $k$th sample, and to single out the $n$th
agent we overload notation using $\hat{\mathbf{y}}_{n}$, and use
$\mathbf{y}_{\text{ego}}$ as the AV’s future trajectory. Given such
predictions, the AV controller $\pi$ outputs ego controls
$\mathbf{u}\in\mathcal{U}$ to anticipate and avoid colliding with other
agents’ future trajectories: $\mathbf{u}=\pi(\mathbf{y})$.

We assume our AV stack performs behavior prediction before control, a common
assumption [31]. While conditioning behavior prediction on ego’s intent
provides more accurate prediction, for sake of simplicity we assume that other
agents do not anticipate the AV’s future, only the AV anticipates the other
agents’ future trajectories in order to avoid collisions.

###  III-B Common Prediction Metrics and Objectives

Common prediction metrics in the literature and in prediction benchmarking
challenges–including Argoverse Forecasting [5], Lyft Prediction [12], Waymo
Open Motion [10], and AIODrive [35]–are summarized in Table I:

TABLE I: Common prediction metrics in the literature. Metric Name | Metric Equation
---|---
Average Displacement Error (ADE) | $||\hat{\mathbf{y}}-\mathbf{y}||_{2}$
Final Displacement Error (FDE) | $||\hat{\mathbf{y}}_{T}-\mathbf{y}_{T}||_{2}$
Minimum-ADE (minADE) | $\min_{k\in[K]}||\hat{\mathbf{y}}^{k}-\mathbf{y}||_{2}$
Minimum-FDE (minFDE) | $\min_{k\in[K]}||\hat{\mathbf{y}}^{k}_{T}-\mathbf{y}_{T}||_{2}$
Miss Rate (MR) | $\frac{1}{K}\sum_{k}\mathds{1}[||\hat{\mathbf{y}}^{k}_{T}-\mathbf{y}_{T}||_{2}>\alpha]$
Negative Log Likelihood (NLL): | $-\log q_{\theta}(\mathbf{y}|\mathbf{x})$

Most metrics compare the Euclidean distance between either the full predicted
state-sequence $\hat{\mathbf{y}}$ (or final state $\hat{\mathbf{y}}_{T}$) with
the true sequence $\mathbf{y}$ (or final state $\mathbf{y}_{T}$) an agent
took, as recorded in data. Probabilistic models are typically trained to
minimize the negative log likelihood (NLL) of the data. All such metrics are
agnostic to road geometry and downstream planning, which implicitly assumes
that all other agents’ forecasts are equally relevant. For example, consider
two pedestrians: one walking ahead of the ego vehicle and one behind. Assuming
independent pedestrian motion, the NLL objective factorizes as: $-\log
q_{\theta}(\mathbf{y}^{\text{ahead}},\mathbf{y}^{\text{behind}}|\mathbf{x})=-\log
q_{\theta}(\mathbf{y}^{\text{ahead}}|\mathbf{x})-\log
q_{\theta}(\mathbf{y}^{\text{behind}}|\mathbf{x})$. Notice that this
prediction metric is equally concerned with both $\mathbf{y}^{\text{ahead}}$
and $\mathbf{y}^{\text{behind}}$. Intuitively, accurate prediction of the
pedestrian ahead of the ego vehicle is more important for safe motion planning
since the ego’s planned path is more likely to intersect with
$\mathbf{y}^{\text{ahead}}$ than $\mathbf{y}^{\text{behind}}$. How can
prediction metrics become “aware” that errors in predicting
$\mathbf{y}^{\text{ahead}}$ have greater downstream consequences than errors
in $\mathbf{y}^{\text{behind}}$?

##  IV CAPO: Control-Aware Prediction Objectives

In this section we propose novel prediction loss functions that consider how
predictions will be used downstream to improve predictive accuracy whenever
prediction errors would cause a large change in control outputs. In Bayesian
decision theory, a decision is evaluated as the expected utility of a decision
$\mathbf{u}$ or controller $\pi$, integrating out any uncertainties [4]. In
our case, it is the future trajectories of other agents that are unknown but
can be probabilistically predicted according to a model with parameters
$\theta$. Following the literature on loss-calibrated variational inference
[16, 6, 15], we define the gain of a decision or controller’s value as a
function of the model parameters $\theta$ that we wish to train.

$\displaystyle\text{Gain}_{\mathbf{x},\mathbf{y},\pi}(\theta)\;$
$\displaystyle=\;\int\text{utility}(\pi,\mathbf{y},\hat{\mathbf{y}},\mathbf{x})q_{\theta}(\hat{\mathbf{y}}|\mathbf{x})\text{d}\hat{\mathbf{y}}.$
(1)

The choice of utility function in Eq. 1 is an open one, that is why we
considered many possible input parmeter; it defines how desirable a course of
actions would be given $\mathbf{x}$ and $\hat{\mathbf{y}}$. Alternatively, an
existing metric like the NLL can simply be weighted without integration. In
the next subsection we discuss some baseline choices for the utility or
weight, and after, we propose two novel methods for computing these weights: a
self-attention method and a counterfactual method.

###  IV-A Baseline Objectives

Most predictive metrics in the literature are agnostic to $\mathbf{u}$ and
simply use a delta function to only score correct trajectory predictions,
recovering the standard log likelihood metric:
$\text{Gain}_{\mathbf{x},\mathbf{y}}(\theta)=\int\delta(\mathbf{y}-\hat{\mathbf{y}})q_{\theta}(\hat{\mathbf{y}}|\mathbf{x})\text{d}\hat{\mathbf{y}}=q_{\theta}(\mathbf{y}|\mathbf{x})$.
However, we are interested in utilities that are a function of $\mathbf{u}$ in
order to weight predictions by their downstream effect on the ego’s control.
For instance, we could score trajectory predictions based on the resultant ego
controls $\pi(\hat{\mathbf{y}})$ matching the ego’s behavior under knowledge
of the true future trajectories $\pi(\mathbf{y})$:
$\text{Gain}_{\mathbf{x},\mathbf{y},\pi}(\theta)=\int\delta(\pi(\mathbf{y})-\pi(\hat{\mathbf{y}}))q_{\theta}(\hat{\mathbf{y}}|\mathbf{x})\text{d}\hat{\mathbf{y}}.$
This integral is unfortunately intractable to derive or estimate, but softer
utility functions can be used instead. One example is
$||\pi(\hat{\mathbf{y}})-\pi(\mathbf{y})||_{1}$, which we include as a
baseline in Table II. Optimizing this controller output error guides the
learning process towards predicting controller inputs (predicted trajectories)
accurately, insofar as they result in the correct control. Any trajectory
errors that do not induce a change in the AV’s control are thus considered
inconsequential and ignored.

###  IV-B Attention-based CAPO

We propose a GRU encoder-decoder architecture with an attention mechanism as
introduced in [34]. Our method weights the agent predictions using attention
factors between agents $\mathbf{x}$ and the AV’s future trajectory
$\mathbf{y}_{\text{ego}}$. The predictive model is a function parameterized by
$\theta$ noted
$q_{\theta}:\mathcal{X}\to\mathcal{P}_{\mathcal{Y}\times\mathcal{Y}_{\text{ego}}}$.
We note $\theta=\\{\theta_{\text{ego}},\theta_{\text{agent}}\\}$ where
$\theta_{\text{ego}}$ is the set of parameters for the ego decoder only;
$\mathcal{X}$ is the past observation space and
$\mathcal{P}_{\mathcal{Y}\times\mathcal{Y}_{\text{ego}}}$ the probability
spaces of future trajectories: $\mathcal{P}_{\mathcal{Y}_{\text{ego}}}$ for
the ego and $\mathcal{P}_{\mathcal{Y}}$ for other agents.

We use an architecture similar to [18, 20] where we train a model with multi-
head attention; the ego agent attends the other agents. The ego predictions
are used as a proxy for the actual planner to compute the importance weights
of other agents:

egoagent1$\vdots$agentNEncoder egoEncoder agent$\vdots$Encoder agent

Ego-attention

Ego-attention

Ego-attention

Decoder egoDecoder agent$\vdots$Decoder agentprediction egoprediction
agent1$\vdots$prediction agentN Figure 2: Diagram of the attention model. All
agent encoders and decoders share their weights. Encoders and decoders are
GRUs. Attention is not used between agents.

The ego-attention blocks in figure Fig. 2 are heads of a multi-head attention
mechanism. The computation performed by each head is given below:

$\begin{matrix}\text{ego}\\\
\text{encoding}\end{matrix}$$\vdots$$\begin{matrix}\text{agent}_{N}\\\
\text{encoding}\end{matrix}$$L_{k}$$L_{v}$$L_{q}$$\mathbf{q}_{0}$$\mathbf{k}_{0}$$\mathbf{v}_{0}$$L_{k}$$L_{v}$$\mathbf{k}_{n}$$\mathbf{v}_{n}$
$Q=\left(\begin{matrix}\mathbf{q}_{0}\end{matrix}\right)$
$K=\left(\begin{matrix}\mathbf{k}_{0}\\\ \vdots\\\
\mathbf{k}_{n}\end{matrix}\right)$ $V=\left(\begin{matrix}\mathbf{v}_{0}\\\
\vdots\\\ \mathbf{v}_{n}\end{matrix}\right)$
$\sigma\left(\frac{QK^{T}}{\sqrt{d_{k}}}\right)V$$\begin{matrix}\text{output}\\\
\text{encoded}\end{matrix}$ Figure 3: Diagram of a self-attention head.

The attention vector is given by

$\alpha=\sigma\left(\frac{QK^{\top}}{\sqrt{d_{k}}}\right)=[\alpha_{0},...,\alpha_{N}],$
(2)

where $Q$ is the query matrix, $K$ the key matrix, and $\sigma$ the softmax
operation that normalizes the attention vector (for details see [34]). The
encoded $\text{output}=\alpha V$ is a weighted mean of the value vectors over
the agents (including ego).

Inspired by [20] the attention model produces outputs in the form of a
sequence of Gaussian mixtures for each agent and is trained to minimize the
NLL for all agents and the ego trajectory predictions. However, for our
application, ego prediction is not the goal, it is only a proxy to compute the
importance weights of the pedestrian contribution to the ego behavior.

We propose to use attention coefficients $\alpha$ as importance factors in a
weighted sum of per-human state prediction loss (as opposed to uniform
weighting). Algorithm 1 summarizes how the model is trained with importance
weighting. If multiple heads are used, the attention coefficients are
averaged:

$\displaystyle w_{n}=\frac{1}{H}\sum_{h=1}^{H}\alpha_{n}^{(h)}$ (3)

The attention predictor imitates a planner that interacts with the other
agents to avoid collision. The only way for the predictor to interact with the
other agents is through attention. Therefore, as the model learns the
correlations between the planner’s trajectories and the agents trajectories,
larger attention coefficients are given to the agents that cause larger
reactions from the controller. It learns this offline and does not need access
to the controller nor its gradient.

Predicting jointly the ego trajectory and the other agents allows us to use
the attention coefficients for concern weighting in a single run. The
coefficient $\alpha_{0}$ quantifies how independent the ego is from other
agents.

This method defines a concern about an agent but not about specific
trajectories of that agent. It can define the concern without using the
controller because it instead uses an offline-learned model that imitates the
controller.

Input: Controller: $\pi:\mathcal{X}\to\mathcal{U}$

1: Record trajectory data $\mathcal{D}=\\{\mathbf{x},\mathbf{y}\\}_{i}$

2: while training do

3: Sample batch $\mathbf{x},\mathbf{y}\sim\mathcal{D}$

4: Run model to estimate $\hat{\mathbf{y}}_{\text{ego}}$ and
$\hat{\mathbf{y}}_{\text{agent}}$ from $\mathbf{x}$

5: Get attention: $\alpha(\mathbf{x})$ $\triangleright$ Eq. 2

6: Compute weight: $w(\alpha(\mathbf{x}))$ $\triangleright$ Eq. 3

7: Update model:
$\theta_{\text{ego}}\leftarrow\theta_{\text{ego}}+\nabla_{\theta_{\text{ego}}}\log
q_{\theta}(\mathbf{y}_{\text{ego}}|\mathbf{x})$$\theta_{\text{agent}}\leftarrow\theta_{\text{agent}}+w(\mathbf{x})\nabla_{\theta_{\text{agent}}}\log
q_{\theta}(\mathbf{y}_{\text{agent}}|\mathbf{x})$

Output: Predictive model
$q_{\theta}:\mathcal{X}\to\mathcal{P}_{\mathcal{Y}\times\mathcal{Y}_{\text{ego}}}$

Algorithm 1 Attention CAPO

###  IV-C Counterfactual Action-Discrepancy CAPO

Our second proposal can also be formulated as a re-weighted maximization
objective, where we weight the log likelihood of each agent’s trajectory in a
scene by its individual contribution to the ego’s control decision. We do this
by first enumerating through each agent in a scene, and computing
counterfactual outputs from the AV’s controller if every agent traversed their
individual trajectory as recorded in the replay buffer, except for agent $n$.
If we resample the trajectory that the $n$th agent might otherwise have taken,
$\hat{\mathbf{y}}^{k}_{n}\sim q_{\theta}(\mathbf{Y}_{n}|\mathbf{x})$, we can
compute the control output that would result:

$\hat{\mathbf{u}}^{k}_{n}\;=\;\pi(\\{\hat{\mathbf{y}}^{k}_{n}\\}\cup\mathbf{y}\setminus\\{\mathbf{y}_{n}\\}),$
(4)

to compare against the control had no agent deviated from their recorded
trajectories:

$\mathbf{u}\;=\;\pi(\mathbf{y}).$ (5)

The difference in these two hypothetical controls corresponds to how much an
individual agent affects the ego vehicle, and can represent the concern
associated with predicting this particular agent in this particular instance
accurately. If the model is probabilistic, then taking multiple samples
$(K>1)$ helps ensure high importance even if the other agent only might cause
a control deviation:

$w_{n}\;=\;\max_{k\in\\{1..K\\}}||\mathbf{u}-\hat{\mathbf{u}}^{k}_{n}||_{1},$
(6)

which we use as weights for predictive model training:

$\theta^{*}\;=\;\operatorname*{arg\,max}_{\theta}\sum_{n=1}^{N}w_{n}\log
q_{\theta}(\mathbf{y}_{n}|\mathbf{x}).$ (7)

We summarize our counterfactual action discrepancy method in Algorithm 2. One
benefit of this approach compared to the attention CAPO is that it is

Input: Controller: $\pi:\mathcal{X}\to\mathcal{U}$

1: Record trajectory data $\mathcal{D}=\\{\mathbf{x},\mathbf{y}\\}_{i}$

2: while training do

3: Sample batch $\mathbf{x},\mathbf{y}\sim\mathcal{D}$

4: Compute hypothetical controls: $\mathbf{u},\hat{\mathbf{u}}^{k}_{n}$
$\triangleright$ Eq. 4–(5)

5: Compute weight: $w(\mathbf{u},\hat{\mathbf{u}}^{k}_{n})$ $\triangleright$
Eq. 6

6: Update model:
$\theta\leftarrow\theta+w(\mathbf{u},\hat{\mathbf{u}}^{k}_{n})\nabla_{\theta}\log
q_{\theta}(\mathbf{y}|\mathbf{x})$

Output: Predictive model $q_{\theta}:\mathcal{X}\to\mathcal{P}_{\mathcal{Y}}$

Algorithm 2 Counterfactual CAPO

###  IV-D Summary of Objectives

There are various choices for utilities, or weights for traditional module
metrics. In Table II we summarize the several baselines methods, including NLL
and our novel proposals.

TABLE II: Comparison of utilities and weighted objectives.

Method | Utility or Weight | Objective $\mathcal{L}(\theta)$
---|---|---
Baselines: |  |
Attention | $\delta(\mathbf{y}-\hat{\mathbf{y}})$ | $q_{\theta}(\mathbf{y}|\mathbf{x})+q_{\theta}(\mathbf{y}_{\text{ego}}|\mathbf{x})$
R2P2 $\text{Gain}_{\mathbf{y}}$ | $\delta(\mathbf{y}-\hat{\mathbf{y}})$ | $q_{\theta}(\mathbf{y}|\mathbf{x})$
R2P2 $\text{Gain}_{\pi 1}$ | $||\pi(\mathbf{y})-\pi(\hat{\mathbf{y}})||_{1}$ | $\mathbb{E}_{\hat{\mathbf{y}}}\left[||\pi(\mathbf{y})-\pi(\hat{\mathbf{y}})||_{1}\right]$
R2P2 $\text{Weight}_{\nabla\hat{\mathbf{y}}}$ | $||\nabla_{\hat{\mathbf{y}}}\pi(\hat{\mathbf{y}})||_{1}$ | $\mathbb{E}_{\hat{\mathbf{y}}}\left[||\nabla_{\hat{\mathbf{y}}}\pi(\hat{\mathbf{y}})||_{1}\right]q_{\theta}(\mathbf{y}|\mathbf{x})$
R2P2 $\text{Weight}_{\nabla\mathbf{y}}$ | $||\nabla_{\mathbf{y}}\pi(\mathbf{y})||_{1}$ | $||\nabla_{\mathbf{y}}\pi(\mathbf{y})||_{1}q_{\theta}(\mathbf{y}|\mathbf{x})$
Ours: |  |
R2P2 $\text{Weight}_{\pi}$ | $||\pi(\mathbf{y})\\!-\\!\pi(\hat{\mathbf{y}})||_{1}$ | $\mathbb{E}_{\hat{\mathbf{y}}}\left[||\pi(\mathbf{y})\\!-\\!\pi(\hat{\mathbf{y}})||_{1}\right]q_{\theta}(\mathbf{y}|\mathbf{x})$
R2P2 $\text{Weight}_{\pi k}$ | $\max_{k}||\pi(\mathbf{y})\\!-\\!\pi(\hat{\mathbf{y}}^{k})||_{1}$ | $\max_{k}||\pi(\mathbf{y})\\!-\\!\pi(\hat{\mathbf{y}}^{k})||_{1}q_{\theta}(\mathbf{y}|\mathbf{x})$
AttentionWeight | $\alpha(\mathbf{x})$ | $\alpha(\mathbf{x})q_{\theta}(\mathbf{y}_{\text{agent}}|\mathbf{x})+q_{\theta}(\mathbf{y}_{\text{ego}}|\mathbf{x})$

##  V Experimental Evaluation

To evaluate our proposed method, we consider a representative scenario that is
commonplace in autonomous driving: pedestrian trajectory prediction. The
majority of pedestrian behaviors can safely be ignored by the AV’s autonomy
stack; however, in rare cases of pedestrian-ego interaction (e.g., road
crossings), accurate prediction of pedestrian behavior becomes crucial in
avoiding collisions. This sparsity of interaction showcases how predictive
models may perform well with respect to traditional metrics (e.g., ADE) while
still leading to suboptimal ego behavior when it matters most. Here, we first
detail our experimental evaluation and implementation of the aforementioned
scenario within the CARLA autonomous driving simulator [9]. We then compare
results between our method and the various baselines discussed in Table II,
where our experiments show that predictive models trained using our CAPO
methods produce safe behavior with fewer collisions relative to other
baselines.

Figure 4: Pedestrian Prediction Scenario. Pedestrians spawn on the sidewalk
(yellow region) and the ego (red car) predicts the pedestrian trajectories
within the next 3 seconds (green). Some pedestrians will cross the road at
right angles (blue). Left: the planner predicts a collision with a crossing
pedestrian and starts slowing (red ego prediction up to blue line but not
further). Right: ego is safely passing the road segment where the pedestrian
has already crossed.

###  V-A CARLA Scenario Design

We implement our pedestrian prediction scenario in the CARLA simulator’s
Town01. A single ego vehicle is commanded to drive down a road that is
adjacent to sidewalks which are populated with pedestrians. Occasionally, a
pedestrian will cross the street and the ego agent must slow to avoid a
collision when necessary.

The ego vehicle adjusts its longitudinal control to balance the competing
priorities of maintaining the current speed limit (45mph) while avoiding
collisions with the pedestrians crossing the road (either their current or
future-predicted distance on the road ahead). This balance is performed by an
Intelligent Driver Model [33]. It uses predictions to estimate the closest
collision distance and controls the vehicle to stop ahead of that point.

Pedestrians spawn at random locations on the sidewalk and are then provided a
long-range navigation goal that is also uniformly sampled from the sidewalk.
When the long-range goal is reached, another is sampled to replace it. To
induce pseudo-random motion, a short-range goal is also generated at each time
step. This goal is generated by projecting a point 4m along the path to the
long-range goal, starting at the pedestrian’s location. The lateral offset
$\beta_{t+1}$ of the short-range goal is generated by sampling from a normal
distribution centered about the previous lateral offset $\beta_{t}$ after it
has been scaled down (to drive it towards the long-range goal):

$\beta_{t+1}=(1-\varepsilon)\beta_{t}+\mathcal{N}(0,\sigma^{2}),$ (8)

where $\sigma$ is the variance of the noise, and $\epsilon\in[0,1)$ is the
commitment to the long-range goal.

When on the sidewalk, pedestrians are programmed to walk at speeds sampled
about 2m/s while navigating around other pedestrians to avoid collisions and,
occasionally, will pause outside of shops. Each different kind of pedestrian
is defined with various noise levels, commitment, and stopping chance.

Pedestrians may also randomly decide to cross the road. The probability
increases if their velocity vector points towards the road and increases
greatly when the pedestrian is close to the road. While crossing, they travel
at 2 m/s in the shortest path possible, i.e., perpendicular to the road
direction. To increase task difficulty, the probability that pedestrians will
cross the road is increased at test time.

TABLE III: Scenario results, 100 episodes. Arrows indicate higher/lower
preferred. Standard errors shown. Best, second.

Predictive Model | Success Rate $\uparrow$ | Collisions $\downarrow$ | Speed (m/s) $\uparrow$ | Jerk (m/$\text{s}^{-3}$) $\downarrow$ | ADE (m) $\downarrow$ | Control Error $\downarrow$
---|---|---|---|---|---|---
Baselines: |  |  |  |  |  |
R2P2 $\text{Gain}_{\mathbf{y}}$ | $89.0\%$ | $11$ |  $9.97$ $\pm 0.222$ |  $8.92$ $\pm 0.250$ |  $2.09$ $\pm 0.024$ |  $0.59$ $\pm 0.012$
R2P2 $\text{Gain}_{\pi 1}$ | $85.0\%$ | $14$ |  $10.45$ $\pm 0.268$ |  $6.65$ $\pm 0.196$ |  $3.48$ $\pm 0.038$ |  $0.63$ $\pm 0.016$
R2P2 $\text{Weight}_{\nabla\hat{\mathbf{y}}}$ | $\underline{94.0}\%$ | $\underline{4}$ |  $9.53$ $\pm 0.216$ |  $8.21$ $\pm 0.140$ |  $\mathbf{1.98}$ $\pm 0.024$ |  $0.60$ $\pm 0.012$
R2P2 $\text{Weight}_{\nabla\mathbf{y}}$ | $91.0\%$ | $9$ |  $9.74$ $\pm 0.216$ |  $8.74$ $\pm 0.184$ |  $\underline{2.00}$ $\pm 0.025$ |  $0.60$ $\pm 0.011$
Attention [20] | $89.0\%$ | $11$ |  $\underline{13.79}$ $\pm 0.214$ |  $\underline{4.48}$ $\pm 0.147$ |  $2.61$ $\pm 0.050$ |  $0.63$ $\pm 0.026$
Oracle distribution | $98.0\%$ | $2$ |  $10.54$ $\pm 0.231$ |  $6.80$ $\pm 0.180$ |  $1.58$ $\pm 0.036$ |  $0.51$ $\pm 0.013$
Our methods: |  |  |  |  |  |
R2P2 $\text{Weight}_{\pi}$ | $93.0\%$ | $7$ |  $8.86$ $\pm 0.188$ |  $9.26$ $\pm 0.194$ |  $2.29$ $\pm 0.022$ |  $\underline{0.58}$ $\pm 0.010$
R2P2 $\text{Weight}_{\pi k}$ | $\mathbf{99.0}\%$ | $\mathbf{1}$ |  $9.46$ $\pm 0.196$ |  $7.89$ $\pm 0.159$ |  $2.14$ $\pm 0.018$ |  $\mathbf{0.55}$ $\pm 0.011$
Attention Weightα | $91.0\%$ | $9$ |  $\mathbf{14.36}$ $\pm 0.217$ |  $\mathbf{4.22}$ $\pm 0.154$ |  $2.58$ $\pm 0.053$ |  $0.64$ $\pm 0.024$

###  V-B Compared models

1) Oracle distribution The pedestrian behavior is simulated with a known
distribution at each time step, which is sampled to produce a trajectory. The
trajectory distribution is approximated by sampling $K=5$ trajectories for
each pedestrian. The planner reacts to the trajectory that would cause the
closest intersection with its desired path. This method is a perfect
probabilistic predictor which is only accessible with simulated data. Its
predictions are not biased towards sampling the most critical trajectories,
and planning with relatively few samples can yield suboptimal results.

2) Gradient weighting Recent work has also investigated weighing prediction
objectives by a measure of local sensitivity of downstream costs to individual
predictions [13]. This method first learns a cost function to evaluate ego
controls given predicted human trajectories, using inverse optimal control.
The method then weights each prediction loss by the gradient magnitude of the
cost outputs w.r.t. the prediction inputs. In our work, we consider using the
controller directly, forming an analogous baseline: using gradients of the
controller w.r.t. the predicted trajectory
$||\nabla_{\hat{\mathbf{y}}}\pi(\hat{\mathbf{y}})||_{1}$, or true trajectory
$||\nabla_{\mathbf{y}}\pi(\mathbf{y})||_{1}$, included in Table II. While we
do not assume differentiable controllers in our own methods, we nevertheless
experiment using a differentiable controller to compare against this baseline
method.

3) Attention weighting As presented in section IV-B. We train this model with
our CAPO method (algorithm 1) and, as a baseline, we compare it with the
unbiased prediction as the predictor [20] would produce.

4) Reparameterized Pushforward Policy (R2P2) we use the likelihood-based
multi-agent prediction algorithm R2P2 [29] as baseline
$\text{Gain}_{\mathbf{y}}$, and also use R2P2 as the base model for all other
predictive models apart from the attention model. R2P2 is a autoregressive
normalizing flow, capable of expressing multimodal agent trajectories, trained
with NLL. We parameterize R2P2 to predict 30 steps with data at 10Hz,
corresponding to a 3s prediction for all pedestrians. We train it with our
CAPO method (algorithm 2) using $K=10$ samples and we use $K=1$ samples at
test time.

###  V-C Metrics

The table III presents our results for 100 sequences. We track the performance
of the system (prediction and planner) with the success rate and the number of
collisions. Three conditions may end a sequence:

  * •

Success: vehicles traverses 200m road without incident.

  * •

Collision: a pedestrian was hurt.

  * •

Time out: the car was too slow ($>60$s).

We also score efficiency and comfort indicators by average speed and average
jerk respectively. Finally, we compute the average pedestrian trajectory
prediction errors and their downstream effect on the planner with the average
displacement error (ADE) and the Control Error equal to
$||\pi(\mathbf{y})-\pi(\hat{\mathbf{y}})||_{1}$. The control error measures
the downstream effect of the prediction error on the ego’s plans.

##  VI Discussion

The results in Table III show that while all methods do reasonably well,
weighting predictive objectives by their downstream effect improves the
downstream performance as illustrated by a low collision count and control
error. While methods such as R2P2 $\text{Weight}_{\nabla\hat{\mathbf{y}}}$
assume a differentiable controller, we find this assumption does not need to
be made, and our methods can work with any type of controller. While our
methods did not score as well on the ADE metric of agents’ trajectories, they
did score best on the metrics that matters more: the control error, and
success rate, thus mitigating error propagated downstream and improving the
end task performance. We compared to objectives weighted by the planner’s
sensitivity $||\nabla_{\hat{\mathbf{y}}}\pi(\hat{\mathbf{y}})||_{1}$, which is
related to [13]. However, such methods assume the planner (or cost function)
is differentiable, which is often not the case in real AV systems. Secondly,
local sensitivity to a point prediction is not necessarily a good measure of
relevance if the gradient is noisy or changes drastically over the full
predictive distribution. While our experiments show encouraging results,
testing with various setups and environments would be needed to give a clear
best method.

##  VII Conclusions

Modular autonomous systems (such as those commonly used in AVs) provide a
number of advantages, but generally incur the disadvantage that individual
components typically do not optimize for system-wide or downstream performance
metrics directly. In this paper, we proposed weighted objectives for learning
prediction models that account for the downstream objective without imposing
stringent requirements on downstream components (such as end-to-end
differentiability). These objectives weight the usual likelihood objective,
either using attention weights derived from a behavior-cloned policy, or using
the impact that substituting predicted trajectories for ground-truth
trajectories has on planner output. Accounting for the downstream objective in
this manner encourages prediction models to focus on what’s important – either
at the agent or individual trajectory level – and, as a result, improves
system-wide performance, as we showed empirically in a pedestrian jaywalking
scenario.

A number of promising avenues exist for future research. First, control-aware
objectives may provide out-of-domain generalization benefits by encouraging
prediction models to focus on relevant aspects of the scene, and ignore
spurious sources of information that are safe to ignore. Second, in this paper
we focused on data collected from an expert. However, this requirement limits
the applicability of the proposed metrics, and a broader coverage of the
state-space resulting from the use of both expert and suboptimal data might
improve the learned prediction models. Finally, although we focused in this
paper on using control-aware weighting for optimizing prediction models. Our
method might equally well be used to define a weighted metric for evaluating
models in a validation setting where training-time access to the downstream
planner is either not available or undesirable.

## References

  * [1] Romina Abachi, Mohammad Ghavamzadeh and Amir-massoud Farahmand  “Policy-aware model learning for policy gradient methods”  In _arXiv preprint arXiv:2003.00030_ , 2020
  * [2] Alex Ayoub et al.  “Model-based reinforcement learning with value-targeted regression”  In _International Conference on Machine Learning_ , 2020, pp. 463–474  PMLR
  * [3] Somil Bansal et al.  “Goal-driven dynamics learning via Bayesian optimization”  In _Conference on Decision and Control (CDC)_ , 2017, pp. 5168–5173  IEEE
  * [4] David Barber  “Bayesian reasoning and machine learning”  Cambridge University Press, 2012
  * [5] Ming-Fang Chang et al.  “Argoverse: 3D tracking and forecasting with rich maps”  In _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2019, pp. 8748–8757
  * [6] Adam D Cobb, Stephen J Roberts and Yarin Gal  “Loss-calibrated approximate inference in Bayesian neural networks”  In _arXiv preprint arXiv:1805.03901_ , 2018
  * [7] Marc Deisenroth and Carl Rasmussen  “PILCO: A model-based and data-efficient approach to policy search”  In _International Conference on machine learning (ICML)_ , 2011, pp. 465–472  Citeseer
  * [8] Priya Donti, Brandon Amos and Zico Kolter  “Task-based End-to-end Model Learning in Stochastic Optimization”  In _Neural Information Processing Systems (NeurIPS)_ 30, 2017, pp. 5490–5500  URL: https://proceedings.neurips.cc/paper/2017/file/3fc2c60b5782f641f76bcefc39fb2392-Paper.pdf
  * [9] Alexey Dosovitskiy et al.  “CARLA: An open urban driving simulator”  In _Conference on robot learning_ , 2017, pp. 1–16  PMLR
  * [10] Scott Ettinger et al.  “Large Scale Interactive Motion Forecasting for Autonomous Driving: The Waymo Open Motion Dataset”  In _arXiv preprint arXiv:2104.10133_ , 2021
  * [11] Amir-massoud Farahmand, Andre Barreto and Daniel Nikovski  “Value-aware loss function for model-based reinforcement learning”  In _Artificial Intelligence and Statistics_ , 2017, pp. 1486–1494  PMLR
  * [12] John Houston et al.  “One thousand and one hours: Self-driving motion prediction dataset”  In _arXiv preprint arXiv:2006.14480_ , 2020
  * [13] Boris Ivanovic and Marco Pavone  “Rethinking Trajectory Forecasting Evaluation”  In _arXiv preprint arXiv:2107.10297_ , 2021
  * [14] Joshua Joseph et al.  “Reinforcement learning with misspecified model classes”  In _International Conference on Robotics and Automation (ICRA)_ , 2013, pp. 939–946  IEEE
  * [15] Tomasz Kuśmierczyk, Joseph Sakaya and Arto Klami  “Variational Bayesian decision-making for continuous utilities”  In _arXiv preprint arXiv:1902.00792_ , 2019
  * [16] Simon Lacoste–Julien, Ferenc Huszár and Zoubin Ghahramani  “Approximate inference for the loss-calibrated Bayesian”  In _Proceedings of the Fourteenth International Conference on Artificial Intelligence and Statistics_ , 2011, pp. 416–424  JMLR WorkshopConference Proceedings
  * [17] Nathan Lambert, Brandon Amos, Omry Yadan and Roberto Calandra  “Objective mismatch in model-based reinforcement learning”  In _arXiv preprint arXiv:2002.04523_ , 2020
  * [18] Edouard Leurent and Jean Mercat  “Social attention for autonomous decision-making in dense traffic”  In _arXiv preprint arXiv:1911.12250_ , 2019
  * [19] Rowan McAllister et al.  “Concrete problems for autonomous vehicle safety: advantages of Bayesian deep learning”, 2017  International Joint Conferences on Artificial Intelligence
  * [20] Jean Mercat et al.  “Multi-head attention for multi-modal joint vehicle motion forecasting”  In _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , 2020, pp. 9638–9644  IEEE
  * [21] Thomas M Moerland, Joost Broekens and Catholijn M Jonker  “Model-based reinforcement learning: A survey”  In _arXiv preprint arXiv:2006.16712_ , 2020
  * [22] Anusha Nagabandi et al.  “Learning image-conditioned dynamics models for control of underactuated legged millirobots”  In _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2018, pp. 4606–4613  IEEE
  * [23] Suraj Nair, Silvio Savarese and Chelsea Finn  “Goal-aware prediction: Learning to model what matters”  In _International Conference on Machine Learning_ , 2020, pp. 7207–7219  PMLR
  * [24] NHTSA  “PE 16-007” Tesla Crash Preliminary Evaluation Report, 2017  URL: https://static.nhtsa.gov/odi/inv/2016/INCLA-PE16007-7876.PDF
  * [25] NTSB  “Preliminary Report Highway: HWY18MH010” Highway Accident Report Collision Between Vehicle Controlled by Developmental Automated Driving System and Pedestrian Tempe, Arizona, 2019  URL: https://www.ntsb.gov/investigations/AccidentReports/Reports/HAR1903.pdf
  * [26] Jonah Philion, Amlan Kar and Sanja Fidler  “Learning to evaluate perception models using planner-centric metrics”  In _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2020, pp. 14055–14064
  * [27] Andrea Piazzoni, Jim Cherian, Martin Slavik and Justin Dauwels  “Modeling Sensing and Perception Errors towards Robust Decision Making in Autonomous Vehicles”  In _arXiv preprint arXiv:2001.11695_ , 2020
  * [28] Siddharth Reddy, Anca D Dragan and Sergey Levine  “Pragmatic Image Compression for Human-in-the-Loop Decision-Making”  In _arXiv preprint arXiv:2108.04219_ , 2021
  * [29] Nicholas Rhinehart, Kris Kitani and Paul Vernaza  “R2P2: A ReparameteRized Pushforward Policy for Diverse, Precise Generative Path Forecasting”  In _European Conference on Computer Vision_ , 2018  Springer
  * [30] Abbas Sadat et al.  “Perceive, Predict, and Plan: Safe Motion Planning Through Interpretable Semantic Representations”  In _ECCV_ , 2020
  * [31] Wilko Schwarting, Javier Alonso-Mora and Daniela Rus  “Planning and decision-making for autonomous vehicles”  In _Annual Review of Control, Robotics, and Autonomous Systems_ 1 Annual Reviews, 2018, pp. 187–210
  * [32] Skanda Shridhar et al.  “Beelines: Evaluating Motion Prediction Impact on Self-Driving Safety and Comfort”  In _arXiv preprint arXiv:2011.00393_ , 2020
  * [33] Martin Treiber, Ansgar Hennecke and Dirk Helbing  “Congested traffic states in empirical observations and microscopic simulations”  In _Physical review E_ 62.2 APS, 2000, pp. 1805
  * [34] Ashish Vaswani et al.  “Attention is all you need”  In _Advances in neural information processing systems_ 30, 2017
  * [35] Xinshuo Weng et al.  “All-In-One Drive: A Comprehensive Perception Dataset with High-Density Long-Range Point Clouds”, 2021
  * [36] Sen Wu, Hongyang R. Zhang and Christopher Ré  “Understanding and Improving Information Transfer in Multi-Task Learning”  In _ICLR_ , 2020