Optimal trajectory generation for autonomous vehicles

From piecewise-jerk planners to onboard nonlinear trajectory optimization

An autonomous driving system needs trajectories that are collision-free, kinematically feasible, and comfortable — at rates that fit a closed loop. This line of work develops optimization-based planners for that stack: first piecewise-jerk quadratic methods for path and speed in Frenet frame, then a nonlinear, MPC-style planner that addresses their limits for complex static-obstacle avoidance.

Gap: Sampling and polynomial methods struggle with comfort, kinematics, and cluttered urban corridors at onboard rates
Approach: Piecewise-jerk QP for path and speed (Apollo); later nonlinear state optimization inside a drivable corridor
Runtime: ~10 ms (Intel i7) / ~20 ms (Drive Orin) for a 6-second static-obstacle trajectory

Guide line & corridor → Piecewise-jerk path/speed → Nonlinear spatiotemporal refine (static obstacles)

What existing approaches lack

Randomized and lattice planners can explore nonholonomic motion, but they underuse road structure and often return trajectories that are uncomfortable at highway speed. Pure polynomial Frenet methods (quintic/quartic end-condition sampling) are fast for simple in-lane motion, yet they lack dense control of the path: complex static clutter, narrow passages, and hard kinematic bounds are difficult to encode without exploding the candidate set.

What production stacks need is direct enforcement of comfort metrics — linear jerk, curvature rate, centripetal acceleration — together with collision constraints, solved fast enough for 10 Hz planning.

Piecewise-jerk path and speed planning

The first approach in this line was a piecewise-jerk formulation: discretize a 1D motion to second order (position, first and second derivatives), and connect consecutive knots with a constant third-order term (jerk). That gives dense control of shape while keeping local second-order continuity — more flexible than fixed polynomials, and amenable to quadratic programming.

Applied laterally in Frenet frame, this yields an optimal path inside a corridor built from the guide line, road bounds, and static obstacles (IEEE IV 2020; deployed on Baidu Apollo). Applied longitudinally, the same idea plans speed along a smooth guide line under centripetal acceleration limits and path–time obstacles for in-lane cruise and stop (IEEE ITSC 2019). Together they form a hierarchical path-then-speed stack that exploits structured roads and runs efficiently enough for onboard use.

Why nonlinear corridor planning came next

Piecewise-jerk planning in Frenet frame has structural limits. Map-frame kinematics — curvature, curvature rate, bicycle-model feasibility — become awkward after the frame transform, so they are often enforced only approximately (e.g., variable bounds). Accurate decoupling also wants a highly smooth guide line. And when static obstacles force irregular corridors and sharp geometric maneuvers, optimizing geometry and timing through loosely coupled 1D problems leaves residual mismatch.

The later nonlinear planner (arXiv 2023) keeps the hierarchical path/speed split for dynamic overrides, but solves static-obstacle avoidance as an MPC-style nonlinear program in map frame: discretize a short horizon in time; optimize populated vehicle states (position, heading, curvature, speed, acceleration) rather than a dense control chain; enforce sparse kinematic equalities between consecutive states; and score comfort through linear jerk and curvature change rate inside a smoothed drivable corridor. Positional increments use Gaussian–Legendre quadrature where closed forms are unavailable. Caching and corridor smoothing keep the solve within about 10–20 ms for a 6-second horizon.

Takeaway

Piecewise-jerk quadratic planners made comfort-aware path and speed planning practical for structured roads; the nonlinear corridor planner extends that line when Frenet approximations are not enough for complex static avoidance — still at rates suitable for commercial onboard stacks.

Related publications

Optimal Trajectory Generation for Autonomous Vehicles Under Centripetal Acceleration Constraints for In-lane Driving Scenarios — IEEE ITSC, 2019 (piecewise-jerk speed).
Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform — IEEE IV, 2020 (piecewise-jerk path).
Optimal Vehicle Trajectory Planning for Static Obstacle Avoidance using Nonlinear Optimization — arXiv:2307.09466, 2023 (nonlinear / MPC-style corridor planning).

Keywords

Quadratic programming · Nonlinear optimization · Vehicle kinematic model · Ipopt