Home / 机器人经典基础 / GPMP2：把轨迹规划写成因子图推断

087｜GPMP2：把轨迹规划写成因子图推断

论文：Jing Dong、Mustafa Mukadam、Frank Dellaert、Byron Boots，Motion Planning as Probabilistic Inference Using Gaussian Processes and Factor Graphs，RSS 2016。原文 · 官方实现。

先接住前作的问题

CHOMP等方法用密集轨迹优化处理平滑与避障，变量规模随检查密度增长；GPMP2用Markov高斯过程把轨迹变量与检查位置解耦，并组合iSAM2的增量求解。

五个前置概念

高斯过程：用均值和协方差描述连续函数的一族概率模型。
Markov 状态：包含预测下一状态所需的信息，使局部条件关系成立。
精度矩阵：协方差的逆，其稀疏性可以揭示条件独立关系。
有符号距离场：空间位置到障碍表面的距离及内外符号。
Bayes Tree：保存变量消元结构，便于增量更新部分计算。

一、问题设定：稀疏轨迹变量怎样兼顾中间时刻的碰撞检查

机械臂绕障碍物移动时，轨迹既要平滑，又不能碰东西。如果每隔很短时间都建立一个独立状态，优化问题会很大；如果只保留少数点，点与点之间又可能穿过障碍物。

问题定义与前人瓶颈

输入是起点、目标、机器人几何与环境距离场，输出是关节空间随时间变化的轨迹。这是基于模型的规划，不是强化学习训练。本文状态包含关节位置和速度；没有从交互奖励学习策略。

CHOMP、STOMP、TrajOpt 等已经能优化轨迹。本文不是首次让轨迹平滑，也不是首次使用高斯过程；其参照之一就是 GPMP。关键差别在于把问题系统地组织成非线性最小二乘和稀疏因子图，利用高效分解及增量更新。

“常速度先验”容易误读为机械臂必须匀速。实际是先验偏好小加速度，优化仍可偏离均值绕障碍。它也不是完整刚体动力学模型：关节轨迹光滑不自动保证电机力矩、接触力、负载和带宽限制全部满足。

二、用Markov高斯过程先验组织稀疏因子图

GPMP2 的办法是：用少量支持状态定义连续时间高斯过程轨迹，通过插值在中间位置计算障碍代价，再把这些约束回传到支持状态。由于选用具有 Markov 结构的先验，优化可写成稀疏因子图，借用 SLAM 中成熟的求解器。

1. 为什么状态要包含速度

第 IV 节采用白噪声加速度模型：

$\ddot q(t)=w(t),\qquad\theta(t)=[q(t)^T,\dot q(t)^T]^T.$

只知道位置通常不足以用一阶 Markov 模型预测下一时刻；加入速度后，相邻状态之间可以建立局部转移因子。这个结构使精度矩阵 $K^{-1}$ 呈块三对角形式。不是所有任意 GP 核都有这种稀疏性，也不是协方差 $K$ 本身必然稀疏。

把白噪声加速度模型积分一次，速度变化为 $\int_0^h w(s)ds$ ；再积分一次，位置的随机变化为 $\int_0^h(h-s)w(s)ds$ 。其中 $h>0$ 是相邻支持状态的时间间隔， $Q_c$ 是加速度白噪声的谱密度矩阵。由积分核的乘积可直接得到论文式(26)：

$\Phi(h)=\begin{bmatrix}I&hI\\0&I\end{bmatrix},\qquad Q(h)=\begin{bmatrix}h^3Q_c/3&h^2Q_c/2\\h^2Q_c/2&hQ_c\end{bmatrix}.$

因此平滑因子残差是 $e_i=\theta_{i+1}-\Phi(h)\theta_i$ ，权重为 $Q(h)^{-1}$ ；符号整体取反不改变平方代价。不同时间间隔不能机械使用同一协方差：位置方差随 $h^3$ 变化，速度方差随 $h$ 变化，位置速度也相关。

原创一关节手算。 取 $h=1,Q_c=1$ ，则 $Q^{-1}=\begin{bmatrix}12&-6\\-6&4\end{bmatrix}$ 。两个残差 $(1,0)$ 与 $(1,1)$ 的二次代价 $e^TQ^{-1}e$ 分别为 12 和 4；同样的位置偏移，若伴随方向一致的速度变化，反而更符合积分模型。这不是任意鼓励高速运动，而是联合动力学统计关系。

若两端状态固定为 $(q_0,\dot q_0)=(0,0)$ 与 $(q_1,\dot q_1)=(1,0)$ ，这一先验的条件均值是三次曲线 $q(t)=3t^2-2t^3$ ， $0\le t\le1$ 。中点为 $q=0.5,\dot q=1.5$ ，不是位置直线插值附上零速度。若障碍位于 $q\in[0.45,0.55]$ ，只看端点会漏撞；插值检查的重要性由此可算出来。模型与插值依据原文II-D、IV-A，该例不代表论文机械臂实验。

