🏠 总目录📚 本教程 09 · 运动规划 ← →

09 · 从地图到可执行路径

格子能走,机器人真的过得去吗?

占据栅格要先考虑机器人尺寸,再按允许的走法连图;势场法另有局部极小等停滞风险。地图信息与规划算法是两层问题。

先核对身体与分辨率,再判断路径和风险;原有公式、四幅图与十一张表继续完整可读。

📑 本页目录(点开跳转)

09 · 运动规划

⏱ 34 分钟 | 阅读参考,不是完成时限;先看“能不能过”,再按需要看势函数和轨迹公式。


🎯 一句话

机器人不能只知道“往目标走”:还要知道自己的身体过不过得去,以及走到那里要花多长时间。

这一章把三件事分开:用地图规划路线,用局部信息调整动作,再给路线安排速度和时间。它们可以配合,不是只能三选一。

先记住一个边界:地图上的路线存在,不代表有宽度、有惯性的小车一定能沿着它走。


🧩 一、断层在哪里

第 8 章已经教过怎样为搜索设计启发式。现在不是把那些方法丢掉,而是先给它们准备一个合适的机器人模型。

前面常用的搜索例子 机器人问题多了什么
城市、棋盘等可列出的状态 位置 $(x,y)$ 连续变化,还可能需要朝向、速度
有限几个后继 行驶方向、距离或控制量可以连续取值
已给出的边和边代价 要判断动作是否可行,再计算距离、时间或能量
已知的地图 可能只有局部观测,还存在误差和未探索区域

连续状态不是说“不能搜索”。它意味着我们需要选择一种有限表示,或者使用不展开搜索树的局部控制方法。

别把“看见多少”与“使用什么算法”绑死

局部观测是当前只知道身边一部分环境;已知全局地图是已有整个规划区域的表示。地图已知也不等于没有误差。

当前条件 可以做什么 还要留心什么
小车只看到身边的障碍 更新局部栅格,做势场或 VFH 避障 没看到的地方不能默认安全;可能需要探索、重规划
已有场地地图 在栅格或几何路线图上搜索 机器人尺寸、定位误差、地图变化
已选出一条路线 为路线安排速度与时间 转弯、加速、刹车和碰撞约束

扫地机、户外小车常需要边走边更新地图。场馆或 RoboCup 小型组可以借助外部摄像头获得位置,但相机也可能有遮挡;车载传感器同样可以逐渐建立全局地图。

所以不能用“车载传感器”直接定义部分可观测,也不能用“头顶有相机”保证完全可观测。参数样条更不是一种感知方式,它负责描述后面的运动轨迹。


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

把地面切成小方格,再判断机器人能在哪些格子之间移动。

概率占据栅格为每个格子维护“这里有障碍”的概率估计。比如 0.9 表示当前模型认为这里很可能被占据,不是说它有 90% 的面积被障碍填满。

为什么保留概率?传感器会出错:超声波可能被斜面反射到别处,玻璃也可能使某些激光测量不可靠。概率模型可以融合多次观测;但重复观测仍有相关误差,并非次数多就必然正确。

用于搜索时,通常还要把格子分成可通行、障碍或未知。概率地图与二值搜索地图是两个处理阶段;并不是任何叫“栅格”的数据结构都必须存概率。

先处理身体,再把格子连成图

如果机器人是半径 10 cm 的圆盘,中心离墙 5 cm 仍会碰撞。不能只检查中心所在格是不是空的。

一个直观办法是:先把障碍向外扩张机器人半径,再把机器人中心当成一个点规划。这就是配置空间的一个简单例子;非圆形机器人转身时,朝向也可能要放进状态。

随后才是建图:

  1. 决定占据阈值、未知格怎么处理,以及需要多少额外安全余量。
  2. 把允许停留的格子当作节点。
  3. 只连接整段运动都合法的邻居。斜着走时,也要检查是否穿过墙角。
  4. 给每条边规定代价,再选择与代价一致的启发式。

