09 · 运动规划

⏱ 34 分钟 ｜ ⭐ 前八章的状态是一格一格的，机器人的状态是连续的实数 —— 这一章讲怎么把连续世界重新变回图

🎯 一句话

要么把连续空间离散化（占据栅格、Voronoi 图）让 A* 重新能用，要么干脆不搜索、直接顺着「目标吸引 + 障碍排斥」的合力往下滑（势场法）—— 后者快得多，但会卡在合力为零的地方永远出不来。

前八章你一直在一个有限、可枚举的状态空间里搜。真机器人不在那种空间里 —— 它在一块地板上，位置是一对实数。

🧩 一、断层在哪里

第 8 章教你怎么给一个搜索问题设计启发式。那套东西的前提你可能没注意到：

⭐ 讲义按最后一条把方法分成两类，这个分法很好用：

第一类适合扫地机、户外小车；第二类适合博物馆、商场、Robocup 小型组这种天花板上装了相机的场地。

🗺 二、占据栅格：把连续空间切回格子

最直接的一招：把环境切成笛卡尔网格，每个格子维护一个「这里有障碍」的概率估计。

⭐ 注意是概率不是 0/1。传感器有噪声：超声波会被斜面弹走，激光会穿过玻璃。 一次读数说「这格空」不可信，几十次融合起来才可信。所以每格存的是一个不断被更新的信念值。

一旦有了栅格，前面几章的东西原封不动地回来了：格子是节点，相邻格子（4 邻接或 8 邻接）是边， 障碍概率超阈值的格子删掉，然后跑 A*，启发式用曼哈顿距离或欧氏距离 —— 正是第 8 章讲的那两个。

⚠️ 分辨率的两难

格子多大是这个方法唯一的旋钮，而它两头都疼：

⭐ 格子数按边长的平方涨：精度提高一个数量级，内存和节点数各涨 100 倍。常见折中是分层 —— 粗栅格搜全局、细栅格避障。

🧲 三、势场法：不搜索，直接滑下去

另一条路彻底放弃搜索。把机器人当成势场里的一个点：目标吸它、障碍推它，它顺着合力走就行。

$$U(q) = U_{att}(q) + U_{rep}(q),\qquad F(q) = -\nabla U(q)$$

吸引项通常取到目标距离的平方（离得越远拉得越狠）：

$$U_{att}(q) = \tfrac12 k_{att}\, d(q, q_{goal})^2$$

排斥项只在障碍附近 $\rho \le \rho_0$ 生效，贴上去时趋于无穷：

$$U_{rep}(q) = \begin{cases}\tfrac12 k_{rep}\left(\dfrac{1}{\rho} - \dfrac{1}{\rho_0}\right)^2 & \rho \le \rho_0\\[4pt] 0 & \rho > \rho_0\end{cases}$$

⭐ 好处非常实在，讲义原话是 very rapid computation：每一步只要算几个向量加法， 不展开任何搜索树；天然反应式，传感器读到什么就算什么，不需要全局地图；障碍在动也无所谓，下一帧重算一遍。

代价在下一节。

💥 四、局部极小：势场法的致命伤

讲义为这一件事专门留了一整页。它不是「偶尔会遇到的小毛病」，它是这个方法的结构性缺陷。

三种典型的卡死：① 障碍正挡在目标前面 —— 机器人、障碍、目标三点共线时，吸引与排斥严格反向，必然存在抵消点； ② U 形（凹形）障碍 —— 滑进凹口后两侧排斥把它顶在中间、吸引把它往里推，来回振荡出不去； ③ 窄通道 —— 门框两侧的排斥叠加，在门口形成一道势垒，本来能过的门被「封」上了。

⭐⭐ 根本原因只有一句

⭐⭐ 势场法就是梯度下降。 它只看当前点的梯度，不展开树、不记录去过哪里、不回溯。 所以它继承了梯度下降的全部毛病：碰到局部极小就停，而且没有完备性 —— A* 保证「有路就一定找得到」，势场法什么都不保证。这是「快」的标价。

⚠️ 更麻烦的是失败是静默的：不崩、不报错、电机还通着电，日志一切正常，机器人只是在原地微微抖动。 工程上要加一个「多久没有靠近目标」的计时器兜底 —— 因为你没法靠看日志发现它卡住了。

