轨迹规划与运动规划

一句话定义

运动规划在"如何从 A 无碰地到 B"的位形空间（C-space）中搜索或优化出一条满足约束、光滑可执行的轨迹，是抓取位姿（来自感知）与关节伺服（下游控制）之间的桥梁。

为什么重要

• 真实环境中充满障碍（桌面、人、设备自碰），"直线路径过去"必然撞；没有规划的抓取 demo 都活不过第一张真桌子。
• 它决定了系统整体的"通行能力"：规划失败 = 任务失败，即使感知与抓取检测全部正确。

前置知识

kp-201。

核心概念

位形空间（C-space）：把机器人所有自由度的取值空间当作点集；障碍通过前向运动学"膨胀"进 C-space（机械臂通常是 6~7 维流形，无法显式表示，靠碰撞检测隐式查询——FCL 库）。

规划算法三大族：

1. 图搜索：栅格化后 A、D（动态环境重规划）。移动底盘导航的主力。
2. 基于采样：PRM（构建路图，多次查询复用）、RRT（快速随机树，单查询）、RRT*（渐近最优）。高维臂规划的主力，OMPL 库实现。
3. 基于优化：CHOMP、STOMP、TrajOpt——把"无碰+光滑+短"写成目标函数迭代优化；及时间参数化 TOPP-RA（给定路径求时间最优速度剖面）。

约束类型：碰撞约束（自碰/环境）、关节限位/速度/加速度限制、末端姿态约束（端着水杯）、闭环约束（双臂持物）。

原理与机制

机械臂操作标准流水线（MoveIt 2 视角）：

抓取目标位姿 ─► IK 求解候选关节解（多解） ─► 过滤（碰撞/奇异/接近关节限位）
   ─► 规划（OMPL: RRTConnect 等） ─► 得到无碰关节路径
   ─► 时间参数化（TOPP/速度限制） ─► 插值点列 ─► 关节控制器跟踪

为什么采样规划在高维可行：不显式建 C-space，只随机撒点 + 碰撞查询；维数升高时确定性格法指数爆炸，随机采样以概率完备性换可行性。RRT 的"偏向目标采样 + 树扩展"让搜索自然绕开障碍。

局部与全局配合（移动机器人）：全局规划给参考路径，局部规划器（DWA/TEB/MPPI）在短时域内避动态障碍——这就是闭环分层（kp-003）在导航中的体现。

公式与图示

优化式规划的最小目标函数：

$c_{obs}$ 为碰撞代价（用 SDF 距离场把"不碰"软化成连续惩罚），$\|\dot{\mathbf{q}}\|^2$ 惩罚抖动。

C-space 视角（2 维示意）：
  ┌──────────────────┐
  │ S ······█······· │   █ = 障碍在 C-space 的像
  │   ······█······· │   · = RRT 树的生长痕迹
  │   ······█···▲··· │   S 起点  G 目标
  │   ······█···G··· │
  └──────────────────┘

直观类比

• 迷宫找路：A* 是拿着地图穷举最优路线；RRT 是撒豆子探路，豆子连成一条道就出发。
• 端汤穿厨房：路径不光要无碰，还要"端得稳"（加速度限制）与"汤不洒"（末端姿态约束）——约束规划的现实版本。

实例与案例

• MoveIt 2 + OMPL：ROS 2 生态标准工具链；RRTConnect 因"双向生长、快、够用"成为默认选择。
• Drake 的 GCS（凸分解）规划（2023 前后）：把无碰空间分解为凸区域图上寻路，兼顾最优性与完备性——研究前沿正走向"采样+优化混合"。
• 导航选型：Nav2 中 SMAC（混合 A*）适合非完整约束车辆，RPP（Regulated Pure Pursuit）适合简单跟踪——全局规划算法按运动学约束选型。

常见误区

• 误区一：规划出的轨迹=可执行轨迹。还要过时间参数化、控制器带宽、振动抑制；直接下发原始 RRT 点列会导致抖动甚至失稳。
• 误区二：忽略动态物体。标准规划假定静态环境；有人走动需重规划或局部避障层兜底。
• 误区三：成功率高就上。要看失败时的行为：RRT 失败通常慢而安全，但部分实现在超时后返回部分路径——部署前必须明确失败语义。

自测题

1. 解释 C-space 以及为什么机械臂规划需要碰撞检测库而不是显式障碍图。
2. 对比 RRT、RRT*、PRM 的适用场景。
3. 抓取检测给出 10 个候选抓取，规划层面如何筛选与执行？

与其他知识点的关系

• kp-201 IK：规划在关节空间进行，IK 是接口。
• kp-205 MPC：MPC 可视为"滚动时域的优化式规划"，含动力学约束。
• kp-504 ROS 2：MoveIt2/Nav2 的工程集成。

延伸阅读

• LaValle, Planning Algorithms（免费在线全书）。
• Choset et al., Principles of Robot Motion。
• MoveIt 2 官方教程 + OMPL 文档（动手首选）。