这一步是在模型中检查“过不过得去”。它不能消除真实感知误差,执行时仍需要监测。

A* 的距离估计要和走法配对

以下以每格边长 1 为单位;障碍只会限制路线,下面的距离先忽略障碍来估计下界。

允许的动作和代价 可用的下界 不要混用的情况
上下左右,每步代价 1 曼哈顿距离;欧氏距离也可,但通常更弱 这里不能偷偷加入斜走一步
上下左右代价 1,斜走代价 √2 欧氏距离;八方向距离也可 曼哈顿距离可能高估
八个方向都只收代价 1 切比雪夫距离:两个坐标差绝对值的最大值 欧氏距离也可能高估

看一个最小例子:从 (0,0) 到 (1,1),若能花 √2 直接斜走,真实最短代价就是 √2,不能再把曼哈顿距离 2 当下界。

若费用是时间或风险,不是距离,还得重新检查单位和下界。怎样判断可容许与一致,回看第 8 章。

⚠️ 分辨率的两难

格子越细,几何细节越多,但节点也越多。 下面只算二维 10 m × 10 m 方形场地,不含朝向和速度维度。

格子边长 格子数 应该怎样理解
20 cm 2 500 保守栅格化可能把窄门抹掉;是否能过还取决于机器人宽度和网格对齐
5 cm 40 000 更细,不代表任何设备都必然算得动
1 cm 1 000 000 从 5 cm 再缩小 5 倍,格数增加 25 倍

例如,一道 30 cm 的门对足够窄的机器人可能可通行,却在粗糙的保守地图中消失;对直径大于 30 cm 的机器人,门本来就过不去。

格子数与边长的平方成反比。边长缩小到原来的 1/10,二维格数增加到 100 倍;这不是一次搜索运行时间也必然精确增加 100 倍。

格子边长只是重要旋钮之一。占据阈值、机器人膨胀范围、未知格策略、邻接关系也会改变结果。粗图规划全局、细图处理局部是一种折中,但两层之间也要核对可通行性。

这里的“有路就能找到”只针对已经建好的有限图。 若采用正确的去重与搜索规则,A* 能找到图里的可达目标;要保证最低代价,还需符合启发式和重开节点等条件。

图中无路,不足以证明连续场地里无路。讨论“随着分辨率不断加细能否最终找到连续路径”是另一层保证,不能用一次固定栅格的成功代替。LaValle 对这些完备性概念作了区分。


🧲 三、势场法:不搜索,直接滑下去

把目标想成往下走的方向,把障碍附近想成不愿靠近的高地。 每一步只看当前位置该往哪边移动,不先搜索整条路线。

本节的 $q$ 是机器人点模型的位置,$q_{goal}$ 是目标位置。$U(q)$ 是人为设计的“高度”,不一定是真实物理能量。

先只读这一条:

$$F(q)=-\nabla U(q)$$

$\nabla U$ 指向高度上升最快的方向,前面的负号把方向反过来。$F$ 在下面代码中是移动指令的方向和大小,不是已经包含质量、惯性和电机限制的物理控制模型。

高度由目标项和障碍项组成

$$U(q)=U_{att}(q)+\sum_j U_{rep,j}(q)$$

att 表示吸引,rep 表示排斥;$j$ 对应第几个障碍。单个障碍只占求和中的一项。

吸引项取到目标距离的平方:

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

$d$ 是距离,正数 $k_{att}$ 控制吸引强度。在二维平面欧氏距离下,这一项给出的方向正好朝向目标,离目标越远,拉得越强。

按需看:障碍附近的排斥公式

对第 $j$ 个障碍,记 $\rho_j$ 为机器人点到障碍的距离,$\rho_0>0$ 为影响范围,$k_{rep}>0$ 为排斥强度。对真实有形状的障碍,应先明确距离是量到边界还是经配置空间处理后的障碍。

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

读法:离得足够远时,这个障碍不产生排斥;进入影响范围后,越近越不愿靠近。$\rho_j=0$ 是公式奇点,不是可以直接代入的正常位置。

