轨迹优化：CHOMP 与 TrajOpt

14.2 节的 RRT 在机械臂的构型空间里撒点、连边，很快就能找到一条无碰撞的路径。但这条路径是一串随机方向的折线：关节速度在每个拐点突变，绕了不必要的远路，有时还贴着障碍物擦过。直接交给电机执行，机械臂会抖、会慢，还可能因为跟踪误差撞上去。OMPL 和 MoveIt 先用捷径化把它简化一遍（14.2 节 FIG 6），但结果仍是折线。常见的做法是“先规划，再优化”：把这条路径按弧长重采样成航点，当作初值，交给轨迹优化器修成光滑、留有余量的轨迹。

6.13 节已经搭好了骨架：把轨迹离散成航点向量，代价 = 平滑项 + 障碍项，用梯度下降、预条件或 Gauss-Newton 求解；还在 2 连杆机械臂上演示了“CHOMP 风格”的预条件下降。那一节回答了“轨迹优化是什么”。本节回答真正写一个运动规划器时会遇到的问题：

• CHOMP 的完整形式：为什么用 \(A^{-1}\) 而不是普通梯度（FIG 1），机器人的身体怎样变成一组球、在 SDF 里取代价（FIG 2），以及为什么障碍代价要按弧长积分，否则优化器会学会“加速冲过”障碍（FIG 3）。
• TrajOpt：把避障当成约束而不是软代价，用 6.7 节的精确罚函数和 6.5 节的信赖域做序列凸规划（FIG 4）；以及只在航点上查碰撞为什么不够，扫掠体怎样补上，又在哪里仍然会漏（FIG 5）。
• 局部极小与初始化：两种方法都只会往“下坡”走。直线初值穿过障碍时它们会卡住，这时需要 14.2 节的采样规划器给出初值，或者多起点重启；顺带介绍不用梯度的 STOMP（FIG 6）。

设想优化进行到一半，只有一个航点 \(\mathbf q_k\) 碰到了障碍物，障碍代价的梯度 \(\mathbf g\) 只在这个航点上非零。普通梯度下降会只把这一个航点推开，轨迹上出现一个尖刺：障碍代价降了，平滑代价却猛增，下一步平滑项又把尖刺拉回去，两者来回拉扯，所以 6.13 节的梯度下降那么慢。

CHOMP 的回答是：步长不该在欧氏度量 \(\|\boldsymbol\delta\|^2\) 下最短，而该在平滑度量 \(\boldsymbol\delta^\top A\boldsymbol\delta\) 下最短，\(A=K^\top K\) 是有限差分矩阵（6.13 节）。在“一阶下降量相同”的条件下，平滑代价增加最少的步长恰好是 \(\boldsymbol\delta\propto\hl{Ainv}{A^{-1}}\mathbf g\)（第 6 节证明）。下图把这三种步长画在一起，三条曲线都被缩放到在第 \(k\) 个航点处位移相同（等于推力箭头），因而带来相同的一阶障碍代价下降，比较它们各自让平滑代价涨了多少。

现在把机器人的身体放进来。CHOMP 不直接计算“连杆和障碍物有多近”，而是把身体近似成一组球：每根连杆上取若干个点 \(\hl{body}{u\in\mathcal B}\)，每个点带一个半径（这里取连杆的半径 0.07）。点 \(u\) 在构型 \(\mathbf q\) 下的工作空间位置是 \(\mathbf x_u(\mathbf q)\)（正运动学），它到障碍物表面的距离直接从环境的 SDF 里查（12.2 节），减去球的半径就是这个球的间隙 \(d\)。间隙小于余量 \(\varepsilon\) 时付出代价 \(\hl{eps}{c(d)}\)，它的梯度沿 SDF 梯度方向把球推开，再经过雅可比转置 \(J_u^\top\) 变成关节空间的力。

下图左边是工作空间：红色圆盘是障碍物（可拖动），红色虚线圈是余量 \(\varepsilon\)，绿色是起始姿态，蓝色是目标姿态，淡色是中间 19 个航点的姿态，青色小圆是 CHOMP 看到的身体球。右边是同一条轨迹在关节空间（构型空间，7.2 节）里的样子：红色区域是会碰撞的构型。初值是关节空间里的直线，它让第二根连杆扫过障碍物。按“运行”，每一步都是完整的 CHOMP 更新：\(\xi\leftarrow\xi-\tfrac1\eta A^{-1}(\nabla F_{\rm obs}+\lambda\nabla F_{\rm smooth})\)。

