轨迹优化：CHOMP 与 TrajOptTrajectory Optimization: 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）。

The RRT of Lesson 14.2 scatters samples in an arm's configuration space, connects them, and quickly finds a collision-free path. But that path is a polyline in random directions: joint velocities jump at every corner, it takes needless detours, and it sometimes grazes obstacles. Hand it straight to the motors and the arm shakes, runs slowly, and may hit something through tracking error. OMPL and MoveIt simplify it by shortcutting first (FIG 6 of Lesson 14.2), but the result is still a polyline. A common practice is "plan, then optimize": resample that path by arc length into waypoints, use them as the initial guess, and let a trajectory optimizer turn it into a smooth trajectory with a safety margin.

Lesson 6.13 built the skeleton: discretize the trajectory into a vector of waypoints, cost = smoothness + obstacle term, solve by gradient descent, preconditioning or Gauss-Newton; it even showed "CHOMP-style" preconditioned descent on a two-link arm. That lesson answered "what is trajectory optimization". This lesson answers the questions you meet when you actually write a motion planner:

• CHOMP in full: why \(A^{-1}\) instead of the plain gradient (FIG 1), how the robot's body becomes a set of spheres that read their cost from an SDF (FIG 2), and why the obstacle cost must be integrated along arc length, or the optimizer learns to "speed through" obstacles (FIG 3).
• TrajOpt: treat collision avoidance as a constraint rather than a soft cost and solve by sequential convex optimization with the exact penalty of Lesson 6.7 and the trust region of Lesson 6.5 (FIG 4); and why checking collisions only at waypoints is not enough, how swept volumes fix it, and where they still miss (FIG 5).
• Local minima and initialization: both methods only go downhill. When the straight-line guess passes through an obstacle they get stuck, and the initial guess must come from a sampling-based planner (14.2) or from several restarts; on the way we meet STOMP, which needs no gradient (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\) 个航点处位移相同（等于推力箭头），因而带来相同的一阶障碍代价下降，比较它们各自让平滑代价涨了多少。

Suppose that halfway through the optimization only one waypoint \(\mathbf q_k\) touches an obstacle, so the gradient \(\mathbf g\) of the obstacle cost is nonzero at that waypoint only. Plain gradient descent pushes just this one waypoint away and the trajectory grows a spike: the obstacle cost drops, but the smoothness cost jumps; the next step the smoothness term pulls the spike back, and the two keep tugging at each other, which is why the gradient descent of Lesson 6.13 was so slow.

CHOMP's answer: the step should be shortest not in the Euclidean metric \(\|\boldsymbol\delta\|^2\) but in the smoothness metric \(\boldsymbol\delta^\top A\boldsymbol\delta\), with \(A=K^\top K\) the finite-difference matrix (Lesson 6.13). Among all steps with the same first-order decrease, the one that adds the least smoothness cost is exactly \(\boldsymbol\delta\propto\hl{Ainv}{A^{-1}}\mathbf g\) (proved in Section 6). The figure draws three such steps together. All three are scaled to the same displacement at waypoint \(k\) (equal to the push arrow), so they buy the same first-order decrease of the obstacle cost; compare how much smoothness cost each one adds.

现在把机器人的身体放进来。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})\)。

Now bring in the robot's body. CHOMP does not compute "how close is the link to the obstacle" directly; it approximates the body by a set of spheres: a few points \(\hl{body}{u\in\mathcal B}\) on each link, each with a radius (here the link radius, 0.07). In configuration \(\mathbf q\) point \(u\) sits at \(\mathbf x_u(\mathbf q)\) in the workspace (forward kinematics); its distance to the obstacle surface is read straight from the environment's SDF (Lesson 12.2), and minus the sphere radius that is the sphere's clearance \(d\). When the clearance is below the margin \(\varepsilon\) the sphere pays the cost \(\hl{eps}{c(d)}\), whose gradient pushes it away along the SDF gradient, and the Jacobian transpose \(J_u^\top\) turns that push into a joint-space force.

In the figure, the left panel is the workspace: red discs are obstacles (draggable), the red dashed ring is the margin \(\varepsilon\), green is the start pose, blue the goal pose, the faint poses are the 19 waypoints in between, and the small cyan circles are the body spheres CHOMP sees. The right panel is the same trajectory in joint space (the configuration space of Lesson 7.2): the red region holds the configurations in collision. The initial guess is a straight line in joint space, which sweeps the second link through the obstacle. Press "Run": every step is the complete CHOMP update \(\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\)）。

