🦉 猫头鹰都在卷机械臂逆解了，你确定不关注一下？

【机器人学习】小白学习第18讲：运动规划 Motion Planning

一句话核心：把"有体积的机器人"缩成一个点，把"障碍物"按机器人的形状长胖——原来难死的规划问题，就变成了"一个点在地图上找路"。

• 本讲目标：学会构造位形空间 C-space，理解四类主流规划方法与它们的"完备性"差别

一、为什么需要这个

小讲 1：从空世界到有障碍——运动规划的标准问题

前面的轨迹规划都偷偷假设世界是空的

前面讲的轨迹规划，都有一个偷偷的前提：工作空间是空的。现实里到处是东西——机械臂要躲焊装夹具和其他机器人，机场的行李车要躲柱子、传送带和路人。

运动规划要回答的那个问题

于是有了本讲的问题：运动规划（motion planning）——在充满障碍的环境中，找一条从起始位形到目标位形、全程不打交道的路径。

说得很实在：人类凭直觉就能在家具之间安全穿行，可要把这套"空间推理"写成机器人能跑的算法，几十年过去了仍然是活跃的研究方向。（所以别指望背一个公式通关，本章记的是几条思路。）

的标准问题与它的四个减法假设

为了把问题拎清楚，给出"标准问题"：机器人 B（单个刚体或运动链）在欧氏工作空间 W = IR^N（N = 2 或 3）里运动，有 p 个已知形状和位置的静止障碍 O₁…O_p，且假设机器人是自由飞行的（不受运动学约束）。问：给定起始和目标位姿，是否存在一条无碰撞路径？存在就给出来，不存在就报告失败。

• 单个刚体在 IR²里搬，叫piano movers’ problem（搬钢琴问题）——就是搬家公司抬着钢琴拐弯抹角那点事。
• 搬到 IR³叫广义搬家问题。

这四个假设（静止障碍、几何已知、只有机器人动、自由飞行）当然都可以放宽，但它们是"减法"：把问题压到纯几何层面，先解决干净的版本，再谈扩展。

停下来想一想：标准问题里的四个假设（障碍静止、几何已知、只有机器人动、自由飞行）各自减掉了什么麻烦？

二、核心概念

小讲 2：位形空间 C-space 与 C-障碍——把机器人缩成一个点

1. 位形空间 C-space——本章最漂亮的想法

思路：用广义坐标 q 把机器人的一个"位姿"对应成一个点。所有可能的 q 构成位形空间 C。这样一来，“机器人动起来"就变成"一个点在 C 里走”。（这和第 2 讲你学过的"用最少的一组数描述位形"是同一件事，只是这里看的不是单个位形，而是全体位形组成的空间。）

常见的 C：

• 平面移动机器人：C = IR² × SO(2)，维数 n = 3（两个平移 + 一个转角）。
• 空间移动机器人：C = IR³ × SO(3)，维数 n = 6。
• 基座固定的 n 关节平面臂：每个关节去掉 2 个自由度，3n − 2n = n维。
• 基座固定的 n 关节空间臂：每个关节去掉 5 个，6n − 5n = n维。
• 独轮车挂一节拖车：位置朝向 + 拖车转角，n = 4。

停下来想一想

注意 C 不一定是个平坦的欧氏空间。例 12.1 的 2R 平面臂：两个关节角都在 [0, 2π) 里，画出来是个正方形；但正方形左边和右边其实是"同一个姿态"，把两边粘起来得到的是一个环面（torus），正确的写法是SO(2) × SO(2)。后果很实际：在这个位形空间里量距离，直接用 ‖q₁ − q₂‖ 会把"实际很近、角度绕了一圈"的两个位姿算成很远。

给了两种距离：一种是物理上正确的d₁(q₁,q₂) = maxp∈B ‖p(q₁) − p(q₂)‖（机器人身上所有点中位移最大者），二是算法上省事的欧氏范数d₂ = ‖q₁ − q₂‖。用第二种时要记得先把角坐标的差按 ±2π 修正。

2. C-障碍与自由位形空间

要把"撞不撞"这件事搬进 C，就得把工作空间里的障碍成像到 C 里：

