Motion planning algorithms for autonomous driving, implemented from first principles in modern C++17. No framework dependencies — just the standard library, GoogleTest for tests, and a small Python script for visualization.
Production autonomous-driving planners are built on a small set of core algorithms. This repository implements them one by one, each with unit tests, reproducible demo scenarios, and quantitative stats (path cost, nodes expanded, runtime), so the behavior of every algorithm is visible and verifiable — see docs/DESIGN.md for the architecture.
| Algorithm | Status | Notes |
|---|---|---|
| A* (4/8-connected) | ✅ done | Octile/Manhattan heuristic, weighted mode, corner-cutting prevention |
| Reeds-Shepp curves | ✅ done | All word families with timeflip/reflect/backwards symmetries |
| Hybrid A* | ✅ done | Exact bicycle-model primitives, dual admissible heuristic, RS analytic expansion |
| RRT / RRT* | ✅ done | Goal biasing, rewiring with subtree cost propagation, benchmarks |
| Trajectory smoothing | ✅ done | Gradient smoothing + trapezoidal velocity profile under accel limits |
Output of plan_demo (Release build, WSL2 on a laptop i7):
scenario success cost expanded time_ms
open_field yes 66.67 151 0.043
u_trap yes 52.87 365 0.043
narrow_gap yes 60.77 263 0.100
| U-trap (escaping a local minimum) | Narrow gap |
|---|---|
 |
 |
Output of parking_demo — kinematically feasible maneuvers for a 4.5 m car
with a ~3.95 m minimum turning radius, blue driven forward, orange in
reverse:
scenario success cost cusps expanded time_ms
perpendicular_parking yes 22.10 1 1450 132.4
parallel_parking yes 14.99 2 1445 123.3
Output of benchmark — sampling planners averaged over 10 seeds
(6000-iteration budget, costs in meters):
scenario planner success cost_m nodes time_ms
open_field A* 10/10 66.67 151 0.04
open_field RRT 10/10 77.89 220 0.30
open_field RRT* 10/10 62.79 5063 248.91
u_trap A* 10/10 52.87 365 0.04
u_trap RRT 10/10 82.71 468 1.57
u_trap RRT* 10/10 50.60 5127 241.06
narrow_gap A* 10/10 60.77 263 0.04
narrow_gap RRT 10/10 70.41 285 0.74
narrow_gap RRT* 10/10 56.93 5379 265.29
Two classic trade-offs, measured: plain RRT is fast but crooked; RRT* spends its full budget rewiring and ends up cheaper than grid A*, because the 8-connected grid overestimates free-space distances while RRT* moves in continuous space.
| RRT (first solution) | RRT* (rewired) |
|---|---|
 |
 |
smooth_demo runs the full pipeline: RRT* plans, the gradient smoother
irons the path out (bending energy + obstacle repulsion, endpoints fixed,
every iterate collision-checked), and the profiler time-parameterizes it —
per-point speed capped by the lateral-acceleration/curvature limit, then
trapezoidal forward/backward passes:
The speed coloring shows the profile braking into both U-turns and stretching out on the straights. The smoothed path is ~2.5 m longer than the raw one — the repulsion term deliberately trades length for obstacle clearance.
Requires a C++17 compiler and CMake ≥ 3.20. GoogleTest is located via
find_package and fetched automatically when not installed.
cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build --parallel
ctest --test-dir build --output-on-failure # run the unit tests
./build/apps/plan_demo out # grid A* demo scenarios
./build/apps/parking_demo out # Hybrid A* parking scenarios
./build/apps/benchmark out # A* vs RRT vs RRT* table
./build/apps/smooth_demo out # smoothing + velocity profile
python3 tools/visualize.py out/*.json --out-dir docs/imagesinclude/mpl/ public headers (GridMap, planners)
src/ implementation
apps/ demo executables
tests/ GoogleTest unit tests
tools/ Python visualization
docs/ design doc + rendered images
面向自动驾驶的运动规划算法库，使用现代 C++17 从零实现，核心库不依赖任何第三方框架。
已实现全部路线图算法：A*（八连通/四连通、加权模式、禁止对角穿墙）、Reeds-Shepp 曲线（全部字族 + 三种对称变换，600 组随机位姿性质测试验证）、Hybrid A*（精确自行车模型运动基元、双可采纳启发式、RS 解析扩展，完成垂直/平行泊车多次换挡机动）、RRT/RRT*（邻域重连 + 子树代价传播，基准显示连续空间的 RRT* 路径代价优于栅格 A*）、轨迹平滑与速度规划（梯度平滑 + 障碍斥力，曲率限速 + 梯形加减速）。每个算法都配有单元测试和可复现的演示场景，输出路径代价、换挡次数、扩展节点数、耗时等量化指标；演示结果由 tools/visualize.py 渲染成上方图片。架构与设计取舍见设计文档。