6. 路径规划算法综述（Survey）

本文范围：算法谱系、Autonomy 能力边界与选型依据。
形式化与代价地图见 §0 指南；模块架构见 §1；各规划器推导见 §3–§5。

6.1 综述定位

维度	本文	其他文档
问题定义 \(J(\tau)\)、栅格离散	不展开	§0.3
导航栈分层、GetPlan 时序	不展开	§1、Navigator 综述
NavFn / Theta* 公式与伪代码	摘要 + 链接	§2 总览 · §3–§5
算法历史、分类、选型	本文	—
几何路径 vs 轨迹、局部时空联合	摘要	Control 轨迹规划综述 §6.2.4

建议阅读顺序

角色	路径
选型 / 集成	§6.6 内置规划器 → §6.6.3 场景矩阵 → §6.7 排错
算法研发	§6.3 分类 → §6.4 时间轴 → §6.5 算法族 → §2 → §3–§5
背景调研	§6.4 → §6.5 → §6.8 业界生态 → §6.10 参考文献

6.2 Autonomy 能力边界

Autonomy planning 是 nav2 兼容的全局栅格规划层：在 Costmap2D 上输出几何路径，不负责局部避障与速度跟踪（见 control）。

1959 Dijkstra → 1990 NavFn → 2010 ROS navfn → 2018 nav2_navfn_planner → 2025 Autonomy
                                                                              ├── NavfnPlanner
                                                                              ├── DijkstraPlanner
                                                                              └── ThetaStarPlanner

能力	状态	说明
栅格势场 / Dijkstra / A*	✅	`navfn_planner`、`dijkstra_planner`
任意角 Theta*	✅	`theta_star_planner`
路径简化 / 平滑	✅	`PathSimplifier`、`SimpleSmoother`（后处理）
重规划 / 路径校验	✅	Navigator + `IsPathValid()`
Hybrid A* / SE(2)	❌	需插件扩展
采样规划 RRT* / OMPL	❌	高维 C-space
增量 D* Lite	❌	动态地图增量修复
学习 / 端到端规划	❌	研究向

6.3 算法分类全景

6.3.1 四维分类法

路径规划算法
├── 搜索空间：离散（栅格/图）· 连续（样条/优化）· 混合（Hybrid A*）
├── 搜索策略：完备（Dijkstra）· 启发式（A*, Theta*）· 采样（RRT）· 优化（CHOMP）
├── 时间维度：静态 · 动态（D*）· 反应式（DWA, MPC）
└── 最优性：最优 · 渐近最优（RRT*）· 有界次优 · 启发式

6.3.2 方法—特性矩阵

方法族	代表	完备性	最优性	路径质量	实时性	Autonomy
图搜索	Dijkstra, A*	完备	离散最优*	中（锯齿）	高	✅
导航函数	NavFn, FMM	完备	近似	中	高	✅ NavFn
任意角	Theta, Field D	完备	近似	高	中	✅ Theta*
采样	RRT, RRT*	概率完备	渐近†	低–中	中	❌
优化	CHOMP, TrajOpt	不完备	局部	高	低–中	后处理 only
混合	Hybrid A*	分辨率完备‡	有界次优	高	中	❌

* 离散图上最优；† RRT* 渐近最优；‡ 分辨率完备。

6.4 发展时间轴

按四个历史阶段分块展示里程碑；完整对照见 §6.4.1。

奠基期 · 1950s–1970s

1959

Dijkstra

单源最短路径，图搜索理论奠基

1968

A*

启发式搜索，f = g + h 框架

1970s

C-space

构型空间概念，运动规划形式化

1979

可视图法

Visibility Graph，多边形障碍最短路径

势场与反应式 · 1980s–1990s

1985

APF

人工势场，引力 + 斥力实时避障

1990

GURVEY / NavFn

导航函数，Eikonal 方程栅格求解

1995

DWA

动态窗口法，局部速度空间采样

1998

FMM

Fast Marching Method，快速行进法

采样革命 · 1998–2011

1998

PRM

