论文笔记（七十九）STOMP: Stochastic Trajectory Optimization for Motion Planning

文章概括

引用：

@inproceedings{kalakrishnan2011stomp,title={STOMP: Stochastic trajectory optimization for motion planning},author={Kalakrishnan, Mrinal and Chitta, Sachin and Theodorou, Evangelos and Pastor, Peter and Schaal, Stefan},booktitle={2011 IEEE international conference on robotics and automation},pages={4569--4574},year={2011},organization={IEEE}
}

Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P. and Schaal, S., 2011, May. STOMP: Stochastic trajectory optimization for motion planning. In 2011 IEEE international conference on robotics and automation (pp. 4569-4574). IEEE.

原文： https://ieeexplore.ieee.org/document/5980280
代码、数据和视频：

系列文章：
请在 $《$文章$》$ 专栏中查找

---

宇宙声明！

引用解析部分属于自我理解补充，如有错误可以评论讨论然后改正！

---

摘要

我们提出了一种基于随机轨迹优化框架的新型运动规划方法。该方法依赖于生成带噪声的轨迹以探索围绕初始（可能不可行）轨迹的空间，然后将这些轨迹合成以生成具有较低代价的更新轨迹。在每次迭代中，我们优化了一个基于障碍物代价和平滑度代价组合的代价函数。由于我们所使用的特定优化算法不需要梯度信息，因此对于那些可能没有可用导数的一般代价（例如对应约束和电机扭矩的代价）也可以被包含在代价函数中。我们在仿真中以及在移动操作系统上分别针对无约束和有约束任务演示了该方法。通过实验，我们展示了STOMP算法的随机性质使其能够克服梯度方法（例如CHOMP）容易陷入的局部最小值问题。

一、引言

近年来，机械手和移动系统的运动规划受到了广泛关注。避免碰撞的运动规划一直是最常见的目标，但在某些情境下，其他目标——如处理约束条件、最小化扭矩或能量以及实现平滑路径——也可能十分重要。特别是在家庭和零售场景中，经常会出现约束满足成为主要目标的情况，例如携带一杯水。移动操控系统可能需要最小化能量消耗，以节约电力并保持长时间的工作。不平滑、抖动的轨迹可能会对执行器造成损害。显然，亟需一种能够应对此类情境的运动规划器。

在本文中，我们提出了一种能够处理一般约束条件的运动规划新方法。我们的方法涉及利用一系列带噪声的轨迹进行随机轨迹优化。在每一次迭代中，都会生成一系列此类轨迹。对生成的轨迹进行仿真以确定其代价，然后利用这些代价来更新候选解。由于该过程中不需要梯度信息，因此可以对一般约束以及额外的不平滑代价进行优化。

我们通过使用PR2移动操控机器人在仿真和实验中验证了该方法。我们考虑了末端执行器的姿态约束，例如要求末端执行器在整个轨迹过程中保持抓取物水平的约束。我们试图利用有符号距离场来测量与障碍物的接近程度，从而最小化碰撞代价。我们进一步施加了平滑度代价，从而生成可以直接在机器人上执行而无需进一步平滑的轨迹。我们还展示了用于执行运动的电机扭矩的优化。

图1. (a) Willow Garage的PR2机器人在家庭环境中操控物体。(b) PR2机器人在仿真中以扭矩最优的方式避开一根柱子。

二、相关工作

基于采样的运动规划算法已在解决操作问题方面取得了极大的成功 [1]，[2]，[3]，[4]，[5]。它们已被用于处理任务空间约束以及在举起重物时施加扭矩约束[6]。虽然基于采样的规划器能够生成可行的计划，但它们在所产生路径的质量上往往不足。通常需要一个次级的捷径步骤来平滑并改进这些规划器输出的计划 [7]。

另一类被应用于操作系统的运动规划器是基于优化的方法。在文献 [8] 中，针对协作移动机械手的运动规划问题采用了一种最优控制方法，将障碍物和约束条件纳入问题描述，并通过离散化后采用数值方法求解所期望的规划。在文献 [9] 中，针对一个$7DOF$机械手使用了协变梯度下降技术进行运动规划。该算法（称为CHOMP）最小化了平滑性和障碍物代价的组合，并且需要两者的梯度信息。
它利用环境的有符号距离场表示来导出障碍物的梯度。我们为我们的方法采用了类似的代价函数，但与CHOMP不同的是，我们的优化方法能够处理那些没有梯度信息的通用代价函数。

三、STOMP 算法

传统上，运动规划被定义为寻找一条从起始配置到目标配置且无碰撞的轨迹的问题。我们将运动规划视为一个优化问题，即寻找一条平滑轨迹，使得与碰撞和约束相关的代价最小化。具体来说，我们考虑持续时间为$T$的轨迹，该轨迹被离散化为$N$个在时间上等间隔的路点。为了使符号表示更简单，我们首先推导一维轨迹的算法；该方法自然可以后续扩展到多维情况。这条一维离散化的轨迹被表示为向量$\theta \in {\mathbb{R}}^N$。我们假设轨迹的起点和终点是给定的，并在整个优化过程中保持不变。

在这里，研究者们将运动规划问题转化为一个优化问题。目标是寻找一条“平滑轨迹”，使得整体的代价最小化。这个代价包含了两部分：

• 状态相关代价 $q({\tilde{\theta }}_i)$：可以包含障碍物代价（例如靠近障碍时代价高）、约束代价（例如运动必须满足某些物理限制）以及扭矩或能量代价。
• 控制代价：用来量化运动过程中各个状态之间的“平滑性”或控制变化，其中用到了轨迹的加速度（平滑的轨迹一般加速度变化平缓）。

现在，我们提出一种算法，该算法迭代地优化这条离散化的轨迹，使其满足任意状态相关代价。尽管在本节中我们将代价函数保持为通用形式，但第四节讨论了我们对于障碍物、约束、能量和平滑度代价的具体表达。我们从下面的优化问题开始：

$$\underset{\tilde{\theta }}{\min }\mathbb{E}[\sum _{i=1}^{N}q({\tilde{\theta }}_i)+\frac{1}{2}{\tilde{\theta }}^T\text{R}\tilde{\theta }](1)$$

其中，$\tilde{\theta }=\mathcal{N}(\theta ,\Sigma )$ 是一个带噪声的参数向量，其均值为$\theta$，方差为$\Sigma$。$q({\tilde{\theta }}_i)$是一个任意与状态相关的代价函数，可以包含障碍物代价、约束和扭矩。$\text{R}$是一个正半定矩阵，用于表示控制代价。我们选择$\text{R}$，使得${\theta }^T\text{R}\theta$表示沿轨迹的加速度平方和。令$\text{A}$为一个有限差分矩阵，当它与位置向量$\theta$相乘时，会产生加速度$\ddot{\theta }$：

如式(2)所示，矩阵$A$可写为：
$$\text{A}=\left[\begin{array}{ccccccc}1& 0& 0& & 0& 0& 0\\ -2& 1& 0& \cdots & 0& 0& 0\\ 1& -2& 1& & 0& 0& 0\\ & ⋮& & \ddots & & ⋮& \\ 0& 0& 0& & 1& -2& 1\\ 0& 0& 0& \cdots & 0& 1& -2\\ 0& 0& 0& & 0& 0& 1\end{array}\right](2)$$

加速度可表示为：
$$\ddot{\theta }=\text{A}\theta (3)$$

加速度向量的内积则为：
$${\ddot{\theta }}^T\ddot{\theta }={\theta }^T({\text{A}}^T\text{A})\theta (4)$$

因此，选择$\text{R}={\text{A}}^T\text{A}$确保了${\theta }^T\text{R}\theta$表示沿轨迹的加速度平方和。

先前的工作 [9] 已经展示了利用协变泛函梯度下降技术对方程$1$的非随机版本进行优化。在本工作中，我们则采用了一种无导数的随机优化方法来优化它。这使得我们能够优化任意的代价函数$q(\tilde{\theta })$，对于这些代价函数，其导数可能不可用或不可微分或不平滑。

对方程$1$中期望关于$\tilde{\theta }$的梯度求导，我们得到：

