6. 轨迹规划综述（Survey）

本文范围：局部轨迹规划与运动控制的动机、历史、分类、算法谱系（含时空联合优化）、工程选型与 Autonomy 能力边界。
形式化与流水线见 §0 指南；架构见 §2；控制器索引见 §5（§5.2–§5.7 对应 controller/10_*–15_* 专题）。

6.1 综述定位

维度	本文	其他文档
\(J(e,u)\) 形式化、FollowPath 时序	摘要	§0 指南 · §2 架构
MPPI / RPP / DWB / TEB 公式	摘要 + 链接	§10–§15 专题
几何路径 vs 轨迹、时空联合谱系	本文 §6.2.4–§6.6.5	Planning 综述（全局几何 · §6.5.4）
动机、发展史、分类、选型	本文	—

建议阅读顺序

角色	路径
选型 / 集成	§6.4.4 路径/轨迹 → §6.6.4–§6.6.5 时空联合 → §0.9.1 / §5.8.1 → §6.15 决策树
算法研发	§6.2 → §6.4.4 → §6.6.4–§6.6.5 → §6.7 → 各 §10–§15
背景调研	§6.5 时间轴 → §6.6.4–§6.6.5 → §6.7.13 预测时空 → §6.13 → §6.18 参考文献

6.2 为什么需要局部运动控制

6.2.1 全局规划 alone 不够

全局规划（Planning）在静态或低频更新的全局代价地图上输出几何路径 \(\mathcal{P}\)，通常假设：

机器人可瞬时达到路径切向速度（忽略动力学）
路径足够平滑、航点足够密
环境变化慢，重规划周期 1–5 Hz

实际运行中会出现：

gap	后果	局部控制职责
动态障碍	全局路径被挡	局部 costmap + 避障 critic
非完整约束	DiffDrive 不能横移	运动学可行 \((v,\omega)\)
动力学限制	加减速有上限	VelocitySmoother / 控制器内约束
定位/地图误差	路径与真实环境偏差	10–50 Hz 闭环纠偏
路径离散	折线、急弯	Lookahead / 优化带

因此导航栈经典分层为：Planning 给「往哪走」，Control 给「现在怎么走」。

6.2.2 deliberative vs reactive

范式	时间尺度	代表	在 Autonomy 中
deliberative	秒级	全局规划、行为树	`planning` + `navigator`
reactive	50–100 ms	DWA、MPPI、势场	`control`
hybrid	双层	Nav2 栈	Planning + Control + 重规划

局部控制器本质是 reactive policy \(\pi(x,\mathcal{P}, \mathcal{C}_{local}) \to u\)，在控制周期内可重复计算，对传感器延迟更鲁棒。

6.2.3 与 Planning 的边界

Planning (1–5 Hz)          Control (10–50 Hz)
─────────────────          ────────────────────
全局几何路径 Path           速度 cmd_vel
静态/全局 costmap           局部 rolling costmap（可选）
无速度 profile              加速度/曲率约束
IsPathValid → 触发重规划    ProgressChecker → Recovery

二者通过 planning_msgs::Path 耦合；路径失效时 Navigator 重调 Planning，不是 Control 的职责。

6.2.4 几何路径、轨迹与局部轨迹规划

导航文献中常混用 path 与 trajectory；在 Autonomy 分层下建议区分：

对象	符号 / 表示	是否含时间	典型产出模块
几何路径	\(\mathcal{P}=\{p_i\}\subset SE(2)\)	❌	`planning`（NavFn / Theta*）
速度剖面	\((v(s),\,\omega(s))\) 或 \(v(t)\)	部分	time-scaling、Smoother
轨迹	\(\tau(t)=(x,y,\theta)(t)\) 或 \((x,y,\theta,v,\omega)(t)\)	✅	控制器 rollout / 优化输出
时空轨迹	\(\{s_k,\,\Delta t_k\}\) 或 \(B\)（TEB）	✅ 显式	TEB、NMPC、CHOMP

局部轨迹规划（本文 Control 层）在几何路径之上补全可行运动与（可选）时间分配：

Planning.Path  ──►  [纯跟踪] PP/RPP/Graceful     ──► cmd_vel(t)
                 ──►  [采样] DWB/MPPI rollout   ──► 隐式 τ(t)
                 ──►  [时空联合] TEB/NMPC/CHOMP ──► τ*(t) + u*(t)

纯几何跟踪：只优化当前 \((v,\omega)\)，不显式存储整条 \(\tau(t)\)（RPP、Graceful）。
滚动 rollout：在预测时域内生成离散 \(\tau_{0:H}\)，执行首步（DWB、MPPI）。
时空联合：决策变量同时含空间形变与时间参数（或控制序列 \(\mathbf{U}_{0:H-1}\)），适合动态障碍、加速度/jerk 约束。详见 §6.6.4。

全局 SE(2) 路径搜索（栅格 A*、Hybrid A*）见 Planning 综述 §6.3；本文聚焦局部与 kinodynamic / 时空 族。

6.3 在导航栈中的位置