概率路线图，高维 C-space 采样

1999

RRT

快速扩展随机树，单查询采样

2000

RRT-Connect

双向 RRT，加速连通

2005

Field D*

任意角栅格搜索，路径更短

2007

Theta*

视线检测任意角 A*，Autonomy 已实现

2011

RRT*

渐近最优采样规划

工程化与自动驾驶 · 2008–至今

2008

Hybrid A*

考虑朝向与运动学约束的栅格 A*

2010

ROS nav stack

move_base + navfn 工程化栈

2014

CHOMP / STOMP

轨迹优化规划器

2018

Navigation2

nav2 插件化架构，行为树导航

2020

nav2_smac

Hybrid A* / State Lattice 插件

2023

学习规划兴起

Neural Planner、Diffusion Policy 等

2025

Autonomy planning

NavFn / Dijkstra / Theta* 三插件成熟

6.4.1 分阶段特征表

阶段	年代	代表方法	核心突破	局限
图搜索奠基	1959–1979	Dijkstra, A*, 可视图	完备性、最优性理论	高维维数灾难
势场反应式	1980–1999	APF, DWA, NavFn	实时、实现简单	局部极小、网格依赖
采样规划	1998–2011	PRM, RRT, RRT*	高维、复杂拓扑	路径不光滑、概率性
任意角/混合	2005–2012	Field D, Theta, Hybrid A*	路径更短、考虑朝向	计算量上升
工程栈整合	2010–至今	nav2, OMPL, Smac	插件化、产品级	调参复杂
学习增强	2020–至今	Neural, Diffusion	大数据泛化	安全可解释性不足

6.5 算法族速览

以下各节为摘要；Autonomy 已实现的三类见 §6.6，完整推导见对应子文档。

6.5.1 经典图搜索

Dijkstra（1959）：\(d(v)=\min_{(u,v)\in E}\{d(u)+w(u,v)\}\)。A*（1968）：\(f(n)=g(n)+h(n)\)，\(h\) 可采纳时离散最优。

算法	复杂度（典型）	Autonomy
Dijkstra	\(O((V+E)\log V)\)	`DijkstraPlanner`、NavFn 桶队列
A*	\(O(b^d)\) 最坏	`navfn_planner` + `use_astar=true`
D* Lite / LPA*	与变化量相关	❌

详见 Dijkstra §4、NavFn §4。

6.5.2 NavFn 导航势场

近似 Eikonal \(\|\nabla\phi\|=F\)：自 \(q_s\) 播种、桶队列传播，再自 \(q_g\) 梯度跟踪。与 FMM 相比：NavFn 用平面波近似，均摊 \(O(1)\) 入队，nav2/Autonomy 默认采用。

详见 NavFn 规划器。

6.5.3 Theta* 任意角规划

A* 扩展时做 Line-of-Sight，允许祖父节点直连，路径逼近欧氏直线。Any-Angle 谱系中 Autonomy 实现 Theta*；PathSimplifier 提供后处理拉直。

方法	Autonomy
Theta* (2007)	✅
Lazy Theta, Field D, Anya*	❌

详见 Theta* 规划器。

6.5.4 采样、优化与反应式（未内置）

族	代表	适用	Autonomy	深入阅读
采样	PRM, RRT, RRT*	高维、窄缝迷宫	❌ 可经 `GlobalPlanner` 插件接 OMPL	Planning 全局
优化	CHOMP, TrajOpt	轨迹平滑、约束	后处理 `SimpleSmoother`（§0.6）	Control §6.7.10–§6.7.11
反应式	APF, DWA, DWB	局部避障、滚动采样	`control` 模块	Control §6.7.4
时空联合	TEB, NMPC, MPPI	动态障碍、显式 \(\Delta t\)	`control`（第三方/待集成）	Control §6.6.4

CHOMP / TrajOpt 在栈中的两种角色：(1) Planning 侧 Path 几何平滑（Autonomy SimpleSmoother 为轻量替代）；(2) Control 侧 局部轨迹优化（含障碍势场、与 TEB/MPPI 选型对照见 Control 综述）。