The naive way is to add up the obstacle cost of every waypoint: \(\sum_t c(\mathbf x_t)\). The trouble is that waypoints are samples in time, so this cost measures "how many samples linger near the obstacle". The optimizer soon finds a loophole: space the waypoints out inside the costly region (that is, make the robot move faster there) and the cost drops, while the geometric path the robot follows has not changed at all. CHOMP's fix is to integrate along arc length: \(\int c(\mathbf x(t))\,\hl{arc}{\|\dot{\mathbf x}(t)\|}\,dt=\int c\,ds\), an integral that does not depend on how the trajectory is parametrized, so the spacing of the waypoints no longer affects the cost. Its functional gradient keeps only the component perpendicular to the motion, plus a curvature term (Section 6).

The figure shows the loophole on a point robot. The red vertical band is a costly region (say the margin zone along a table edge); every path from left to right has to cross it, so its cost can only be redistributed, never removed. The upper lane uses the naive cost and the lower lane CHOMP's arc-length cost; everything else is identical (both preconditioned by \(A^{-1}\), same \(\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 次）。

The second task of FIG 2 exposed CHOMP's weak spot: the obstacle term is only a cost, and where it ties with the smoothness term the optimizer stops in collision. TrajOpt (Schulman et al., 2013/2014) states the problem differently: smoothness is the objective, collision avoidance a constraint, namely signed distance \(\mathrm{sd}\ge d_{\rm safe}\) for every waypoint (or segment), every link and every obstacle. The constraints are nonconvex, so it runs sequential convex optimization:

1. Convexify: at the current trajectory \(\bar\theta\), linearize every signed distance: \(\mathrm{sd}(\theta)\approx\mathrm{sd}(\bar\theta)+\hat{\mathbf n}^\top J_{\mathbf p}(\bar\theta)(\theta-\bar\theta)\), where \(\mathbf p\) is the point of the link nearest to the obstacle and \(\hat{\mathbf n}\) the unit normal pointing from the obstacle to it. In joint space this is one half-plane at a waypoint.
2. Exact penalty: move the constraints into the objective as \(\mu\sum|d_{\rm safe}-\mathrm{sd}|^+\) (with \(|x|^+=\max(x,0)\)). This is the \(\ell_1\) version of the penalties of Lesson 6.7: once \(\mu\) exceeds the largest Lagrange multiplier its minimizer satisfies the constraints exactly, with no need to send \(\mu\to\infty\) as the quadratic penalty does. With slack variables the subproblem becomes a convex QP.
3. Trust region: the linearization is trustworthy only near \(\bar\theta\), so each variable gets a box \(|\theta-\bar\theta|\le\hl{trust}{\Delta}\). After solving the QP, compare the decrease the model predicted with the true decrease of the penalized objective; if their ratio \(\rho\) exceeds a threshold, accept and enlarge the box, otherwise reject, shrink the box and solve again on the same linearization. This page uses the defaults of the open-source TrajOpt implementation (its sco library): threshold 0.25, enlarge 1.5 times, shrink to 0.1 times, initial box 0.1 and initial \(\mu=10\). It is the trust-region logic of Lesson 6.5, with a box instead of a disc.
4. When this round stops (the box cannot shrink any further, or the predicted decrease is below \(10^{-4}\)) and the constraints are still violated, multiply \(\mu\) by 10 and start over (at most 5 times).

第一个任务里的“隧穿”是离散检查的通病（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 节）。

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

The "tunnelling" of the first task is the classic weakness of discrete checks (Pitfall 2 of Lesson 6.13): two safe waypoints do not make the motion between them safe. TrajOpt's fix: for every segment and every link, take the convex hull of the link's shapes at times \(t\) and \(t+1\), \(\hl{hull}{\mathrm{conv}(A(\theta_t)\cup A(\theta_{t+1}))}\), and require its signed distance to the obstacle to be \(\ge d_{\rm safe}\). The distance between the hull and a convex obstacle comes from GJK, the penetration depth from EPA; the nearest point \(\mathbf p\) lies on an edge of the hull and is a convex combination of points at the two times, \(\mathbf p=\alpha\mathbf p_0+(1-\alpha)\mathbf p_1\), so the linearization hands the gradient to \(\theta_t\) and \(\theta_{t+1}\) in the proportions \(\alpha\) and \(1-\alpha\) (Section 6).

