End-to-end Interpretable Neural Motion Planner

In plain words

Self-driving cars usually split the job into separate parts: spotting other vehicles, guessing where they will go, and then choosing a path. This paper builds one neural network that takes the car's laser scans and a road map and goes straight to a "cost map" over the space the car could reach in the next few seconds. Low cost means a good place to be at a given moment.

The car does not learn a steering command directly. It draws many candidate drivable paths, scores each one by adding up the cost map along it, and drives the cheapest one. The network also outputs the other vehicles it sees and where they are heading, so a person can inspect why a path was chosen. Because the cost map is learned, it can represent several possible futures at once, for example going left or staying in lane.

TL;DR

• One convolutional network maps LiDAR sweeps plus an HD map to 3D vehicle detections, future vehicle motion and a planning cost volume §3.1.
• The planner samples physically plausible trajectories, sums cost-volume values along each, and executes the minimum-cost one §3.2.
• Training combines perception losses with a max-margin planning loss that pushes the human-driven trajectory below sampled alternatives §3.3.
• Author-reported results are on a private multi-city North American dataset, not a public benchmark §4.

Problem

End-to-end driving from sensors to controls suffers from compounding error and is hard to interpret or to constrain with prior knowledge §1. Modular stacks are interpretable but each module is trained for its own metric, so gains in one do not translate to the whole system, and uncertainty does not propagate well §1. The paper targets urban driving with traffic lights, yielding and multiple road users, with a planning horizon of a few seconds §1 §4.

The planning objective is a minimisation over trajectories of the sum of a learned cost at each timestep eq. 1. That minimisation is not solved exactly: the paper calls it NP-hard under physical constraints and uses sampling instead §3.2.

Method

The model has three parts that share one backbone §3.1:

1. Input: ten LiDAR sweeps, corrected for ego-motion into the current frame, are rasterised into a 3D occupancy grid by stacking the height bins along the channel axis, so no 3D convolutions are used [§3.1, Input representation]. The HD map is rasterised into M channels (road, intersections, lanes, lane boundaries, traffic lights, etc.) and concatenated §3.1.
2. Backbone: five blocks of Conv2D layers. The outputs of the first four blocks are resized to a quarter of the input size, concatenated, and fed to block five §3.1, Backbone.
3. Heads. A perception head predicts, for each of 12 predefined anchors per location, a vehicle-presence score and regression targets for each future frame: offsets, size and heading as sine and cosine [§3.1, Perception Header]. A cost volume head uses two deconvolution layers to restore the BEV resolution, followed by a final convolution with one filter per planning timestep §3.1, Cost Volume Head. Cost values are clipped to [-1000, 1000] after the network §3.1.

Planning is done by sampling. A candidate trajectory is a clothoid path plus a constant-acceleration velocity profile §3.2, Output Parameterization. Its cost is the sum over future timesteps of the cost volume read at the trajectory's waypoint for that timestep eq. 1 §3.2. The sampled candidate with the lowest cost is executed §3.2.

Clothoid paths are chosen because a vehicle with a bicycle model cannot follow arbitrary point sequences. The paper approximates the path curvature as proportional to the steering angle §3.2. The clothoid is written with Fresnel integrals and a scale factor a eq. 2:

\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] eq. 2

where S and C are the Fresnel integrals eq. 3 eq. 4 §3.2. The velocity profile is linear in time, with initial speed and constant acceleration §3.2.

Training uses a multi-task loss, total = perception + beta times planning eq. 5:

\mathcal{L} = \mathcal{L}_{\text{perception}} + \beta \mathcal{L}_{\text{planning}} eq. 5

The perception loss is a classification cross-entropy over anchors eq. 6 plus alpha times a regression term summed over frames t = 0 to T eq. 7. Anchors whose best ground-truth IoU is above 0.4 are positives, with a nearest-neighbour fallback for unassigned boxes; hard negative mining is used §3.3.

The planning loss is a max-margin loss. The ground-truth trajectory is the positive, and N sampled trajectories are negatives §3.3. For each negative, the per-timestep hinge terms add a cost gap, a distance term and a traffic-rule penalty; the largest negative term is the loss eq. 8:

\mathcal{L}_{\text{planning}} = \sum_{\{(x^t, y^t)\}} \max_{1 \leq i \leq N} \sum_{t=1}^{T} \left[ \hat{c}^t - c_i^t + d_i^t + \gamma_i^t \right]_+ eq. 8

Here c-hat is the cost volume at the ground-truth point, c_i the cost at the i-th negative point, d_i the distance between the ground truth and the negative, and gamma_i the penalty applied only at timesteps where the negative breaks a traffic rule §3.3.

Inference pseudocode. The paper describes these steps in prose; no algorithm block is given §3.1 §3.2.

# Plan one step from sensors and map  [§3.1] [§3.2]
# Input:  sweeps (10 LiDAR frames), hd_map (M channels)
# Output: chosen trajectory, T waypoints in BEV