接下来的实验把障碍简化成点,距离量到点中心。它用于研究停滞,不代表真实圆盘机器人已经完成碰撞检查。

它常适合快速响应:无需展开搜索树,可以根据新观测重算局部指令。但计算量仍取决于考虑多少障碍、如何查距离,不能只凭“向量加法”断言在所有场景都更快。

动态障碍不是“下一帧重算就解决了”。 还要考虑感知延迟、刹车距离、步长和运动约束,否则计算方向正确也可能来不及避让。


💥 四、局部极小与其他停滞:势场的风险

合力为零,只说明这一步没有方向,不说明到达了目标。

先分清两个词:局部极小像一个小碗底,足够近的地方没有更低的点;若其他邻近位置都更高,就是严格局部极小。鞍点则在某些方向像谷底,在另一些方向仍有下坡路。

两者都可能让恰好停在该点的简单梯度控制器不动。下面的原实验属于第二种,不能把它叫成二维局部极小。

势场停滞:合力为零,可是还没到目标起点目标合力 = 0吸引力 → 拉向目标排斥力 ← 推离障碍这个控制器在此没有移动方向对称障碍示意,不表示真实通道已封闭
看左右两支力:它们抵消,上下又因对称抵消。这里只画停滞机制,不是按实验坐标比例绘制;是否能绕行还要由地图和机器人尺寸决定。

三类常见风险,不是每次都必然发生

情境 为什么可能卡住 不要多推一步
障碍位于目标前方,初值对称 吸引与排斥可能抵消,控制器也没有横向偏转 零合力点未必是局部极小
U 形或其他凹障碍 局部吸引把机器人往凹处带,退出可能需要先远离目标 停住还是振荡,与势函数及离散控制参数有关
窄通道 两侧排斥区叠加,算法不愿进入本来可通行的空间 先确认机器人实际能过,别把物理过不去误叫算法失败

⭐ 根本原因只有一句

本节的简单控制器只做局部下降,没有系统尝试整条绕行路线。

它不记录搜索分支,也不回溯,所以不能保证“连续场地里有路,就一定到目标”。相对地,A* 的保证也是在已经建好的图以及正确算法条件内,不是对现实世界无条件保证。

这不是说所有势函数方法都毫无保证。精心构造的导航函数、调和势函数有各自的理论条件;不能把一般吸引加排斥模型的缺陷扩大到整个方法家族。

停滞也不是日志里永远看不见。可以同时记录目标距离、实际位移、合力大小和安全距离;持续无进展时触发停车、重规划或人工处理。“一段时间没接近目标”是告警信号,不是数学上证明无路。

三种补救,各有各的代价

办法 怎么做 仍要核对的边界
随机扰动 停滞时尝试不同方向,再继续下降 随意扰动本身没有到达保证,也可能再进另一个坑;动作仍要安全
调和势场 用拉普拉斯方程和合适边界条件构造势函数 可排除不希望出现的内部局部极小,但仍可能有鞍点;通常需全局几何与数值求解
全局与局部分工 例如栅格 A* 提供路线,局部控制器响应邻近障碍 是常用组合思路,不是唯一方案;两层冲突、地图更新和停车策略也要处理

想了解一般势场、导航函数与调和函数的差别,可按需看 LaValle 教材第 8 章相关部分。这里不要求先会解微分方程,先记住“保证取决于构造和条件”。

🔨 独立跑一遍:1337 步停住

这段只需 Python 标准库。它保留原实验的起点、目标、三处点障碍、步长和停止阈值;不是可直接驱动真实小车的避障程序。

# 对称点模型:合力接近零时停住;下面的停点不是二维局部极小。
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

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:
        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。force 汇总吸引与三个排斥;p += 0.001 * F 是离散移动;停止条件检查的是合力,不是“已经到达目标”。

代码对极小距离使用了数值保护,但没有碰撞处理;本次运行没有接近那个保护阈值。不要把跳过奇点计算当成避障成功。