$${\nabla }_{\tilde{\theta }}\left(\mathbb{E}\right[\sum _{i=1}^{N}q({\tilde{\theta }}_i)+\frac{1}{2}{\tilde{\theta }}^{T}R\tilde{\theta }\left]\right)=0(5)$$
这导致：
$$\mathbb{E}(\tilde{\theta })=-{\text{R}}^{-1}{\nabla }_{\tilde{\theta }}\left(\mathbb{E}\right[\sum _{i=1}^{N}q({\tilde{\theta }}_i)\left]\right)(6)$$
进一步分析得到：
$$\mathbb{E}(\tilde{\theta })=-{\text{R}}^{-1}\mathbb{E}\left({\nabla }_{\tilde{\theta }}\right[\sum _{i=1}^{N}q({\tilde{\theta }}_i)\left]\right)(7)$$
上述表达式可以写成如下形式：
$$\mathbb{E}(\tilde{\theta })=-{\text{R}}^{-1}\delta {\hat{\theta }}_G$$
其中$\delta {\hat{\theta }}_G$现在定义为梯度估计，其表达式为：
$$\delta {\hat{\theta }}_G=\mathbb{E}\left({\nabla }_{\tilde{\theta }}\right[\sum _{i=1}^{N}q({\tilde{\theta }}_i)\left]\right)(8)$$

从式 (5) 到式 (6)

式 (5) 是最优性条件（梯度为零）： $${\nabla }_{\tilde{\theta }}\left(\mathbb{E}\right[\sum _{i=1}^{N}q({\tilde{\theta }}_i)+\frac{1}{2}{\tilde{\theta }}^{T}R\tilde{\theta }\left]\right)=0.$$
利用梯度运算的线性和期望算子的可交换性（在常见条件下可将梯度和期望对换），将梯度作用到两部分：

1. 状态代价部分 $${\nabla }_{\tilde{\theta }}\mathbb{E}[\sum _{i}q({\tilde{\theta }}_i)]=\mathbb{E}[{\nabla }_{\tilde{\theta }}\sum _{i}q({\tilde{\theta }}_i)].$$

2. 二次控制代价部分 $${\nabla }_{\tilde{\theta }}\mathbb{E}\left[\frac{1}{2}{\tilde{\theta }}^{T}R\tilde{\theta }\right]={\nabla }_{\tilde{\theta }}\left(\frac{1}{2}{\theta }^{T}R\theta \right){|}_{\theta =\mathbb{E}(\tilde{\theta })}=R\mathbb{E}(\tilde{\theta }).$$ （因为 $\frac{1}{2}{\theta }^{T}R\theta$ 对 $\theta$ 的梯度是 $R\theta$。）

把两部分加起来并令其为零： $$\mathbb{E}[{\nabla }_{\tilde{\theta }}\sum _{i}q({\tilde{\theta }}_i)]+R\mathbb{E}(\tilde{\theta })=0.$$
移项可得 $$R\mathbb{E}(\tilde{\theta })=-\mathbb{E}[{\nabla }_{\tilde{\theta }}\sum _{i}q({\tilde{\theta }}_i)]⟹\mathbb{E}(\tilde{\theta })=-R^{-1}\mathbb{E}[{\nabla }_{\tilde{\theta }}\sum _{i}q({\tilde{\theta }}_i)],$$ 这就是式 (6)。


从式 (6) 到式 (7)

式 (6) 已经是 $$\mathbb{E}(\tilde{\theta })=-R^{-1}{\nabla }_{\tilde{\theta }}(\mathbb{E}[\sum _{i}q({\tilde{\theta }}_i)]).$$ 在这里，用到了梯度和期望可交换这一点，将 $${\nabla }_{\tilde{\theta }}\mathbb{E}[\sum _{i}q({\tilde{\theta }}_i)]=\mathbb{E}[{\nabla }_{\tilde{\theta }}\sum _{i}q({\tilde{\theta }}_i)].$$ 所以直接把右边写成“期望的梯度”，就得到了式 (7)： $$\mathbb{E}(\tilde{\theta })=-R^{-1}\mathbb{E}[{\nabla }_{\tilde{\theta }}\sum _{i}q({\tilde{\theta }}_i)].$$

先前的方法 [9] 使用解析的泛函梯度来推导迭代的梯度下降更新规则。虽然这可能是高效的，但它要求代价函数是平滑且可微的。此外，即使文献 [9] 中并未提出这一点，对于给定的代价函数$J(\theta )$，其Hessian矩阵的正定性条件${\nabla }_{\theta \theta }J(\theta )>0$也是收敛性的必要保证。我们提出的梯度估计方法正是受限于基于梯度的优化在处理不可微或不平滑代价函数时的局限性。受概率匹配领域中已有工作的启发 [10]，以及路径积分强化学习领域的近期研究 [11]，我们提出了一种如下形式的估计梯度：
$$\delta {\hat{\theta }}_G=\int \delta \theta d\text{P}(9)$$

解析的泛函梯度下降（Analytic Functional Gradient Descent）

• 什么是泛函梯度？ 轨迹$\theta (t)$本质上是一个函数（从时间到配置空间的映射），其代价$J[\theta ]$是作用在这个函数上的“泛函”。泛函梯度就是对整个函数做微小扰动时，代价的变化速度——可看作“函数空间里的梯度”。

• 解析推导 指直接对模型中定义的光滑代价$J(\theta )$求导，得到一个闭式表达式，进而写出梯度下降的更新规则。

• 要求平滑可微：只有当$q({\theta }_i)$和加速度二次项都可微，且拼成的复合代价在参数空间中光滑，才能保证这些导数存在并且计算准确。

Hessian正定性保证收敛

• Hessian矩阵${\nabla }_{\theta \theta }J(\theta )$是$J$对$\theta$的二阶导数矩阵。若在整个可行域内该矩阵都是正定的（所有特征值>0），则意味着$J$是严格凸的。

• 严格凸保证：梯度下降不会陷入鞍点或局部凹陷，最终会全局收敛到唯一的最优解。

基于梯度的方法在非光滑代价上的局限

• 真实世界中，障碍物避撞代价往往在边界处不连续（从“安全”到“碰撞”急剧跳变），或者约束代价出现不可微拐角。
• 这时候解析梯度不存在或数值不稳定，导致基于梯度的优化（尤其依赖Hessian的牛顿法、拟牛顿法）效果大打折扣。

受概率匹配（Probability Matching）和路径积分强化学习(Path Integral RL) 启发的随机梯度估计

• 核心思想：不直接计算不可得的导数，而是通过采样在参数空间（或控制空间）中画噪声样本，再对这些样本按代价高低加权、累积，得到一个“方向指示”——即梯度估计。
• 这种方法无需解析导数，只要能评估每个噪声扰动下的代价，就能更新参数。

$$\delta {\hat{\theta }}_G=\int \delta \theta d\text{P}(9)$$

• $\delta \theta$ 表示“对参数向量 $\theta$ 的一个小扰动”，通常由零均值、高斯分布等随机产生。
• $d\text{P}$ 是在这些扰动上定义的概率测度，用来决定每个样本对最终梯度估计的贡献大小。

本质上，上式表示在概率度量$\text{P}=\exp \left(-\frac{1}{\lambda }\text{S}(\tilde{\theta })\right)$下，噪声$\delta \theta$（即参数向量$\tilde{\theta }$中的扰动）的期望。其中，$\text{S}(\tilde{\theta })$是定义在轨迹上的状态相关代价，其形式为：
$$\text{S}(\tilde{\theta })=[\sum _{i=1}^{N}q({\tilde{\theta }}_i)]$$
因此，随机梯度现在被表述为：
$$\delta {\hat{\theta }}_G=\int \exp \left(-\frac{1}{\lambda }\text{S}(\theta )\right)\delta \theta d(\delta \theta )(10)$$

$$\delta {\hat{\theta }}_G=\int \exp \left(-\frac{1}{\lambda }\text{S}(\theta )\right)\delta \theta d(\delta \theta )(10)$$

• $\lambda$ 是温度／探索参数，控制“高代价干扰”的衰减程度；$\lambda$ 越小，则高代价扰动被抑制得越厉害。
• $\exp (-S/\lambda )$ 就是路径积分框架中常见的权重：代价低（好路径）的样本权重大，代价高（坏路径）的样本权重小。

尽管我们的优化过程是静态的，即不需要执行实际的轨迹回放，但我们的梯度估计过程与路径积分随机最优控制框架中值函数梯度的计算方法有密切联系 [11]。更准确地说，在随机最优控制框架中的目标是找到能够最小化性能指标的最优控制。在路径积分随机最优控制形式中，这些控制在每一个状态${\text{x}}_{t_i}$上以以下方式计算：
$$\delta \hat{\text{u}}=\int p(\text{x})\delta \text{u}$$

与路径积分随机最优控制的对应关系

• 在随机最优控制（Path Integral Control）中，寻找最优控制序列 { u t i } \{u_{t_i}\} {uti​​} 以最小化从当前状态到终止状态的累积代价。
• 控制更新类似：
  $$\delta \hat{\text{u}}=\int p(\text{x})\delta \text{u}p(\tau )\propto \exp (-S(\tau )),$$
  其中 $\tau$ 是一条完整的状态—控制轨迹，$S(\tau )$ 是该轨迹代价。