三种补救，各有各的代价

🔨 二十行跑一遍卡死

# 势场法：一跑就卡在局部极小
import math

GOAL = (9.0, 5.0)
OBST = [(6.0, 5.0), (6.0, 6.2), (6.0, 3.8)]      # 三个障碍竖排，正挡在起点和目标中间
K_ATT, K_REP, RHO0 = 1.0, 8.0, 2.0               # RHO0：排斥力作用半径，超出就是 0

def force(p):
    fx = K_ATT * (GOAL[0] - p[0])                # ⭐ 吸引力：指向目标
    fy = K_ATT * (GOAL[1] - p[1])
    for o in OBST:
        dx, dy = p[0] - o[0], p[1] - o[1]
        rho = math.hypot(dx, dy)
        if 1e-9 < rho <= RHO0:                   # ⭐ 排斥力：背离障碍，贴近时急剧变大
            g = K_REP * (1.0 / rho - 1.0 / RHO0) / (rho ** 3)
            fx += g * dx
            fy += g * dy
    return fx, fy

p = [1.0, 5.0]
for step in range(1, 100001):
    fx, fy = force(p)
    n = math.hypot(fx, fy)
    if n < 1e-4:                                 # ⭐ 合力≈0：再也动不了了
        print("第 %d 步合力≈0，停在 (%.2f, %.2f)，离目标还差 %.2f" %
              (step, p[0], p[1], math.dist(p, GOAL)))
        break
    p[0] += 0.001 * fx
    p[1] += 0.001 * fy
else:
    print("10 万步都没到，停在 (%.2f, %.2f)" % (p[0], p[1]))

实跑输出：第 1337 步合力≈0，停在 (4.97, 5.00)，离目标还差 4.03。 起点到目标一共 8 个单位，它走了不到 4 个就永久停住了。

📡 五、VFH：不要把方向信息加没了

向量场直方图（Vector Field Histogram）是讲义给的「更不容易卡住」的方案，四步： ① 维护持续更新的笛卡尔直方图栅格（就是占据栅格）； ② 按机器人当前位置和朝向转成极坐标直方图 —— 横轴方向角，纵轴那个方向上的障碍密度； ③ 找出候选谷（连成一片的低障碍密度扇区）；④ 挑一个最接近目标方向的谷，朝那儿走。

⭐ 它和势场法的差别就在第 2 步。 势场法把所有障碍的排斥向量加成一个合力，而求和是有损的： 两个障碍中间有条缝，排斥向量一相加，缝就被抹平成「前方整体有阻力」，机器人看不见缝在哪儿。 VFH 保留的是沿方向的整个分布，缝在直方图上是一个显眼的低谷 —— 所以它能在势场法只会硬顶的地方找到穿过去的方向。

🕸 六、地图全知道时：Voronoi 与 Delaunay

天花板上有相机、障碍位置全部已知时，有比栅格聪明得多的做法。

Delaunay 三角剖分（讲义的贪心构造）：在障碍最近点之间加线段，按长度从短到长依次加， 与已有线段相交的一律不加，加完得到一张铺满自由空间的三角网。 Voronoi 图是它的对偶 —— 每条 Voronoi 边都是「到两个最近障碍等距」的点的集合。

所以沿 Voronoi 边走，等价于「随时离最近的障碍尽量远」 —— 正是你希望一台会晃、定位有误差的机器人走的路。

流程是：建 Voronoi 图 → 剪掉太窄、机器人过不去的弧 → 在剩下的图上跑 A* → 把路径转成轨迹。 ⭐ 价值在规模：同一块场地细栅格几万个节点，Voronoi 图只有几十条边，小三个数量级，而且天生落在「安全」的位置上。

⏱ 七、最短的路不等于最快的路

⭐ 一条有长直段的路可以先加速再减速；一条更短但拐弯很多的路，每个弯都要减速。长的那条反而更快。

要优化时间，路径就不能只是折线，得是能表达速度的曲线。讲义用参数三次样条，每一段写成

$$P(t) = \begin{pmatrix}P_x(t)\\ P_y(t)\end{pmatrix} = a\,t^3 + b\,t^2 + c\,t + d$$