6.5.5 动态环境与重规划

策略	Autonomy
周期性 / 失效触发 `GetPlan()`	✅ Navigator
`IsPathValid()`	✅
D* Lite 增量修复	❌

当前策略：全局规划 + 路径校验 + 触发重规划，非增量 D*。

与 Control 的分工：Planning 输出 无时间 的几何 Path；局部轨迹 \(\tau(t)\)、时空联合（TEB、MPC、MPPI rollout）与 time-scaling 均在 control 层完成。全局侧 SimpleSmoother 仅做几何后处理，不等价于 TEB/NMPC 的时间分配。详见 Control 综述 §6.2.4–§6.6.4。

6.6 Autonomy 内置规划器选型

本节是工程读者最常用部分：对比三插件并给出场景与决策树。

6.6.1 定性对比

维度 (1–5)	NavFn Dijkstra	NavFn A*	DijkstraPlanner	ThetaStarPlanner
计算速度	★★★★★	★★★★	★★★★★	★★★
路径长度	★★★	★★★	★★★	★★★★★
路径平滑	★★★	★★★	★★★	★★★★
确定性	★★★★★	★★★★★	★★★★★	★★★★★
窄通道	★★★★	★★★★	★★★★	★★★ (8-连通)
大地图	★★★	★★★★	★★★	★★★
调参难度	★★	★★	★★	★★★

6.6.2 量化参考（500×500 栅格，量级）

指标	NavFn Dijkstra	NavFn A*	Theta*
首次规划延迟	20–80 ms	10–50 ms	50–200 ms
路径航点数	多	多	少
路径/直线比	1.2–1.5×	1.2–1.5×	1.0–1.1×
内存	\(O(N)\) 势场	\(O(N)\) 势场	\(O(N)\) g+parent

* 随硬件、地图、起终点变化，仅供选型量级。

6.6.3 场景选型矩阵

场景	地图	通道	推荐规划器	关键配置
室内仓储 AGV	静态 SLAM	宽	`navfn_planner`	默认
办公室服务机器人	静态 + 激光	中	`theta_star_planner`	`how_many_corners=8`
窄走廊	静态	窄	`navfn_planner` 或 Theta* 4-连通	`how_many_corners=4`
医院/商场	大范围	混合	`navfn` + `use_astar=true`	大地图提前终止
调试/回归测试	任意	—	`dijkstra_planner`	确定性输出
探索未知区	部分未知	—	任意	`allow_unknown=true`
动态人群	实时更新	—	NavFn + 高频重规划	缩短 costmap 超时
泊车/倒车 SE(2)	—	—	❌ Hybrid A*	插件扩展

6.6.4 选型决策树

需要全局几何路径？
├── 否 → control 局部规划（DWA / TEB / MPPI）
└── 是
    ├── 地图未就绪？ → 先建图 / 定位
    └── 是
        ├── 要任意角短路径？
        │   ├── 是 → theta_star_planner
        │   │       ├── 窄通道 → how_many_corners=4
        │   │       └── 开阔 → how_many_corners=8
        │   └── 否
        │       ├── 要确定性？ → dijkstra_planner
        │       ├── 大地图 (>2048²)？ → navfn + use_astar=true
        │       └── 默认 → navfn_planner
        └── 目标在膨胀区边缘？ → 增大 tolerance (0.1→0.3 m)

6.7 工程质量与排错

6.7.1 代价地图与规划

规划质量高度依赖 Costmap2D：代价值语义、膨胀半径、图层顺序直接影响 NavFn / Theta* 行为。
不重复展开 — 见 §0.4 代价地图、Map · Costmap2D、架构 §1.2.1。

关键约束：\(r_{\mathrm{inflation}} \geq r_{\mathrm{robot}}\)，否则点机器人假设下路径可能穿墙。

6.7.2 质量影响因素（鱼骨图）

路径规划效果不佳时，从「地图 · 算法 · 模型 · 环境 · 集成 · 下游」六维排查：

上层原因（人 · 机 · 法）