最朴素的写法是把每个航点的障碍代价加起来：\(\sum_t c(\mathbf x_t)\)。问题在于，航点只是时间上的采样，代价衡量的却是“在障碍附近停留了多少个采样”。优化器很快会发现一个漏洞：只要让航点在障碍区里排得稀一点（等价于让机器人在那里跑得快一点），代价就降了，而机器人走过的几何路径一点没变。CHOMP 的解决办法是按弧长积分：\(\int c(\mathbf x(t))\,\hl{arc}{\|\dot{\mathbf x}(t)\|}\,dt=\int c\,ds\)，这个积分与轨迹怎样参数化无关，航点疏密不再影响代价。它的泛函梯度只保留垂直于运动方向的分量，再加一个曲率项（第 6 节）。

下图用一个点机器人看这个漏洞。红色的竖带是一片代价区（比如贴近桌边的余量区），从左到右的任何路径都必须穿过它，所以代价只能被重新分配，无法消除。上面一条用朴素代价，下面一条用 CHOMP 的弧长代价，其余完全相同（都用 \(A^{-1}\) 预条件，同样的 \(\lambda\)）。

FIG 2 的第二个任务暴露了 CHOMP 的软肋：障碍项只是代价，和平滑项打平手时就停在碰撞里。TrajOpt（Schulman 等，2013/2014）换了一个提法：平滑度是目标，避障是约束，即对每个航点（或每段）、每根连杆、每个障碍物要求带符号距离 \(\mathrm{sd}\ge d_{\rm safe}\)。约束是非凸的，所以它做序列凸规划：

1. 凸化：在当前轨迹 \(\bar\theta\) 处，把每个带符号距离线性化：\(\mathrm{sd}(\theta)\approx\mathrm{sd}(\bar\theta)+\hat{\mathbf n}^\top J_{\mathbf p}(\bar\theta)(\theta-\bar\theta)\)，\(\mathbf p\) 是连杆上离障碍物最近的点，\(\hat{\mathbf n}\) 是从障碍物指向它的单位法向量。在关节空间里，这就是每个航点上的一个半平面。
2. 精确罚函数：约束写进目标，变成 \(\mu\sum|d_{\rm safe}-\mathrm{sd}|^+\)（\(|x|^+=\max(x,0)\)）。这是 6.7 节罚函数的 \(\ell_1\) 版本：只要 \(\mu\) 大于最大的拉格朗日乘子，它的最小点就恰好满足约束，不需要像二次罚函数那样让 \(\mu\to\infty\)。引入松弛变量后，子问题是一个凸 QP。
3. 信赖域：线性化只在 \(\bar\theta\) 附近可信，所以给每个变量加一个盒子 \(|\theta-\bar\theta|\le\hl{trust}{\Delta}\)。解完 QP 后比较“模型预测的下降”与“真实罚函数的下降”，比值 \(\rho\) 大于阈值就接受并放大盒子，否则拒绝、缩小盒子，在同一个线性化上再解一次。本页用 TrajOpt 开源实现（sco 库）的默认值：阈值 0.25、放大 1.5 倍、缩小到 0.1 倍，初始盒子 0.1、初始 \(\mu=10\)。这是 6.5 节的信赖域逻辑，只是盒子代替了圆。
4. 当这一轮停下来（盒子缩到不能再小，或模型预测的下降已小于 \(10^{-4}\)）而约束仍不满足时，把 \(\mu\) 乘以 10 重来（最多 5 次）。

第一个任务里的“隧穿”是离散检查的通病（6.13 节的误区 2）：两个航点都安全，不代表它们之间的运动安全。TrajOpt 的办法是对每一段、每根连杆，取它在 \(t\) 和 \(t+1\) 时刻两个形状的凸包 \(\hl{hull}{\mathrm{conv}(A(\theta_t)\cup A(\theta_{t+1}))}\)，要求这个凸包到障碍物的带符号距离 \(\ge d_{\rm safe}\)。凸包与凸障碍物的距离用 GJK 算，穿透深度用 EPA 算；最近点 \(\mathbf p\) 落在凸包的一条边上，可写成两个时刻上的点的凸组合 \(\mathbf p=\alpha\mathbf p_0+(1-\alpha)\mathbf p_1\)，于是线性化把梯度按 \(\alpha\) 和 \(1-\alpha\) 分给 \(\theta_t\) 和 \(\theta_{t+1}\)（第 6 节）。