The hull is only an approximation of the swept volume. For a translating body the two are identical; for a rotating one, the tip of a link travels on an arc that bulges beyond the line joining its two end positions, while the hull only reaches that chord. The figure lets you find both situations by hand: one the discrete check misses but the hull catches, and one the hull misses too.

在关节空间里看，一个小障碍物往往对应一条细长的红带（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 思想用在轨迹上）。下图在一个直线初值会失败的场景里比较这些组合。

Seen in joint space, a small obstacle often turns into a long thin red band (as in FIG 2 and FIG 4). When the straight guess crosses the band, CHOMP and TrajOpt can only push perpendicular to it and never reach its end; if the band is long enough they get stuck inside it, or stop at a position "half on the left, half on the right". This is not a badly tuned step size or weight; it is the shape of the cost itself: going downhill from the straight line never reaches the basin that goes around the band.

Three common remedies: (1) let the RRT of Lesson 14.2 find a collision-free path first, resample it by arc length into waypoints and use it as the initial guess, so the optimizer only has to smooth it (real systems usually shortcut it first, as in Lesson 14.2; FIG 6 uses the raw RRT path so that you can see what the optimizer does); (2) multiple restarts: add several smooth random perturbations to the straight guess (sampled from \(\mathcal N(0,A^{-1})\), so they are smooth themselves), run each, and keep the best; (3) switch to the sampling-based optimizer STOMP (Kalakrishnan et al., 2011): each step draws several trajectories with smooth noise, averages them with weights from the cost at every time step, and applies a smoothed update through \(A^{-1}\), with no gradient (the MPPI/CEM idea of Lesson 6.11 applied to trajectories). The figure compares these combinations on a scene where the straight guess fails.

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

A trajectory \(\xi:[0,1]\to\mathcal C\) maps time to configurations. CHOMP minimizes

离散成 \(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\) 处一阶导数连续）：

Discretized into \(n\) interior waypoints with time step \(\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{const}\) with \(A=\frac1{\Delta t}K^\top K\) (one tridiagonal matrix per joint; \(\mathbf b\) comes from the fixed ends). The obstacle cost \(c\) acts on the clearance \(d=\mathrm{SDF}(\mathbf x_u)-r_u\) of a body sphere (the form of the CHOMP paper, with a continuous first derivative at \(d=0\) and \(d=\varepsilon\)):

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

Near the current trajectory \(\xi_i\), take the minimizer of "linearized cost + a step penalty in the metric \(A\)" (derived in Lesson 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\)。

Why it is the "cheapest in smoothness" direction (FIG 1): minimize \(\tfrac12\boldsymbol\delta^\top A\boldsymbol\delta\) subject to \(\mathbf g^\top\boldsymbol\delta=\gamma\); the Lagrange condition (Lesson 6.7) gives \(A\boldsymbol\delta=\nu\mathbf g\), i.e. \(\boldsymbol\delta=\nu A^{-1}\mathbf g\), with cost \(\gamma^2/(2\,\mathbf g^\top A^{-1}\mathbf g)\). The Euclidean step \(\boldsymbol\delta\propto\mathbf g\) costs \(\gamma^2\,\mathbf g^\top A\mathbf g/(2\|\mathbf g\|^4)\). With the push on waypoint \(k\) only, the ratio of the two is \(A_{kk}(A^{-1})_{kk}\); for the velocity metric \(A_{kk}=2\) and \((A^{-1})_{kk}=k(n+1-k)/(n+1)\) (the Green's function of the tridiagonal matrix, which is the "tent"), so the ratio is \(2k(n+1-k)/(n+1)\): \((n+1)/2\) at the middle and \(2n/(n+1)\lt2\) next to an end.

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

Taking the functional gradient of the arc-length integral (the Euler-Lagrange equation), every body point contributes

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

\(\hat{\mathbf x}'\) is the unit direction of motion in the workspace and \(\boldsymbol\kappa\) the curvature vector of the path. The projection \(I-\hat{\mathbf x}'\hat{\mathbf x}'^\top\) removes the push along the direction of motion, so waypoints are not shoved "along the road", which is why the waypoint spacing of the blue lane in FIG 3 stays even; the curvature term straightens bends where the cost is high. In the implementation \(\mathbf x'\) and \(\mathbf x''\) are central differences of neighbouring waypoints. Note that this gradient is taken of the continuous functional and then discretized; it is not the exact gradient of the discrete cost. That is harmless in practice, but it is one reason CHOMP is usually run without a line search.

原问题是 \(\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\) 罚函数，并在每次迭代用它的凸近似：

The original problem is \(\min_\theta f(\theta)\) s.t. \(g_i(\theta)\le0\), \(h_j(\theta)=0\) (here \(g_i=d_{\rm safe}-\mathrm{sd}_i\), and joint limits, end-effector poses and more can be added). TrajOpt minimizes the \(\ell_1\) penalty and uses its convex model at every iteration:

\(\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\) 从未被增大。

\(\tilde f\) is a quadratic model of \(f\) (on this page \(f\) is quadratic already). Each \(|\cdot|^+\) brings a slack variable with \(s_i\ge0\) and \(s_i\ge g_i(\bar\theta)+\nabla g_i^\top(\theta-\bar\theta)\), so the subproblem is a convex QP; this page solves it with a small primal-dual interior-point method. The "exactness" of the \(\ell_1\) penalty comes from the KKT conditions (Lesson 6.7): if \(\theta^\star\) is a local optimum satisfying a constraint qualification with multipliers \(\lambda^\star\), then for \(\mu\gt\max_i|\lambda_i^\star|\) it is also a local minimizer of \(\phi\). In FIG 4, \(\mu=10\) is already enough, so \(\mu\) is never raised.

对两个凸体 \(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}\) 给出，于是

