第 9 章运动规划¶

第 8 章给一条已知路径配时间，先要找到那条无碰几何路径：给定起止位姿与障碍，求一条不与障碍相交的关节空间连续曲线。这是机械臂自动避障与移动机器人全局路径的算法核心。下面的三条线——精确法、图搜索近似法、采样式规划——共用一套取舍：完备性、最优性、可扩展性，三者最多同时满足两个；路径最终交第 8 章时标化。

1. C-space 与位阻¶

规划在构型空间（configuration space, C-space）里做：\(n\) 个自由度的机器人，其所有关节角组合构成 \(\mathbb{R}^n\) 中的一点 \(q\)，即一个构型。工作空间（world）里的障碍经正运动学（第 3 章）反卷成 C-space 里的位阻（configuration-space obstacle, C-obstacle）——凡是让机器人某部分碰到障碍的构型集合。

于是"无碰路径"变成 C-space 里从 \(q_{\text{start}}\) 到 \(q_{\text{goal}}\)、避开全部 C-obstacle 的曲线。把机器人的几何与关节耦合全部吸收进 C-obstacle 后，规划器面对的是"高维空间里的一个点绕障碍走"，问题被统一，代价是维度等于自由度数、C-obstacle 形状复杂难解析表达。实际系统里 C-obstacle 从不显式构造，只在采样点上做碰撞检测来回答"这是不是自由构型"——这也是采样式方法能工作的前提。

2. 精确法与近似法概览¶

按解的保证分两族：

精确法：给完备答案（要么最短路、要么证明无解）。二维多边形障碍中的点机器人可用可视图（visibility graph，连障碍顶点）求最短折线；自由空间可解析分解时用单元分解（cell decomposition，如 BCD、梯形分解）。可证最优，但只低维可用。
近似法：把空间离散或采样，在图/网格上搜索。有概率完备（样本足够多时若解存在则几乎必找到，但不保证有限步内）与分辨率依赖。高维（机械臂 6–7 自由度、移动底盘带约束）几乎只能用近似法。

势场法（人工势场，goal attraction + obstacle repulsion）实现最简，但局部极小会卡死，多用作局部避障或 RRT 内的引导，不作全局完备规划器。

三种"保证"要分清：完备（有解必找到、无解必报告）、概率完备（若有解，样本趋于无穷时找到的概率趋于 1，但不给时限）、分辨率完备（在给定离散化下完备，细到能穿过通道才有效）。工程选型看任务：离线、环境固定、要求可复现的场合倾向确定性方法；在线、高维、只要求快速给出可行解的场合才接受概率完备。

势场法的局部极小不靠调参消除：常见补救是加随机扰动跳出、用导航函数（harmonic potential，无局部极小但计算贵）替代，或者只把它当 RRT 的采样引导，到达性仍由树搜索保证。

三类方法共用一套取舍，终点都交第 8 章时标化：

flowchart LR
    cs["C-space 与位阻"] --> exact["精确法<br/>完备+最优，代价高"]
    cs --> gs["图搜索近似<br/>栅格 / A*"]
    cs --> sample["采样式<br/>PRM / RRT"]
    exact --> path["无碰几何路径"]
    gs --> path
    sample --> path
    path --> ts["第 8 章：时标化成轨迹"]

3. 图搜索与 A*¶

近似法把问题变成图上的最短路：占用栅格（occupancy grid）把 C-space 切成格、标自由/占据，或用 roadmap 把自由构型连成边。经典搜索逐级推进：BFS 给无权图最短跳数、Dijkstra 给带权最短路、A* 在 Dijkstra 上加启发函数 \(h(q)\) 估计到目标的剩余代价，只要 \(h\) 可采纳（不高估真实代价，如忽略障碍的欧氏距离），A* 即保证找到最优路径且比 Dijkstra 扩展更少节点。栅格分辨率越细越接近连续最优，但节点数与内存按维度指数增长——这正是高维机械臂弃用栅格、改用采样的原因。图搜索的产物是一条折线路径，须交第 8 章「时间参数化与速度加速度约束」配时间才可执行。

A* 的四个实现细节决定它好不好用：启发加权 \(f=g+\varepsilon h\)（\(\varepsilon>1\)）大幅减少扩展节点、代价是路径略长，实时系统普遍这么换；障碍膨胀按机器人外接半径放大占据格，把"点机器人"近似成"有体积机器人"；距离度量与连通方式要匹配（八连通配对角距离、四连通配曼哈顿距离），否则启发不可采纳、最优性丢失；路径后处理用视线检查把折线拉直，比加密分辨率便宜得多。

4. 采样式 PRM 与 RRT¶

高维 C-space 用采样式规划（sampling-based planning）绕开显式离散：

概率路线图（PRM, probabilistic roadmap）：随机撒一批自由构型为节点，近邻间若连线无碰就连边，建多查询路线图，再在图上搜索。适合环境不变、多目标的场合。
快速扩展随机树（RRT, rapidly-exploring random tree）：从起点长一棵树，每次取随机样本、找树上最近点、朝样本走一步（steer）加新节点，样本带偏向 goal 的偏置（goal biasing）以加速收敛。单查询、适合带微分约束与前向仿真，是移动机器人在线规划的主流。