$$\begin{array}{r}C{O}_i=q\in C:B(q)\cap O_i\ne \varnothing \\ \text{（}12.3\text{）}\\ CO=\cup C{O}_i\text{（}C-\text{障碍区域）}\\ C_{free}=C-CO\text{（自由位形空间）}\\ \text{（}12.4-12.5\text{）}\end{array}$$

**直觉：**CO_i就是"所有会让机器人和第 i 个障碍碰上（含接触）的位形"的集合。于是规划问题被压缩成一句极干净的话：**qs 和 qg 是否落在 C_free的同一个连通分支里？**是就一定能找到，不是就报告失败。

怎么构造 CO_i？按机器人形状逐级加难：

• 点机器人：C 就是 W 的副本，C-障碍就是障碍的副本。
• 球形机器人：用球心（代表点）描述位形，C 仍是 W 的副本，但 C-障碍要"长大"——把障碍沿各方向长胖一个球半径。（理由：C-障碍的边界正好是"机器人贴着障碍滑动时球心留下的轨迹"。）在 IR² 里长出来的结果是广义多边形：直边 + 圆弧。
• 多边形机器人（只平动，朝向固定）：代表点取某个顶点，同样用"长大"手续；结果仍是多边形，且当机器人和障碍都凸时结果也是凸的。代表点选哪个顶点会改变 CO_i的形状，但不改变"有没有解"这件事。
• 又能平动又能转的多边形：C = IR² × SO(2)（局部可看成 IR³），做法是把每个固定朝向 θ 都切一刀，然后把这些切片沿 θ 轴堆起来。
• 机械臂：麻烦得多。除了"连杆撞障碍"，还有自碰撞（连杆互撞）。要得到 CO 的边界，得用逆运动学把"某个连杆刚好蹭到障碍"的所有关节角全找出来。，工作空间的障碍形状非常简单，映射出来的 C-障碍却是一团怪东西。

好消息：并不是所有方法都需要把 CO 显式算出来。概率法和人工势场 + 控制点就完全绕开了这一步——这也是它们在高维里更实用的原因。

图 1 位形空间的构造思想。左：机器人贴着障碍滑动一圈，其代表点的轨迹正好是"长大"后的障碍；右：把这一圈作为 C-障碍后，机器人就只是一个点了

小讲 3：回缩法与单元分解——两张完备的路网

3. 回缩法 Retraction：把 Cfree 压成一张路网

思路是：先造一张路标图 roadmapR ⊂ C_free，它概括了 C_free的连通性；以后要解决具体某次规划，只要把 qs、qg 各自"收缩（retract）"到路网上，再在网上找一条路。

挑的路网是广义 Voronoi 图。定义"间隙（clearance）"γ(q)为 q 到 C-障碍边界的最短距离，把决定这个距离的边界点叫 q 的邻居；那么 Voronoi 图就是"邻居不止一个"的那些点：

$$\begin{array}{r}V(C_{free})=q\in C_{free}:card(N(q))>1\\ \text{（}12.8\text{）}\end{array}$$

**直觉：**Voronoi 图是"离所有障碍都尽量远"的那条脊线——贴着它走，安全裕度最大。它的弧由直线段和抛物线段拼成，可以解析地算出来。

把任意点 q 收缩到图上很简单：沿间隙梯度的方向（就是从最近边界点指向 q 的那条射线）一直走，第一次撞到 V 的地方就是连接点 r(q)。

算法四步：① 建 V；② 算 r(q_s)、r(q_g)；③ 在 V 上找一串相邻弧把两点连起来（可用广度/深度优先，想要最短就用 A*）；④ 拼成完整路径。

性质：完备（complete）——有解一定找得到，没解一定报错。时间复杂度 O(v log v)（v 是 C_free边界的顶点数）。而且 V 算一次可以反复用，属于**多查询（multiple-query）**型。

4. 单元分解 Cell Decomposition

把 C_free切成一堆形状简单的单元（cell），满足两条：同一单元内任意两点，很容易连出一条无碰撞路径；相邻两单元之间，也很容易穿过去。于是可以做一张连通图（节点 = 单元，边 = 相邻），在图上搜出一串相邻单元，叫它通道（channel），再从通道里抽出具体路径。