子问题	模块	频率
定位	`localization`	10–50 Hz
全局规划	`planning`	1–5 Hz
局部控制	`control`	10–50 Hz
编排	`navigator`	事件驱动

Planning.Path ──→ ControllerServer ──→ cmd_vel ──→ 底盘
                      ↑        ↑
              local/global    odom + TF
                 costmap
                      ↓（可选）
              VelocitySmoother

Autonomy control 对齐 Nav2 nav2_controller + nav2_velocity_smoother + Goal/Progress Checker 体系。

6.4 问题形式化

6.4.1 状态与控制

差速/全向模型常用 \(SE(2)\) 或扩展状态：

\[ x = (x, y, \theta, v_x, v_y, \omega_z)^\top, \quad u = (v_x^{cmd}, v_y^{cmd}, \omega_z^{cmd})^\top \]

给定参考路径 \(\mathcal{P}=\{p_0,\ldots,p_N\}\)，求 \(\pi\) 使跟踪误差与控制代价最小：

\[ \min_{\pi} \int_0^T \Big( \|e_p\|_Q^2 + w_\theta e_\theta^2 + u^\top R u \Big) dt \]

约束 \((x,y)\in\mathcal{C}_{free}\)，\(|v|\leq v_{max}\)，\(|a|\leq a_{max}\)，DiffDrive 另有 \(v_y=0\)。

6.4.2 误差定义

全局误差：\(e_p = [x-x_g,\, y-y_g]^\top\)，\(e_\theta = \mathrm{AngleDiff}(\theta,\theta_g)\)。

Frenet 误差（沿路径）：\(e_y\) 为有符号横向距离，\(e_s\) 为弧长误差。几何控制器（PP、Stanley）多在 Frenet 或 lookahead 坐标下设计。

6.4.3 约束与能力矩阵

约束	说明	几何跟踪	DWB/MPPI	TEB/MPC
几何避障	footprint 不碰撞	弱/可选	✅	✅
非完整	DiffDrive	✅	✅	✅
运动学	\(R_{min}\)	RPP/MPPI	部分	✅
动力学	\(a_{max}\)	Smoother	仿真内	✅
时间最优	\(\min T\)	❌	弱	TEB ✅

6.4.4 轨迹优化问题（局部）

给定初值路径或 pose 序列，局部轨迹规划常写为：

\[ \min_{\tau,\,\mathbf{U}} \; \sum_{k=0}^{H-1} \ell\bigl(x_k,u_k\bigr) + \ell_f(x_H) \quad \text{s.t.}\quad x_{k+1}=f(x_k,u_k),\; (x_k,u_k)\in\mathcal{F} \]

\(\ell\)：跟踪 \(\mathcal{P}\)、避障、控制 effort、时间（\(\Delta t_k\) 或 \(\|u\|\)）。
\(\mathcal{F}\)：非完整、\(v,a,\omega\) 界、碰撞约束（硬约束或罚函数）。
与 MPPI：同一 OCP 结构；MPPI 用路径积分采样近似 \(\arg\min\)，NMPC/TEB 用 NLP/SQP/LM 显式求解（§15 MPC）。

Time-scaling（仅对几何路径补时间，不做空间 deform）：\(s(t)\) 单调，\(\dot{s}\) 受 \(v_{max}(s)\)、\(\kappa(s)\) 限制（Bang-coast-bang、S 曲线）。RPP/Graceful 的减速区属于启发式 time-scaling，非全局时间最优。

6.5 发展时间轴

按五个阶段梳理；对照表见 §6.5.1。

几何跟踪奠基 · 1980s–1990s

1985

Pure Pursuit

CMU 农业机器人；lookahead 曲率跟踪

1990

Feedback Linearization

非完整系统反馈线性化跟踪

1995

Dynamic Window (DWA)

Fox et al. 速度空间实时避障

1995

Stanley

DARPA Grand Challenge 横向控制

ROS 工程化 · 2000s–2010s

2000s

move_base

global_plan + base_local_planner (DWA/Trajectory Rollout)

2009

TEB

弹性带时空优化；动态障碍

2010

DWB

Critic 插件化 DWA；ROS navigation 标准

2014

MPC 普及

嵌入式 QP/SQP 求解器进入移动机器人

2013

CHOMP

轨迹优化协变梯度；障碍势场 + 平滑

2014

TrajOpt / STOMP

序列凸优化 · 随机轨迹优化并行

Nav2 时代 · 2018–2022

2018

Navigation2

Behavior Tree + 插件化 controller

2019

Regulated Pure Pursuit

Nav2 默认；曲率/障碍/目标速度调节

2017–18

MPPI / Path Integral MPC

Williams et al. 信息论随机 MPC

2022

nav2_mppi_controller

Nav2 官方 MPPI + Critics 生态

平滑与全栈整合 · 2023–至今

2023

Graceful Controller

Nav2 平滑跟踪；Park & Kuipers 2011 工程化

2023

Velocity Smoother 独立节点

Nav2 加速度约束后处理

2024+

Learning / Diffusion Policy

