6.2 机器人运动规划

6.2 机器人运动规划

不变量侦探所：机械臂真正要躲的，不是墙，是墙在抽象空间里投下的阴影

让一台七轴机械臂从桌面抓取零件、让仓库里的自动搬运车绕过货架会合、让手术器械顺着血管蜿蜒前行——这些任务放到调度中心眼里，几乎都长成同一个问题：给出一条从起点构型到终点构型的连续路径，路径上每个点都不与环境相撞。表面看这是几何题，离得够远就行；可真做起来你会发现，它更像一道拓扑题。机器人在哪个空间里"想事情"，障碍物会把这片空间戳出怎样的洞，起点终点落没落在同一个连通块里——这些结构事实，往往比任何路径平滑算法都更先决定成败。

构型空间：把整台机器人缩成一个点

机器人的一个"姿势"，由它所有关节角与自由度的取值共同决定。把这些取值并成一个向量，机器人的全部可能姿势就对应某个高维空间里的一个点，这个空间叫构型空间（Configuration Space，简称 C-space 或 \mathcal{C}）。一个 n 自由度的机械臂，构型空间大体是一个 n 维环面 T^n：每个转动关节对应一个圆周 S^1，若干圆周的乘积就是环面。轮式底盘要考虑位置还要考虑朝向，构型空间是 \mathbb{R}^2 \times S^1；自由飘浮的刚体（卫星、无人机）则活在 \mathbb{R}^3 \times SO(3)。用欧拉角硬拼出三个转动自由度的做法会在某些姿态失灵（万向节锁死），原因正是三维转动群 SO(3) 与三维环面并不同胚——这是把流形硬压进欧氏坐标的代价。

把机器人缩成点之后，环境障碍必须换算到同一个空间里。凡是让机器人本体与环境（或机器人自己与身体其他部位）发生重叠的构型，合起来构成构型障碍（C-obstacle，记 \mathcal{C}_{\text{obs}}）。对点机器人而言，障碍不过是把工作空间里的物体沿机器人半径方向"膨胀"一圈（Minkowski 和）；对铰接的多连杆机器人，膨胀的结果要复杂得多，常常是弯曲、非凸甚至多分支的曲面。扣除障碍后剩下的

就是自由构型空间。运动规划的全部前提，都压在这条公式上：起点与终点若落在 \mathcal{C}_{\text{free}} 的同一个连通分支里，路径原则上存在；若落在不同分支，则无论采样多少次、优化多努力，路径都注定不存在。可行性由拓扑决定，这排在算法设计之前，是不折不扣的"先取证、后办案"。

对 2D 情形，自由空间的样子很容易想象，障碍物轮廓挡住的部分就是禁止区。可构型空间的拓扑会玩花招：环面沿关节角方向首尾相接，机械臂转过一整圈又回到同一构型，于是地图的两条边粘在一起，障碍在接缝处可以被"剪开"再绕过去。真正棘手的场景来自铰接与高维：一个六自由度机械臂的构型空间是 6 维的，障碍在其中表现为边界复杂的高维禁止区域，人脑的 3D 直觉在这里彻底失灵。这正是机器人运动规划区别于一般避障的困难来源。

从"看得见的几何"起步的经典算法

低维、静态、多边形化的世界里，几套经典算法给出过漂亮的回答。

可见图（visibility graph）法直接把问题变成图搜索。把起点、终点与所有多边形障碍的顶点取作节点，凡是两节点之间的直线段不穿过任何障碍内部，就连一条边，于是得到一张"视线图"。这张图之所以可靠，依赖一个几何事实：绕过多边形障碍的最短路径，必然是一段段贴着障碍顶点的折线，中间没有悬空的弯。因此在这张图上跑 Dijkstra，得到的就是欧氏意义下的最短无碰路径。它简单、精确，可代价也明确：顶点一多边就爆炸，只适用于二维平面与多边形障碍，还要处理"擦着顶点过"这类数值敏感问题。

单元分解法是另一路：把自由空间沿障碍边界切成若干梯形或简单单元，单元之间能互通就建邻接边，同样化为图搜索。它的完整性有保障，但剖分成本随维度暴涨，实际只在小规模低维场景讨得到便宜。

势场法走向另一个极端。给目标点设一个引力势，给障碍物设一片斥力势，机器人沿总势场的负梯度方向下滑，好像小球滚向谷底。它反应快、天然适合在线避障，却背着一个著名隐患：凹形障碍的怀抱会形成势场局部极小，机器人滑进去就停在原地，永远到不了目标。势场法因此极少被单独信赖，通常要与全局规划器配合，作为局部执行层存在。

这些算法各擅胜场，却共享同一道天花板：它们要么穷举几何细节，要么贪恋局部梯度，一旦维度升高或障碍复杂，就迅速失去工程意义。高维世界的出路，落在"以概率换完备"的采样思想上。

采样路线图：用随机性覆盖高维空间

在高维构型空间里，把自由空间剖成格子再搜，格子数随维度指数膨胀，直接做到底不现实。采样法换了个思路：不追求穷尽，只要求"样本足够多时，答案几乎必然出现"，这被称为概率完备性。

**概率路线图（PRM）**面向多查询场景。它在 \mathcal{C}_{\text{free}} 里随机撒点，把彼此距离足够近、且连线无碰撞的点连成边，先离线搭一张路线图；真正收到规划请求时，只需把起点终点接到图上，再做一次图搜索。环境不变、查询频繁的场合，PRM 的建图成本被反复摊薄，非常划算。它的短板是窄通道：若自由空间里只余一条细长走廊，随机点落入走廊的概率按体积比趋于 0，路线图就可能把两片本该连通的区域画成两个孤岛——这不是算力不足，而是采样分布和流形几何拧着劲。