def plan(sweeps, hd_map):
    # Rasterise the 10 ego-corrected sweeps into an occupancy grid  [§3.1]
    occupancy = rasterise_sweeps(sweeps)         # (H, W, Z*T')
    # Concatenate map channels along the channel axis  [§3.1]
    x = concatenate(occupancy, hd_map)           # (H, W, Z*T' + M)
    # Run the shared backbone  [§3.1 Backbone]
    features = backbone(x)
    # Predict the planning cost volume, one channel per timestep  [§3.1]
    cost_volume = cost_head(features)            # (H, W, T)
    # Sample candidate trajectories  [§3.2 Sampling]
    candidates = sample_trajectories()
    # Sum the cost-volume values along each candidate  [§3.2]
    costs = [trajectory_cost(s, cost_volume) for s in candidates]
    # Return the candidate with minimum cost  [§3.2]
    return candidates[argmin(costs)]

Sampling of candidate trajectories. The paper gives the ranges but not every branch; the line and circle branches are not described §3.2.

# Sample one candidate trajectory  [§3.2 Sampling]
# Input:  v0 (initial speed), steer0 (initial steering angle)
# Output: waypoints, one per output timestep

def sample_trajectory(v0, steer0):
    # Path family: line 0.5, circle 0.25, clothoid 0.25  [§3.2]
    family = sample_family([0.5, 0.25, 0.25])
    # Clothoid branch: scale a drawn uniformly from [6, 80] m  [§3.2]
    scale = uniform(6.0, 80.0)
    # Constant acceleration drawn from [-5, 5] m/s^2  [§3.2]
    accel = uniform(-5.0, 5.0)
    # Locate the start on the curve from steer0  [§3.2, wording not exact]
    start = curve_position_from_steering(steer0, scale)
    # Project the path to output timesteps using the velocity profile  [§3.2]
    waypoints = project(family, scale, start, v0, accel)
    return waypoints

Training pseudocode, perception and planning terms, with the paper's labels.

# Perception loss for one frame  [§3.3] [eq. 7]
# Inputs: anchor scores, labels, regression outputs and targets

def perception_loss(scores, labels, regs, targets, alpha):
    # Classification cross-entropy over anchors  [§3.3] [eq. 6]
    cla = cross_entropy(scores, labels)
    # Weighted smooth-L1 over regression, summed over frames 0..T  [eq. 7]
    reg = sum_over_frames(smooth_l1(regs, targets))
    return cla + alpha * reg

# Max-margin planning loss for one ground-truth trajectory  [§3.3] [eq. 8]
# Inputs: cost_volume (H, W, T), gt_traj (T, 2), neg_trajs (N negatives)

def planning_loss(cost_volume, gt_traj, neg_trajs, gamma):
    # Cost volume at the ground-truth waypoints  [eq. 8, c-hat]
    gt_costs = index_cost(cost_volume, gt_traj)   # (T,)
    totals = []
    for neg in neg_trajs:
        # Cost volume at the negative's waypoints  [eq. 8, c_i]
        neg_costs = index_cost(cost_volume, neg.waypoints)
        # Per-step L2 distance to ground truth  [eq. 8, d_i]
        dist = l2_distance(gt_traj, neg.waypoints)
        # Traffic-rule penalty, only where the rule is broken  [eq. 8]
        penalty = gamma * neg.violations          # (T,), 0 or gamma
        hinge = relu(gt_costs - neg_costs + dist + penalty)
        totals.append(sum(hinge))
    # Keep only the worst negative  [eq. 8, max over i]
    return max(totals)

Hyperparameters

Stated values come from the paper's text and figures §3.1 §3.2 §3.3 §4. The paper has no appendix and no training section. Optimiser, learning rate, batch size, epochs and augmentation are all not stated.

Results

Author-reported. Setup: a private dataset of driving scenarios collected in several North American cities, 10 input frames, 7 output frames over 3 s §4. Baselines are described in §4.1 and §4.2. Model size, hardware and training time are not stated.

Planning metrics tab. 1. Lower is better for all columns. The table is placed at the end of the sampling part of the method §3.2.

Motion forecasting tab. 2. Errors in metres. Columns are along-trajectory L2, across-trajectory L2, L1 and L2, each at 0, 1, 2 and 3 s.

Detection mAP tab. 4. Columns are IoU thresholds.

Ablation tab. 3. Model 5 is the full model §4.3. "Det" and "Plan" mark the losses used. Input 5 or 10 marks the number of input frames. Penalty is the traffic-rule term gamma.

Qualitative results: the cost volume is visualised for several timesteps overlaid on the scene; the paper describes multi-modal, lane-following and collision-avoiding behaviour in the examples §4.2, Cost Map Visualization fig. 3. The paper gives no quantitative score for these figures.

Paper vs code

No official code.

Search: the arXiv abstract page lists no code, project or GitHub link; the paper text contains no URL; a GitHub repository search for the exact title and for a title-keyword query, both via the GitHub search API, returned zero results. No repository was cloned.