• 这与上面对 $\theta$ 的样本化更新一一对应，只不过目标空间从“轨迹参数”变为“控制输入”。

其中，$\delta \text{u}$是采样的控制量，$p(\text{x})$表示从${\text{x}}_{t_i}$开始到终止状态${\text{x}}_{t_N}$的每条轨迹${\tau }_i$的概率。该概率被定义为$p(\text{x})=\exp (-S({\tau }_i))$，其中$S({\tau }_i)$为路径${\tau }_i=({\text{x}}_{t_i},\dots ,{\text{x}}_{t_N})$的代价。因此，$p(\text{x})$与路径代价$S({\tau }_i)$成反比，即代价越高的路径对最优控制的贡献越小，代价越低的路径贡献越大。上述过程会对每一个状态${\text{x}}_{t_i}$重复，直到终止状态${\text{x}}_{t_N}$（即多阶段优化）。然后控制量根据$\text{u}=\text{u}+\delta \hat{\text{u}}$进行更新，并生成新的轨迹。在我们的场景中，我们假设每个状态的代价$q({\theta }_i)$仅依赖于参数${\theta }_i$本身，且不将未来或过去的代价归因于当前状态。因此，我们通过定义局部轨迹代价$S({\theta }_i)=q({\theta }_i)$来简化问题，即移除了代价的累积项。我们发现这种简化在第五节的实验中显著加快了收敛速度。

分阶段／分状态的重复更新

• 在控制框架中，通常对每一个时刻的控制都做一次上述加权采样估计，并不断迭代更新直到终止。
• 在本文的方法里，轨迹参数(\theta)是一次性整体更新，不需实际回放轨迹，仅在离线通过采样模拟完成更新。

本工作中的简化假设

• 局部代价：我们假设每个时间步上的代价 $q({\theta }_i)$ 只依赖于当前参数 ${\theta }_i$，而不与未来或过去的参数关联（即不累积历史）。
• 于是把全局累积 $S(\tilde{\theta })={\sum }_{i}q({\tilde{\theta }}_i)$ 简化为多个独立的“局部” $S({\theta }_i)=q({\theta }_i)$ 并分别采样更新。
• 这一简化大大减少了采样方差和计算开销，使得收敛速度在实验中有显著提升。

STOMP 的最终更新公式列在表 I 中。以下几点值得进一步讨论：

A. 探索

为了使带噪声轨迹的控制代价保持较低，我们从一个零均值的正态分布中采样噪声$ϵ$，其协方差矩阵为${\Sigma }_{ϵ}={\text{R}}^{-1}$，如图2(b)所示。与协方差$\Sigma =\text{I}$的采样方式相比，这种方式具有多个优点：
(1) 采样的$ϵ$具有较低的控制代价${ϵ}^{\text{T}}\text{R}ϵ$，因此可以在不显著增加轨迹控制代价的情况下探索状态空间；
(2) 这些带噪声的轨迹在物理系统上可以顺利执行；
(3) 这些采样不会导致轨迹偏离起点或终点。

在基于轨迹的点对点运动强化学习中，实现朝向目标的探索是非常理想的，此类情形下已有动态系统设计可以满足这一性质[12]。

图2. (a) 每条曲线表示对称矩阵${\text{R}}^{-1}$的一列/一行。(b) 从零均值、协方差为${\Sigma }_{ϵ}={\text{R}}^{-1}$的正态分布中采样得到的$20$个随机扰动$ϵ$样本。

为什么要用 ${\Sigma }_{ϵ}=R^{-1}$ 来采样噪声？

1. 回顾 $R$ 的含义 在前面我们介绍过， $$R=A^{T}A$$ 其中 $A$ 是一个二阶有限差分矩阵，用来计算离散轨迹的“加速度”。因此， $${ϵ}^{T}Rϵ={ϵ}^T{A}^{T}Aϵ=(Aϵ{)}^T(Aϵ)=\sum _i({\ddot{ϵ}}_i{)}^2$$ 正好是噪声轨迹 $ϵ$ 的“加速度平方和”——也就是它的控制代价。

2. 取协方差为 $R^{-1}$ 的直观意义

   • $R$ 是正定的，故可逆；其逆 $R^{-1}$ 自身也是一个对称正定矩阵。

   • 从概率的角度看，我们把 $ϵ\sim \mathcal{N}(0,R^{-1})$——即在 $N$ 维空间中以 $R^{-1}$ 为协方差采样。

   • 图 2(a) 正好把 $R^{-1}$ 的每一列（因为对称，也可以看作每一行）画了出来：

     每一条彩色曲线表示如果你在第 $j$ 个时间点上投一个“单位噪声”，该噪声如何通过协方差矩阵在整个轨迹上“扩散”并产生平滑的、钟形（bell‑shaped）分布。

   • 这样的协方差结构意味着：相邻时间点的噪声高度相关，而与起点和终点的相关性又被特殊处理（确保持零），从而天然生成“中间最大、两端为零”的平滑波形。

---

与 $\Sigma =I$（各分量独立同分布）相比，$R^{-1}$ 采样的三个优点

我们要在不破坏平滑性，也不偏离起终点的前提下，对轨迹做试探性扰动（探索）。

1. 扰动本身的控制代价 ${ϵ}^{T}Rϵ较低$

   • 如果直接用 $ϵ\sim \mathcal{N}(0,I)$，那么 $\mathbb{E}[{ϵ}^{T}Rϵ]=\mathrm{tr}(R)$，可能非常大。
   • 而若 $ϵ\sim \mathcal{N}(0,R^{-1})$，则
     $$\mathbb{E}[{ϵ}^{T}Rϵ]=\mathbb{E}[\mathrm{tr}(ϵ{ϵ}^{T}R)]=\mathrm{tr}(R\underset{=R^{-1}}{\underbrace{\mathbb{E}[ϵ{ϵ}^T]}})=\mathrm{tr}(I)=N,$$
     与维度 $N$ 成正比，却不依赖于 $R$ 的大小或轨迹的长度。
   • 结果：我们在探索时，不会因为噪声太“粗暴”而瞬间飞出可行平滑轨迹空间，保证“扰动够用”但“代价可控”。

2. 在物理系统上的可执行性（平滑性）

   • 由于 $R^{-1}$ 在时序上引入了强烈的邻时相关，采样得到的 $ϵ$ 曲线几乎没有高频抖动，看起来就是一条“平滑的波浪”——而非白噪声那样拉裂轨迹。

   • 图 2(b) 中我们画了 20 条 $\mathcal{N}(0,R^{-1})$ 的样本：

     这种蓝色的、在两端锥形收敛到 0 的样本曲线，正是对平滑、边界不偏离的最佳体现。

   • 好处：机器人或执行器不用应对突变的速度/加速度指令，机械结构上也更安全、寿命更长。

3. 不偏离起点和终点（边界条件严格满足）

   • 因为 $A$ 在构造时，就把第一行和最后一行设计成了只读取端点（见式 (2)）；进而 $R=A^{T}A$ 强制任何“单位冲击”在边界处都要乘上系数 0。
   • 所以 $ϵ\sim \mathcal{N}(0,R^{-1})$ 的所有样本都天然满足 ${ϵ}_1={ϵ}_N=0$，绝不会搬动起点和终点。

---

在轨迹型点对点强化学习中，为什么需要“朝向目标”的探索？

• 点对点任务：机器人从固定起点到达固定终点，只要尽快、安全到达即可。
• 理想的探索：
  • 希望扰动能“顺着”往目标方向微调——而不是在起点附近瞎走一通。
  • 这样才能更快试出哪一类轨迹能到达目标，并以此加速学习。

文献 [12] 中提出了动态系统设计，即在运动规划里引入“吸引子”或“虚拟弹簧”机制，使得所有探索样本都自然带有指向目标的趋势。

在我们的设置里，虽然没有显式建弹簧，

• 但通过 ${\Sigma }_{ϵ}=R^{-1}$ “围绕当前轨迹”做的平滑扰动，加上后续的更新规则（参见前面的 $\mathbb{E}(\tilde{\theta })=-R^{-1}\delta {\hat{\theta }}_G$），
• 实际上也具备了一定的“收敛指向性”：
  1. 扰动不会偏离边界，
  2. 更新时综合了轨迹上的所有局部代价，
  3. 因而每一步都会“向着降低代价（更接近目标）”的方向前进一步。

为啥要“给噪声做个过滤器”？

想象你有一根两端固定的细杆（轨迹），你想在它中间抖出一些小波浪来“试试不同的路线”。