地图与感知

分辨率过粗
膨胀半径不足
静态层未加载
障碍层延迟
未知区策略误设

算法与参数

规划器选型不当
tolerance 过小
allow_unknown 误设
Theta* 切角穿墙
A* / Dijkstra 选型

机器人模型

footprint 不准确
robot_radius 偏差
点机器人假设
运动学约束未建模

路径规划质量不佳

下层原因（环 · 系 · 果）

环境与任务

窄通道 / 多障碍
动态障碍未更新
多目标路点复杂
大地图规模过大

系统集成

TF 坐标系错误
costmap 未及时更新
规划频率过低
后处理简化失真

下游执行

局部规划跟不上
控制器跟踪偏差
航点过密或过疏

6.7.3 因素—对策速查表

分支	典型症状	优先检查	对策
地图与感知	路径穿墙	`inflation_radius`、障碍话题	增大膨胀，确认 `/scan`
算法与参数	找不到路径	`tolerance`、goal 在膨胀区	增大 tolerance，换目标
机器人模型	身体蹭障碍	`footprint` vs `robot_radius`	使用 footprint
环境任务	窄缝失败	8-连通切角	Theta* 改 4-连通或 NavFn
系统与集成	路径漂移	`frame_id`、TF	统一到 costmap global frame
下游执行	走不到位	航点间距、控制频率	禁用 DP 简化，查 controller

更多排错见 §0.8 故障排查。

6.8 业界生态

6.8.1 ROS / nav2 规划器

规划器	包	Autonomy
NavFn	nav2_navfn_planner	✅
Dijkstra	nav2_navfn (mode)	✅ 独立插件
Theta*	nav2_theta_star_planner	✅
Smac 2D / Hybrid / Lattice	nav2_smac_planner	❌ 可插件扩展
OMPL	ompl_interface	❌
Nav2 Route	nav2_route	❌

6.8.2 其他框架

框架	全局规划	特点
Navigation2	插件多种	ROS 2 事实标准
Autonomy	NavFn / Dijkstra / Theta*	对齐 nav2，autolink
Move Base (ROS 1)	navfn	legacy
Apollo	Open Space + EM	自动驾驶

6.9 开放问题与路线图

2025 Autonomy
栅格三剑客成熟

2026 Hybrid A*
拓扑路由层

2027 D* Lite
增量重规划

2028 学习辅助
启发式 A*

2029+ 端到端
Nav Policy

优先级	方向	预期收益
P0	Hybrid A* 插件	SE(2) 泊车、倒车
P0	拓扑路由层	长距离大图效率
P1	D* Lite	动态环境增量修复
P1	OMPL 集成	高维扩展
P2	学习启发式	搜索加速
P2	多机 MAPF	仓储多车

6.10 术语表

术语	解释
完备性	若解存在则算法必能找到
可采纳启发式	\(h(n)\leq h^(n)\)，保证 A 离散最优
任意角路径	不限于栅格方向的直线段组合
导航函数	全空间单调指向目标的势场
膨胀	障碍周围安全缓冲区
重规划	环境变化后重新计算路径
几何路径	无时间参数的 \(SE(2)\) 航点序列；Planning 产出
局部轨迹	含 \(t\) 或控制序列的 \(\tau(t)\)；Control 产出（见 Control 术语表 §6.17）

6.11 参考文献

教材

里程碑论文

年份	论文	贡献
1959	Dijkstra	最短路径
1968	Hart et al., A*	启发式搜索
1990	Eriksson & Borenstein, GURVEY	NavFn 前身
1998	Kavraki et al., PRM · LaValle, RRT	采样规划
2007	Nash et al., Theta*	任意角
2008	Dolgov et al., Hybrid A*	运动学栅格
2011	Karaman & Frazzoli, RRT*	渐近最优

工程

6.12 相关文档

§0 指南 · §1 架构 · §2 规划器总览
NavFn · Dijkstra · Theta*
Navigator 导航编排 · Control 轨迹规划综述（局部时空联合 · §6.6.4）