For two convex bodies \(A\) (a robot link) and \(O\) (an obstacle), let \(\mathbf p_A\in A\), \(\mathbf p_O\in O\) be the closest pair of points (the two ends of the minimum translation vector when they penetrate) and \(\hat{\mathbf n}\) the unit vector from \(\mathbf p_O\) to \(\mathbf p_A\) (reversed under penetration). Treat \(\mathbf p_A\) as a point fixed on the link; its motion with the joints is given by the Jacobian \(J_{\mathbf p_A}\), so

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

The line above is computed when the page loads: on the straight initial guess of FIG 4 it compares the analytic gradients of both the discrete and the swept constraints with central finite differences, entry by entry. The formula holds where the closest pair is unique; when the closest feature switches (say from one end of the link to the other), \(\mathrm{sd}\) is only piecewise smooth, which is one more reason the trust region is indispensable.

每步采样 \(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\) 为该时间步的障碍代价加一个小的加速度惩罚。

Each step samples \(K\) noises \(\boldsymbol\epsilon_k\sim\mathcal N(0,\sigma^2R^{-1})\) (\(R\) is the acceleration metric, so the noise is smooth), computes a cost \(S_{k,i}\) per time step \(i\), and weights with \(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}\), and finally \(\theta\leftarrow\theta+M\delta\theta\), with \(M\) equal to \(R^{-1}\) with every column scaled so that its largest entry is \(1/n\). This page uses \(K=20\), \(h=10\), and \(S\) is the obstacle cost of that time step plus a small acceleration penalty.

本页所有结果都只是几何路径 \(\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 节的转录），代价是问题更大、更非凸。

Every result on this page is only a geometric path \(\mathbf q(s)\); the "time" between waypoints is just an index. Before execution one still chooses a time law \(s(t)\) so that the velocity \(\mathbf q'(s)\dot s\) and acceleration \(\mathbf q'(s)\ddot s+\mathbf q''(s)\dot s^2\) stay within every joint's limits. This is a one-dimensional problem in \(s\): in the \((s,\dot s)\) phase plane the time-optimal solution is a bang-bang curve ("accelerate fully while you can, brake fully when you must"), and TOPP-RA turns it into a chain of small LPs via reachability analysis, which is very fast. The other route is to put time (or the dynamics) into the optimization variables directly (the transcription of Lesson 6.13), at the price of a larger and more nonconvex problem.

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

Where it sits in a system: the typical arm pipeline is "perception and mapping (a distance field or meshes) → a sampling-based planner (OMPL) returns a path → CHOMP / TrajOpt / STOMP smooth it and add margin → time parameterization → joint controllers track it". Mobile robots and drones often put trajectory optimization into a receding horizon instead: each control cycle re-optimizes a short piece on a local distance field (the MPC of Lesson 13.4 is this idea plus dynamics), with map representations covered in Lesson 15.3. Task and motion planning (Lesson 14.4) decides at a higher level "which object first, and where to put it", then hands each piece to the methods of this lesson.