数据驱动局部策略（研究→产品探索）

2025

Autonomy control

Nav2 架构移植；Checker/Smoother 就绪，插件待接

6.5.1 分阶段特征表

阶段	年代	代表方法	核心突破	局限
几何跟踪	1985–1995	PP, Stanley	极简、低延迟、可证明局部稳定	不避障、无时间剖面
反应式采样	1995–2010	DWA, DWB	实时局部避障、critic 可组合	局部最优、单步/短 horizon
轨迹优化	2010–2018	CHOMP, TrajOpt, STOMP	空间轨迹平滑 + 障碍代价	局部最优、初值敏感
时空联合	2009–2018	TEB, timed-elastic, NMPC	位姿 + \(\Delta t\) 联合优化	计算量、非凸
随机 MPC	2017–2022	MPPI	非线性+约束统一框架	batch 算力需求
工程栈	2018–至今	Nav2 插件族	产品级组合	插件矩阵复杂
学习增强	2020–至今	RL, Diffusion	大数据泛化	安全可解释性

6.5.2 Autonomy 在技术谱系中的位置

1995 DWA → 2010 DWB → 2018 nav2_dwb → 2022 nav2_mppi / nav2_regulated_pp
                                              │
                                        2025 Autonomy control
                                        ├── Checker ×5 ✅
                                        ├── VelocitySmoother ✅
                                        ├── ControllerServer 骨架 ⏳
                                        └── 插件 RPP/Graceful/MPPI ⏳

Autonomy 定位于 nav2 兼容的局部控制层：接口与配置对齐，优先落地 RPP/Graceful，MPPI 为可选重算力方案。

6.6 算法分类体系

6.6.1 四维分类

局部轨迹规划
├── 按方法
│   ├── 几何：PP, Stanley, Graceful, RPP
│   ├── 采样：DWA, DWB, MPPI
│   ├── 轨迹优化（空间）：CHOMP, TrajOpt, STOMP
│   ├── 时空联合：TEB, EB, NMPC, kinodynamic RRT*
│   └── 学习：RL, imitation, diffusion
├── 按是否显式时间
│   ├── 无时间剖面：PP, RPP, Graceful
│   ├── 隐式（rollout 步长）：DWB, MPPI
│   └── 显式 $\Delta t$ / $H$：TEB, MPC, CHOMP 离散化
├── 按感知
│   ├── 纯跟踪（仅路径+odom）
│   ├── 障碍感知（local costmap）
│   └── 预测性（+ prediction 模块）
├── 按模型
│   ├── 差速 SE(2)
│   ├── 全向 SE(2)+vy
│   └── Ackermann / 履带
└── 按最优性
    ├── 启发式（PP, DWB）
    ├── 滚动最优（MPC, MPPI）
    └── 局部 NLP（TEB, TrajOpt）

6.6.2 方法—特性矩阵

方法族	代表	避障	路径质量	实时性	调参	Autonomy
几何跟踪	PP, RPP, Graceful	弱–中	中–高	★★★★★	低	⏳ RPP/Graceful
速度采样	DWB	强	中	★★★★	高	❌
随机 MPC	MPPI	强	高	★★★	高	⏳ 配置
轨迹优化	CHOMP, TrajOpt	强	高	★★★	高	❌
时空联合	TEB	强	高	★★★	高	❌
经典 NMPC	acados, CasADi	强*	高	★★★	很高	❌
学习	End-to-end	变化	变化	推理快	数据	❌

* 依赖模型与约束建模质量。

6.6.3 按感知依赖

类型	costmap	odom	预测	代表
纯跟踪	❌	✅	❌	Pure Pursuit
障碍感知	✅	✅	❌	DWB, MPPI
预测避障	✅	✅	✅	TEB + prediction

6.6.4 时空联合轨迹规划

动机：仅跟踪几何路径无法协调何时到达路径各点；动态障碍需要预测时间维上的冲突。时空联合方法在优化变量中同时包含空间配置与时间参数（或等价地，固定 \(\Delta t\) 下的控制序列 \(\mathbf{U}\)）。

族	决策变量	时间如何进入	代表	Nav2 / Autonomy
Timed Elastic Band	位姿 \(s_k\) + 段时长 \(\Delta t_k\)	显式 \(\Delta t_k\)；共弧 + 速度界	TEB (Rösmann)	第三方 `teb_local_planner` · ❌
Elastic Band（经典）	仅位姿链	事后 time-scaling	EB, NavFn 局部	—
MPC / NMPC	\(\mathbf{U}_{0:H-1}\) 或 \(x_k\)	固定 \(H\Delta t\) 网格	acados, CasADi	❌ · 见 §15
MPPI	采样 \(\mathbf{U}^{(k)}\)	rollout 步长 \(\Delta t\)	nav2_mppi	⏳ · §11
CHOMP / TrajOpt	离散 \(x_k\) 或 waypoints	均匀 \(\Delta t\) 或弧长参数	MoveIt, 研究栈	❌
Kinodynamic 搜索	状态-时间 \((x,t)\) 图	边带 \(\Delta t\)	Hybrid A* 局部、RRT*	Planning 层为主