• 精确分解：所有单元的并正好等于C_free。用凸多边形（具体是梯形分解），靠扫描线算法：让一条线扫过整个区域，每次经过 C_free的一个顶点，就从该顶点沿扫描线向两侧发射延长线直到撞上边界。凸性保证了"同一单元内直线行得通"。完备，复杂度 O(v log v)，也是多查询型。缺点是出来的路径是折线，得再用曲线拟合打磨。
• 近似分解：单元形状预先定死（二维用正方形），通常只是 C_free的由欠缺刻画的近似。做法：先把 C 四等分，把单元分成自由 / 占用 / 混合三类，在连通图上找路；如果通道里有混合单元，就把它再四分，如此递归——这就是四叉树（quadtree）（三维是八叉树，IRⁿ是 2ⁿ-树）。分辨率完备（resolution complete）：只要允许的最小单元取得足够小，有解就一定能找到。优点是实现极其简单（只需要"递归切格子 + 碰撞检测"）。

常见误区

致命点：维数灾难。2ⁿ-树的叶子数最多是 2^np（n 是维数、p 是层数），所以近似分解的复杂度对维数 n 和分辨率双双呈指数增长。的判断是：这类方法实际上只在不超过 4 维的位形空间里可用。这正是下面概率法出场的理由。

小讲 4：概率规划、人工势场与机械臂该怎么选

5. 概率规划 Probabilistic Planning

核心动作只有三个：随机采样→碰撞检测→把不撞的点连起来。不撞就收进图里，撞了就扔。关键是全程不需要显式构造 C-障碍。

PRM（概率路标图，）：每轮在 C 里按均匀分布撒一个点 q_rand，做碰撞检测；不撞就加进路图，并尝试用局部规划器（最常用：扔一条直线过去、沿线段采样逐点检测）连到附近的已有节点。循环直到迭代次数用完或连通分量数足够少，最后把 qs、qg 连到同一个连通分量上。

双向 RRT：不再维护一张覆盖全域的图，而是从 qs 和 qg 处各长出一棵树，每轮在树上找离随机点最近的节点 q_near，沿这个方向走固定步长 δ得到 q_new，若这一段无碰撞就把它挂到树上。随机点 q_rand本身不进树，只负责指方向。长一段时间后，两棵树轮流互相"延长"去够对方，接上就完成。

这套扩展虽然简单，却天生偏爱还没去过的地方——树会自动往空白处铺开，这就是它在高维里也不拉胯的原因。

还能处理非完整机器人：直线段对独轮车不成立，于是把"局部路径"换成一组运动基元（motion primitives）（固定 v 下 ω 取 −ω̄、0、+ω̄，得到左转、直行、右转三种弧），用它们拼接。若改用 Reeds–Shepp 曲线（第 17 讲）当基元，还能保证稠密性。

性质：概率完备（probabilistically complete）——有解时，找到解的概率随运行时间趋于 1。所以无解时它会一直跑，实际必须设最大迭代次数。另外，PRM 对窄通道很头疼：均匀撒点时落进某区域的概率正比于它的体积，窄缝当然难中签；补救办法是改用非均匀分布。

6. 人工势场 Artificial Potentials

前面几种都要求事先知道障碍的几何。势场法的看点是在线：边走边用传感器看，随时修改。做法是把代表点在 C 里放进一个人工势能场 —— 目标处挖一个"坑"吸引它，障碍周围堆一圈"墙"排斥它，然后顺着负梯度走。

吸引势，e = q_g− q：

• 抛物面型Ua1 = ½ ka‖e‖²⟹ 力fa1 = ka e：离目标越远拉力越大。
• 圆锥型Ua2 = ka‖e‖⟹ 力fa2 = ka e/‖e‖：力的大小恒定，不会随误差无限增长，但在 q_g处无定义。
• 折中做法：远处用圆锥、近目标（‖e‖ ≤ 1）用抛物面，力就处处连续。

排斥势：先假设 C-障碍已被分成若干凸块 CO_i，对每块定义

$$U_{r,i}(q)=(k_{r,i}/\gamma )(1/{\eta }_i(q)-1/{\eta }_{0,i}{)}^{\gamma }\text{，当}{\eta }_i(q)\le {\eta }_{0,i}\text{；否则为}0$$

