0. Planning 路径规划指南

autonomy/planning：全局路径规划，对齐 nav2 nav2_planner。输入起终点 + Costmap2D，输出 planning_msgs::Path。

本文 §0	其他文档
快速开始、配置、排错	§1 架构 · §2 规划器 · §6 综述

---

0.1 文档地图

角色	阅读顺序
新手	§0.2 → §1 → §2
集成	§0.2、§0.5、§0.8 → §6.6 选型
算法	§6 → §2 → §3–§5

§	文档	内容
0	本指南	上手、配置、排错
1	架构	分层、GetPlan、插件加载
2	规划器总览	三插件对比与索引
3–5	NavFn · Dijkstra · Theta*	算法专题
6	综述	历史、分类、选型

---

0.2 快速开始

1. config/planner/planner.lua — 启用插件、设 default_planner_id

2. config/autonomy.lua — planning = AUTONOMY_PLANNER

3. 启动后 Autonomy 构造 PlannerServer

default_planner_id = "navfn_planner"
planner_plugins = { "navfn_planner", "dijkstra_planner", "theta_star_planner" }

auto server = std::make_shared<PlannerServer>(CreateOptions("config"));
auto path = server->GetPlan(start, goal, "navfn_planner", []{ return false; });

运行时序见 §1.3。插件对比见 §2。

---

0.3 问题形式化

求 \(\tau:[0,1]\to\mathcal{C}_{\mathrm{free}}\)，\(\tau(0)=q_s\)，\(\tau(1)=q_g\)，最小化路径长度与障碍代价：

\[ J(\tau)=\int_0^1 \big(w_l\|\tau'(s)\|+w_c\,c(\tau(s))\big)\,ds. \]

离散栅格 \(C\) 上由各插件搜索；分类与约束见 §6.3。

---

0.4 代价地图

规划读 Costmap2D 快照；代价值为各插件公共输入。

值	含义	NavFn	Theta*
0	FREE	通行	通行
1–252	梯度	\(F_{ij}=C_n+\kappa_F c_{ij}\)	\(\tau(n)\propto c_n/252\)
253 / 254	INSCRIBED / LETHAL	阻塞	LOS 阻塞
255	UNKNOWN	allow_unknown	allow_unknown

NavFn 参数推导 §3 NavFn §3.2；图层与膨胀 Map · Costmap2D、§1.2.1。

---

0.5 配置与 API

config/planner/planner.lua

字段	说明	默认建议
default_planner_id	默认插件	navfn_planner
planner_plugins	启用列表	三插件
path_simplify_epsilon	DP 简化，0=关	0.0
auto_smooth_after_plan	规划后平滑	false
costmap	全局地图子配置	见 Map 文档

PlannerServer

API	用途
GetPlan(…)	单次规划
IsPathValid(…)	路径仍可行？
GetCostmapWrapper()	共享 costmap

节点 planner_server。分层、插件加载、扩展见 §1。

---

0.6 路径后处理

由 §0.5 中 path_simplify_epsilon、auto_smooth_after_plan 控制；默认均关（简化可能使绕障路径坍缩）。流水线位置见 §1.3。

---

0.7 插件扩展

见 §1.6 扩展自定义规划器。

---

0.8 故障排查

现象	先查
NoValidPathCouldBeFound	连通性、tolerance、goal 自由区
StartOccupied / GoalOccupied	位姿、膨胀半径
PlannerTimedOut	costmap->isCurrent()、地图尺寸
PlannerTFError	frame_id、TF
穿墙 / 贴障	膨胀、footprint
路径锯齿	theta_star_planner 或后处理

结果码表 §1.5 · 选型 §6.6 · 系统排查 §6.7.3。