与分层栈的关系：

                    ┌─ 仅空间 deform ─ CHOMP, TrajOpt
Planning.Path ──────┼─ 空间 + 显式 Δt ─ TEB
                    └─ 控制序列 u_k ─── MPC, MPPI, DWB

TEB 是 Nav2 生态中最典型的显式时空联合局部规划器：\(B=\{s_1,\Delta T_1,\ldots,s_n\}\)（第三方 teb_local_planner，Autonomy 未集成）。
MPC/MPPI 不显式优化 \(\Delta t_k\)，但 \(H\) 与 \(\Delta t\) 固定时，\(\{x_k\}_{0:H}\) 仍是一条离散时空轨迹。
Planning 输出的 Path 不含 \(t\)；时空联合发生在 Control（或 move_base 的 local planner 角色）。

选型提示：动态障碍多、需 jerk/加速度协调 → TEB / MPPI / NMPC；静态窄道、低算力 → RPP / Graceful（几何 + 启发式减速）。

6.6.5 Elastic Band 与时空参数化演进

局部「deform 路径 + 满足约束」的思想可沿时间是否显式优化分为三代：

阶段	表示	时间	代表	特点
Elastic Band (EB)	位姿链 \(\{s_k\}\)	事后 time-scaling	Metzig & Günther, EB 局部规划	仅空间 deform；\(\Delta t\) 由 \(v_{max}\) 导出
Timed EB / TEB	\(s_k\) 与 \(\Delta T_k\) 交错	优化变量	Rösmann et al. 2009–2017	同一 NLP 协调 kinodynamic + 动态障碍
控制序列 MPC	\(x_k,u_k\) 网格	固定 \(H\Delta t\)	NMPC, MPPI, DWB	不显式 \(\Delta T_k\)，但 \(\{x_k\}\) 即离散时空轨迹

Planning.Path ──► EB：deform {s_k} ──► time-scale ──► cmd_vel
              ──► TEB：min V({s_k, ΔT_k})
              ──► MPC/MPPI：min Σ ℓ(x_k,u_k), x_{k+1}=f(x_k,u_k)

EB → TEB：从「先形后时」到「形时同优化」，是 Nav2 生态选用 TEB 而非纯 EB 的主因。
TEB vs MPC：TEB 稀疏 band + LM；MPC 固定 horizon 网格 + SQP/采样。动态障碍多时 TEB 可接 prediction 时间窗；MPC/MPPI 在 critic/约束中编码预测轨迹（见 §6.7.13）。
Hybrid A*：在 Planning 层做 kinodynamic 全局搜索 \((x,t)\)；局部仍常用 TEB/MPPI 跟踪或短 horizon 重优化（Planning §6.5.4）。

6.7 核心算法详解

各算法动机 → 核心公式 → 优劣 → Nav2/Autonomy 状态。完整推导见 §10–§15 专题。

6.7.1 Pure Pursuit（1985）

动机：农业/户外低速场景，只需沿路径走，无需显式避障（障碍由全局规划处理）。

在路径上取 lookahead 距离 \(L_d\) 处目标点，几何曲率：

\[ \kappa = \frac{2 y^*}{L_d^2}, \quad \omega = \kappa v \]

优点	缺点
\(O(n)\) 极低延迟	不避障
工业验证数十年	急弯切内道
参数仅 \(L_d\)	无速度规划

Autonomy：controller_utils 中 GetLookAheadPoint 等已就绪，作为 RPP 核心。

6.7.2 Regulated Pure Pursuit（Nav2 2019）

动机：PP 在急弯、近障碍、近目标时速度过大 → 在 PP 上叠加曲率调节、障碍减速、接近目标减速。

\[ v_{cmd} = v_{pref} \cdot f_{curv}(\kappa) \cdot f_{obs}(d_{obs}) \cdot f_{goal}(d_{goal}) \]

Nav2 默认 indoor 控制器。Autonomy Phase 1 推荐首个插件（工具函数已有）。

详见 §12 RPP。

6.7.3 Stanley（2005 DARPA）

动机：高速自动驾驶需同时压横向与航向误差。

\[ \delta = e_\theta + \arctan\!\left(\frac{k_e e_y}{v + \varepsilon}\right) \]

室内 AGV 较少用；Autonomy 未规划移植。

6.7.4 DWA / DWB（1995 / 2010）

动机：全局路径可能被新出现的局部障碍阻断，需在 \((v,\omega)\) 空间快速搜索可行速度。

DWA：动态窗口 \(\mathcal{V}_d\) = 可达速度 ∩ 安全速度，评分 \(G=\sum w_i S_i\)。

DWB：Nav2 将 DWA 插件化——Trajectory Generator + 多个 Critic（PathDist, GoalDist, ObstacleFootprint, RotateToGoal…）。

优点	缺点
成熟、生态丰富	critic 权重调参复杂
实时局部避障	高维 (vx,vy,ω) 采样爆炸
可组合行为	轨迹前向仿真近似

