Self-driving cars need planners that are fast, safe, and smooth — all at once. This paper splits the problem in two: first find a good path, then figure out how fast to drive it. The path planner (HSL-RRT*) reuses work from the previous planning cycle instead of starting fresh every time, which makes it faster in practice. The speed planner uses an NMPC controller to generate a velocity profile that respects vehicle limits and keeps passengers comfortable.
Approach
The planner uses path-velocity decomposition — separating the problem into two stages:
- Path planning — find a collision-free geometric path
- Velocity planning — optimize a velocity profile along that path
This decomposition reduces complexity and enables each stage to be solved with specialized algorithms.
HSL-RRT*
The core contribution is HSL-RRT* (Historical data-enhanced Rapidly-exploring Random Graph), a sampling-based path planner that reuses historical data from previous planning cycles to grow path trees more efficiently.
Rather than rebuilding the tree from scratch each cycle, HSL-RRT* seeds the new search with branches from prior iterations. This matters in on-road driving where the environment changes incrementally — most of the previously computed structure remains valid.
Velocity Optimization with NMPC
Once a path is selected, a Nonlinear Model Predictive Controller (NMPC) generates the velocity profile. The NMPC respects:
- Vehicle kinematic and dynamic constraints
- Passenger comfort limits (jerk, lateral acceleration)
- Speed limits and traffic context
Results
Comparative experiments against Apollo EM show HSL-RRT* outperforming in:
- Complex multi-obstacle navigation
- Lane change maneuvers
- Pull-over operations
- Traffic merging scenarios
Videos
Apollo EM vs HSL-RRT*: Navigate through many obstacles (EM fail, HSL-RRT* success)
Apollo EM vs HSL-RRT*: Short lane change (EM fail, HSL-RRT* success)
HSL-RRT*: Pull-over
HSL-RRT*: Merge into traffic after emergency pull-over