Deep Structured Reactive Planning

As mentioned in the main paper, we assume that the trajectory history of the other actors, including their bounding box width/height and heading, are known to the ego-agent at the given timestep. Hence, we directly feed the trajectories of all actors to our model, transformed to the current ego-agent coordinates.

In addition, we add dataset-dependent context to the model. In UrbanCity and Nuscenes, we use both LiDAR sweeps and HD maps as context. Meanwhile, we directly use the input representation provided by the CARLA dataset [42], which contains a rasterized LiDAR representation but no map data.

Here, we present additional details of the actor-specific and interaction energies of our model. As mentioned in the main paper, the actor-specific energies are parameterized with neural nets, consisting of a few core components. The first component is a backbone network that takes in the input representations to compute intermediate spatial feature maps. Then, given the past trajectory for each actor, we sample $K$ trajectories for each actor using a realistic trajectory sampler, given in Sec. A-D. These future actor trajectory samples, as well as the past actor trajectories and backbone feature map, are in turn passed to our unary module to predict the actor-specific energies for each actor trajectory. Finally, our interaction energy is determined by computing collision and safety distance violations between actor trajectories.

Our interaction energy is a pairwise energy between two actor trajectory samples that contains two non-learnable components: collision cost and safety distance cost. The collision detection is efficiently implemented using a GPU kernel computing the IOU among every pairwise sample from two actors, given their timestep, positions, bounding box width/height, and heading. The output is a $N\times N\times K\times K$ matrix, where a given entry is 1 if the trajectory sample of actor $i$ and trajectory sample of actor $j$ collide at any point in the future, and 0 if not. The collision energy is only computed over future samples, not past samples.

Similarly, the safety distance violation is computed over all future actor samples. For a given pairwise sample between actor $i$ and actor $j$ at a given timestep, the distance from the center point of actor $i$ is computed to the polygon of actor $j$ (minimal point-to-polygon distance). If the distance is within a given safety threshold, then the energy is the squared distance of violation within the threshold. Note that unlike collision energy, this matrix is not symmetric. This choice was made to use a GPU kernel that efficiently computes point distances to polygons in parallel.

We follow the discrete trajectory sampler used in [59, 60]. The sampler first estimates the initial speed/heading of each actor given the provided past trajectory. From these values, the sampler samples from three trajectory modes: a straight line, circular trajectory, or spiral trajectory with $[0.3,0.2,0.5]$ probability. Within each mode, the control parameters such as radius, acceleration are uniformly sampled within a range to generate a sampled trajectory. We use 50 trajectories per actor (including the ego-agent) for CARLA, and 100 trajectories per actor on UrbanCity and Nuscenes. Additional details can be found in [59, 60].

We mention in Sec. III-B of the main paper that we exclude the actor-actor interaction term as a cost from our reactive planning objective. The primary reason for this is due to computational reasons. To illustrate this, we distribute the full expectation in the objective over the costs:

First, we note that the last summation term implies computing a full $N\times N\times K\times K$ matrix (containing both the interaction cost between any pair of samples from two actors, as well as the probabilities) for every value of ${\mathbf{y}}_{0}$. For our values of $N,K$, one of these matrices will generally fit in memory on a Nvidia 1080Ti GPU, but additionally batching by the number of ego-agent samples (which is $N$) will not. Moreover, we note that the Loopy Belief Propagation algorithm used for obtaining actor marginals will provide marginal probabilities $p_{{\mathbf{y}}_{i}}$ and pairwise probabilities $p_{{\mathbf{y}}_{i},{\mathbf{y}}_{j}}$ [37] for all actor samples, which directly gives us the conditional actor marginal probabilities $p_{{\mathbf{y}}_{i}|{\mathbf{y}}_{0}}$ with one LBP pass. However, $p_{{\mathbf{y}}_{i},{\mathbf{y}}_{j}|{\mathbf{y}}_{0}}$ is not readily provided by the algorithm, requiring us to run LBP for every value of ${\mathbf{y}}_{0}$ to obtain these conditional pairwise marginals.