Autonomy：未实现；动态避障需求高时可评估移植或直接用 MPPI。

详见 §13 DWB。

6.7.5 MPPI（2017–2022）

动机：统一处理非线性动力学、多目标代价、约束，避免手工设计 DWB critic 权重；利用 GPU/多核并行采样。

路径积分近似：

\[ u^* \approx \sum_{k=1}^{K} w_k u^{(k)}, \quad w_k = \frac{\exp(-S(u^{(k)})/\lambda)}{\sum_j \exp(-S(u^{(j)})/\lambda)} \]

Nav2 nav2_mppi_controller：\(K \sim 2000\)，\(H \sim 56\)，多 CostCritic（Obstacle, PathFollow, Goal, PreferForward…）。

优点	缺点
非线性+约束自然融合	算力需求大
Critics 可扩展	temperature、noise_std 敏感
支持 Omni/Ackermann	需与 control_frequency 对齐

Autonomy：controller.lua 已含完整 MPPI 配置；插件待实现。

详见 §11 MPPI。

6.7.6 TEB（2009–2017）

动机：经典 Elastic Band 只 deform 位姿，时间靠事后 scaling；DWB/MPPI 的 rollout 步长固定但无显式 \(\Delta t_k\) 优化。TEB 在 Timed Elastic Band 中联合优化 \(s_k\) 与 \(\Delta T_k\)，将 kinodynamic 与避障写入同一稀疏 NLP（Rösmann et al., IROS 2017）。

\[ B = \{ s_1,\, \Delta T_1,\, s_2,\, \ldots,\, \Delta T_{n-1},\, s_n \}, \quad \min_B \tilde{V}(B) = \sum_k \big( \phi_{\mathrm{obs},k} + \phi_{\mathrm{kin},k} + \chi_{\mathrm{time},k} + \ldots \big) \]

优点	缺点
时空联合，动态障碍可预测	非凸，LM 局部最优
显式 \(v_k,a_k\) 由 \(\Delta T_k\) 导出	参数/权重多
car-like / 差速共框架	Autonomy 未集成

适合人群、叉车等动态场景；可接 Prediction 模块。

详见 Rösmann et al. (IROS 2017; IEEE RAM 2017)。

6.7.7 Graceful Controller（2023）

动机：室内服务机器人要平滑到达（初始对齐、减速区、少倒车），比 RPP 更强调 UX。

基于 Park & Kuipers (ICRA 2011) 平滑控制律 + Nav2 工程封装（slowdown_radius、collision check）。

Autonomy 配置已预留，Phase 2 推荐。

详见 §10 Graceful。

6.7.8 经典 NMPC（与 MPPI 区分）

动机：显式求解有限时域 OCP，硬约束保证性更好（泊车、高速）。

\[ \min_{\mathbf{U}} \sum_{k=0}^{H-1} \ell(x_k,u_k) + \ell_f(x_H) \quad \text{s.t. 动力学与约束} \]

求解：SQP / RTI / acados。与 MPPI（采样+softmax）互补。Autonomy 未实现，见 §15 MPC。

6.7.9 Learning-based 局部控制

路线	动机	现状
端到端	省掉手工 critic	仿真→真机 gap、安全
RL local planner	自适应复杂环境	训练成本高
Diffusion policy	多模态轨迹	2024+ 研究热点

工程产品仍以 Nav2 经典控制器为主；学习方案多作辅助（启发式、perception-to-cost）。

6.7.10 CHOMP（2009–2013）

动机：在固定时间网格上对离散轨迹 \(x_{0:H}\) 做协变梯度下降，用障碍势场 + 平滑正则 deform 路径，无需手工 DWA 评分权重。

\[ \min_{x_{0:H}} \sum_k \Big( c_{\mathrm{obs}}(x_k) + \lambda \|\nabla x_k\|^2 + \mu \|\nabla^2 x_k\|^2 \Big) \]

特点	说明
空间轨迹优化	时间通常均匀 \(\Delta t\)；非显式 \(\Delta t_k\) 决策
局部方法	初值敏感，易陷局部极小
生态	源于 MoveIt! 机械臂；平面移动机器人有移植

Autonomy 未集成；与 TEB 相比缺少显式 timed band，与 MPPI 相比无约束硬保证。

文献：Ratliff et al., CHOMP: Gradient Optimization Techniques for Efficient Motion Planning, ICRA 2009；Zucker et al., 2013 协变版本。

6.7.11 TrajOpt / STOMP（2013–2014）

TrajOpt（Schulman et al.）：将碰撞等不等式约束序列凸化（SCS），在 waypoints 上迭代求解局部轨迹；适合低维 C-space，移动机器人多用于离线或短 horizon 重规划。

STOMP（Kalakrishnan et al.）：对轨迹加随机扰动，用加权更新（与 MPPI 同属随机优化族，但更新对象为整条轨迹形状而非控制序列）。