**RRT（快速探索随机树）**则面向单查询。它从起点长出一棵树：每次随机采样一个构型，找到树上离它最近的节点，朝采样点方向试探着延伸一小段，若这段无碰就挂到树上。随机采样把树的生长不断拽向未探索区域，于是树能很快铺满自由空间；只要样本量足够，起点与终点同处一个连通分支时，树"几乎必然"够到终点。RRT 速度快、实现简单，是当下机器人领域应用最广的采样器之一。

RRT* 在 RRT 的扩展上加了"重连"步骤：新节点入树后，检查附近已有的节点，凡是经新节点绕行更短的就改写父指针，相当于一边探索一边修剪最短路径树。它继承了概率完备性，还多出渐进最优性——样本趋于无穷时，找到的路径代价收敛到理论最优。代价是每个节点的邻居查询与重连开销显著上升，靠近最优的速度也并非总能让人满意。

采样法的成败高度依赖两点：度量选得对不对，以及自由空间里"好采"的区域占多大比例。把采样向窄通道倾斜（如桥接采样、障碍附近加密），或让树沿势场方向偏置生长，都是工程上常见的补丁。就适用场景而言，PRM 适合静态环境中的大量查询，RRT 适合动态环境里的单次快速求解，RRT* 适合允许较长计算时间、希望路径尽量短的场合。

障碍不只是挡路，还在自由空间里凿洞

把障碍物想成单纯的"禁区"，会漏掉它们真正的威力：障碍会改变 \mathcal{C}_{\text{free}} 的拓扑，而拓扑决定路径的"本质差异"。

二维平面上，每堵把自由空间切成两半的障碍，都让从一侧到另一侧出现至少两类走法——从左边绕，或从右边绕。两条路径如果在自由空间内可以彼此连续变形而不撞障碍，就属于同一个同伦类；绕左与绕右属于不同同伦类，在障碍间"绕了几圈"的信息由基本群 \pi_1(\mathcal{C}_{\text{free}}) 记录。这个区分在工程上有实义：优化算法通常只在同一个同伦类内部搜索，若起点终点间的全局最优恰恰落在"先绕远再抄近"的另一个类里，局部优化就永远够不到它。拓扑感知的规划器会显式枚举同伦类（比如为每条路径记录它环绕了哪些障碍、绕向如何），确保每个类的代表路径都被纳入比较。

障碍的凸性也暗中作祟。凸障碍的构型障碍大体规整，可见图、二分空间划分这些手法尚能招架；非凸障碍（L 形走廊、环形墙、带有内腔的结构）会让构型障碍变得非凸乃至分成好几块，几何捷径随之失效，而势场法最怕的凹形陷阱恰恰由非凸障碍造就。采样法对凸性不敏感，但同样受困于窄通道：窄缝本质上是自由空间里的"细脖子"，测地距离远大于欧氏距离，均匀采样在这里注定低效。

高维构型空间则连"画出来"都是奢望。人类能感知的维度只有三维，十自由度以上机械臂的自由空间只能靠指标间接把握：连通分支数、洞数这类 Betti 数不会因为维度升高而消失，它们仍是判断"是否存在可行路径"的可靠罗盘。换句话说，维度越高，我们越要依赖不变量而非几何想象来导航。

图注：从机器人本体到可执行规划的一整条推理链。运动学约束把本体翻译成构型空间，障碍建模在构型空间里圈出禁区，扣除禁区后先做拓扑体检（连通性、洞、空腔），再做同伦分类，最后才轮到具体的路径搜索。

构型之后还有动力学这道坎

几何无碰的路径，对真实机器人未必可执行。汽车不能原地掉头，机械臂的关节力矩有上限，飞行器受气动耦合拖累——规划若只盯着 \mathcal{C}_{\text{free}}，产出的路径可能要求机器人在物理上做不到的运动。为此，路径规划常要升级为轨迹规划：给路径附上时间，生成 q(t),\dot q(t),\ddot q(t)，并逐点核对动力学方程

算出的关节力矩是否落在电机能力范围内。这一类带微分约束的问题，解法通常把轨迹离散成时空节点做多目标优化（时间最短、足够平滑、离障碍够远），或把动力学直接编进采样过程，让树只在"动力学可达"的方向上生长。非完整约束的存在还会改写可达集：底盘构型空间里看似连通的区域，受最小转弯半径限制后，真正够得着的子集可能出现尖点与缺口。于是"有效自由空间"的边界不再只是静态障碍的投影，而是受动力学约束削出来的可达域——空间结构随系统状态演化，静态拓扑随之升级为动态图景。

纵观全章，运动规划中值得信任的从来不是某条具体路径，而是路径赖以存在的结构：自由空间是否连通、路径落在哪个同伦类、动力学是否允许这段位移。采样、优化、平滑都只是在这套结构里做工；结构若判了死刑，再精巧的算法也救不回来。侦探办案要看清案发现场的通道与隔墙，机器人规划要看清构型空间的连通与缠绕——两者其实在追同一个真相：能去，还是不能去。

图注：一条从传感器数据走向可执行轨迹的典型流水线。几何重建把点云变成障碍模型，拓扑分析压缩出结构代表，采样法产出初始路径，动力学优化再把它打磨成机器人真跑得起来的轨迹。拓扑分析的作用是把后两步从"碰运气"变成"按图索骥"。