Background Planning Stack

Kelly, A. (2013). Mobile Robotics:
Mathematics, Models, and Methods. Cambridge University Press

Background Planning Stack

Kelly, A. (2013). Mobile Robotics:
Mathematics, Models, and Methods. Cambridge University Press

Background Motion Planning

Background Motion Planning

1. Sample points in $(s, l)$ road coordinates.
2. Generate kinematically feasible paths connecting sampled coordinates.
3. For each path, generate a set of trajectories - each with a constant acceleration profile.
4. Assign costs to each trajectory. (Exact formulation not specified).
5. Choose final vertex to maximize forward progress and minimize time and cost.
6. Backtrack from final vertex to construct the minimum cost trajectory.

Note: GPU acceleration required to meet realtime demands.

"Motion Planning for Autonomous Driving with Conformal Spatiotemporal Lattice" McNaughton et al.

1. Construct a graph by sampling points (spatial nodes) at regular intervals along the road.
2. Assign obstacle and lane-centering costs to each spatial node in the graph.
3. Use the Bellman-Ford algorithm to construct a min. cost path through the graph.
4. Use a path following algorithm (pure pursuit) to generate a kinematically feasible smooth path.
5. Use constraint satisfaction to generate a reference velocity profile based on the speed limit and curvature of the road.
6. Generate a pool of similar trajectories by sampling in space and velocity.
7. Rank the pool of trajectories, and select the first ranked trajectory.

"Runtime-Bounded Tunable Motion Planning for Autonomous Driving" Gu et al.

Solve the Boundary Value Problem connecting $\mathbf{X_s} = [x_0, y_0, \theta_0, \kappa_0]^\intercal$ to $\mathbf{X_e} = [x_e, y_e, \theta_e, \kappa_e]^\intercal$ while respecting vehicle kinematics.

1. Parameterize vehicle controls as polynomials with unknown coefficients.
2. Initialize the polynomial coefficients by solving the terminal heading and terminal curvature constraints explicitly.
3. Use a shooting method to forward simulate the parameterized trajectory.
4. Optimize over the polynomial coefficients to bring the final path state $\mathbf{X_f}$ into agreement with $\mathbf{X_e}.$
5. More detail to follow soon.

"Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control" Kelly et al.

Graph-Search Methods

• The Conformal Lattice runs at 10Hz, using a GTX 260 to reoptimize and evaluate ~400,000 trajectories per cycle.
• The Augmented Graph Planner runs at 15-20Hz and evaluates ~4,000 spatial nodes and ~20,000 augmented nodes per cycle.

Variational Methods

• While these methods can be made arbitrarily complex, the trajectory generator is extremely lightweight. Expect at least $650,000\frac{\text{paths}}{\text{sec}}$ per CPU thread.

• Probe the global search space with a series of local optimizations.


• Leverage the structure of the highway environment to prioritize exploration.

• Execute the first trajectory that achieves the current goal and is found to be sufficiently safe and comfortable.


• Perfect answers are useless if they are found too late.

• The highway driving environment is naturally segmented by local traffic.
• It is convenient to abstract away from individual trajectories.
• All trajectories that terminate on the blue line are considered part of the same pattern.

Pattern Planning
Control Flow

• Generate a ranked list of maneuver patterns.
• Take the top ranked pattern, and generate a trajectory using it.
• If the trajectory is safe and (optionally) comfortable, execute it.
• Else, return to the list of patterns and evaluate the next ranked pattern.

Pattern Planning
Pattern Ranking

• Define a maneuver pattern for each neigboring gap.
• At steady state, consider the pattern from the previous planning cycle first.
• At steady state, consider the lane-keeping pattern next.
• At steady state, consider lane-change maneuvers in order by estimated safety.
• If the behavioral planner commands a lane change, consider those maneuvers first.

Pattern Planning
Emergency Response

• If all maneuvers are infeasible, remove the comfort requirement.
• Prioritize emergency braking maneuvers.
• Re-evaluate the maneuver pattern list and immediately execute the first safe trajectory that is found.

$\kappa = \frac{tan(\delta)}{L}$

$ \boldsymbol{\dot x}(t) = \boldsymbol{f}(\boldsymbol{x}(t), \boldsymbol{u}(t), t) = \begin{bmatrix} v \cdot cos(\theta) \\ v \cdot sin(\theta) \\ v \cdot \kappa \\ u_{\dot k}(t) \\ a \\ u_{\dot a}(t) \\ \end{bmatrix} $

$ \boldsymbol{x} = \begin{bmatrix} x & y & \theta & \kappa & v & a \end{bmatrix}^\intercal \hspace{4mm} \boldsymbol{u} = \begin{bmatrix} \dot{\kappa} & \dot{a} \end{bmatrix}^\intercal $

Given a control sequence parameterized by $\begin{bmatrix} c_0...c_{n-1} & d_0...d_{n-1} & T_f \end{bmatrix}^\intercal$ and an initial state $\mathbf{X_{s}}$, the vehicle state can be found by numerically integrating the kinematic equations:

$ \mathbf{X}(t) = \mathbf{X_s} + \int_0^t{\mathbf{f}(\mathbf{x}(t'), \mathbf{u}(t'), t') dt'} $

1. Set $\mathbf{u}_{\dot{a}}(t) = 0$
2. Set $T_f = R(1+\frac{\theta^2}{5})$
3. Select coefficients of $\mathbf{u}_{\dot{a}}(t)$ to satisfy endpoint heading and curvature constraints.
4. Use Newton's Method with the cost function $(\mathbf{X}(T_f)-\mathbf{X_e})^\intercal \mathbf{D} (\mathbf{X}(T_f)-\mathbf{X_e})$ to iteratively move the trajectory endpoint to the desired position $\mathbf{X_e}$ by perturbing $T_f$ and $c_0...c_{n-1}$.

• Paths can be scaled to the unit circle and cached for future use.


• At runtime, scale the problem to the unit circle and select the most similar path. Scale the cached parameters to the size you need and use them to warm-start the optimization.

• Use the procedure from the previous slide to generate a path without regard for obstacles, velocities, or accelerations.
• Reoptimize with initial parameters $\begin{bmatrix} c_0...c_{n-1} & 0...0 & T_f\end{bmatrix}$.
• Use a cost function that measures the following path costs in addition to endpoint state matching costs.
  • min. distance to an obstacle
  • max. lateral acceleration
  • max. deviation from the road centerline
  • etc.

• Comparing Planners is difficult because there is no universally applicable objective standard for trajectories.


• SwerveSim places two or more vehicles with different planning algorithms in otherwise identical vehicles in an identical traffic-filled world.


• Vehicles under test are not aware of each other. They are competing to incur the lowest cost as they traverse the SwerveSim track.


• SwerveSim gives the user control of the cost function used to score the planners.
  • Available cost terms will include common trajectory cost terms as well as computational cost metrics.