方法	优化对象	与 MPPI/TEB 关系
TrajOpt	waypoint / \(x_k\)	确定性 NLP；无 softmax
STOMP	轨迹分布	随机采样轨迹；无 MPC 滚动
MPPI	\(\mathbf{U}_{0:H-1}\)	滚动 + 路径积分
TEB	\(s_k,\,\Delta T_k\)	显式时空联合

Autonomy 未集成；动态局部场景更常选 MPPI（Nav2 官方）或 TEB（第三方）。

6.7.12 Time-scaling 与 jerk 约束

动机：全局 Path 几何可行但无速度剖面；工业 AGV 需限制 \(v,a,jerk\)（Graceful / 轮椅场景，见 §10）。

常见两阶段：

几何路径 \(\mathcal{P}(s)\)（Planning 或 Pure Pursuit 隐式）
Time-scaling \(s(t)\)：\(\dot{s}\leq v_{max}(s)\)，\(\ddot{s}\leq a_{max}\)，有时加 jerk 界

Park & Kuipers (2011) 在平滑控制律上通过 motion target 序列实现有界 \(v,a,jerk\)；Nav2 VelocitySmoother 在 \(u\) 层做后处理（§4）。这与 TEB/NMPC 的单阶段时空联合互补：实现简单 vs 全局协调。

6.7.13 预测感知时空规划

动机：静态 costmap 假设障碍不动；行人、叉车需 \((x,y,t)\) 冲突检测。Prediction 模块输出障碍未来轨迹 \(\hat{o}_j(t)\)，局部规划在时空上留 clearance。

集成方式	做法	代表
TEB 动态项	\(\phi_{\mathrm{dyn},k}(s_k,t_k)\) 惩罚与 \(\hat{o}(t)\) 距离	`teb_local_planner` + prediction
MPPI critic	rollout 中采样障碍位置	`ObstacleCritic` + 预测接口
VO / ORCA	速度空间半平面约束	多机/人群；常与 DWA 结合
重规划触发	`IsPathValid` + 高频 `GetPlan`	Navigator 默认策略（非显式时空 NLP）

Autonomy：prediction 与 control 尚未闭环；工程上可先 Navigator 高频重规划 + MPPI 障碍 critic，再演进 TEB 式显式时空项。详见 Prediction 行为预测。

6.7.14 Velocity Obstacles 与 kinodynamic 局部搜索

Velocity Obstacles (VO)：在 \((v,\omega)\) 或 \((v_x,v_y)\) 空间剔除 \(T\) 秒内与动态障碍碰撞的速度集；ORCA 用线性约束近似，适合多智能体局部协调。

\[ \mathcal{V}_{safe} = \mathcal{V}_{max} \setminus \bigcup_j VO_j(x, \hat{o}_j, \hat{v}_j) \]

与 DWB/MPPI 关系：DWB 在离散 \((v,\omega)\) 采样上可附加 VO 可行性过滤；MPPI 通过 ObstacleCritic + 预测 rollout 实现软约束。TEB 则在 band 上写硬/软时空障碍势。

Kinodynamic 局部 RRT* / Hybrid A*（短 horizon）：在 \((x,t)\) 或 \((x,y,\theta,v)\) 上搜索，输出可行轨迹片段；算力高于 TEB/MPPI，多用于泊车、非结构化环境，Autonomy 未内置。全局 Hybrid A* 见 Planning 综述 §6.2。

6.8 辅助组件

局部控制不仅是「算 \((v,\omega)\)」，Nav2 将到达判定、卡住检测、速度后处理独立为可组合组件。

6.8.1 Goal Checker

类型	判定	典型场景
SimpleGoalChecker	XY + yaw	默认导航
PositionGoalChecker	仅 XY	不要求最终朝向
StoppedGoalChecker	XY + yaw + \(\|v\|<\epsilon\)	充电对接

Autonomy ✅ 三种均已实现。见 §3 Checkers。

6.8.2 Progress Checker

检测机器人是否在时间窗口内有足够位移/转角，否则 Navigator 触发 Recovery。

Autonomy ✅ Simple / Pose 两种；时间窗口待完善。

6.8.3 VelocitySmoother

动机：控制器输出可能阶跃，底盘驱动器/电机有 \(a_{max}\) 与 deadband。

约束 \(\Delta v\) per cycle，支持 OPEN/CLOSED_LOOP。Autonomy 算法 ✅，节点接线 ⏳。

见 §4 VelocitySmoother。

6.9 Autonomy control 能力边界

6.9.1 已实现

组件	状态
`ControllerInterface` 插件接口	✅
Goal Checker ×3	✅
Progress Checker ×2	✅
VelocitySmoother / OdomSmoother	✅
几何工具（lookahead、圆-线段交点）	✅
Lua → `ControllerOptions`	✅
`ControllerServer` 骨架	⏳ 部分

6.9.2 待实现（优先级）

组件	优先级	说明
FollowPath 控制循环	P0	`ComputeControl()` 等
Autolink 插件加载	P0	参照 `PlannerServer`
RPP / Graceful 插件	P1	首个可用控制器
CheckerOptions Lua 接线	P1	proto 已有
MPPI 插件	P2	可选编译
VelocitySmoother 节点	P2	pub/sub