• ηi(q)= q 到 CO_i的距离；η0,i= 影响半径（出了这个圈势场为 0）；典型取 γ = 2。
• 势值在障碍边界处趋于无穷（γ 越大越陡），这就是那堵推不倒的墙。
• 总势Ut = Ua + Σ Ur,i，总力ft = −∇Ut = fa + Σ fr,i。

怎么用它运动？给了三种：把 f_t当广义力τ = ft（路径最平滑，因为被动力学过滤了一遍）；当单位质点的加速度q̈ = ft；或者直接当期望速度q̇ = ft（修正最快、最安全，且无局部极小时可保证渐近稳定地停在 q_g）。最常用的是第三种，配欧拉积分qk+1 = qk + T ft(qk)——说白了就是最速下降法在优化 U_t。

由于它能根据传感器读数实时改变运动，本质上是一个反馈控制律，这类方法也叫反应式规划（reactive planning）。

图 2 人工势场法的局部极小陷阱：目标在两个障碍正后方，代表点被卡在两堵虚拟墙之间的"阴影区"，吸引力与排斥力恰好平衡

7. 机械臂这一档该怎么选

机械臂的难处是维数高（通常 n ≥ 4）而且有转动关节。两条对策：

• 离线：先用保守放大的办法降维——比如六轴拟人臂，把后三个关节（球形手腕）连同末端在全部行程内扫过的体积打包成一个整体，规划只针对底座、肩、肘三个关节，维数降到 3。代价是明显保守。剩下交给概率法：它在高维里表现最好，而且压根不看你 C-障碍长什么样。
• 在线：把势场直接建在工作空间 W 上，作用在布置于机械臂上的一组控制点上（末端一个，各连杆至少一个）：末端受"吸引 + 排斥"，其余控制点只受排斥。用雅可比转置把工作空间的梯度搬回位形空间：τ = −Σ JTi(q) ∇Ur(pi) − JTP(q) ∇Ut(pP)。这一步你眼熟吧——正是第 3 讲"雅可比把速度从关节空间映到操作空间，它的转置把力搬回来"。

代价要记住：工作空间里的势场会让局部极小问题更严重——各个控制点上的力可能在关节层面互相抵消，把机器人锁在一个"力平衡"位形上（右边就是这种情况）。

停下来想一想：点机器人、球形机器人、多边形机器人构造 C-障碍的共同手法是什么？PRM 和双向 RRT 为什么完全不需要把 CO 显式算出来？

三、关键公式

小讲 5：C-space 三件套、三种完备性与四类方法一览

1. C-space 三件套

$$\begin{array}{r}C{O}_i=q\in C:B(q)\cap O_i\ne \varnothing \\ CO=\cup C{O}_i\\ C_{free}=C-CO\end{array}$$

符号	含义
C	位形空间：一个位形 ↔ 一个点。含角坐标时整体是流形（如 2R 臂是环面 SO(2)×SO(2)）
B(q)	机器人在位形 q 下占据的工作空间区域
COi	C-障碍：所有会和第 i 个障碍相碰的位形。构造办法是让障碍"长大"机器人自身尺寸
Cfree	自由位形空间。规划 ⟺ 在 Cfree 里从 qs 走到 qg

直觉："机器人是点、障碍很胖"和"机器人很胖、障碍是原样"是同一个碰撞问题的两种画法，前者让所有后续算法变得简单。

2. 三种"完备性"——最容易混的一组概念

$$\begin{array}{r}\text{完备（}complete\text{）：有解必定找到，无解必定报错（回缩法、精确单元分解）}\\ \text{分辨率完备（}resolutioncomplete\text{）：只要分辨率足够细，有解就找到（近似单元分解、}best-first\text{、数值导航函数）}\\ \text{概率完备（}probabilisticallycomplete\text{）：有解时，找到解的概率}\to 1\text{（}PRM\text{、双向}RRT\text{、随机化}best-first\text{）}\end{array}$$

**直觉：**完备 = “保证给答案”；分辨率完备 = “给足磁盘和时间就保证给答案”；概率完备 = “一直跑下去，几乎一定会给答案，但它永远不会告诉你’没有路’”。人工势场连这三条都不满足。