为什么这次是鞍点?真正的局部极小又长什么样?

只看原实验的对称水平线,停点像一个低处;但是向上或向下偏一点,势能还能降低。有横向下坡路,所以它不是二维小碗底。

同一种势函数也能构造真正的小碗底。保留目标 (9,5),改成两个障碍 (6,4)、(6,6),把排斥强度设为 $16+8\sqrt2$,其他强度与影响半径分别取 1、2。

此时 (5,5) 的吸引与排斥抵消,而且附近各个方向的二阶变化都为正。它是一个离目标还有 4 的严格局部极小;这与原来的三障碍鞍点是两个不同的实验。

想亲自核对:分别验证鞍点与局部极小,代码可独立运行

下面重新定义势函数,不依赖上一个代码框。原实验的驻点用二分求出;第二个实验的 (5,5) 是按参数构造的精确驻点。

import math

GOAL = (9.0, 5.0)
RHO0 = 2.0

def potential(p, obstacles, strength):
    total = math.dist(p, GOAL) ** 2 / 2
    for obstacle in obstacles:
        r = math.dist(p, obstacle)
        if r == 0:
            return math.inf
        if r <= RHO0:
            total += strength / 2 * (1 / r - 1 / RHO0) ** 2
    return total

def force(p, obstacles, strength):
    result = [GOAL[i] - p[i] for i in range(2)]
    for obstacle in obstacles:
        delta = [p[i] - obstacle[i] for i in range(2)]
        r = math.hypot(*delta)
        if 0 < r <= RHO0:
            factor = strength * (1 / r - 1 / RHO0) / r ** 3
            result = [result[i] + factor * delta[i] for i in range(2)]
    return result

old_obstacles = [(6.0, 5.0), (6.0, 6.2), (6.0, 3.8)]
lo, hi = 4.0, 5.8
for _ in range(80):
    mid = (lo + hi) / 2
    if force((mid, 5), old_obstacles, 8)[0] > 0:
        lo = mid
    else:
        hi = mid
p = ((lo + hi) / 2, 5.0)
center = potential(p, old_obstacles, 8)
side = potential((p[0], 5.01), old_obstacles, 8)
assert side < center
print("原驻点 x=%.8f;侧移后的势能变化=%.8f" % (p[0], side - center))

new_obstacles = [(6.0, 4.0), (6.0, 6.0)]
strength = 16 + 8 * math.sqrt(2)
q = (5.0, 5.0)
assert math.hypot(*force(q, new_obstacles, strength)) < 1e-12
# 两个障碍关于 y=5 对称;此点的 Hessian 是 curvature 乘单位矩阵。
curvature = 7 + 2 * math.sqrt(2)
assert curvature > 0
base = potential(q, new_obstacles, strength)
increments = [potential((5 + dx, 5 + dy), new_obstacles, strength) - base
              for dx, dy in [(0.01, 0), (-0.01, 0), (0, 0.01), (0, -0.01)]]
assert min(increments) > 0
print("新驻点 (5,5);两个二阶特征值均为 %.6f" % curvature)
print("四个邻近试点的势能都更高:", min(increments) > 0)

输出中的原驻点 x 约为 4.96551423,侧移势能变化约为 −0.00002673。第二例的两个二阶特征值都是 9.828427,四个邻近试点也都更高。

“二阶特征值”在这里描述不同方向的局部弯曲:两个都正,结合驻点附近二阶连续可微且梯度为零,可判定严格局部极小。只测试四个点本身还不是证明;本例的解析结果是 $H=(7+2\sqrt2)I$。

如果想核对这个解析式:对影响范围内、距离为 $r>0$ 的单个排斥项,径向二阶导数是 $k_{rep}\left(\frac{3}{r^4}-\frac{2}{\rho_0r^3}\right)$。在本例 $r=\sqrt2$ 处,两个对称障碍的交叉项抵消,再加吸引项的单位矩阵,得到上面的 $H$。