We acknowledge that the actor-actor interaction term can capture situations that the actor-specific term does not, specifically where the ego-agent’s actions led to dangerous interactions between two other actors (e.g. the SDV causes a neighboring car to swerve and narrowly collide with another car). We can potentially approximate the term by only considering neighboring actors to the SDV – we leave this for future work.

We observe an additional level of flexibility in this model: being able to interpolate between our reactive/non-reactive objectives, which have thus far been presented as distinct. A potential advantage of interpolation is the ability to customize between conservative, non-reactive driving to efficient navigation depending on the user’s preference. Recall that our reactive objective is defined as $f={\mathbb{E}}_{{\mathcal{Y}}_{r}\sim p({\mathcal{Y}}_{r}|{\mathbf{y}}_{0},{\mathcal{X}};{\mathbf{w}})}[C({\mathcal{Y}},{\mathcal{X}};{\mathbf{w}})]$, which can be simplified into

(see Sec. III-B, III-C). Similarly, our non-reactive baseline objective is defined as $f_{\text{nonreactive}}={\mathbb{E}}_{{\mathcal{Y}}_{r}\sim p({\mathcal{Y}}_{r}|{\mathcal{X}};{\mathbf{w}})}[C({\mathcal{Y}},{\mathcal{X}};{\mathbf{w}})]$, and can be simplified into

The key to interpolation between these two objectives lies within our conditional prediction model for a given actor ${\mathbf{y}}_{i}$ within the reactive objective: $p({\mathbf{y}}_{i}|{\mathbf{y}}_{0},{\mathcal{X}};{\mathbf{w}})$, currently conditioned on a single ego-agent plan. We can modify the conditioning to be on a set: $S^{{\mathbf{y}}_{0}}$ with $k,1\leq k\leq K$ elements, which are the top-$k$ candidate trajectories closest to ${\mathbf{y}}_{0}$ by L2 distance. Then, we define $p({\mathbf{y}}_{i}|S^{{\mathbf{y}}_{0}},{\mathcal{X}};{\mathbf{w}})=\frac{1}{Z}\sum_{\bar{{\mathbf{y}}}_{0}\in S^{{\mathbf{y}}_{0}}}{p({\mathbf{y}}_{i},\bar{{\mathbf{y}}}_{0},{\mathcal{X}};{\mathbf{w}})}$, where $Z$ is a normalizing constant. Intuitively, conditioning actor predictions on this set implies that actors do not know the exact plan that the SDV has, but may have a rough idea about the general intent. When $|S^{{\mathbf{y}}_{0}}|$ is 1, we obtain our reactive model. When $|S^{{\mathbf{y}}_{0}}|$ is $K$, it is straightforward to see that $Z=1$, and hence we obtain our actor marginals $p({\mathbf{y}}_{i}|{\mathcal{X}};{\mathbf{w}})$ used in the non-reactive model. Moreover, when $|S^{{\mathbf{y}}_{0}}|=K$ the actor-specific cost term $\sum_{i,{\mathbf{y}}_{i}}p_{{\mathbf{y}}_{i}|S^{{\mathbf{y}}_{0}}}C_{\text{traj}}^{{\mathbf{y}}_{i}}=\sum_{i,{\mathbf{y}}_{i}}p_{{\mathbf{y}}_{i}}C_{\text{traj}}^{{\mathbf{y}}_{i}}$ no longer depends on the candidate SDV trajectory ${\mathbf{y}}_{0}$; hence we can remove it from the planing objective, which results in the non-reactive objective.

Please see Sec. D for experimental results in our simulation scenarios at different conditioning set sizes.

Additionally, it is fairly straightforward to show that the non-reactive objective is closely related to maximizing the marginal likelihood of the ego-agent. Let the marginal likelihood of the ego-agent be denoted as $p({\mathbf{y}}_{0}|{\mathcal{X}};{\mathbf{w}})$.

The lower bound in 18 is obtained through Jensen’s inequality. Now, note that if we try to maximize this term through ${\mathbf{y}}_{0}$, we obtain our non-reactive planning objective (which is a minimization over the joint costs):

This implies that maximizing the marginal likelihood of the SDV trajectory under our model can be considered as a non-reactive planner.