6.9.3 与 Nav2 能力矩阵

能力	Nav2	Autonomy
FollowPath	✅	⏳
Regulated PP	✅	❌
Graceful	✅	⏳ 配置
MPPI	✅	⏳ 配置
DWB	✅	❌
TEB	✅	❌
Goal/Progress Checker	✅	✅
Velocity Smoother	✅	✅ 未接线

6.10 工程质量与影响因素

6.10.1 控制频率

频率	适用	说明
10 Hz	低算力 / 慢速 AGV	延迟明显
20 Hz	默认推荐	与 MPPI `dt=0.05` 一致
50 Hz	高速 / 全向	需更强 CPU/GPU

6.10.2 Lookahead 与容差

参数	小	中	大
\(L_d\)	跟踪紧、易抖	0.4–0.6 m 常用	切弯、平滑
`xy_goal_tolerance`	精度高	0.25 m	易报到达
`required_movement_radius`	易触发 recovery	0.5 m	卡住不敏感

6.10.3 鱼骨图：跟踪质量不佳

上层原因

路径与规划

航点过疏/过密
全局路径急弯
重规划滞后
Path 坐标系错误

控制器与参数

控制器选型不当
lookahead 不匹配速度
MPPI critic 权重失衡
max_vel 与地图不匹配

机器人模型

footprint 偏差
非完整未建模
滑移/打滑

局部跟踪质量不佳

下层原因

感知与地图

local costmap 延迟
膨胀不足
动态障碍未入图

定位与 TF

odom 漂移
map→odom 跳变
控制频率 < 定位频率

执行与平滑

VelocitySmoother 过限
deadband 过大
底盘 PID 饱和

6.10.4 因素—对策速查

分支	症状	对策
路径	切弯、振荡	增大 \(L_d\)；Planning 换 Theta*
控制器	贴障、卡住	换 MPPI/DWB；调 ObstacleCritic
定位	路径相对漂移	查 TF；提高 control 频率
平滑	起步慢、到不了	调 `max_accel`；CLOSED_LOOP
Checker	早报到达	收紧 tolerance 或 StoppedGoalChecker

更多见 §0.19 排错。

6.11 算法复杂度对比

算法	复杂度	20 Hz 典型耗时	内存
Pure Pursuit	\(O(n)\)	< 0.1 ms	低
Regulated PP	\(O(n)\)	< 0.5 ms	低
Graceful	\(O(n)\)	< 1 ms	低
DWB	\(O(n_{traj}\cdot n_{sample})\)	1–5 ms	中
MPPI (\(2000\times56\))	\(O(K\cdot H)\)	5–20 ms	高
TEB	\(O(n_b \cdot n_{iter})\)	10–50 ms	中
CHOMP / TrajOpt	\(O(H \cdot n_{iter})\)	10–80 ms	中
NMPC	\(O(n_x^3 H)\)	5–30 ms	中–高

* 随 CPU/GPU、costmap 大小变化。

6.12 与其他模块的耦合

planning ──Path──► controller_server ──cmd_vel──► 底盘
    │                    ↑    ↑
global costmap      local costmap
    │                    ↑
   map ◄──scan── perception
    │
localization ──odom/TF──► controller
    │
navigator ──FollowPath──► controller
         ◄──goal/progress check──┘

模块	数据	说明
`planning`	`Path`	全局参考
`map`	global/local costmap	MPPI/DWB 避障
`localization`	odom, TF	闭环
`navigator`	FollowPath, recovery	编排
`prediction`	动态障碍（可选）	TEB/MPPI 扩展

6.13 业界生态

6.13.1 Nav2 控制器生态

控制器	包	算法	Autonomy
Regulated PP	nav2_regulated_pure_pursuit	几何+调节	⏳
Graceful	nav2_graceful_controller	平滑跟踪	⏳ 配置
MPPI	nav2_mppi_controller	随机 MPC	⏳ 配置
DWB	nav2_dwb_controller	DWA+critics	❌
Rotation Shim	nav2_rotation_shim	对齐辅助	❌
TEB	teb_local_planner (3rd party)	弹性带	❌

6.13.2 其他框架

框架	局部控制	特点
Navigation2	插件多种	ROS 2 标准
Autonomy	对齐 Nav2	autolink
ROS 1 move_base	DWA/TEB	legacy
Apollo	MPC + 规划	自动驾驶
Isaac Sim / Nav2	MPPI GPU	仿真验证

6.14 工程选型矩阵

场景	推荐控制器	Goal Checker	关键参数
室内差速通用	Graceful / RPP	SimpleGoalChecker	lookahead 0.4–0.6 m
动态避障	MPPI	SimpleGoalChecker	batch_size 2000
窄通道	Graceful + collision	SimpleGoalChecker	`use_collision_detection`
充电对接	RPP / Graceful	StoppedGoalChecker	tolerance 0.05 m
只到位置	任意	PositionGoalChecker	—
低算力嵌入式	RPP / PP	SimpleGoalChecker	10 Hz
全向机器人	MPPI (Omni)	SimpleGoalChecker	`vy_std > 0`
仿真调试	RPP	SimpleGoalChecker	OPEN_LOOP smoother
高速户外	Stanley / MPC	StoppedGoalChecker	模型精确
人群密集	MPPI / TEB	SimpleGoalChecker	高 obstacle critic；时空联合
需显式 \(\Delta t\) / jerk 协调	TEB / NMPC	StoppedGoalChecker	见 §6.6.4