• 如果直接在每一点上随机抖动（就像白噪声），杆子中间会抖得乱七八糟，两端也可能被抖动跑偏。
• 我们希望：
  1. 抖动很自然地在中间出现（中间能有些起伏），
  2. 两端保持固定不动（起点和终点不变），
  3. 抖动不要太“猛”（不要一会儿抖得太费力，增加能量/加速度开销）。

这个“过滤器”就是矩阵 $R$（本质上是一个离散化的“二阶差分”算子，衡量弯曲和加速度）。

• 它的逆 $R^{-1}$ 就像给抖动做了一个低通滤波：高频、突变的噪声被滤掉，只剩下“平滑的、钟形的”波浪。
• 同时，滤波后保证了两端始终贴着墙（噪声值为零），不把起点和终点撬动。

---

抖动“小但有效”的秘诀

当你从 $\mathcal{N}(0,R^{-1})$（也就是波浪滤波后的噪声分布）里抽样时，你会得到这种特性：

1. 中间起伏，两端贴墙
   • 波浪最高在中间，抖动幅度往两头逐渐变小，到端点时恰好抖为零。
2. 抖动平滑，不甩锅
   • 曲线像水波一样，没有尖锐的拐点，机械执行时不会因为“突变加速度”而抖坏零件。
3. 耗能可控
   • 由于“滤波”后每一次抖动的内部加速度平方和固定在一个合理范围内，不会突然甩出一个耗能巨大、轨迹失控的样本。

---

这样做的好处

• 探索更安全：系统不会因为“太猛的随机”而跑偏或出故障。
• 保持目标方向：虽然只是抖动，但因为起终点不动，整体仍朝向目标。
• 效率更高：每次抖动都在“平滑可行”的轨迹空间里，更新时更快找到更优路径。

---

B. 轨迹更新

在生成$K$条带噪声轨迹之后，我们对每条轨迹的每个时间步计算其代价$S({\theta }_k,i)$（见表1，步骤2(a)）。在步骤2(b)中，我们对每条带噪声轨迹的每个时间步计算其概率$P({\theta }_k,i)$。参数$\lambda$用于调节指数化代价的敏感度，并可以在每个时间步自动优化，以最大化对观测到的代价的区分度。

我们在步骤2(b)中计算指数项如下：

$$e^{-\frac{1}{\lambda }S({\theta }_k,i)}=e^{-h\frac{S({\theta }_k,i)-\min S({\theta }_k,i)}{\max S({\theta }_k,i)-\min S({\theta }_k,i)}}(11)$$
其中$h$是一个常数，在我们的所有评估中选择$h=10$。$\max$和$\min$操作作用于所有的带噪声轨迹$k$。然后，在步骤3中，对每个时间步的噪声更新通过对该时间步所有带噪声参数的概率加权凸组合计算得到。

最后，在步骤4中，我们使用矩阵$\text{M}$对噪声更新进行平滑，然后在步骤5中更新轨迹参数。矩阵$\text{M}$通过对${\text{R}}^{-1}$的每一列进行缩放得到（如图2(a)所示），使得每列中的最大元素的幅值为$1/N$。这种缩放确保没有更新后的参数超过带噪声轨迹所探索的范围。与$\text{M}$的乘法确保了更新后的轨迹仍然平滑，因为它本质上是投影到图2(a)中所示的${\text{R}}^{-1}$的基向量上。

这些轨迹更新可以被视为比标准梯度下降更新规则更安全。新的轨迹本质上是已经评估过的带噪声轨迹的一个凸组合，也就是说，不会由于噪声梯度评估而突然跳转到状态空间中未探索的区域。我们的迭代更新规则类似于期望最大化（EM）算法，在该算法中，我们更新轨迹采样分布的均值，使其匹配上一轮采样所得到的代价分布。如果采样被认为是足够密集的，那么这一过程能够保证平均代价不增加[10][13]。另一个优点是，该算法不需要梯度步长参数；算法中唯一需要设定的参数是探索噪声的幅度。

一、整体思路：从采样到更新

1. 采样阶段
   • 我们已经用 ${\Sigma }_{ϵ}=R^{-1}$ 从当前轨迹 $\theta$ 周围采样了 $K$ 条“平滑扰动”轨迹，记作
     $${\theta }_k=\theta +{ϵ}_k,k=1,2,\dots ,K.$$
2. 评估阶段
   • 对每条轨迹的每个时间步 $i$，都可以算出一个局部代价 $S({\theta }_k,i)$，比如避障代价、能量代价之和。
3. 加权阶段
   • 用 指数化（softmax-like） 的方式把代价 $S({\theta }_k,i)$ 转换成概率权重 $P({\theta }_k,i)$，低代价的样本权重大，高代价的权重小。
4. 重构更新
   • 对每个时间步 $i$ 上的噪声 ${ϵ}_k(i)$ 做 概率加权平均，得到一个“期望扰动” ${\Delta }_i$。
   • 再对整个扰动向量 $\Delta =({\Delta }_1,\dots ,{\Delta }_N)$ 做一次 平滑投影（乘以矩阵 $M$），保证更新后的轨迹依然平滑、不跑偏边界。
5. 轨迹迭代
   • 最后令 $\theta \leftarrow \theta +M\Delta$，进入下一轮循环。

---

二、关键数学细节

1. 指数化代价——式 (11)

$$e^{-\frac{1}{\lambda }S({\theta }_k,i)}⟶e^{-h\frac{S({\theta }_k,i)-{\min }_{k}S({\theta }_k,i)}{{\max }_{k}S({\theta }_k,i)-{\min }_{k}S({\theta }_k,i)}}.$$

• 目的：把原本可能跨度很大的代价值 $S$ “压缩”到 $[0,1]$ 区间，再做指数，避免出现极大或极小的权重不稳定。

• 内部操作

  1. 归一化 $\tilde{S}=\frac{S-S_{\min }}{S_{\max }-S_{\min }}$，把所有样本的代价都映射到 $[0,1]$。
  2. 放大／敏感度：乘以常数 $h$（文中取 10），让权重对代价差距更敏感。
  3. 指数衰减：再做 $e^{-h\tilde{S}}$，于是代价最小的样本 $\tilde{S}=0$ 权重 $=1$，代价最大的样本 $\tilde{S}=1$ 权重 $=e^{-h}\approx 4.5\times 10^{-5}$，落差很大，体现“好轨迹贡献多、差轨迹贡献少”。

• 最后再把所有 $k$ 上的指数结果做归一化——除以它们的和——就得到了真正的概率分布 $$P({\theta }_k,i)=\frac{e^{-h\tilde{S}({\theta }_k,i)}}{{\sum }_{j=1}^K{e}^{-h\tilde{S}({\theta }_j,i)}}.$$

2. 噪声更新：概率加权的凸组合

• 对每个时间步 $i$，我们把所有采样的扰动 ${ϵ}_k(i)$ 按照权重 $P({\theta }_k,i)$ 做“加权平均”： $${\Delta }_i=\sum _{k=1}^{K}P({\theta }_k,i){ϵ}_k(i).$$
• 这是一个凸组合，保证 ${\Delta }_i$ 落在所有 ${ϵ}_k(i)$ 所在的区间里，不会“跳”到没探索过的区域——因此更新很安全。

3. 平滑投影：乘以矩阵 $M$

• 虽然每一步 ${\Delta }_i$ 本身来源于“平滑扰动采样”，但它们各自是独立加权的，还可能在时间上稍有不连贯。
• 矩阵 $M$ 就是对 $R^{-1}$ 再做一次按列缩放，确保每列最大值是 $1/N$，然后把整个 $\Delta$ 向“平滑基”上投影一次： $${\Delta }_{\text{平滑}}=M\Delta .$$
• 直觉：等同于再给 $\Delta$ 做一次“低通滤波”，消除残余的高频抖动，同时保证任何一个时刻的更新幅度不会超过原来那些采样轨迹所能到达的最大偏移量。

---

三、为什么比标准梯度下降“更安全”？

1. 不需要手动调节学习率
   • 传统梯度下降要选一个步长 $\alpha$，否则可能发散或收敛太慢。这里唯一需要调的就是采样噪声的幅度。
2. 更新留在已探索区域
   • 由于更新是所有带噪路径的凸组合，绝不会突然跳到“连尝试都没尝试过”的状态空间深处。
3. 类似 EM 思想
   • 把采样—评估看作“E 步”——算出每条轨迹的后验概率；
   • 把加权平均看作“M 步”——更新轨迹均值，使得新的分布“更匹配”那些代价低的样本。
   • 在充分采样的情况下，这样一轮一轮下来，平均代价只会上不下——保证了稳定收敛。

---

四、唯一超参数：噪声幅度