凸包只是扫掠体的近似。物体平移时两者完全一样；转动时，连杆末端走的是圆弧，圆弧会鼓出两端连线之外，而凸包只连到弦。下图让你亲手找出两种情况：离散检查漏掉而凸包抓住的，以及凸包也漏掉的。

在关节空间里看，一个小障碍物往往对应一条细长的红带（FIG 2、FIG 4 都是）。直线初值横穿红带时，CHOMP 和 TrajOpt 只能沿着垂直于红带的方向推，推不到红带的尽头；红带太长，它们就卡在带子里，或者停在“一半在左、一半在右”的位置。这不是步长或权重没调好，而是代价函数本身的形状：从直线出发，“往下坡走”到不了绕过红带的那个盆地。

三种常见对策：(1) 用 14.2 节的 RRT 先找一条无碰撞路径，按弧长重采样成航点作为初值，优化器只负责把它修光滑（实际系统通常先做 14.2 节的捷径化；FIG 6 为了让你看清优化器做了什么，直接用未捷径化的 RRT 路径）；(2) 多起点重启：在直线初值上叠加若干条光滑的随机扰动（从 \(\mathcal N(0,A^{-1})\) 采样，所以本身是光滑的），各跑一遍取最好的；(3) 换用基于采样的优化器 STOMP（Kalakrishnan 等，2011）：每步生成若干条带光滑噪声的轨迹，按每个时间步的代价给它们加权平均，再用 \(A^{-1}\) 平滑更新，不需要梯度（6.11 节的 MPPI/CEM 思想用在轨迹上）。下图在一个直线初值会失败的场景里比较这些组合。

轨迹 \(\xi:[0,1]\to\mathcal C\) 把时间映到构型。CHOMP 最小化

离散成 \(n\) 个内部航点、步长 \(\Delta t=1/(n+1)\)，\(\mathcal F_{\rm smooth}=\frac1{2\Delta t}\sum\|\mathbf q_{i+1}-\mathbf q_i\|^2=\tfrac12\xi^\top A\xi+\mathbf b^\top\xi+\text{常数}\)，\(A=\frac1{\Delta t}K^\top K\)（每个关节各一份三对角矩阵，\(\mathbf b\) 来自固定的两端）。障碍代价 \(c\) 作用在身体球的间隙 \(d=\mathrm{SDF}(\mathbf x_u)-r_u\) 上（CHOMP 原文的形式，在 \(d=0\) 与 \(d=\varepsilon\) 处一阶导数连续）：

在当前轨迹 \(\xi_i\) 附近，取“线性化代价 + \(A\) 度量下的步长惩罚”的最小点（6.13 节已推导）：

为什么它是“平滑代价最便宜”的方向（FIG 1）：在 \(\mathbf g^\top\boldsymbol\delta=\gamma\) 的约束下最小化 \(\tfrac12\boldsymbol\delta^\top A\boldsymbol\delta\)，拉格朗日条件（6.7 节）给出 \(A\boldsymbol\delta=\nu\mathbf g\)，即 \(\boldsymbol\delta=\nu A^{-1}\mathbf g\)，代价为 \(\gamma^2/(2\,\mathbf g^\top A^{-1}\mathbf g)\)。欧氏步长 \(\boldsymbol\delta\propto\mathbf g\) 的代价是 \(\gamma^2\,\mathbf g^\top A\mathbf g/(2\|\mathbf g\|^4)\)。当推力只在第 \(k\) 个航点上时，两者之比是 \(A_{kk}(A^{-1})_{kk}\)；对速度度量 \(A_{kk}=2\)，\((A^{-1})_{kk}=k(n+1-k)/(n+1)\)（三对角矩阵的格林函数，正是那顶“帐篷”），所以比值 \(=2k(n+1-k)/(n+1)\)：中点处为 \((n+1)/2\)，紧挨端点处为 \(2n/(n+1)\lt2\)。

对弧长积分求泛函梯度（欧拉-拉格朗日方程），每个身体点贡献

\(\hat{\mathbf x}'\) 是工作空间里的单位速度方向，\(\boldsymbol\kappa\) 是路径的曲率向量。投影 \(I-\hat{\mathbf x}'\hat{\mathbf x}'^\top\) 去掉了沿运动方向的推力，所以航点不会被推着“顺路溜走”，这正是 FIG 3 蓝色一条航点间距保持均匀的原因；曲率项则在代价高的地方把弯道拉直。实现里 \(\mathbf x'\)、\(\mathbf x''\) 用相邻航点的中心差分计算。注意这个梯度是对连续泛函求的，再离散化，并不是离散代价的精确梯度，这在实践中无妨，但意味着 CHOMP 一般不配合线搜索使用。