The ESP architecture largely follows the details specified in the original paper, with slight modifications similar to the insights discovered in [10]. First, we use the same whisker featurization scheme as specified in the paper, but due to memory limitations in UrbanCity we sample from a set of three radii $[1,2,4]$ as opposed to the original 6. Our past trajectory encoder is a GRU with hidden state 128 that sequentially runs across the past time dimension and takes in the vehicle-relative coordinates, width/height, and heading as inputs. Moreover, given that our scenes can have a variable number of actors as opposed to constant number in the original paper, we use $k$-nearest neighbors with $k=4$ to select the nearest neighbor features at every future timestep. Finally, we found that in the autoregressive model setting, training using direct teacher forcing [26] by conditioning the next state on the ground-truth current state caused a large mismatch between training and inference. Instead, we add white noise of 0.2m to the conditioning ground-truth states during training to better reflect error during inference.

The PRECOG planning objective is given by:

where the second term represents the goal likelihood, and the first term represents the ”multi-agent” prior, which is a joint density term that can be readily evaluated by the model. In order to plan with the PRECOG objective, one must optimize the ego-agent latent $\mathbf{z}^{r}$ over an expectation of latents sampled from other actors $\mathbf{Z}^{h}$.

The joint density term can be evaluated by computing the log-likelihood according to the decoded Gaussian at each timestep $\log q(f(\mathbf{Z}))=\log q(S)=\sum_{t=1}^{T}q(\mathbf{S}_{t}|\mathbf{S}_{1:t-1})=\sum_{t=1}^{T}\mathcal{N}(\mathbf{S}_{t};\mathbf{{\mu}_{t}},\Sigma_{t})$. Meanwhile, the authors use an example of a goal state penalizing L2 distance as an example of goal likelihood (assuming a Gaussian distribution), which admits a straightforward translation into our definition of a goal energy, for both goal states and goal lanes. Hence, we use the same goal energy definition given in Sec. 4-A of the main paper to compute the goal likelihood. We weight the prior likelihood and goal likelihood with two hyperparameters $\lambda_{1},\lambda_{2}$, which are determined from the validation sets.

In practice, we implement the PRECOG objective as follows: we sample 100 ego-agent latents $\mathbf{z}^{r}$, effectively using random shooting rather than gradient descent. In discussion with the authors they confirmed that results should be similar. Then for each ego-agent latent, we sample 15 joint actor latent samples $\mathbf{Z}^{h}$ as a Monte-Carlo approximation to the expectation. We then evaluate the goal/prior likelihood costs for each candidate ego-agent latent and select the ego-agent latent with the smallest cost. Evaluation of the planning costs for all candidate samples can be efficiently done in a batch manner using one forward GPU pass. Note that in selecting the optimal ego-agent latent $\mathbf{z}^{r}$, there is an intricacy that since PRECOG is a joint autoregressive model, the ego-agent latent does not correspond to a fixed ego-agent trajectory, as the final trajectory will depend on the other actor latents. We avoid this challenge in simulation execution by replanning sufficiently often (0.3s) and also observing that the other actor latents do not generally perturb the ego-agent trajectory too much.

As indicated in Table I of the main paper, the PRECOG model underperforms both our non-reactive and reactive objectives based on the energy-based model. We qualitatively analyzed some of the simulation scenarios and offer some hypotheses for the results. The first is that since PRECOG does not explicitly define a collision prior, it’s possible that the model does not try to avoid collision in all cases, especially on test scenarios that are out-of-distribution from the training data (especially in CARLA, where the traffic density in simulation is higher than in training). The second is that sampling in latent space does not guarantee a diverse range of trajectories for the ego-agent. In fact, we notice that in some turning scenarios where we set the goal state to be the result of a turn, the ego-agent still goes in a straight line, even when the prior likelihood weight goes to 0 and the number of ego-agent samples is high (tried setting to 1000). We hypothesize that this is partially due to test distribution shift. Nevertheless, we find learned autoregressive models promising to keep in mind for the future.

We showcase a qualitative example comparing PRECOG with our model in the next section, in Fig. 6.