Autonomy 当前建议：FollowPath 循环完成后，Phase 1 上 RPP 验证闭环；Phase 2 Graceful；Phase 3 MPPI。Checker + Smoother 可先行单元测试。

6.15 选型决策树

需要沿 Path 跟踪并输出 cmd_vel？
├── 否 → 非 control 职责（behavior / manipulation）
└── 是
    ├── 环境动态障碍多？
    │   ├── 是 → 需时空联合？ → TEB / NMPC；否则 MPPI / DWB
    │   └── 否
    │       ├── 强调平滑 UX？ → Graceful
    │       ├── 要低延迟/嵌入式？ → RPP / Pure Pursuit
    │       └── 默认 → RPP（Nav2 默认等价）
    ├── 全向底盘？ → MPPI Omni 模型
    ├── 充电/精密停靠？ → StoppedGoalChecker
    └── 算力/频率
        ├── MPPI 需 GPU 或多核 + ≥20 Hz
        └── RPP/Graceful 10–20 Hz 即可

6.16 未来趋势与路线图

2025 Autonomy
Checker+Smoother 就绪

2026 RPP / Graceful
首个插件

2027 MPPI 可选
GPU 加速

2028 Prediction
+ MPPI/TEB

2029+ 学习辅助
critic / policy

方向	说明
GPU MPPI	batch 10k+，Isaac/Nav2 验证
规划-控制联合 OCP	单 NLP 替代 Path + Controller 分层（研究多、产品少）
显式时空 + 预测	Prediction 模块 → TEB/MPPI 动态约束
Semantic control	语义地图调速（人行道/坡道）
多机协调	速度障碍、MAPF 与 local trajectory 结合
Safe RL / Diffusion traj	学习生成 \(\tau(t)\)，约束校验器兜底

6.17 术语表

术语	英文	说明
局部轨迹规划	Local Trajectory Planning	输出 \(\tau(t)\) 或 \(u(t)\)；Nav2 中常称 local planner / controller
几何路径	Geometric Path	\(\mathcal{P}\subset SE(2)\)，无时间
时空轨迹	Spatiotemporal Trajectory	含 \(t\) 或 \(\Delta t_k\) 的 \(\tau\)
时空联合优化	Spatiotemporal / Kinodynamic Planning	同时优化空间与时间（TEB、NMPC）
Timed Elastic Band	TEB	位姿 + \(\Delta T_k\) 交错参数化
Elastic Band	EB	仅位姿链；时间后处理
Velocity Obstacle	VO / ORCA	动态障碍速度可行集
轨迹优化	Trajectory Optimization	CHOMP, TrajOpt, STOMP
Time-scaling	Time Scaling	固定几何路径，只求 \(s(t)\)
轨迹跟踪	Trajectory Tracking	沿 Path/\(\tau\) 闭环
Lookahead	Look-ahead Distance	前瞻 \(L_d\)
动态窗口	Dynamic Window	可达速度集 \(\mathcal{V}_d\)
Critic	Critic Function	DWB/MPPI 评分项
非完整约束	Nonholonomic	DiffDrive \(v_y=0\)
Path Integral MPC	MPPI	采样型随机 MPC
Stateful Checker	—	XY 达标后只检 yaw

6.18 参考文献

教材

里程碑论文

年份	文献	贡献
1992	Coulter, Pure Pursuit (CMU-RI-TR-92-01)	PP
1997	Fox et al., DWA (IEEE RAM)	动态窗口
2006	Thrun et al., Stanley (JFR)	横向控制
2009	Ratliff et al., CHOMP (ICRA)	梯度轨迹优化
2011	Kalakrishnan et al., STOMP (ICRA)	随机轨迹优化
2011	Park & Kuipers, Smooth Control (ICRA)	Graceful 理论基础
2013	Zucker et al., CHOMP covariant (IJRR)	协变轨迹优化
2014	Schulman et al., TrajOpt (IJRR)	序列凸轨迹优化
2017	Rösmann et al., TEB (IEEE RAM)	时空弹性带
1998	Fiorini & Shiller, VO (IJRR)	速度障碍
2011	van den Berg et al., ORCA (IROS)	多智能体局部避障
2017	Williams et al., IT-MPC (ICRA)	MPPI
2022	Macenski et al., Nav2 (Science Robotics)	工程栈

工程

6.19 相关文档

§0 指南 · §2 架构
§3 Checkers · §4 Smoother · §5 控制器总览
Planning 综述（全局几何 · §6.5.4） · Navigator 综述
Prediction 行为预测（动态障碍 · TEB/MPPI 扩展）