2. MAP 目标包含什么

论文第 II 节把目标写成：

$\theta^*=\arg\min_\theta\frac12(\theta-\mu)^TK^{-1}(\theta-\mu) +\frac12h(\theta)^T\Sigma_{obs}^{-1}h(\theta).$

第一项偏好符合先验的平滑轨迹，第二项惩罚靠近或进入障碍的状态。 $μ$ 是先验均值， $h$ 是障碍残差， $\Sigma_{obs}$ 控制代价强度。这里似然是人为构造的规划偏好，并未通过大量碰撞频率校准；不能把其数值解释成真实世界的安全概率。

当 $\Sigma_{obs}$ 变大，其逆矩阵变小，障碍代价反而变弱。因此“协方差参数更大，所以避障力度更大”方向是反的。论文图 6 正好展示参数过大时的失败。

原论文图6：不同障碍噪声参数下的机械臂轨迹对照 — Jing Dong、Mustafa Mukadam、Frank Dellaert、Byron Boots（2016），图 6，PDF 第 6 页；原文与图注。原版权保留。

读图：左右使用不同的障碍代价尺度；左侧成功与右侧失败说明参数会改变避障权衡，不是任意设定都稳健。

3. 障碍距离怎样进入关节空间

机器人用一组球近似，正运动学把球心映射到工作空间。距离场给出球心到障碍的有符号距离，再减半径得到近似表面间隔。低于安全余量时产生 hinge 残差，远离障碍时为零。

这个链条也暴露了误差源：球体覆盖不充分、距离场分辨率太低、地图有误差，都可能让代价低估碰撞。几何代价精确求解与真实机器人绝对不碰撞之间，还隔着模型质量。

4. 支持状态少，检查位置可以多

对 $t_i<\tau<t_{i+1}$ ，插值状态可由邻接支持状态表达：

$\theta(\tau)=\mu(\tau)+\Lambda(\tau)(\theta_i-\mu_i)+\Psi(\tau)(\theta_{i+1}-\mu_{i+1}).$

中间时刻的障碍残差因此只连接两个支持状态，保留局部稀疏结构。图 2 区分优化支持点、碰撞检查点和执行输出点：它们无需具有相同密度。

原论文图2：系统状态、碰撞检查与输出轨迹在时间轴上的不同分辨率 — Jing Dong、Mustafa Mukadam、Frank Dellaert、Byron Boots（2016），图 2，PDF 第 4 页；原文与图注。原版权保留。

读图：空心点表示离散状态，较密点表示插值检查或输出；插值增密并没有同时增加全部优化变量。

连续时间表示不意味着检查了无限多个时刻。论文通过有限插值点评价障碍，不能据此宣称对任意薄障碍都具有形式化连续碰撞保证。输出进一步加密也不能修复已经漏掉的碰撞。

插值障碍残差依赖支持状态而非新增独立轨迹点。下面检查残差回传，再把增量耗时与成功率放在一起。

三、插值检查障碍并回传支持状态，核对增量更新

5. 增量推断与伪代码

输入：起终状态、机器人球体模型、已构建距离场
以少量支持状态初始化一条轨迹
加入起终状态先验与相邻 GP 平滑因子
在支持点及插值点加入障碍因子
重复：
    计算残差和雅可比
    求解稀疏、带阻尼的线性化系统并更新支持状态
    达到迭代或相对改进阈值后停止
独立检查所得轨迹是否满足可行性要求
若目标或当前状态变化：
    替换相关因子，复用原轨迹及 Bayes Tree
    用 iSAM2 更新受影响部分，再次检查结果
输出：可行轨迹，或明确失败

这是教学流程；批量版用 Levenberg–Marquardt，增量实现不具有完全相同的阻尼策略。起终状态在论文中以很小协方差软约束实现，作者认为实验误差可忽略，不是数学上严格零误差的硬等式。

例子与五个前置概念

自构例子，未运行。 用起点、中点、终点三个支持状态表示一条路径。三个状态都在障碍外，但两段之间可能穿过一个小盒子。插值点发现盒子后，其梯度会同时影响相邻支持状态，让整段弯开；如果检查点恰好都绕过很薄的障碍，仍可能漏检。这个例子解释计算节省与安全覆盖是两个问题。

四、同时读求解时间、成功率与任务改变

Table II：对两台机械臂同时读成功率、平均与最大耗时 — 原论文结果图／表摘引：Jing Dong、Mustafa Mukadam、Frank Dellaert、Byron Boots（2016），Table II，PDF 第 8 页；核对原文。原版权保留。

读图：对两台机械臂同时读成功率、平均与最大耗时。增量版本明显省时却丢掉部分成功问题，因此不能把成功子集上的速度直接宣布为同可靠性加速。

实验怎么比较

GPMP2 与增量 iGPMP2 的成功率和成功运行耗时分别统计；GP 障碍权重是建模选择，不是已校准的真实碰撞概率。