原实验若把初始 y 改成 5.001,本页实测会离开原鞍点,却又停在另一非目标位置。因此“打破对称”不等于“保证到目标”。


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

势场把障碍影响合成一个方向;VFH 先保留各个方向的拥挤程度,再挑方向。

VFH 是 Vector Field Histogram,向量场直方图。下面是它的阅读顺序,不是可以直接替代全部控制细节的伪代码。

  1. 更新二维直方图栅格。 累积“某个位置可能有障碍”的证据。原始方法用确定度数值(certainty value,表示障碍存在的证据强度)和简化更新,不能把它直接等同于完整贝叶斯概率占据图。
  2. 转成以机器人为中心的极坐标直方图。 横向依次看方向角,竖向数值表示该方向的障碍密度。
  3. 寻找候选谷。 连续一片低于阈值的方向构成一个谷;它保留了可能有通道的方向信息。
  4. 在谷内选方向并控制速度。 通常偏向目标方向,同时考虑谷宽、选择稳定性与实际可通行余量。

可以把三个方向的密度想成左边高、中间低、右边高:中间可能存在缝隙。一个合力向量不再保留这种三方向分布;直方图仍能区分它们。

VFH 方向密度示意:中间低谷保留候选方向 方向从负60度到正60度,示意密度依次为8、6、2、1、2、7、8。阈值为3,负20度到正20度连续低于阈值,构成一个候选谷。数值仅用于解释读法,不是机器人实测。 保留每个方向的信息,才能看见中间的低谷 障碍密度(示意单位) 阈值 3 −60°−40° −20°0° 20°40° 60° 86 21 278 方向角;0° 是这里选定的前方 绿色连续低于阈值:先作为候选谷,再判断能不能走
先看低于虚线的三根柱:中间的方向信息被保留下来。这是解释读法的七扇区小例子,不是 VFH 原论文参数或实机测量;扇区选取还需后续规则。

但“低谷”只是当前模型中的候选,不是碰撞安全证明。窄谷可能容不下机器人,未知障碍也可能在下一次观测出现。原始论文还区分宽谷、窄谷以及具体扇区和速度的选取。

比较项 占据栅格 + A* 本节的简单势场 VFH
地图范围 可用局部或全局图;保证只覆盖当前建图范围 可用邻近障碍信息 累积局部栅格信息用于当前决策
有路必到吗 正确实现可穷尽有限图,但连续场地另说 不保证 一般不保证到达全局目标
保留的信息 格子层面的空间与邻接关系,并非连续世界全部细节 最终移动指令是一个合力向量 用方向密度分布来选局部方向

VFH 针对势场压缩方向信息的部分缺点作了改进;不能脱离地图、速度和评价指标,断言它对所有场景都“明显更稳”。


🕸 六、地图已知时:Voronoi 与 Delaunay

除了铺满小格子,也可以用障碍之间的几何关系,提取一张更稀疏的路线图。

这里先看静态、已建模的平面环境。地图可以来自外部相机,也可以来自预先测绘;几何方法不以安装哪一种相机为前提。

Delaunay 不是“先连最短的边”

对普通平面点集,Delaunay 三角剖分要求:每个三角形的外接圆内部,不包含其他输入点。外接圆就是经过三个顶点的圆。

在没有退化情况时,这种剖分与点集的 Voronoi 图互为对偶:两个点的 Voronoi 区域共享一条边时,这两个点之间对应一条 Delaunay 边。四点共圆时可能有不唯一的合法选择。CGAL 的官方定义以空圆条件为准。

为什么不按长度从短到长加边,只避开交叉?看一个整数例子:

按凸四边形顺序排列的点 坐标
A (0,0)
B (3,3)
C (1,3)
D (0,1)
短对角线不一定符合 Delaunay:同一四点的两种剖分 两栏使用相同尺度。四点为 A零零、B三三、C一三、D零一,不是矩形。左栏选 AC,长根号10;ABC外接圆的圆心O为二一,D落在圆内,所以不合格。右栏选更长的BD,长根号13;ABD和BCD都满足空圆条件,因此合格。右栏不画两个圆,以免遮挡四点和对角线。 AC 短一些,但不合格 BD 更长,却合格 AC = √10 ≈ 3.16 BD = √13 ≈ 3.61 A (0,0)B (3,3) C (1,3)D (0,1) O (2,1) D 在圆内 ABC 外接圆 OD² = 4,小于半径平方 5 A (0,0)B (3,3) C (1,3)D (0,1) 选择 BD ABD、BCD 两个三角形 各自的外接圆内都没有第四点
先看左边红色 D:它落在 ABC 的外接圆内,所以较短的 AC 不合格;右边换成较长的 BD,两侧三角形才都满足空圆条件(两栏四点坐标与比例相同)。

AC 长 √10,比 BD 的 √13 短,所以短边贪心会先选 AC。但 ABC 外接圆的圆心为 (2,1),半径平方为 5;D 到圆心的距离平方为 4,落在圆内。AC 不满足 Delaunay 条件,正确的对角线是 BD。

点集三角剖分覆盖的是点集的凸包,不能自动当作“已经避开所有障碍的自由空间”。真实障碍有边界,机器人也有宽度,需要约束建模、净空与碰撞检查。

Voronoi:读懂“等距”,不要读成“处处最远”

普通点集 Voronoi 边上的点,到相邻的两个最近输入点等距。机器人规划的广义版本则要看距障碍边界的距离,还可能包含场地边界。

Voronoi 边 = 到两个最近障碍等距的点障碍 A障碍 B障碍 C三条边的交点粗线:沿等距骨架走;仍需检查净空与可通行性
看三条细线:它们是两两中垂线的相应部分,交点到三点近似等距。圆形符号标记点障碍;真实障碍的大小、形状以及场地边界需要另行建模。

这张骨架能帮助路线留在障碍之间较宽敞的位置,给定位误差留余量,但净空沿骨架也会变化。

例如,障碍点 (−1,0)、(1,0) 的 Voronoi 边是竖直中线。线上 (0,0)、(0,1)、(0,2) 都等距,最近障碍距离却分别为 1、√2、√5。不能把等距换成“每一步都是整个场地里最安全的位置”。

把几何骨架变成可用路线

  1. 根据障碍与场地边界建立适合当前模型的几何表示。
  2. 结合机器人外形,剪掉净空不足或存在碰撞的连接。
  3. 把起点、目标接入路线图,接入段也要检查碰撞。
  4. 在剩下的图上用正确的代价与启发式搜索。
  5. 转成可执行轨迹后,再核对碰撞和运动约束。

Delaunay 中连接障碍的边可以描述“障碍之间哪一道间隙允许通过”;它本身不是让机器人沿着边穿进障碍的行驶命令。不要把这种间隙图与 Voronoi 骨架路线当成同一张图。

稀疏骨架有时比细栅格小很多,但规模由地图决定。旧的“几万个节点变成几十条边、少三个数量级”只有在给出具体地图和计数方式时才能核验,不是普遍保证;节点数和边数也应分别比较。


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

路径回答从哪里经过,轨迹还回答什么时候到那里。

长一点的直路可能允许持续加速;短一点却有急弯的路可能要反复减速。因此最小距离和最小时间可能给出不同选择,不是每一条长路都更快。

折线也能加上时间安排,但尖角可能迫使机器人停下。平滑曲线是构造可执行轨迹的一种方式,不是表达速度的唯一方式。

一小段三次曲线,四种量各管什么

$$P(t)=a\,t^3+b\,t^2+c\,t+d,\qquad 0\le t\le s$$

符号 在这一段中的作用
$P(t)$ 时刻 t 的位置;平面中有 x、y 两个分量
$P'(t)$ 速度:位置随时间怎么变
$P''(t)$ 加速度:速度随时间怎么变
$s>0$ 走完整段的时间
$a,b,c,d$ 决定曲线的四个系数;二维时都是向量