3. 四类方法一览

方法	核心结构	完备性	适合
回缩法（Voronoi 图）	一维路网 + 图搜索	完备	低维、要最大安全裕度、多查询
精确单元分解	梯形单元 + 连通图	完备	低维多边形 Cfree、多查询
近似单元分解	四叉树 / 八叉树	分辨率完备	实现最简单，但只到 4 维左右
PRM / 双向 RRT	随机采样 + 碰撞检测	概率完备	高维、不想算 CO、多查询 / 单查询
人工势场	虚拟力场 + 梯度下降	都不满足（有局部极小）	在线 / 反应式，障碍事先未知

停下来想一想：分辨率完备和概率完备的说法差在哪？为什么概率法永远不会告诉你没有路？

小讲 6：两张图——C-障碍怎么长胖，势场怎么卡住

停下来想一想：图 1 里 C-障碍的边界是怎么画出来的——机器人贴着障碍滑一圈时，是哪个点在画线？图 2 里小球卡住时，吸引力与排斥力是什么关系？

四、例题 / 动手

例 1 动手构造一次 C-障碍

一个半径 R = 0.25 m 的圆形移动机器人（所以用球心代表位形，C = W = IR²），工作空间里有一个 1 m × 1 m 的方形障碍，占据区域 x ∈ [2, 3]、y ∈ [1, 2]。

**第 1 步：长胖。**把障碍沿各方向外扩 R，得到 C-障碍：

KaTeX parse error: Undefined control sequence: \piR at position 134: … + 2R(a + b) + \̲p̲i̲R̲^{2} = 1 + 2 \t…

原来 1 m² 的障碍变成了 2.196 m² 的"胖障碍"——机器人越大，可走的地方越少，这就是 C-space 里最直观的代价。

**第 2 步：判碰撞。**判断机器人在 q = (3.1, 2.1) 处是否安全：算该点到原方形障碍的距离——在 x、y 两个方向都超出了，距离 = √(0.1² + 0.1²) = 0.141 m < 0.25 m，撞了，所以 q ∉ C_free。换成 q = (3.3, 2.1)：距离 = √(0.3² + 0.1²) = 0.316 m > 0.25 m，安全。

**第 3 步：想清楚一件事。**你在 C 里画的那个"点"，其实代表真实的机器人；你在 C 里画的自由路径，在 W 里就是直径 0.5 m 的圆盘滚过去——只要它不进 C-障碍，圆盘就绝不会碰到障碍。这就是整套方法值钱的地方：碰撞检测被一次性地"预支"进 C-障碍的构造里了。

例 2 给势场算一组数，看看"墙"有多陡

取 k_r= 1，γ = 2，影响半径 η₀= 0.5 m。

机器人到障碍的距离 η	排斥势 Ur	排斥力模长 ‖fr‖
η = 0.50（恰好在影响边界）	0	0
η = 0.25	½(4 − 2)² = 2	(1/0.0625)·(2)¹ = 32
η = 0.10	½(10 − 2)² = 32	(1/0.01)·(8)¹ = 800

读出来的两点：（1）排斥力在靠近障碍时暴涨，这是有意的——它保证你撞不进去；（2）也正是这种暴涨，加上力的方向全由"局部最近点"决定，让它在凹角、双墙夹缝这类地方特别容易互相抵消，于是有了图 2 的陷阱。提到的一个例外值得记住：如果所有 C-障碍都是球（sphere world），叠加出来的总势只有孤立鞍点、没有局部极小。

动手建议：写一个 30 行的 Python，在 20×20 的网格环境里合成抛物面吸引势 + 单个方形障碍的排斥势，然后从起点做最速下降迭代到目标。再手工在障碍后面加一个障碍做出"阴影区"，看机器人是不是真的卡住了。

停下来想一想：例 1 里 1 m² 的方形障碍长胖之后变成多少平方米？例 2 里 η 从 0.25 缩到 0.10，排斥力的模长涨了多少倍？

五、常见误区

C-space 不是把工作空间按比例缩小

常见误区

误区 1：“C-space 就是把工作空间按比例画小一点。”