Pitfalls

• No training details. The paper has no appendix and no training section. Optimiser, learning rate, batch size, number of epochs, augmentation, anchor sizes, loss weights alpha and beta, the number of negatives N, the penalty gamma, and the hard-negative ratio are all not stated. An implementation needs its own choices for each, and the card cannot resolve them.
• Dataset is private and unnamed. The paper describes a multi-city North American dataset with the split sizes §4, but does not name it or say it is public. The reported numbers cannot be reproduced from public data.
• Notation clash on T. T' is the number of input sweeps (10) and T is the planning horizon (7 output frames). Equation 8 sums over t = 1 to T, while the perception loss sums over t = 0 to T eq. 7 eq. 8. Read them with the right offset, since a loss off by one frame changes the target alignment.
• Backbone downsampling conflict. The text says the backbone has a downsampling rate of 4 §3.1, Backbone, but it also has MaxPool after each of the first three blocks, which alone gives a factor of 8 §3.1. Two cost head deconvolutions with stride 2 restore a factor of 4, not 8, so the stated BEV resolution only follows if the rate is 4. The paper does not resolve this. Check the official weights or code before fixing the architecture.
• Curvature formula is only exact for a = 1. The paper states curvature as proportional to arc length, kappa(xi) = pi xi §3.2, with scale a in the clothoid form eq. 2. From the Fresnel form with argument xi/a, the curvature of the scaled curve is pi xi / a^2. This is my derivation from eq. 2, not a statement in the paper. Implement the clothoid with the explicit scale and check the curvature numerically.
• Clothoid start point is vague. The paper says the initial steering angle is used to find the position on the curve §3.2, without a formula. The pseudocode helper is a placeholder for that step, and its definition is not given.
• Line and circle branches are not described. The path family is sampled with probabilities 0.5, 0.25 and 0.25 §3.2, but only the clothoid branch has parameters given. The line and circle parameterisations are not stated.
• Speed units and sign conventions. The sampling range for a is given in metres, the acceleration in m/s^2, and the initial speed is the SDV speed. The paper says the limit is 15 m/s for city driving §3.2. Whether the limit caps the sampled speed is not stated.
• Negative sampling is biased. Negatives violate the initial-state constraints with probability 0.8 §3.3, so most negatives are easy. The paper says this is intentional to start learning §3.3, but the effect of the 0.8 value is not studied.
• Max-margin loss is sparse. The planning loss only constrains the cost at waypoints of sampled trajectories. Cost values far from every sample are unconstrained. The clipping to [-1000, 1000] §3.1 is stated to keep the cost from drifting, which suggests this happens in practice.
• Claims in the text that the table does not support. The text claims a clearly lower future collision rate at every horizon, by a wide margin §4.1. At 0.5 s all methods are 0.00, and at 1.0 s Ours-NMP, Ego-motion and IL are all 0.01 tab. 1. The text says ACC has "about 0" lane violation §4.1, but tab. 1 gives 0.24, 0.46 and 0.64 for ACC.
• Ego-motion and IL win on L2. tab. 1 shows lower planning L2 for Ego-motion and IL at all horizons. The paper says this is because they optimise L2 directly and collide more §4.1. Do not read the L2 column as a planning-quality score.
• Detection claim. The paper says Ours-NMP is best "at 0.7 IoU" §4.2. tab. 4 also shows it best at 0.6 and 0.8 and not best at 0.5 (IntentNet 94.4) or 0.9 (Pixor 7.6).
• Ablation is one run per row. No variance or seeds are reported, so differences of about 0.1 mAP in tab. 3 may be noise.
• Table and figure numbers are from source order. tab. 1 is the planning table in the method section, tab. 2 the motion forecasting table, tab. 3 the ablation, and tab. 4 the detection table. fig. 1 is the model overview in the introduction, fig. 2 the trajectory illustration and fig. 3 the cost volume visualisation §4.2. These numbers assume LaTeX float order and should be checked against the rendered PDF.
• Equation numbers are from source order. Eq. 1 is the planning minimisation, eq. 2 the clothoid, eq. 3 and eq. 4 the Fresnel integrals, eq. 5 the total loss, eq. 6 the classification loss, eq. 7 the perception loss and eq. 8 the planning loss. The regression definitions in the perception section are display math without a label and are not numbered.
• Version and venue. The arXiv comment names CVPR 2019 (oral), and this arXiv record is version 1 from 2021-01-17 [arXiv]. Use the version recorded in the metadata.

Lineage

• Builds on: the LiDAR detection and motion-forecasting networks it names as baselines, including FaF, IntentNet and Pixor §4.2. It adopts the occupancy-grid input of IntentNet §3.1. The backbone is adapted from Pixor §3.1.
• Compared against: Ego-motion forecasting, imitation learning, adaptive cruise control and a planner with a manually designed cost §4.1.
• Superseded by: not known from content in this wiki.