原问题是 \(\min_\theta f(\theta)\) s.t. \(g_i(\theta)\le0\)，\(h_j(\theta)=0\)（这里 \(g_i=d_{\rm safe}-\mathrm{sd}_i\)，还可以加关节限位、末端位姿等）。TrajOpt 最小化 \(\ell_1\) 罚函数，并在每次迭代用它的凸近似：

\(\tilde f\) 是 \(f\) 的二次近似（本页的 \(f\) 本身就是二次的）。每个 \(|\cdot|^+\) 引入一个松弛变量 \(s_i\ge0\)、\(s_i\ge g_i(\bar\theta)+\nabla g_i^\top(\theta-\bar\theta)\)，子问题就是一个凸 QP；本页用一个小型原始-对偶内点法求解。\(\ell_1\) 罚函数的“精确性”来自 KKT 条件（6.7 节）：若 \(\theta^\star\) 是满足约束规范的局部最优解、乘子为 \(\lambda^\star\)，则当 \(\mu\gt\max_i|\lambda_i^\star|\) 时 \(\theta^\star\) 也是 \(\phi\) 的局部极小点。FIG 4 里 \(\mu=10\) 已经足够，所以 \(\mu\) 从未被增大。

对两个凸体 \(A\)（机器人连杆）和 \(O\)（障碍物），设 \(\mathbf p_A\in A\)、\(\mathbf p_O\in O\) 是最近点对（穿透时是最小平移向量的两端），\(\hat{\mathbf n}\) 是从 \(\mathbf p_O\) 指向 \(\mathbf p_A\) 的单位向量（穿透时取反）。把 \(\mathbf p_A\) 看成固定在连杆上的点，它随关节的运动由雅可比 \(J_{\mathbf p_A}\) 给出，于是

上面这一行是页面加载时现算的：在 FIG 4 的直线初值上，把离散和扫掠两种约束的解析梯度与中心有限差分逐项比较。这个公式在最近点对唯一时成立；当最近的特征切换（例如从连杆的一端换到另一端），\(\mathrm{sd}\) 只是分段光滑的，这也是信赖域必不可少的原因之一。

每步采样 \(K\) 条噪声 \(\boldsymbol\epsilon_k\sim\mathcal N(0,\sigma^2R^{-1})\)（\(R\) 是加速度度量，所以噪声是光滑的），对每个时间步 \(i\) 计算代价 \(S_{k,i}\)，再按 \(P_{k,i}\propto\exp\!\big(-h\,\frac{S_{k,i}-\min_kS_{k,i}}{\max_kS_{k,i}-\min_kS_{k,i}}\big)\) 加权：\(\delta\theta_i=\sum_kP_{k,i}\,\epsilon_{k,i}\)，最后 \(\theta\leftarrow\theta+M\delta\theta\)，\(M\) 是把每列最大值缩放到 \(1/n\) 的 \(R^{-1}\)。本页取 \(K=20\)、\(h=10\)，\(S\) 为该时间步的障碍代价加一个小的加速度惩罚。

本页所有结果都只是几何路径 \(\mathbf q(s)\)，航点之间的“时间”只是编号。执行前还要选一个时间律 \(s(t)\)，使速度 \(\mathbf q'(s)\dot s\) 和加速度 \(\mathbf q'(s)\ddot s+\mathbf q''(s)\dot s^2\) 不超过每个关节的限值。这是关于 \(s\) 的一维问题：在 \((s,\dot s)\) 相平面上，时间最优解是“能加速就全力加速、该减速就全力减速”的 bang-bang 曲线，TOPP-RA 用可达性分析把它变成一串小 LP，速度很快。另一条路是把时间（或动力学）直接放进优化变量（6.13 节的转录），代价是问题更大、更非凸。

在整个系统里的位置：机械臂的典型流水线是“感知建图（距离场或网格）→ 采样规划器（OMPL）给出路径 → CHOMP / TrajOpt / STOMP 修光滑并留余量 → 时间参数化 → 关节控制器跟踪”。移动机器人和无人机则常把轨迹优化放进滚动时域：每个控制周期在局部距离场上重新优化一小段（13.4 节的 MPC 就是这种思路加上动力学），地图的表示方式见 15.3 节。任务与运动规划（14.4 节）则在更上层决定“先抓哪个、放在哪里”，再把每一段交给本节的方法。