不是。第一，维数可能都不对：平面移动机器人要去 C = IR² × SO(2)，是三维；六轴臂是六维；而 2R 平面臂的 C 是一条"上下左右都连着自身"的环面，压根不是一块平面。第二，"长胖"是把机器人的形状和朝向融进障碍，如果机器人能转向，还得把每个朝向各切一片再沿 θ 轴堆起来。所以值得记住的永远是那句定义：COi = { q : B(q) ∩ Oi ≠ ∅ }。

格子剖得再细也躲不开维数灾难

常见误区

误区 2：“格子剖得够细就一定能行。”

理论上对（分辨率完备），代价是维数灾难。2ⁿ-树的叶子数上界是 2^np：维数 n 和层数 p 双双在指数上。所以的判断很明确——近似单元分解实际上只适用于不超过 4 维的位形空间，再高就得改用随机采样（PRM / RRT）：既然无法穷举，就随机抽若干点，只要这些点足够代表连通性即可。

势场法有局部极小，所以它不完备

常见误区

误区 3：“势场法机器人一定会被吸到目标。”

未必——这正是它最大的坑。叠加出来的总势Ut除了在 q_g有个全局极小，还常常有若干局部极小（典型就是障碍正后方的"阴影区"，图 2）。最速下降一脚踩进吸引域就停在那儿，明明有路却到不了，所以势场法不完备。给的补救：(a) best-first 算法（沿网格广度优先地把局部极小的吸引域"灌满"，总能爬出去，但也只对 ≤3 维可行）；(b) 加了随机游走的 best-first（概率完备）；© 干脆换导航函数——构造上就没有局部极小的人工势（包括网格上的"波前扩散"式数值导航函数），代价是计算量大、且只能离线用。

三种完备不是一回事

常见误区

误区 4：把三种"完备"当成一回事。

回缩法和精确分解是完备的；近似分解和 best-first 是分辨率完备的；PRM 与双向 RRT 只是概率完备。也就是说，概率法永远不会告诉你"没有路"——它只会一直跑到你设的迭代上限，然后假装失败。这一点做实验时极易误判。

停下来想一想：2ⁿ-树的叶子数是 2^np，这个式子里的 n 和 p 分别是什么，为什么它对维数呈指数增长？

六、本讲小结

停下来想一想：运动规划、轨迹规划、运动控制这三层分别回答哪个问题？哪一层是唯一关心时间的？

要点回顾

1. 运动规划补上了前面讲缺的那一块：有障碍时怎么找路。标准问题假设障碍静止且已知、机器人自由飞行。
2. 位形空间 C-space是全章的地基：一个位形缩成一个点，障碍物按机器人形状"长大"成 C-障碍COi，规划就变成"在 C_free里两点连一线"。代价是 C 常常不是欧氏空间，而是带角坐标的流形（2R 臂是环面）。
3. 四类主流方法各自一本账：回缩法用广义 Voronoi 图（最大安全裕度、完备、多查询）；单元分解用梯形分解 / 四叉树（实现最简单、分辨率完备，但怕高维）；概率法PRM / 双向 RRT（不需要显式 CO、高维表现最好、概率完备）；人工势场（唯一适合在线反应式，但有局部极小、不完备）。
4. 机械臂：离线用降维 + 概率法；在线把势场建在工作空间的控制点上，靠雅可比转置JT(q)把梯度搬回关节空间。
5. 完备性的三种档次要能脱口而出：完备 / 分辨率完备 / 概率完备。

一句话总结：运动规划的全部聪明都花在一句话上——把机器人缩成一个点、把障碍长胖，然后所有"躲障碍"的算法都退化成你熟悉的找路问题。

回扣全书的三层分工：
运动规划（本讲）回答"走哪条路"：在 C_free里找一条几何上可行的无碰撞路径——它不关心时间，也不关心动力学。
轨迹规划回答"路上怎么走"：给这条几何路径配上时间律 s(t)，满足速度、加速度、（对非完整系统还要满足）运动学约束，甚至做到时间最优。
运动控制回答"怎么真的走准"：用编码器、视觉、里程计的反馈，在有扰动、有模型误差的情况下把这个参考轨迹跟住。
三层串起来，就是一台机器人从"我要去那儿"到"我真的到了"的完整链条。