给定这一段起点（$t=0$）和终点（$t=s$）的位置和速度，四个系数向量就唯一解出来了。

⭐ 关键改动在 $s$ 上。传统做法固定 $s = 1$（$t$ 只是形状参数）；讲义把 $s$ 当成这一段花掉的时间去最小化，约束是运动学能力：

$$\frac{\lVert P''(t)\rVert}{A} + \left(\frac{\lVert P'(t)\rVert}{V}\right)^{2} \le 1,\qquad 0 \le t \le s$$

$A$、$V$ 是最大加速度和最大速度，这一条同时管住「别超速」和「弯别拐太急」。

完整算法三步（讲义原文）：① Delaunay 三角剖分把场地变成图；② A* 搜索，路径由参数三次样条拼成、代价是时间不是长度； ③ ⭐ 梯度下降调路点，把整条曲线抹平。

⭐⭐ 第 3 步值得单独说一句：这里的梯度下降不是在训练神经网络。变量是几十个路点的坐标， 目标是整条轨迹的总时间，梯度解析可算 —— 没有数据集、没有标签、没有 GPU。 答案是被算出来的，不是被学出来的。讲义说这套系统真部署在 Robocup F180 小型组。

🔗 这一章连到哪里

✅ 检查点

1. 前八章的搜索和运动规划之间，最根本的那条断层是什么？
2. 占据栅格里每个格子存的为什么是概率而不是「有 / 没有」？
3. 一块 10 m × 10 m 的场地，格子边长从 5 cm 缩到 1 cm，格子数从多少变成多少？为什么是这个倍数？
4. 势场法为什么会卡在局部极小？用一句话说清根本原因。三种典型的卡死场景各是什么？
5. VFH 和势场法的关键差别在哪一步？为什么这个差别让它更不容易卡住？
6. 「沿 Voronoi 边走」在几何上意味着什么？为什么这对机器人有好处？
7. 最优轨迹规划的第 3 步用梯度下降，它优化的变量是什么、目标函数是什么？

🛑 可以停在这里

⚡ 走神救援

前八章的搜索都建在「状态可枚举」上 —— 20 个城市、100 个格子都数得完。真机器人不行：位置是一对实数，动作连续，代价是时间。出路一：离散化。 占据栅格切成网格，每格存「有障碍」的概率（不是 0/1，传感器有噪声），超阈值的删掉，剩下的当节点跑 A*。旋钮只有格子边长，两头都疼：20 cm 会把一道 30 cm 宽的门整体判成障碍；1 cm 在 10 m × 10 m 场地上是 100 万格（5 cm 时 4 万格，边长缩到 1/5、格子数涨 25 倍，按面积走）。出路二：不搜索 —— 势场法。 目标吸引、障碍排斥，顺着合力滑，每步只要几个向量加法，不要全局地图。⚠️ 代价是局部极小，三种典型：三点共线、U 形凹障碍、窄通道两侧排斥叠加把门封死。⭐⭐ 根本原因一句话 —— 势场法就是梯度下降：只看当前梯度，不展开树、不回溯，完备性直接没了。示例代码里机器人第 1337 步合力归零，停在离目标还差 4.03 的地方（全程才 8）；失败还是静默的，不崩不报错，得靠「多久没靠近目标」的超时才发现。补救三条：随机扰动（无保证）、调和势场（要全局地图）、⭐ 势场只做局部避障、全局交给 A*（工程标准答案）。VFH 改的就是「把排斥向量加成一个合力」这一步 —— 求和会把缝隙抹平，极坐标直方图保留整个方向分布，缝是一个显眼的低谷。地图全知道时用 Delaunay 三角剖分（最近点之间按长度从短到长加线段，相交的不加），它的对偶 Voronoi 图每条边到两个最近障碍等距 —— 沿边走 = 离障碍最远，剪掉过窄的弧后只剩几十条边。最后：最短的路不等于最快的路，长直段能加速；轨迹用参数三次样条，把段时长 $s$ 当成要最小化的量，三步走 —— Delaunay、样条上的 A*、⭐ 梯度下降调路点（优化路点坐标、目标是总时间，没有数据集也没有标签）。

下一节 👉 10-博弈树与Minimax.html