选定正时长 s,再给定两端的位置与速度,才能唯一解出这四个系数。 改变 s 时系数通常也要重算,尤其当端点速度仍固定为实际物理速度时。

一个可以看出速度变化的小例子

只沿一条直线从位置 0 走到位置 1,两端速度都取 0。用 $u=t/s$ 表示已经经过的时间比例:

$$P(t)=3u^2-2u^3,\qquad u=t/s$$

这段轨迹从静止起步,中途较快,到终点又停下。把时长从 1 改为 2,同一位置变化用更长时间完成,因此速度与加速度都会下降。

下面的代码只需标准 Python,独立打印起点、中点、终点的位置和速度,以及整段的最大速度和最大加速度大小。

def sample(t, duration):
    u = t / duration
    position = 3 * u ** 2 - 2 * u ** 3
    velocity = (6 * u - 6 * u ** 2) / duration
    acceleration = (6 - 12 * u) / duration ** 2
    return position, velocity, acceleration

for duration in (1.0, 2.0):
    print("时长 %.0f" % duration)
    for ratio in (0.0, 0.5, 1.0):
        t = ratio * duration
        p, v, a = sample(t, duration)
        print("t=%.1f, 位置=%.3f, 速度=%.3f, 加速度=%.3f" % (t, p, v, a))
    # 此特定三次段的解析极值,不是任意轨迹只查三点就够。
    print("最大速度=%.3f, 最大加速度大小=%.3f" %
          (1.5 / duration, 6 / duration ** 2))
时长 中点位置 整段最大速度 整段最大加速度大小
1 0.5 1.5 6
2 0.5 0.75 1.5

数字说明“花多少时间”会改变运动要求。这个例子不含障碍和转弯,不是在复现机器人完整轨迹规划器。

约束必须说清是哪一种

记 A 为选定的加速度尺度、V 为选定的速度尺度,二者都为正。最简单的限制是速度大小不超过 V、加速度大小不超过 A;其他模型也可能把两种能力耦合起来。

下面保留一种示意性的耦合限制来练习读公式,它不是所有机器人的通用动力学定律:

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

读法:加速度占用一部分允许量,速度也占用一部分,两者相加不能超过 1。它比“两项各自不超过 1”更严格:加速度为 0.8A、速度为 0.8V 时,左边是 0.8+0.64=1.44,仍然不满足。

约束写着“所有 t”,就不能只查起终点。例如上面时长为 1 的三次段两端速度都为 0,中间却达到 1.5;只检查端点会漏掉中途超速。

这个示意约束并非对下面论文式 (14) 的转录。真实小车还可能有最小转弯半径、不可横移、驱动力和摩擦模型,必须以采用的机器人模型为准。

历史例子:RoboCup 的路线与样条联合优化

Thomas、Blair、Barnes 在 2003 年研究了用于 RoboCup 小型组的规划器。它展示了“不训练神经网络,也能直接优化运动”的思路;不要把一个特定规划器简化成无条件最优的三步口诀。

  1. 用几何图表达间隙。 Delaunay 边提供可调路点所在的间隙,先按可通行性剪枝。
  2. 搜索候选,再评价轨迹时间。 原算法用直线距离形成快速搜索估计和时间下界,再把候选转换为三次轨迹评价,并不是简单把所有 A* 边权改成一个固定时间。
  3. 局部调整路点状态。 不仅调整位置,还调整速度,以降低总时间;边界约束与候选路线限制仍然存在。

论文采用动态约束的近似处理,局部改进也可能陷在次优结果。因此这里的 A* 子过程不能替整套连续轨迹优化承诺全局最优。原论文作者上传正文的 Table I、Table II 与结论说明了这些步骤和边界。

“梯度下降式优化”在这里服务于轨迹参数,不是神经网络权重。优化目标是总时间,约束来自机器人与环境;不需要标签数据,也不以是否使用 GPU 来定义方法。


