摘要
- 该方法（论文）的发表时间为2009年，旨在解决传统高维运动规划算法（sampled based） “既冗余又不足” 的痛点。
- 其核心是提出一种名为CHOMP（Covariant Hamiltonian Optimization for Motion Planning，协变哈密顿运动规划优化） 的新型轨迹优化方法，来优化提升采样生成轨迹的质量。
- CHOMP 核心优势：无需输入路径满足 “无碰撞” 前提，可直接将粗糙初始轨迹优化为平滑、无碰撞的可执行轨迹，甚至能作为独立运动规划器使用，同时兼顾高阶动力学优化与更广的收敛范围。
基本符号
-
配置空间维度：$R^m$
-
轨迹起点与终点：$q_{init}, q_{goal} \in R^m$
-
离散轨迹：由 $n$ 个 waypoints 构成，$q_1,\ldots,q_n$（不包括起点与终点），轨迹 $\xi = [q_1^T, \ldots, q_n^T]^T \in R^{nm}$
-
机器人中的点的集合：$\mathcal{B}$
-
机器人中的采样点：$u \in \mathcal{B}$
算法原理
CHOMP 的本质是基于协变梯度下降的轨迹优化，通过定义 “轨迹成本函数”，迭代最小化成本以实现轨迹改进。其核心逻辑可拆解为三部分：成本函数设计、协变梯度更新、几何约束处理（障碍物、关节限制）。
成本函数设计
CHOMP 将轨迹成本分为两类，总目标函数为：
\[\mathcal{U}(\xi) = f_{\text{prior}}(\xi) + f_{\text{obs}}(\xi)\]-
先验项 $f_{\text{prior}}(\xi)$：衡量轨迹的平滑性与动力学特性，与环境无关，确保轨迹 “物理可执行”。也就是说轨迹越平滑，这项成本越低。那么如何衡量轨迹的平滑性呢？可以从速度、加速度等角度进行评估，很显然速度小，轨迹缓，加速度小，轨迹平稳。再结合轨迹的离散表述形式，$\xi = [q_1^T, \ldots, q_n^T]^T$，可通过相邻路径点的有限差分来表示不同路径点的速度或加速度。因此，先验项的数学表达式：
\[f_{\text{prior}}(\xi) = \frac{1}{2} \sum_{d=1}^{D} w_d \| K_d \xi + e_d \|^2\]-
$D,d$：差分的阶次，一阶差分对应速度，二阶差分对应加速度；
-
$w_d$：权重，控制各阶导数的重要性；
-
$K_d$：有限差分矩阵，一阶的形式如下，高阶的形式可以类推
\[K_1 = \left[ \begin{array}{cccccc} 1 & 0 & 0 & \cdots & 0 & 0 \\ -1 & 1 & 0 & \cdots & 0 & 0 \\ 0 & -1 & 1 & \cdots & 0 & 0 \\ \vdots & \vdots & \ddots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & -1 & 1 \\ 0 & 0 & 0 & \cdots & 0 & -1 \\ \end{array} \right] \otimes I_{m \times m} \in R^{(n+1)m \times nm}\] -
$e_d$：常数向量，包含轨迹起点和终点的固定贡献，一阶的形式如下，高阶的形式可以类推
\[e_1 = \begin{bmatrix} -q_0^T, 0, \ldots, 0, q_{n+1}^T \end{bmatrix}^T \in R^{(n+1)m}\]$q_0$ 就是 $q_{init}$，$q_{n+1}$就是 $q_{goal}$。
先验项形式上是平方导数之和，因此理论上可以表示为二次型的形式：
\[f_{\text{prior}}(\xi) = \frac{1}{2} \xi^T A \xi + \xi^T b + c\]将先验项展开如下：
\[f_{\text{prior}}(\xi) = \frac{1}{2} \sum_{d=1}^{D} w_d \left( \xi^T K_d^T K_d \xi + 2 \xi^T K_d^T e_d + e_d^T e_d \right)\]因此可以得到，$A = \sum_{d=1}^{D} w_d K_d^T K_d$，$b = \sum_{d=1}^{D} w_d K_d^T e_d$，$c = \frac{1}{2} \sum_{d=1}^{D} w_d e_d^T e_d$。
由于 $K_d$ 是满秩的有限差分矩阵，$K_d^TK_d$ 为对称正定矩阵，且权重 $w_d > 0$，因此 $A$ 必然是对称正定矩阵，这一性质是后续协变梯度下降收敛的关键保障。
-
-
障碍物项 $f_{obs}$：衡量轨迹与障碍物的距离，确保 “无碰撞”。
该算法是在工作空间（三维物理空间）而非配置空间计算成本。通过预计算 “符号距离场（SDF）” 获取任意点到最近障碍物的距离（负值表示在障碍物内，正值表示在外部），再将机器人身体离散为多个采样点，积分所有采样点的 “障碍物成本”（距离越近、成本越高）。特别地，成本积分基于工作空间弧长而非时间，避免机器人通过 “快速冲过障碍物区域” 降低成本，确保避障稳健性。
论文原文是基于连续轨迹 $q(t)$ 讨论障碍物项，而在实际迭代优化中，轨迹需以离散形式处理，而论文中没有给出障碍物项转化为离散表达式，这里也先讨论连续形式，而后给出补充。
-
连续轨迹 $q(t)$ 的障碍物项
\[f_{\text{obs}}[q] = \int_0^1 \int_{\mathcal{B}} c(x(q(t), u)) \left\| \frac{d}{dt} x(q(t), u) \right\| du dt\]-
$x(q, u) : \mathbb{R}^m \times \mathcal{B} \mapsto \mathbb{R}^3$：类似正运动学函数
当机器人处于配置空间的 $q$ 点时，机器人点集中的 $u$ 点在三维工作空间中的坐标位置。
-
$c(x)$：cost function
论文中没有给出代价方程的具体形式。一般的常见定义方式为 $c(x) = \max(0, \epsilon - d(x))^2$ ，其中 $\epsilon$ 是安全距离。
可以看到障碍物项二重积分的内部积分，并非简单的将机器人的每个点的代价积分，而是将每个点的代价与该点此刻的速度乘积后再去积分，避免机器人通过 “快速冲过障碍物区域” 降低成本。
此时，障碍物项 $f_{obs}$ 是工作空间下的函数，而先验项 $f_{prior}$ 是配置空间下的函数，而轨迹优化是在配置空间下进行的，因此需要将 $f_{obs}$ 变换到配置空间下，或者说将 $\bigtriangledown f_{obs}$ 变换到配置空间下（轨迹优化实际上用到的是成本函数的梯度）。
通过对障碍物项的泛函分析求梯度（ $f_{obs}$ 本质上是泛函，我也不太会），可以得到 $\bar{\nabla} f_{\text{obs}}$（$f_{obs}$在配置空间下的梯度）：
\[\bar{\nabla} f_{\text{obs}} = \int_{\mathcal{B}} J^T \| x' \| \left[ \left( I - \hat{x}' \hat{x}'^T \right) \nabla c - c \kappa \right] du\]- $J$：机器人的运动学雅可比矩阵（维度 $3 \times m$，$\delta x = J \delta q$）
- $x’$：机器人身体元素 $u$ 在工作空间下的速度，$x’ = \frac{d}{dt}x(q(t),u)$
- $\hat{x’}$：速度方向的单位向量，$\hat{x}’ = \frac{x’}{| x’ |}$
- $I - \hat{x}’ \hat{x}’^T$：投影矩阵，该矩阵的作用是将任意向量投影到 “与速度方向垂直的工作空间平面”
- $\nabla c$：cost function的梯度，$\nabla c = \frac{dc}{dd} \nabla d$（其中 $\nabla d$ 在构建 SDF 时就已经生成了）
- $κ$：轨迹在工作空间的曲率向量，$\kappa = \frac{1}{| x’ |^2} \left( I - \hat{x}’ \hat{x}’^T \right) x’’$，描述轨迹的弯曲程度与弯曲方向，用于修正 “因轨迹弯曲导致的成本梯度偏差”
-
-
离散轨迹 $\xi$ 的障碍物项
该论文的障碍物项的设计思路在连续轨迹的讨论中已经深刻体现了，在实际应用过程中，需要进行离散化，
-
将关于时间的导数项替换为有限差分项：
\[\|x'\| = \|\frac{d}{dt}x(q(t),u)\| \longrightarrow \| \frac{x(q_i, u) - x(q_{i-1},u))}{\Delta t} \|\] -
对机器人身体 $\mathcal{B}$ 的积分，通过 “离散采样身体元素” 实现
\(\int_{\mathcal{B}} du \longrightarrow \Sigma_{k=1}^{N_u}\Delta u\) $N_u$ 为身体采样点数量，$\Delta u$ 为采样间隔权重。
最终，离散轨迹 $\xi$ 的障碍物项可以表示为：
\[f_{\text{obs}}(\xi) \approx \sum_{i=1}^{n} \sum_{k=1}^{N_u} c(x(q_i, u_k)) \cdot \left\| \frac{x(q_i, u_k) - x(q_{i-1}, u_k)}{\Delta t} \right\| \cdot \Delta t \cdot \Delta u\]同时，离散轨迹 $\xi$ 的障碍物项在配置空间下的梯度可以表示为：
\[\nabla_{\xi} f_{\text{obs}} = \left[ \nabla_{q_1} f_{\text{obs}}, \nabla_{q_2} f_{\text{obs}}, \ldots, \nabla_{q_n} f_{\text{obs}} \right]^T \in R^{nm}\]从泛函的梯度变为对每个轨迹点做偏导，
\[\] -
-
协变梯度更新
-
核心目标
CHOMP 的最终目标是找到使总成本函数 $\mathcal{U}(\xi) = f_{prior}(\xi) + f_{obs}(\xi)$ 最小化的轨迹 $\xi$。由于 $\mathcal{U}(\xi)$ 通常是非线性、非凸函数（尤其障碍物项 $f_{obs}$ 依赖 SDF，可能存在多个局部极小值），无法直接求全局最小值，因此采用迭代优化策略： 从初始轨迹 $\xi_k$ 出发，每次迭代找到一个更优的轨迹 $\xi_{k+1}$，使 $\mathcal{U}(\xi_{k+1}) < \mathcal{U}(\xi_k)$，逐步逼近局部最小值。
“协变（Covariant）” 的核心含义是：轨迹的更新方向与轨迹的表示方式无关。
-
成本函数的一阶泰勒展开
为简化优化问题，在当前轨迹 $\xi_k$ 的邻域内（即 $\xi$ 与 $\xi_k$ 差异较小时），用一阶泰勒展开近似成本函数 $\mathcal{U}(\xi)$。
\[\mathcal{U}(\xi) \approx \mathcal{U}(\xi_k) + g_k^T (\xi - \xi_k)\]其中，$g_k = \nabla \mathcal{U}(\xi_k)$。
但是，直接优化这个泰勒展开式显然是不可行的：1. 这是一个线性函数，只要梯度不为零，可以找到 $\xi$ 使得该式无限小；2. 当新找的 $\xi$ 离 $\xi_k$ 太远时，泰勒展开的前提（邻域）就失效了，展开式会严重偏离真实成本函数，优化就失效了。因此，需要对优化的步长进行限制，即使得 $\xi$ 在 $\xi_k$ 的邻域内。
-
正则项约束增量
\[\mathcal{U}(\xi) \approx \mathcal{U}(\xi_k) + g_k^T (\xi - \xi_k) + \frac{\lambda}{2} \| \xi - \xi_k \|_M^2\]$\lambda > 0$ 是正则化系数，控制正则化强度 ——$\lambda$ 越大，对增量的约束越强，轨迹更新越平缓；$\lambda$ 越小，增量允许越大，优化收敛越快但可能不平滑。
论文中，正则项选择使用黎曼范数（$|\delta|M^2 = \delta^TM\delta$，原文中 $M$ 选为 $A$），本质是为了让 “轨迹增量的约束标准” 与机器人运动的动力学特性、几何意义完全匹配。$A = \sum{d=1}^{D} w_d K_d^T K_d$，所以 $| \xi - \xi_k |_A^2$ 其实意味着轨迹增量对于轨迹的速度、加速度的影响，直接反映轨迹的动力学代价。
-
无约束二次函数的极值求解
最终优化上述带有正则项的二次式：
\[\xi_{k+1} = \arg\min_{\xi} \left\{ \mathcal{U}(\xi_k) + g_k^T (\xi - \xi_k) + \frac{\lambda}{2} \| \xi - \xi_k \|_M^2 \right\}\]令右式关于 $\xi$ 的梯度为零：
\[g_k + \lambda M(\xi - \xi_k) = 0 \implies \xi_{k+1} = \xi_k - \frac{1}{\lambda}M^{-1}g_k\]
几何约束处理
传统关节限制处理要么在目标函数中添加惩罚项，要么直接将超限关节重置到极限值（$L_1$ 投影），易导致轨迹突变。CHOMP 采用基于A矩阵 norm 的平滑投影：
- 计算 $L_1$ 投影的更新向量 $v$；
- 用黎曼度量变换 $v$：$\bar{v} = A^{-1}v$；
- 缩放 $\bar{v}$ 使更新后的轨迹 $\bar{\xi} = \xi + \alpha \bar{v}$ 恰好消除最大关节超限；
- 若仍有超限，重复迭代。
论文中没有解释为什么使用这种方法，这种方法有效的原因是什么也没有解释。