项目	原文设置
问题规模	WAM 七自由度机械臂 24 个规划问题；PR2 右臂七自由度 90 个问题，含书架和工业场景
平台	GTSAM 实现，OpenRAVE 仿真；单线程 3.4 GHz i7；另把成功轨迹在真实 WAM 上验证
离散化	WAM 通常 101 状态；插值版用 11 支持状态、每间隔九个插值点；PR2 对应 51 检查状态
对照	GPMP、CHOMP、STOMP、TrajOpt，以及有/无插值的 GPMP2
预算	GPMP2 最多 100 次迭代或相对改进低于 $10^{-4}$ ；部分基线最多 250 次，终止规则不同
初始化与调参	多数以常速度直线初始化，GPMP 例外；每个数据集调到接近默认且表现较好的参数，预算未量化
增量测试	WAM 32、PR2 28 个目标改变重规划问题；不等于动态障碍预测的全面评测
统计边界	时间表只统计成功运行；未报告统一随机种子置信区间，不能忽略失败者后只比较平均速度

四组关键证据

表 I.A：速度与成功率要一起读。 WAM 插值版成功率 95.8%，平均成功耗时 0.068 s；无插值版 91.7%、0.120 s。条件平均耗时下降约 43.3%，计算为 $(0.120-0.068)/0.120$ 。TrajOpt-11 更快，0.027 s，但成功率只有 20.8%；不能只取最快数字宣布它最适合任务。

表 I.B：不同场景收益不同。 PR2 插值版 94.4%、0.033 s，TrajOpt-51 为 93.3%、0.860 s。约 26.1 倍是两者成功样本平均耗时之比，包含实现、参数和成功子集差异；不是 GP 插值这一模块单独产生的倍数。

表 II：增量加速付出了代价。 WAM 重规划从 30.21 ms 降到 2.38 ms，但成功率从 100% 降至 90.6%；PR2 从 26.70 ms 降至 4.27 ms，成功率从 96.4% 降至 75.0%，下降 21.4 个百分点。作者正文称损失较小，但在需要可靠执行的应用中，后一差异不能轻描淡写。

图 6 与失败分析。 障碍参数过大可产生不可行解。作者把所测失败归因于不可行局部极小，并指出随机重启未实现。它直接限制“优化完成就规划成功”的说法。起点变化、目标大幅移动时旧轨迹可能是坏初值，iGPMP2 不能只依靠复用保证成功。

证据见原文第 V–VI 节及表 I–II。机械臂真实演示验证的是选出的成功轨迹；没有报告每个仿真问题在硬件上重复试验的统一成功率。

五、保留软约束、局部解和插值检查密度

贡献是把规划与估计共享的稀疏求解结构用起来，同时分离轨迹参数密度和碰撞检查密度。作者明确承认没有全局最优保证，处理一般非线性不等式约束的能力有限。

我的分析是，系统总延迟还包括感知、距离场构建、碰撞验证和执行接口；论文规划内核耗时不直接等于闭环机器人反应时间。软障碍代价也不等于安全证明。若要在未知环境执行，应把距离场更新、地图误差与轨迹跟踪误差一起纳入验证。

六、追问：增加插值检查点而不增加支持状态，优化问题哪些部分会变大？

增加插值检查点而不增加支持状态，优化问题哪些部分会变大？

参考答案：状态变量数可保持不变，但障碍残差的评价与雅可比装配会增加。插值把新检查关联到相邻支持状态，不代表检查成本为零。应分别记录变量数、因子数与成功率，避免把少变量直接等同于固定总耗时。

教学参照：Factor Graphs and GTSAM。这里借用其分开变量、因子与当前估值的讲法。

七、衔接时间参数化与增量求解

模块组合：TOPP-RA：先算哪里还能刹住，再决定哪里可以加速。几何轨迹确定后另求满足动力学约束的时间分配。
模块组合：iSAM2：把不断增长的SLAM问题变成局部树更新。复用增量因子图求解，但重规划成功率仍需单独报告。

八、自测与参考答案

问题 1：为什么使用 GP 不意味着要从示范数据训练一个模型？

答案： 本文 GP 是根据运动平滑偏好设计的轨迹先验，核来自选定随机微分方程；优化求的是满足任务的轨迹，而非从示范学习策略或未知动力学。

问题 2： $\sigma_{obs}$ 增大时，为何可能更容易碰撞？

答案： 它进入障碍项的协方差，代价用逆协方差加权。协方差越大，障碍残差权重越小，优化器更愿意用靠近或穿过障碍换取平滑。名称不能代替读公式。

问题 3：iGPMP2 平均只需几毫秒，为什么还不能保证快速完成任务？

答案： 时间只对成功运行统计，失败还需恢复或重规划；增量结果成功率低于批量版，而且感知和执行耗时未包含在这个数字里。实际应用应同时评估截止时间内成功率、失败恢复成本和最终执行可行性。