• 整个算法除了采样时用的噪声协方差（即“探索幅度”）和指数权重里的 $h$，就没别的超参数——没有梯度步长、没有动量系数，也不需要估 Hession。
• 你只要控制“想探索多大范围”，算法即可自适应地把代价差异转化为更新幅度。

---

总结

• “B. 轨迹更新” 其实是：
  1. 从当前轨迹周围采样多条平滑扰动；
  2. 按代价给它们打分并做 softmax，低代价样本占大权重；
  3. 把这些扰动按权重合并，并再做一次平滑滤波；
  4. 把这条“加权平滑后的扰动”加回到当前轨迹上，得到下一代轨迹。
• 这样一来，每一步更新都是“在已探索到的、合理的轨迹区间里”做的最优凸组合；不需要算导数、不需要调步长、也不会乱跳，因而既稳健又高效。

整体比喻：多人给你出“分段路线”然后你汇总成一条最优路线

1. “让朋友们各自试跑”——采样 $K$ 条轨迹

   • 你让 $K$ 个朋友各自开车，从家到公司，但他们都稍微走不同的绕行路线（这些绕行就是“加了噪声”的轨迹）。

2. “每段路打个分”——计算每条轨迹每个时间点（路段）的代价

   • 比如第 $i$ 段路，如果前面那条朋友的路线很堵，他的代价就高；如果畅通，他的代价就低。
   • 我们把这些分数记做 $S({\theta }_k,i)$。

3. “把分数变成权重”——指数化并归一化

   • 分数越低（路越好），权重就越大；分数越高（路越差），权重就越小。
   • 数学上我们用一个“软化的指数函数”（看起来像给好路段打 100 分，坏路段打 1 分），再把所有朋友的权重加起来做归一。
   • 这样，每个人每段路就都有一个“贡献值” $P({\theta }_k,i)$。

4. “加权平均，合成最优路段”—— weighted average 噪声

   • 对于路段 $i$，你把所有朋友在这段路的“绕行偏移”按权重平均，得到一个“大家都偏爱”的最佳偏移 ${\Delta }_i$。
   • 这就像取所有人给段路的建议，越多人觉得好的建议占比越大。

5. “再给路线做一次平滑润色”——乘以矩阵 $M$

   • 虽然第 4 步算出了每段的最佳偏移，但它们之间可能有一点“折线”或小抖动。
   • 矩阵 $M$ 就像一个“平滑滤镜”：把这些偏移再轻轻过一遍，消除尖锐折点，让整条路线看起来像一条连贯的弧线。
   • 而且 $M$ 的设计保证：任何一段的修正都不会超过朋友们原来走过的最远那一步——不会越界。

6. “更新你的路线”

   • 最终，你把这条“加权又平滑”的偏移路线，加回到原来走法上，得到下一轮更优的路线。
   • 然后再让朋友们在新路线附近再试一次……不断循环，路线越来越好。

为什么比普通梯度下降更“友好”？

• 不用算导数，也不用调学习率：你只管决定让朋友们绕行（设置噪声力度），剩下的“打分→加权→平滑→更新”一步到位，自动找到最优方向。
• 只在“试过的地方”动：每次更新都是朋友们实地跑过的组合，不会突然跳到完全没试过、很危险的路段。
• 理论保证不倒退：如果朋友们试的路线够多，这种“采样＋加权平均”方法会让平均分数（也就是整体代价）一轮比一轮低。

四、机械臂的运动规划

在本节中，我们讨论如何将表 I 中的随机轨迹优化算法应用于高维机器人机械臂的运动规划问题。我们将介绍如何设计一个代价函数，使得其能够同时用于避障、任务约束优化以及关节扭矩的最小化。

A. 设置

表 I 中的算法是为一维离散轨迹推导出来的。将其扩展到多维轨迹，只需在每次迭代中对每个维度分别执行采样和更新步骤即可。因此，该算法的计算复杂度随问题维度呈线性增长。在机器人运动规划的应用中，我们在关节空间中表示轨迹，轨迹具有固定的持续时间和离散化分辨率。我们假设起始和目标配置在关节空间中已知。

• 关节空间（Joint Space）：
  • 机械臂的姿态可以用末端执行器在笛卡尔空间的位置／姿态来描述，也可以直接用各个关节的角度（或转角）来描述。
  • 关节空间的好处：直接对应机器人自身的驱动指标，常用于避障和动力学规划。
• 固定持续时间
  • 指从开始运动到完成目标，这段时间 (T) 是预先设定好的（比如 2 秒、5 秒），不会动态伸缩。
• 离散化分辨率
  • 把连续的时间区间 ([0,T]) 平均切分成 (N) 小段（比如每 0.1 秒一段，共 50 段），形成 (N) 个时间点。
  • 离散化后，就能用向量和矩阵运算来处理轨迹优化，而不是求解连续的变分问题。

通俗比喻：

想象你要做一场 10 秒的舞蹈，把音乐切成 100 小拍，每拍都要决定身体各个关节的位置。舞蹈总时长和拍子数事先定好，你只需为每拍、每个关节选一个角度。

---

• 起始配置：机械臂在 $t=0$ 时刻的关节角度向量 ${\theta }_{\mathrm{start}}$。
• 目标配置：在 $t=T$ 时刻应达到的角度 ${\theta }_{\mathrm{goal}}$。
• 已知且固定：优化过程中这两个边界条件不能变动——所有采样和更新都必须满足两端不偏移。

意义：

• 这就给了一个“锚点”——好比轨迹的左右两端被钉死。算法优化的任务是“在钉死的两端之间、在给定时间和分辨率下，找到一条平滑、避障、最省能量的角度变化曲线”。

B. 代价函数

我们使用的代价函数包含障碍物代价$q_o$、约束代价$q_c$和扭矩代价$q_t$：
$$q(\theta )=\sum _{t=0}^T{q}_o({\theta }_t)+q_c({\theta }_t)+q_t({\theta }_t)(12)$$
额外的平滑性代价${\theta }^T\text{R}\theta$已被包含在方程(1)的优化问题中。

总的代价函数结构（式 12）

$$q(\theta )=\sum _{t=0}^T(q_o({\theta }_t)+q_c({\theta }_t)+q_t({\theta }_t))$$

• 含义：整个轨迹 $\theta$（在 $t=0$ 到 $t=T$ 这 $T+1$ 个离散时间步上）所对应的代价，就等于每一个时刻上三种代价的累加。
• 三种代价：
  1. 障碍物代价 $q_o$：告诉我们距离障碍物有多危险。
  2. 约束代价 $q_c$：例如关节极限、速度／加速度上限等必须满足的“软”约束。
  3. 扭矩代价 $q_t$：希望关节用力尽量小，节省能量、保护机械部件。
• 为什么这么设计：
  • 把各种需求（避障、满足物理／任务约束、省力）都统一到一个可加的框架里，优化时只需最小化一个总和。
  • 既可保证“安全”（障碍物不碰撞），又可保证“可行”（不超出机械极限）和“高效”（扭矩／能量消耗少）。

注意：文中提到的额外 “平滑性代价” $\theta^T R,\theta$ 已经被包含在最初的优化目标（式 (1)）里，这里就不重复写到式 (12) 中了。

1）障碍物代价：

我们采用一种类似于以往优化型运动规划工作 [9] 中使用的障碍物代价函数。我们从环境中获取布尔体素表示，该表示可以通过激光扫描仪或三角网格模型获得。尽管我们的算法可以优化这种非平滑的布尔值代价函数，但如果使用一个可以获得局部梯度（或通过采样估计梯度）的函数，则可以实现更快的收敛。我们计算该体素图的有符号欧几里得距离变换（EDT）[14]。在整个体素网格中计算得到的有符号EDT$d(x)$，提供了从当前位置到最近障碍物边界的距离信息，无论是在障碍物内部还是外部。EDT的值在障碍物内部为负，在边界处为零，在障碍物外部为正。因此，EDT提供了关于穿透深度、接触情况和接近程度的离散化信息。

我们将机器人本体$\mathcal{B}$近似表示为一组重叠的球体，记为$b\in \mathcal{B}$。我们要求每个球体中所有点到最近障碍物的距离至少为$ϵ$。这一约束可以简化为：球体的中心点到障碍物的距离至少为$ϵ+r$，其中$r$是球体的半径。因此，我们的障碍物代价函数如下所示：
$$q_o({\theta }_t)=\sum _{b\in \mathcal{B}}\max (ϵ+r_b-d(x_b),0)\Vert {\dot{x}}_b\Vert (13)$$
其中，$r_b$是球体$b$的半径，$x_b$是球体$b$在时间$t$时刻在三维工作空间中的位置，由机器人的运动学模型计算得到。如果直接将每个时间步的代价进行简单求和，机器人可能会选择快速穿越代价较高区域以降低总体代价。将代价乘以该球体在工作空间中的速度大小$(\Vert {\dot{x}}_b\Vert )$可以避免这一效应[9]。