两者都概率完备：样本无穷多时若存在通路则几乎必然找到。但基础 RRT 不保证最优——它先够到目标就停，路径常曲折。

落地时瓶颈很少在扩展逻辑，而在碰撞检测：它占规划总时间的绝大比例，所以有缓存、包围体层次（BVH）、连续碰撞检测（扫掠体）这些优化。窄通道是采样式方法的经典难点，随机样本落入通道的概率极低，RRT 类会出现"两边都长满树、就是不连通"，工程上用桥测试（bridge test）或把通道附近作为偏置采样区来缓解。

## 10 x10 场景、竖直墙带缝，起点(1,1)终点(9,9)，直线距离 11.31 为不可达下界
## RRT  命中耗时约 395 节点，路径长度 20.86

同一场景 RRT 给出 \(20.86\) 的路径，而障碍使其无法贴近直线距离 \(11.31\)——RRT 只保证可行、不保证最优（需 RRT* 等渐近最优变体随样本增加才逐步收敛）。

5. RRT* 与路径平滑¶

RRT*（RRT-star）在 RRT 的每步插入后做两件事换取渐近最优：①选择父节点（choose-parent），在所有半径 \(r\) 内的近亲里选使起点到该节点代价最小的父；②重连（rewire），若经新节点能降低邻居代价就改其父。近邻半径取 \(r=\gamma\big(\frac{\log n}{n}\big)^{1/d}\)，随样本数衰减的速率满足覆盖条件时，代价依概率 1 收敛到最优。

## 同场景：
## RRT  路径长度 20.86（395 节点，先够到就停）
## RRT* 路径长度 16.18（2302 节点，全程 rewire 优化）
## 直线不可达下界 11.31；RRT* 比 RRT 明显缩短

RRT* 用更多节点把路径从 \(20.86\) 压到 \(16.18\)，逼近不可达下界 \(11.31\)，直观演示渐近最优。代价是样本数与计算量上升。落地常用路径平滑（post-hoc shortcut）补救：对已有路径贪心尝试跳过中间点直连，无碰则删点。

## RRT* 原始 25 点、长 16.18 → shortcut 后 9 点、长 16.02

本例 shortcut 把 \(25\) 个航点减到 \(9\) 个、长度 \(16.18\to16.02\)——主要收益在减少拐点、路径更直更易时标化，长度增益则取决于原始路径的绕行程度。

渐近最优有前提：它针对固定环境下的单一查询，样本越多越好；动态环境里旧树基本作废，重规划要沿用上一时刻的树重连而不是从头长。近邻查询靠 k-d 树把每步代价从线性降到对数，否则样本数一上去就慢到不可用。代价函数还可以换成任意关心的量（路径长度、最小间隙、能耗），换个代价重新 rewire 就得到对应的最优解，这是它比栅格搜索灵活的地方。

6. 带运动模型的移动底盘规划¶

移动底盘的构型不只位置，还含朝向（如 \((x,y,\theta)\)），且受非完整运动学约束（第 10 章）——不能横移，只能沿前进方向滚动。此时"两点直线连边"通常违反运动模型，规划必须在满足约束的前提下扩展：

基于控制输入的 RRT：steer 不沿直线走，而是施加有限控制（如 \((v,\omega)\)）前向积分运动学，得到可执行的弧线分支。
运动基元 / 栅格：用 Dubins / Reeds–Shepp 曲线（含/不含倒车的最短常曲率路径）作局部连接，或在状态栅格里按离散控制转移搜索（Lattice / Hybrid A*）。

这类"带运动模型的规划"保证路径不仅是几何无碰、而且运动学可跟踪，衔接第 8 章时标化与底盘跟踪控制（归 control-note）。机械臂的多构型约束（连杆碰撞、奇异）则更多靠 C-obstacle 与逐点碰撞检测处理。

7. 要点¶

规划在 C-space 做：障碍经 FK 反卷成位阻，无碰路径 = 高维自由空间里绕开 C-obstacle 的曲线。
精确法（可视图 / 单元分解）可证最优但只低维；高维机械臂与移动底盘只能用近似法（栅格 / 采样）。
图搜索 BFS / Dijkstra / A* 在离散图上求最优，A* 靠可采纳启发省扩展；栅格分辨率与维度相冲突。
采样式 PRM / RRT 概率完备但不天然最优（固定场景 RRT 路径 20.86，直线不可达下界 11.31）。
RRT* 用 choose-parent + rewire、半径 \(\gamma(\log n/n)^{1/d}\) 换渐近最优（同场景压到 16.18）；shortcut 平滑减航点（25→9）。
移动底盘带非完整约束，须按运动模型扩展（控制输入 RRT、Dubins/Reeds–Shepp、Hybrid A*），保证可跟踪后再交时标化。

参考文献与延伸阅读¶

LaValle, S. M. Planning Algorithms. Cambridge University Press, 2006. 书籍主页
Karaman, S., & Frazzoli, E. Sampling-Based Algorithms for Optimal Motion Planning. The International Journal of Robotics Research, 2011. DOI
Kavraki, L. E., Švestka, P., Latombe, J.-C., & Overmars, M. H. Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces. IEEE Transactions on Robotics and Automation, 1996. DOI

第 9 章 运动规划¶