🔗 这一章连到哪里

去哪 为什么
08-启发式怎么设计.html 把连续空间表示成图之后,还要给搜索选择不高估剩余费用的启发式
10-博弈树与Minimax.html 下一站面对的不是静态障碍,而是会主动改变局面的对手
机器学习与深度学习基础 09 · 优化器与学习率 对照梯度、鞍点、局部极小;相似的数学工具不代表不同任务的收敛保证相同
强化学习基础 02 · MDP 对照状态、动作与动力学模型;已知模型和未知模型都可能用 MDP 建模,未知不等于 MDP 的定义

若观测不足以确定当前状态,还要考虑如何表示不确定性。不要一边用局部传感器,一边默默假设完整状态永远已知。MDP 的马尔可夫条件关注给定当前状态与动作后,历史是否还带来额外的转移信息,不是“是否已经知道动力学”。


✅ 检查点

  1. 前面城市或棋盘搜索的例子,与机器人的运动规划相比,状态、动作和可行性多了什么问题?
  2. 概率占据栅格为什么保留“有障碍的概率”?这是否表示用于搜索的格子不能再分成可通行与障碍?
  3. 10 m × 10 m 场地,格子边长从 5 cm 缩到 1 cm,格子数从多少变成多少?为什么是这个倍数?
  4. 简单势场为什么可能停在非目标位置?原三障碍实验是鞍点还是局部极小?共线、U 形与窄通道各有什么风险?
  5. VFH 与把排斥向量加成一个合力,关键差别在哪一步?为什么看见低谷还不等于保证能到目标?
  6. 沿 Voronoi 边走,在几何上意味着什么?为什么通常有利于留出净空,但不等于处处净空最大?
  7. RoboCup 历史轨迹规划例子的局部优化在调什么变量、优化什么目标?为什么不能据此承诺全局最优?
👀 答案
  1. 位置和控制量可能连续,还需要机器人尺寸、朝向、速度等状态。规划前要决定有限表示或控制模型,检查整段动作是否碰撞,并给出时间、距离或能量代价。

  2. 概率用于表达和融合有误差的观测;用于搜索时仍可按阈值等规则分成可通行、障碍、未知。这两个阶段不矛盾,重复观测也不自动消除系统误差。

  3. 从 40 000 到 1 000 000,增加 25 倍。边长变为 1/5,二维格数按边长平方的倒数变化,因此乘以 $5^2$。

  4. 简单控制器只利用局部梯度,不系统搜索绕行路线;零合力既可能是局部极小,也可能是鞍点。原实验在对称轨迹上接近鞍点。共线可能导致抵消,U 形可能把下降方向困在凹处,窄通道可能被排斥区挡住;都应结合具体参数与机器人可通行性判断。

  5. 势场最终压缩为一个向量,VFH 在方向选择前保留极坐标障碍密度分布。低谷提供候选方向,却未自动解决机器人尺寸、未知障碍、速度限制与全局死胡同。

  6. 到两个最近障碍等距,适合构造障碍间的骨架路线;真实规划要量到障碍边界。净空沿边可能变化,还需剪除过窄连接并检查起终点接入,不是全场最大净空定理。

  7. 调整路点位置和速度等轨迹参数,目标是减少总时间,受几何和运动约束限制。该研究使用候选路线与近似、局部调整;局部下降可能次优,不能把图搜索子过程的保证扩展到整个连续问题。


本节回顾

⚡ 走神救援

  • 先给机器人留出身体空间,再谈格子或几何图上的路线。
  • 合力为零不等于到目标:原实验停在鞍点,另一个参数例才是真正的局部极小。
  • VFH 保留方向分布,Voronoi 提供等距骨架;两者都不能跳过安全与模型检查。
  • 路径告诉你经过哪里,轨迹还要安排何时到达;最短距离不一定最短时间。

本章区分了地图、局部控制和带时间的轨迹。用1337 步实验核对三者的作用。

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

打卡记录保存在你的浏览器里,首页能看到总进度