障碍物代价 $q_o$ 的设计思路

---

为什么要一个“体素化”的布尔表示？

• 体素（Voxel）：就像三维版的像素，把工作空间切成一格格小的立方体。
• 布尔表示：每个小格要么“占据” （obstacle），要么“空”（free）。
• 优点：
  • 可以直接从激光雷达点云或三角网格模型快速转换得到。
  • 简单、离散、很好地兼容后面的距离变换和梯度估计。

---

有符号欧几里得距离变换（Signed Euclidean Distance Transform，EDT）

• 普通的距离变换：对每个空格，计算到最近障碍物边界的距离；但无法分清是在障碍物内部还是外部。
• 有符号版本：
  • 如果在障碍物内部，输出负值；
  • 在边界上刚好为零；
  • 在障碍物外部，为正值。
• $d(x)$ 的含义：
  • $|d(x)|$ 是点 $x$ 到障碍物表面的最短欧氏距离；
  • $\mathrm{sign}(d)$ 告诉你“穿透”还是“相离”。
• 作用：让代价函数能够知道“我离障碍物多远？离得越近就越危险；如果已经穿进去了，穿得越深也更严重。”

---

机器人本体的球体近似

• 为什么要近似为球体集合？
  • 机械臂的连杆、关节等外形通常比较复杂，直接计算每一个表面点到障碍物的距离非常昂贵。
  • 一组大小、位置不同的球体能够粗略包围整个臂体，且“球面到障碍物的距离”有简单闭式（中心距离减半径）。
• 记号：$\mathcal{B}$ 表示所有这些球体的集合，每个球体记为 $b$，半径为 $r_b$，中心位置为 $x_b$。

---

障碍物代价公式（式 13）

$$q_o({\theta }_t)=\sum _{b\in \mathcal{B}}\underset{\text{穿透或过于靠近的量}}{\underbrace{\max (ϵ+r_b-d(x_b),0)}}\times \underset{\text{速度大小}}{\underbrace{\Vert {\dot{x}}_b\Vert }}$$

拆解看它到底在算什么：

部分	含义
$ϵ+r_b-d(x_b)$	如果球体 $b$ 的中心到障碍物边界的距离 $d(x_b)$ 小于 $ϵ+r_b$，说明球 $b$ 穿过或太靠近边界了。 这里 $ϵ$ 是一个安全距离阈值，保证即便没穿透也要留一点余量。
$\max (\cdot ,0)$	只有在“穿透或太近”时才产生正值代价，其他情况代价为 0。

---

为什么乘以速度 $\Vert {\dot{x}}_b\Vert$？

• 避免“快速穿越高代价区域”

  • 如果不乘速度，算法可能干脆在很危险（代价高）的区域一闪而过：处于危险区的时间很短，积分代价反而可能低于“慢慢挪”那种长时间在轻度危险区的情形。

  • 乘上速度后：在极危险区快速穿越依然会因速度大而带来高代价；相当于“穿越距离 × 速度” 的惩罚，抑制那种“高速撞墙式”探索。

• 物理直觉
  • 在现实里，高速碰撞产生的能量和破坏往往更大，速度越高，穿透同样距离的冲击力越大。这个项把这种物理效应也加进了成本中。

2）约束代价：

我们通过将末端执行器的位置和/或姿态的约束违反程度的大小加入代价函数，来对这些约束进行优化：
$$q_c({\theta }_t)=\sum _{c\in C}|v_c({\theta }_t)|(14)$$
其中，$C$是所有约束的集合，$v_c$是一个函数，用于计算约束$c\in C$在时间$t$时刻的违反程度。

为什么要“代价化”约束？

• 在运动规划里，常有各种必须满足的限定——比如末端执行器必须经过某个路径点、保持一定朝向，或工具尖端不得超出工作面。
• 硬性约束（如 “关节角度绝不能超出极限”）如果直接用“可行域”来剪掉不满足的解，优化搜索空间会被切得七零八落，算法常常求解困难甚至卡死。
• 因此我们把这些约束软化：允许在优化过程中偶尔轻微违反，但对违反程度进行“惩罚”——代价越大，优化越倾向于回到可行区域。

---

约束代价的数学表达

$$q_c({\theta }_t)=\sum _{c\in C}|v_c({\theta }_t)|.$$

• ${\theta }_t$：轨迹在时间 $t$ 上的关节配置。
• $C$：所有需要考虑的“约束项”的集合。每一个索引 $c$ 代表一个具体的约束，比如“末端位置到目标点的距离”、“夹具姿态偏离基准的角度”、“关节 3 的速度不得超 1 rad/s”……等等。
• $v_c({\theta }_t)$：专门计算“约束 $c$”在当前配置下的违反程度（violation magnitude）。
  • 若当前满足该约束，通常定义 $v_c({\theta }_t)=0$。
  • 若违反了，比如末端离目标点 5 cm，则 $v_c=+0.05$（单位米）；若姿态偏差了 10°，则 $v_c=+10°$。
• 绝对值 $|v_c|$：保证无论正负偏差，都给出正的惩罚量；有些约束的自然定义可能带符号（比如超过上界是正、低于下界是负），绝对值让它统一成“多大就惩罚多少”。

---

常见的 $v_c({\theta }_t)$ 设计示例

1. 末端位置到固定目标点 ${\mathbf{p}}_{\mathrm{target}}$： $$v_{\mathrm{pos}}({\theta }_t)=\Vert \underset{\begin{array}{c}\text{通过}\\ \text{正向运动学}\\ \text{计算末端位置}\end{array}}{\underbrace{x_{\mathrm{ee}}({\theta }_t)}}-{\mathbf{p}}_{\mathrm{target}}\Vert .$$

   • 如果末端偏离目标 0.1 m，代价就是 0.1；偏离更大，惩罚更高。
   • $|v_{\mathrm{pos}}|$ 与欧氏距离成正比，让优化往离目标更近的方向走。

2. 末端姿态（欧拉角或四元数）与参考姿态差异： $$v_{\mathrm{ori}}({\theta }_t)=\angle (\underset{\text{当前四元数}}{\underbrace{q_{\mathrm{ee}}({\theta }_t)}},q_{\mathrm{ref}}),$$
   例如采用四元数间的夹角距离，单位是度或弧度。

3. 关节角度上下界软约束： 假设关节 $i$ 的可行范围是 $[{\theta }_i^{\min },{\theta }_i^{\max }]$，那么 $$v_{\mathrm{joint}}({\theta }_t)=\{\begin{array}{ll}{\theta }_t^i-{\theta }_i^{\max },& {\theta }_t^i>{\theta }_i^{\max },\\ {\theta }_i^{\min }-{\theta }_t^i,& {\theta }_t^i<{\theta }_i^{\min },\\ 0,& \text{其它情况}.\end{array}$$ 再取绝对值得到非负惩罚。

4. 速度／加速度上限：与角度类似，把超过阈值的部分作为 $v_c$。

---

为什么要把所有 $v_c$ 相加？

• 多目标兼容：机器人在执行任务时，往往不止一个约束。我们把所有约束的违反量都加起来，得到当前时刻总体的“约束惩罚”。
• 线性可加：每个约束的单位、量纲可能不同，但都转成了“数值惩罚”后（统一在物理单位或归一化后），相加就能反映哪个解在综合约束上离可行域更远。

---

L1（绝对值）惩罚的优缺点

• 优点：
  • 对大偏差的“拉回”作用更强，能快速压缩过界行为。
  • 在遇到突变或不可微点时，还能用子梯度方法进行优化，算法较为稳定。
• 缺点：
  • 在代价函数处于不光滑拐角（$v_c=0$ 位置）时，梯度不存在，但我们的随机采样优化并不强依赖精确导数，能自然处理这种情况。
  • 如果想要更平滑的优化路径，也可改成 L2（二次）惩罚 $v_c^2$，但那会使得远离可行域的解受到平方级的猛烈惩罚，可能导致收敛性能不稳定。

---

约束代价在整个优化中的角色

• 与障碍物和扭矩代价并行：在式 (12) 中，$q_c$ 与 $q_o$、$q_t$ 同级别地累加。
• 平滑代价之外的“几何/任务”保证：前面 ${\theta }^{T}R\theta$ 保证轨迹在时间上足够平滑；$q_c$ 则保证在几何空间（位置、姿态、关节范围）上足够接近任务要求。
• 软化 vs. 强制：所有约束都以“软惩罚”形式进入代价，无需解时刻强制满足，但优化趋向于把 $q_c$ 降到 0，从而最终满足所有约束。

3）扭矩代价：

给定一个合适的机器人动力学模型，我们可以使用逆动力学算法 [15] 来计算在跟踪期望轨迹时每个关节所需的前馈扭矩。每一时刻的电机扭矩是关节状态及其导数的函数：
$${\tau }_t=f({\text{x}}_t,{\dot{\text{x}}}_t,{\ddot{\text{x}}}_t)(15)$$
其中${\text{x}}_t={\theta }_t$表示时间$t$时刻的关节状态，${\dot{\text{x}}}_t$和${\ddot{\text{x}}}_t$分别是关节速度和加速度，可通过对$\theta$进行有限差分得到。

我们通过将这些扭矩的大小加入代价函数来实现扭矩的最小化：
$$q_t({\theta }_t)=\sum _{t=0}^T|{\tau }_t|dt(16)$$

为什么要考虑扭矩代价？

• 背景：在机械臂运动规划中，不仅希望轨迹“走得安全”（避障、满足几何约束）、“走得平滑”（不大起大落），还要“走得省力”——减少电机输出扭矩可以节省能耗、降低部件磨损、延长使用寿命。

• 扭矩代价的角色：把所需的电机扭矩大小也作为一个代价项，优化时会自然倾向于那些既能完成任务又扭矩需求较低的轨迹。

---

动力学模型与逆动力学（式 15）

$${\tau }_t=f(x_t,{\dot{x}}_t,{\ddot{x}}_t)\text{\hspace{1em}(15)}$$

• $x_t={\theta }_t$：在时间 $t$ 时刻，关节状态向量 ${\theta }_t$ 就是所有关节角度（或位置）拼在一起的向量。
• ${\dot{x}}_t$ 和 ${\ddot{x}}_t$：分别表示关节速度和加速度，它们可以通过对离散轨迹做有限差分来近似计算： $${\dot{x}}_t\approx \frac{{\theta }_{t+1}-{\theta }_{t-1}}{2\Delta t},{\ddot{x}}_t\approx \frac{{\theta }_{t+1}-2{\theta }_t+{\theta }_{t-1}}{(\Delta t{)}^2}.$$ 这里 $\Delta t$ 是相邻时间步之间的间隔。
• 函数 $f$：代表“逆动力学”算法，它知道机械臂的质量分布、惯性矩阵、科氏与离心力、重力等物理特性，并根据下面的刚性体动方程计算出实现给定位置、速度、加速度所需的电机扭矩：
  $$\underset{\begin{array}{c}\text{惯性项}\\ \text{(转动惯量×加速度)}\end{array}}{\underbrace{M(x_t){\ddot{x}}_t}}+\underset{\begin{array}{c}\text{科氏/离心力}\end{array}}{\underbrace{C(x_t,{\dot{x}}_t){\dot{x}}_t}}+\underset{\text{重力项}}{\underbrace{g(x_t)}}={\tau }_t.$$
  • $M$ 是质量／惯性矩阵；
  • $C$ 是速度相关的科氏矩阵；
  • $g$ 是重力向量。
• 通俗理解：

  想让机械臂在关节空间做“弯一弯、甩一甩”的动作，就像让你举重：你需要多少力（扭矩）取决于你的速度变化（加速度）、身体质量分布（惯性）以及要克服的重力和离心力。

---

从力学到代价——为什么用扭矩的绝对值

逆动力学给出了每个关节在每个时刻所需的扭矩 ${\tau }_t$（可以是一个向量，包含所有关节的扭矩）。但我们只关心“总共用了多大力气”，于是引入扭矩代价：
$$q_t({\theta }_t)=\sum _{t=0}^T\left|{\tau }_t\right|dt\text{\hspace{1em}(16)}$$

• $|{\tau }_t|$：对所有关节扭矩的绝对值求和，转成一个非负实数，表示“当时刻 $t$ 需要的总力气大小”。
• 乘以 $dt$：因为我们将连续的扭矩任务离散化到每个时间步，乘 $dt$ 实际上在做数值积分——将“力矩随时间的累积”转化为一个标量总成本。
• 为什么要绝对值？
  • 扭矩可能是正也可能是负（正扭矩和负扭矩都是输出“力气”），绝对值把它们都看成“耗费”，不让正负抵消。
  • 如果用平方 $\Vert {\tau }_t{\Vert }^2$，则大的扭矩会被平方放大，更加惩罚峰值；用绝对值则更线性、更“省心”。

---

扭矩代价的物理与优化意义

1. 能量消耗
   • 电机提供的扭矩和角速度共同决定功率输出。虽然精确能量=$\int \tau \cdot \omega dt$，但把$\left|{\tau }_t\right|dt$ 做为近似，也能有效抑制大扭矩、高速组合带来的高能耗。
2. 平滑度的补充
   • 前面 ${\theta }^{T}R\theta$ 保证几何上的轨迹平滑（加速度最小）；扭矩代价进一步保证动态执行过程也不会出现大力冲击。
3. 机械寿命与安全
   • 持续高扭矩或扭矩脉冲会加速机械磨损、产生过热和振动。加入扭矩代价后，优化会偏好“力气分布均匀、小幅度”的动作，延长装备寿命。

---

小结与连贯

• 式 (15)：利用逆动力学模型 $f(x,\dot{x},\ddot{x})$ 计算每个时刻、每个关节所需扭矩。
• 式 (16)：把这些扭矩的绝对值随时间积分，形成“扭矩代价” $q_t$，并将它与障碍物代价 $q_o$、约束代价 $q_c$ 以及平滑度代价一起累加，构成总代价进行优化。
• 效果：最终输出的轨迹，不仅安全、可行、平滑，而且“费力最少”，在能耗、机械应力等方面都具有良好的性能。

C. 关节限位

关节限位问题可以通过在代价函数中增加惩罚项以惩罚违反关节限位的行为来处理。然而，我们更倾向于在探索阶段直接通过将带噪声轨迹$\theta +ϵ$在关节范围内进行截断来消除违反限位的情况，即对可行值集合进行$L_1$投影。由于带噪声轨迹始终在关节限位范围内，每次迭代中的更新轨迹也必然满足限位条件，因为它是带噪声轨迹的凸组合。此外，由于噪声的凸组合会通过矩阵$\text{M}$进行平滑处理，因此最终更新的轨迹会平滑地接触关节限位，而不是以高速撞击限位。

关节限位是什么问题？

• 物理约束：每个机械臂的关节都有最小角度和最大角度，超过这个范围就会撞到机械结构、损坏设备，或者使驱动电机负载过大。
• 运动规划要保证：生成的轨迹中，所有时刻每个关节的角度都严格落在 $[{\theta }_{\min },{\theta }_{\max }]$ 这个区间内。

---

两种处理思路：惩罚 vs. 投影

1. 惩罚项（Penalty）

   • 在代价函数里加一项：如果某个时刻某个关节角度超出范围，就对“超出量”进行惩罚，让优化“自觉”避开那片区域。
   • 缺点：
     • 依赖于惩罚权重的调节（太小无效、太大又会影响其它需求）；
     • 即使代价很高，也可能在迭代中“碰”到边界再退回来，造成不稳定。

2. 投影（Projection）

   • 探索阶段：给当前轨迹加噪声得到一批候选轨迹后，马上把所有超出范围的角度“截断”回 $[{\theta }_{\min },{\theta }_{\max }]$。
   • 更新阶段：只在已经“投影”到合法区间里的轨迹之间做组合，天然保证更新后轨迹也合法。

---

$L_1$ 投影到底是什么意思？

• 数学描述：给定一个向量 $\theta +ϵ$（原轨迹加上噪声），我们想找一个最接近它、同时落在合法区间的点。
• $L_1$ 投影：最小化 $\Vert {\theta }_{\mathrm{new}}-(\theta +ϵ){\Vert }_1$（各关节角度偏差绝对值之和）且 ${\theta }_{\mathrm{new}}\in [{\theta }_{\min },{\theta }_{\max }]$。
• 简化操作：对于每个关节单独剪裁（clamp）： $${\theta }_{\mathrm{new}}^i=\{\begin{array}{ll}{\theta }_{\min }^i,& {\theta }^i+{ϵ}^i<{\theta }_{\min }^i,\\ {\theta }_{\max }^i,& {\theta }^i+{ϵ}^i>{\theta }_{\max }^i,\\ {\theta }^i+{ϵ}^i,& \text{otherwise}.\end{array}$$
• 通俗比喻：「你想要把手伸出一米，铰链只能开到90°，那就把手角度卡在90°；想缩回两米，卡在0°。」每个关节独立“拉偏了就推回来”。

---

为什么更新后也一定满足限位？

• 更新是所有“合法噪声轨迹”的凸组合：
  1. 在探索阶段，每条带噪声的轨迹都被投影到合法区间里。
  2. 在更新阶段，新的轨迹是这些合法轨迹的“加权平均”（权重和为1、且非负）。
• 凸组合的性质：任何合法点集合的凸组合，仍然落在这个合法集合里。
• 结果：无论怎么迭代，轨迹都永远不会跑出关节限位范围。

---

平滑“接触”关节限位，而不是“撞上去再退回来”

• 如果直接在更新后才硬性截断，会出现：
  • 轨迹本来“飞”出范围，再被猛地拉回去——就像一辆车撞上护栏后再掉头，轨迹不连续且震荡大。
• 这里的做法：
  1. 先截断再更新，保证所有参与“平均”过程的样本都不越界；
  2. 再乘以平滑矩阵 $\text{M}$，给最终更新的轨迹再做一次时序滤波。
• 效果：
  • 当最佳轨迹“正好”要贴近极限时，它会缓缓地、平滑地滑到边界上，而不会高速“弹”上去、再弹回来。
  • 这对于物理执行尤为关键，能够避免机械振动和碰撞冲击。

---

小结

• 关节限位 是机械臂运动规划中的硬性边界；
• 投影剪裁（$L_1$ 投影）让探索样本始终合规，无需在代价里调重惩罚；
• 凸组合＋平滑 保证每次迭代都平滑地“贴”在合法范围内，不会出现越界或撞击式震荡；
• 这样，优化在保持安全边界的同时，还能保持轨迹平滑与算法稳定。

五、实验

STOMP 是一种执行局部优化的算法，即它寻找的是局部最优轨迹而非全局最优轨迹。因此，其性能会依赖于用于优化的初始轨迹。不能指望 STOMP 在合理时间内解决典型的运动规划基准问题，例如 alpha puzzle [16]。在本节中，我们评估了将 STOMP 用作机器人手臂的运动规划器以执行操控机器人在日常任务中可能面临的典型任务的可行性。我们首先在 Willow Garage PR2 机器人的仿真环境中进行实验，随后展示其在真实机器人上的性能表现。

A. 仿真

图3. (a) 用于评估 STOMP 作为机器人手臂运动规划器的仿真设置。(b) 两个储物柜之间的初始直线轨迹。© 由 STOMP 优化后的轨迹，用于避开储物柜的碰撞，并满足夹爪保持竖直方向的约束。

我们创建了一个仿真环境，其中包含一个带有15个储物柜的架子，如图3(a)所示。这其中有7个储物柜是PR2的7自由度右臂可以触及的。我们测试了手臂在所有可达储物柜对之间的双向轨迹规划，共产生42个不同的规划问题。由于该算法具有随机性，我们将每个实验重复进行了5次。每次使用的初始轨迹是在机器人配置空间中的一条直线。每条轨迹持续时间为5秒，被离散为100个时间步。

这些规划问题在两种场景下重复进行了实验，分别使用了不同的代价函数。在无约束场景中，代价函数仅包含障碍物代价和轨迹平滑代价。该场景中的成功意味着生成了一条无碰撞的轨迹。在有约束场景中，我们在代价函数中加入了一个约束代价项。该任务约束要求机器人始终保持夹爪朝上，也就是说夹爪就像是手中握着一杯水。具体地说，夹爪的滚转角和俯仰角被约束在±0.2弧度范围内。该场景中的成功意味着生成了一条既无碰撞又在容差范围内满足任务约束的轨迹。

我们还在无约束场景中使用相同的代价函数评估了基于梯度的优化器 CHOMP[9]的性能。STOMP 的探索噪声幅度和 CHOMP 的梯度下降步长均经过调节，以获得良好且稳定的性能。两个算法的迭代次数上限均为500次。对于 STOMP，每次迭代生成 $K=5$ 条带噪声的轨迹样本，并在更新步骤中使用从前几轮中挑选出的额外5条最优样本。

表II展示了这些实验所得的结果。在无约束场景中，STOMP在所有实验中均生成了无碰撞的轨迹。相比之下，CHOMP在接近$30\%$的实验中失败，可能是因为其梯度下降过程陷入了代价函数的局部极小值（该结果是使用标准的 CHOMP 梯度更新规则获得的，而非其哈密顿蒙特卡洛变体。详情请参见第六节）。尽管CHOMP通常需要更多迭代次数以获得成功，但两者的执行时间相当。这是因为STOMP的每次迭代需要评估多条轨迹的代价，但其更新步长通常更大且更稳定，相比CHOMP的梯度更新方式更具优势。

在有约束场景中，$93.3\%$的实验结果生成了同时满足无碰撞和任务约束的轨迹。
图5(a)展示了其中一个有约束规划问题在十次试验中轨迹代价的迭代演化过程的平均值。

这些结果是在算法以一条在配置空间中穿越环境、通常会碰撞的朴素直线轨迹初始化的情况下获得的（见图3(b)）。该算法能够将轨迹从环境中“推出”避免碰撞，同时优化诸如任务约束和扭矩等次级代价项。优化后的轨迹可直接在机器人上执行，即无需进一步进行平滑处理，这在基于采样的规划器中是常见需求[7]。

为了测试代价函数中与最小化扭矩相关的部分，我们对图4中所示的规划问题在有无扭矩项的情况下分别进行了10次试验。两种情形下优化后的运动所产生的扭矩如图5(b)所示。由于运动速度较慢，大部分扭矩用于重力补偿，因此运动开始和结束时的扭矩在两种情况下基本相同。规划器在运动中段找到了状态空间中的某些位置，在这些位置上支持手臂重量所需的扭矩更小。 有趣的是，规划器通常会找到两种解决方案之一，如图4(b)与4(c)所示：(1) 整个手臂被向下拉，或 (2) 手肘折叠回自身，这两种方式所需的扭矩都比完全伸展的姿态要低。

图4. 用于评估扭矩最小化的规划问题。(a) 未进行扭矩最小化时得到的规划：手臂处于伸展状态。(b,c) 进行扭矩最小化后得到的两种不同规划结果。在(b)中，整个手臂被向下拉；在(c)中，手肘被折叠回自身：这两种解决方案都需要更小的重力补偿扭矩。图5(b)展示了这些动作所需的扭矩。

图5. (a) STOMP在一个有约束的规划任务中进行10次试验所得的轨迹代价的迭代演化过程。(b) 图4所示规划问题中所使用的前馈扭矩，在进行和未进行扭矩优化的两种情况下的结果，取10次试验的平均值。

B. 实际机器人

附带的视频展示了在家庭环境中，使用 STOMP 规划的轨迹在 Willow Garage 的 PR2 机器人上的执行效果。

C. 代码与结果复现

本研究中编写的所有软件均已在BSD开源许可协议下发布，并基于机器人操作系统（ROS）[17]。有关安装该软件以及复现本文中实验结果的进一步说明可参见[18]。

六、讨论

文献[9][19]中讨论了一种CHOMP的哈密顿蒙特卡洛（Hamiltonian Monte Carlo, HMC）变体，作为一种在CHOMP梯度更新规则中引入随机性的规范方法。虽然该方法在理论上是合理的，但我们发现其在实际应用中较为困难。它引入了多个需要调节的附加参数，并且为了获得成功的解需要进行多次随机重启。相比之下，我们的算法几乎无需参数调节，不需要代价函数的梯度，并采用一种稳定的更新规则，在某些假设下可以保证平均代价不增加。

七、结论

我们提出了一种算法，用于在存在障碍物的环境中为高维度机器人操作臂规划平滑轨迹。该规划器采用一种无导数的随机优化方法，对可能是不可微或不平滑的代价函数进行迭代优化。我们在仿真环境和移动操作平台上展示了该算法在避障、任务约束优化以及最小化用于执行轨迹的电机扭矩等方面的应用。

未来工作的一个方向是将该局部轨迹优化器与轨迹库方法相结合，使其能够回忆在相似情境下使用过的轨迹，并以此作为进一步优化的起点[20]。STOMP算法也可应用于基于轨迹的强化学习问题中，在这些问题中，代价只能通过在真实系统中执行得到；我们计划在后续工作中进一步探索这些方向。

致谢

本研究在Mrinal Kalakrishnan和Peter Pastor于Willow Garage实习期间完成。本研究还部分得到了以下资助机构的支持：国家科学基金会（NSF）项目编号ECS-0326095、IIS-0535282、IIS-1017134、CNS-0619937、IIS-0917318、CBET-0922784、EECS-0926052、CNS-0960061；DARPA高级机器人操作项目；美国陆军研究办公室；冈川基金会；ATR计算神经科学实验室。Evangelos Theodorou获得了Myronis奖学金的支持。