Part I · 第 6 章 优化 · 6.13Part I · Ch. 6 Optimization · 6.13

轨迹优化初探A First Look at Trajectory Optimization

规划器找到一条能走的路，轨迹优化把它变成一条好走的路。把整条轨迹拆成一串路点，当作一个长长的向量，再写一个代价：平滑、避开障碍、到达目标，剩下的就是前面学过的优化：梯度、预条件、Gauss-Newton。机械臂绕过障碍物、四足机器人的步态、无人机的穿越飞行，都从这个朴素的想法出发。 A planner finds a path that can be driven; trajectory optimization turns it into a path that is good to drive. Cut the whole trajectory into a string of waypoints, treat them as one long vector, and write a cost: smooth, clear of obstacles, reaching the goal. What remains is the optimization you already know: gradients, preconditioning, Gauss-Newton. Arms swinging around obstacles, legged gaits and drone fly-throughs all start from this plain idea.

约 30 分钟~30 min 前置：6.3 梯度下降 · 6.6 Newton、Gauss-Newton 与 LM · 6.9 非线性最小二乘Prereqs: 6.3 Gradient descent · 6.6 Newton, Gauss-Newton & LM · 6.9 Nonlinear least squares 4 个交互4 interactives

    1为什么机器人需要它Why robots need this

    路径规划（第 IV 部分的 RRT、A*）擅长回答"能不能到达"：它给出一串无碰撞的路点，但常常是歪歪扭扭的折线，不管电机的速度和加速度，也不管能耗。直接把这样的折线交给机械臂执行，要么抖，要么超出关节限制。

    轨迹优化回答"怎样到达得最好"：它把轨迹本身当作未知数，在满足约束的前提下最小化一个代价（耗时、能量、平滑度、离障碍物的距离……）。结果是一条连续、平滑、合乎动力学的轨迹，可以直接执行。现代的机器人栈大都是"先规划、再优化"，或者在 MPC（6.8 节）里每个周期都做一次短时域的轨迹优化。

    好消息是，你已经具备了所有工具。一条离散化的轨迹就是一个高维向量；轨迹的代价是这个向量的函数；求解就是 6.3、6.6、6.9 节的算法，只是维数比较大、结构比较稀疏。本课带你从最简单的"二维点绕过圆"出发，看清这件事的每一个部件。

    Path planners (RRT and A* in Part IV) are good at "can we get there at all?": they return a string of collision-free waypoints, often a jagged polyline, ignorant of motor speeds and accelerations, and of energy use. Hand such a polyline to an arm and it either shakes or exceeds joint limits.

    Trajectory optimization answers "how do we get there best?": it treats the trajectory itself as the unknown and minimizes a cost (time, energy, smoothness, clearance from obstacles, ...) subject to constraints. The result is a continuous, smooth, dynamically sensible trajectory that can be executed directly. Most modern robot stacks "plan, then optimize", or run a short-horizon trajectory optimization every cycle inside MPC (Lesson 6.8).

    The good news is that you already have every tool. A discretized trajectory is a high-dimensional vector; its cost is a function of that vector; solving it is the algorithms of Lessons 6.3, 6.6 and 6.9, just in higher dimension and with sparse structure. This lesson starts from the simplest case, "a 2D point going around a disc", and shows every part of the machinery.

    2直觉：一条橡皮筋，被障碍物推开Intuition: an elastic band pushed off by obstacles

    在起点 \(\mathbf s\) 与终点 \(\mathbf g\) 之间放 \(N-1\) 个路点 \(\mathbf q_1,\dots,\mathbf q_{N-1}\)（两端固定 \(\mathbf q_0=\mathbf s,\ \mathbf q_N=\mathbf g\)）。整条轨迹就是一个 \(2(N-1)\) 维的向量。代价由两部分组成：

    • 平滑项（弹簧）：\(F_s=\tfrac{w_s}{2}\sum_{k=0}^{N-1}\|\mathbf q_{k+1}-\mathbf q_k\|^2\)。每一段都像一根弹簧，想缩短自己。它的负梯度在路点 \(k\) 处是 \(w_s\big(\mathbf q_{k-1}+\mathbf q_{k+1}-2\mathbf q_k\big)\)：把路点拉向它两个邻居的中点。
    • 障碍项（排斥）：路点离障碍物表面的符号距离 \(d_k\) 小于安全边距 \(\varepsilon\) 时，罚 \(\tfrac{w_o}{2}(\varepsilon-d_k)^2\)。它的负梯度把路点沿远离圆心的方向推。

    两种力一拉一推，平衡的地方就是局部最优。下图用 5 个路点把这件事摆出来：拖动路点，看每个路点上的受力箭头：绿色是平滑项的拉力，红色是障碍项的推力，琥珀色是合力（\(=-\nabla F\)）。合力为零的轨迹，就是最优轨迹。

    Place \(N-1\) waypoints \(\mathbf q_1,\dots,\mathbf q_{N-1}\) between a start \(\mathbf s\) and a goal \(\mathbf g\) (ends fixed, \(\mathbf q_0=\mathbf s,\ \mathbf q_N=\mathbf g\)). The whole trajectory is one \(2(N-1)\)-dimensional vector. The cost has two parts:

    • Smoothness (springs): \(F_s=\tfrac{w_s}{2}\sum_{k=0}^{N-1}\|\mathbf q_{k+1}-\mathbf q_k\|^2\). Each segment acts like a spring wanting to shorten. Its negative gradient at waypoint \(k\) is \(w_s\big(\mathbf q_{k-1}+\mathbf q_{k+1}-2\mathbf q_k\big)\): it pulls the waypoint toward the midpoint of its two neighbours.
    • Obstacle (repulsion): when a waypoint's signed distance \(d_k\) to the obstacle surface is below a safety margin \(\varepsilon\), penalize \(\tfrac{w_o}{2}(\varepsilon-d_k)^2\). Its negative gradient pushes the waypoint away from the disc's center.

    One pulls, one pushes; where they balance is a local optimum. The figure lays this out with 5 waypoints: drag a waypoint and read the arrows on each: green is the smoothness pull, red the obstacle push, amber the net force (\(=-\nabla F\)). A trajectory with zero net force everywhere is an optimal trajectory.

    FIG 1橡皮筋：平滑的拉力与障碍的推力The elastic band: smoothness pull vs. obstacle push
    红色圆盘是障碍物（可拖动），虚线圈是安全边距。小圆点是可以拖动的路点，起点（左）与终点（右）也可以拖。"下降"按钮用小步长的梯度下降让整条轨迹沿合力方向流动：它流到哪里，合力就缩到零。 The red disc is an obstacle (draggable) and the dashed ring the safety margin. The small dots are draggable waypoints; the start (left) and goal (right) can be dragged too. The "Descend" button lets gradient descent with a small step make the whole trajectory flow along the net force: wherever it settles, the net force has shrunk to zero.
    试一试Try this
    • 把一个路点拖进障碍物的边距里：红色推力出现，它的大小和 \((\varepsilon-d)\) 成正比。Drag a waypoint into the obstacle's margin: a red push appears, proportional to \((\varepsilon-d)\).
    • 按"下降"，让轨迹自己找到平衡位置；读出合力的最大值（应该很小）。Press "Descend" and let the trajectory find its own equilibrium; read the maximum net force (it should be tiny).
    • 先把所有路点手动拖到障碍物上方，下降一次；再重置，把它们拖到下方，再下降。两次得到不同的轨迹，代价也不同：这就是局部最优。Manually drag all waypoints above the obstacle and descend; then reset and put them below and descend again. Two different trajectories with different costs: local optima.
    核心洞见Key insight

    轨迹优化 = 在"路点向量"的空间里做优化。没有什么新算法：代价是"平方和 + 惩罚"，所以 6.9 节的最小二乘全部适用；整个问题在最简单的情形下是非凸的（障碍物把空间挖掉了一块），所以结果取决于初值落在障碍物的哪一边。

    Trajectory optimization = optimization in the space of the "waypoint vector". There is no new algorithm: the cost is "sum of squares plus penalties", so all of the least squares of Lesson 6.9 applies; and the problem is nonconvex even in the simplest case (an obstacle carves a hole out of space), so the result depends on which side of the obstacle the initialization lands.

    3动手玩：三种求解器绕障碍物Play: three solvers around the obstacles

    把路点增加到 \(N=30\)，再放三个障碍物。三种求解器从同一条穿过障碍物的直线出发：

    • 梯度下降：\(\mathbf x\leftarrow\mathbf x-\eta\nabla F\)。每个路点只"知道"它自己的力，信息一格一格地沿着轨迹传，所以很慢（平滑项让问题的条件数约为 \(N^2\)，6.3 节）。
    • 预条件（CHOMP 风格）：\(\mathbf x\leftarrow\mathbf x-\eta\,A^{-1}\nabla F\)。\(A=K^\top K\) 是平滑项的 Hessian（有限差分矩阵）。乘上 \(A^{-1}\) 等于让力"沿整条轨迹抹开"，一个路点受到的推力会带动整条曲线一起变形。
    • Gauss-Newton / LM：把平滑项和障碍项都写成残差，解 \((J^\top J+\lambda D)\boldsymbol\delta=-J^\top\mathbf r\)（6.9 节）。\(J^\top J\) 是带状稀疏矩阵，所以每步的成本不高。

    前两种用回溯法选步长（保证代价下降），LM 用阻尼的接受 / 拒绝。拖动障碍物、起点、终点，改变权重，看三者的反应。

    Raise the waypoints to \(N=30\) and add three obstacles. Three solvers start from the same straight line through the obstacles:

    • Gradient descent: \(\mathbf x\leftarrow\mathbf x-\eta\nabla F\). Each waypoint only "knows" its own force, and information crawls along the trajectory cell by cell, so it is slow (the smoothness term gives a condition number of about \(N^2\), Lesson 6.3).
    • Preconditioned (CHOMP style): \(\mathbf x\leftarrow\mathbf x-\eta\,A^{-1}\nabla F\). \(A=K^\top K\) is the Hessian of the smoothness term (the finite-difference matrix). Multiplying by \(A^{-1}\) "spreads the force along the whole trajectory": a push on one waypoint drags the entire curve with it.
    • Gauss-Newton / LM: write smoothness and obstacle terms as residuals and solve \((J^\top J+\lambda D)\boldsymbol\delta=-J^\top\mathbf r\) (Lesson 6.9). \(J^\top J\) is banded and sparse, so each step is cheap.

    The first two choose the step size by backtracking (guaranteeing a cost decrease); LM uses damping with accept / reject. Drag the obstacles, start and goal, change the weights, and watch how the three react.

    FIG 2轨迹优化的三种求解器Three solvers for trajectory optimization
    总代价 \(F\)（对数）与迭代次数total cost \(F\) (log) vs. iteration
    灰色虚线是初始的直线轨迹。三条彩色曲线是三种求解器当前的轨迹（红：梯度下降，绿：预条件，蓝：Gauss-Newton）。读数里每种方法给出代价的两个部分（平滑 / 障碍）和轨迹到障碍物的最小间隙（负数表示进入了障碍物）。三种方法不一定收敛到同一个局部最优。 The grey dashed line is the initial straight trajectory. The three colored curves are the current trajectories of the solvers (red: gradient descent, green: preconditioned, blue: Gauss-Newton). The readout gives each method's two cost parts (smoothness / obstacle) and the trajectory's minimum clearance from obstacles (negative means inside an obstacle). The three need not converge to the same local optimum.
    • 按"运行"：预条件和 Gauss-Newton 在几次迭代里就让代价降到很低，而 梯度下降 还在慢慢爬。Press "Run": preconditioned and Gauss-Newton bring the cost very low in a few iterations, while gradient descent is still crawling.
    • 运行结束后，把一个障碍物拖到当前轨迹上，再按"运行（保留轨迹）"：轨迹从现在的位置出发重新避让（热启动）。After a run, drag an obstacle onto the current trajectory and press "Run (keep trajectory)": it re-avoids starting from where it is (a warm start).
    • 把障碍权重 \(w_o\) 调到很小：轨迹径直穿过障碍物（最小间隙为负）。再调大：轨迹被推得越来越远。Turn the obstacle weight \(w_o\) very low: the trajectory slices straight through (negative clearance). Raise it and the trajectory is pushed ever farther.

    4加上动力学：打靶法与配点法Adding dynamics: shooting vs. collocation

    前面的轨迹只是几何曲线。真实机器人必须服从动力学 \(\mathbf x_{k+1}=f(\mathbf x_k,\mathbf u_k)\)（\(\mathbf x\) 是状态，\(\mathbf u\) 是控制）：不能凭空横移，也不能瞬间改变速度。把"轨迹 + 动力学"变成一个有限维优化问题，叫作转录 (transcription)。主要有两条路：

    打靶法 (shooting)Shooting配点 / 多重打靶 (collocation)Collocation / multiple shooting
    变量Variables只有控制 \(\mathbf u_0,\dots,\mathbf u_{N-1}\)，状态由前向仿真算出Only controls \(\mathbf u_0,\dots,\mathbf u_{N-1}\); states come from forward simulation状态和控制一起 \((\mathbf x_k,\mathbf u_k)\)States and controls together, \((\mathbf x_k,\mathbf u_k)\)
    动力学Dynamics自动满足（隐含在仿真里）Satisfied automatically (inside the simulation)作为等式约束 \(\mathbf x_{k+1}-f(\mathbf x_k,\mathbf u_k)=\mathbf 0\)（"缺口 defect"）Equality constraints \(\mathbf x_{k+1}-f(\mathbf x_k,\mathbf u_k)=\mathbf 0\) (the "defects")
    矩阵结构Matrix structure变量少但稠密：每个状态依赖之前所有控制Few variables but dense: each state depends on all earlier controls变量多但稀疏（带状）：约束只连接相邻时刻More variables but sparse (banded): constraints couple only neighbouring steps
    长时域Long horizon不稳定系统上误差指数放大，条件很差Errors amplify exponentially on unstable systems; badly conditioned每个约束只涉及一步，不放大；初值可以直接给"想去的状态序列"Each constraint spans one step, no amplification; the initial guess can directly be a desired state sequence

    用一个最简单的不稳定系统 \(x_{k+1}=a\,x_k+u_k\)（\(a>1\) 时状态会自己发散，像倒立摆）看这两种做法在数值结构上的差别。上方是同一个小改动（第一个控制 \(u_0\) 加 \(\pm0.05\)）经 \(N\) 步前向仿真放大后的状态；下方是两种转录各自的雅可比矩阵：左边是打靶法的 \(\partial\mathbf x/\partial\mathbf u\)（下三角，稠密），右边是配点法的约束雅可比 \(\partial\mathbf c/\partial(\mathbf x,\mathbf u)\)（只有 3 条对角线）。

    So far the trajectory has been a geometric curve. A real robot must obey dynamics \(\mathbf x_{k+1}=f(\mathbf x_k,\mathbf u_k)\) (\(\mathbf x\) the state, \(\mathbf u\) the control): it cannot slide sideways from nowhere or change velocity instantly. Turning "trajectory + dynamics" into a finite-dimensional optimization problem is called transcription. There are two main routes:

    ShootingCollocation / multiple shooting
    VariablesOnly controls \(\mathbf u_0,\dots,\mathbf u_{N-1}\); states come from forward simulationStates and controls together, \((\mathbf x_k,\mathbf u_k)\)
    DynamicsSatisfied automatically (inside the simulation)Equality constraints \(\mathbf x_{k+1}-f(\mathbf x_k,\mathbf u_k)=\mathbf 0\) (the "defects")
    Matrix structureFew variables but dense: each state depends on all earlier controlsMore variables but sparse (banded): constraints couple only neighbouring steps
    Long horizonErrors amplify exponentially on unstable systems; badly conditionedEach constraint spans one step, no amplification; the initial guess can directly be a desired state sequence

    Take the simplest unstable system \(x_{k+1}=a\,x_k+u_k\) (for \(a>1\) the state diverges by itself, like an inverted pendulum) and see how the two differ in numerical structure. The upper plot shows the same small change (adding \(\pm0.05\) to the first control \(u_0\)) amplified through \(N\) forward steps; below are the Jacobians of the two transcriptions: on the left the shooting \(\partial\mathbf x/\partial\mathbf u\) (lower triangular, dense), on the right the collocation constraint Jacobian \(\partial\mathbf c/\partial(\mathbf x,\mathbf u)\) (just 3 diagonals).

    FIG 3打靶的指数放大 vs. 配点的带状结构Shooting's exponential amplification vs. collocation's band
    矩阵里每个小方格的颜色浓淡（不透明度）与元素绝对值的对数成正比（越淡越接近 0，空白为零）。可以看到打靶法的元素范围从 1 一直到 \(a^{N-1}\)，而配点法的所有非零元素都是 \(1\)、\(-a\)、\(-1\) 这样的数量级。右下读数里的条件数是真实算出来的：打靶法的 \(\Gamma^\top\Gamma\)（\(\Gamma=\partial\mathbf x/\partial\mathbf u\) 是左边的矩阵；把 \(\mathbf x\) 当作要跟踪的目标时的 Hessian），和配点法约束雅可比 \(C\) 的 \(CC^\top\)。 Each cell's colour strength (opacity) is proportional to the log of the entry's magnitude (fainter means nearer zero, blank means zero). Shooting's entries span from 1 up to \(a^{N-1}\), whereas every nonzero entry of the collocation Jacobian has the order of \(1\), \(-a\), \(-1\). The condition numbers in the readout are actually computed: \(\Gamma^\top\Gamma\) for shooting (\(\Gamma=\partial\mathbf x/\partial\mathbf u\) is the left matrix; this is the Hessian when \(\mathbf x\) is a tracking target) and \(CC^\top\) for the collocation constraint Jacobian \(C\).
    • 把 \(a\) 拖到 1.2、\(N\) 拖到 30：扇形张开到很大，打靶法的条件数爆炸，而配点法的条件数几乎不变。Drag \(a\) to 1.2 and \(N\) to 30: the fan opens enormously, shooting's condition number explodes, while collocation's barely changes.
    • 把 \(a\) 拖到 0.9（稳定系统）：扇形收拢。打靶法对稳定系统的问题要小得多，这就是为什么打靶法在短时域、稳定系统上依然很常用。Drag \(a\) to 0.9 (a stable system): the fan closes. Shooting is far less troublesome on stable systems, which is why it is still common for short horizons and stable systems.
    和动态规划的关系Relation to dynamic programming

    打靶法只以 \(\mathbf u\) 为变量，所以也可以用 6.12 节的动态规划来解：iLQR / DDP 就是"沿当前轨迹线性化动力学、二次化代价、再做一次 LQR 的 Riccati 反向递推"，是打靶法的一个特别高效的求解器。它的每次迭代代价是 \(O(N)\)，而不是稠密矩阵的 \(O(N^3)\)。配点法则交给通用的稀疏非线性规划求解器（如 IPOPT、SNOPT）。

    Shooting has only \(\mathbf u\) as variables, so it can also be solved by the dynamic programming of Lesson 6.12: iLQR / DDP is "linearize the dynamics along the current trajectory, quadratize the cost, then do a Riccati backward recursion as in LQR", a particularly efficient solver for shooting. Each iteration costs \(O(N)\), not the \(O(N^3)\) of a dense matrix. Collocation is handed to general sparse nonlinear-programming solvers (e.g. IPOPT, SNOPT).

    5再看数学The math

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 中对应的力会被高亮。Tip: hover the highlighted symbols in the formulas; the matching forces light up in FIG 1.

    从连续轨迹到有限维向量From a continuous trajectory to a finite vector

    连续问题是在函数空间里最小化泛函：

    The continuous problem minimizes a functional in function space:

    $$ \min_{\mathbf q(\cdot)}\ J[\mathbf q]=\int_0^T\Big(\tfrac12\|\dot{\mathbf q}(t)\|^2\ +\ c_{\rm obs}\big(\mathbf q(t)\big)\Big)\,dt\qquad\text{s.t.}\quad\mathbf q(0)=\mathbf s,\ \ \mathbf q(T)=\mathbf g $$

    取 \(N\) 个等间隔时刻，用差分近似导数 \(\dot{\mathbf q}_k\approx(\mathbf q_{k+1}-\mathbf q_k)/h\)，积分换成求和，得到 FIG 1、FIG 2 的有限维代价（\(h\) 吸收进权重里）：

    Take \(N\) equally spaced instants, approximate the derivative by a difference \(\dot{\mathbf q}_k\approx(\mathbf q_{k+1}-\mathbf q_k)/h\), and replace the integral by a sum to obtain the finite-dimensional cost of FIG 1 and FIG 2 (\(h\) absorbed into the weights):

    $$ F(\mathbf x)=\underbrace{\tfrac{w_s}{2}\|K\mathbf x+\mathbf k_0\|^2}_{\hl{smooth}{\text{smoothness}}}\ +\ \underbrace{\tfrac{w_o}{2}\sum_{k}\max\!\big(0,\ \varepsilon-d(\mathbf q_k)\big)^2}_{\hl{obs}{\text{obstacle}}},\qquad \mathbf x=(\mathbf q_1,\dots,\mathbf q_{N-1}) $$

    \(K\) 是一阶差分矩阵（带状，每行 \((-1,\ 1)\)），\(\mathbf k_0\) 来自固定的端点。把平滑项改成二阶差分 \((1,-2,1)\)，惩罚的就是加速度，轨迹更圆滑，但矩阵条件更差（约 \(N^4\)）。

    \(K\) is the first-difference matrix (banded, rows \((-1,\ 1)\)) and \(\mathbf k_0\) comes from the fixed endpoints. Use second differences \((1,-2,1)\) in the smoothness term and you penalize acceleration: rounder trajectories but a worse-conditioned matrix (about \(N^4\)).

    梯度与 CHOMP 的预条件The gradient and CHOMP's preconditioning

    代价的梯度是平滑项和障碍项两部分之和：

    The gradient of the cost is the sum of the smoothness and obstacle parts:

    $$ \nabla F=\hl{smooth}{w_s\big(A\mathbf x+K^\top\mathbf k_0\big)}\ +\ \hl{obs}{\nabla F_{\rm obs}},\qquad A=K^\top K,\qquad \big[\nabla F_{\rm obs}\big]_k=-w_o\big(\varepsilon-d_k\big)\,\nabla d(\mathbf q_k)\ \ (d_k\lt\varepsilon) $$

    在 \(\mathbf x\) 附近，要求步长 \(\boldsymbol\delta\) 使"线性化的代价 + 在度量 \(A\) 下的二次罚"最小：

    Near \(\mathbf x\), pick the step \(\boldsymbol\delta\) minimizing "linearized cost + a quadratic penalty in the metric \(A\)":

    $$ \boldsymbol\delta^\star=\arg\min_{\boldsymbol\delta}\ \nabla F^\top\boldsymbol\delta+\frac{\eta^{-1}}2\,\boldsymbol\delta^\top A\,\boldsymbol\delta\ =\ -\eta\,A^{-1}\nabla F $$

    普通梯度下降用的是欧氏度量（\(A\to I\)），在这个度量下"局部推一个路点"和"整条曲线变形"是同样"费力"的；换成 \(A\)，度量衡量的是轨迹的速度能量，平滑地改变整条曲线便宜得多，于是梯度被"沿轨迹抹开"。这就是 CHOMP（Covariant Hamiltonian Optimization for Motion Planning）里"covariant"的含义。因为 \(A\) 是带状矩阵，\(A^{-1}\mathbf v\) 的计算只需 \(O(N)\)（三对角求解）。

    Plain gradient descent uses the Euclidean metric (\(A\to I\)), under which "pushing one waypoint locally" and "deforming the entire curve" are equally "expensive". Replace it by \(A\) and the metric measures the trajectory's velocity energy: deforming the whole curve smoothly is much cheaper, so the gradient is "spread along the trajectory". That is the "covariant" in CHOMP (Covariant Hamiltonian Optimization for Motion Planning). Since \(A\) is banded, computing \(A^{-1}\mathbf v\) takes only \(O(N)\) (a tridiagonal solve).

    残差形式与 Gauss-NewtonThe residual form and Gauss-Newton

    令残差 \(\mathbf r=\big(\sqrt{w_s}(K\mathbf x+\mathbf k_0),\ \sqrt{w_o}\max(0,\varepsilon-d_k)\big)\)，则 \(F=\tfrac12\|\mathbf r\|^2\)，雅可比 \(J\) 的平滑部分是常数 \(\sqrt{w_s}K\)，障碍部分每个活跃路点一行：\(-\sqrt{w_o}\nabla d(\mathbf q_k)^\top\)。GN / LM 的一步就是解 \((J^\top J+\lambda D)\boldsymbol\delta=-J^\top\mathbf r\)，\(J^\top J\) 是 \(2\times2\) 块的带状矩阵（路点只与邻居耦合），所以 FIG 2 的蓝线只需几步。

    Let the residual be \(\mathbf r=\big(\sqrt{w_s}(K\mathbf x+\mathbf k_0),\ \sqrt{w_o}\max(0,\varepsilon-d_k)\big)\), so \(F=\tfrac12\|\mathbf r\|^2\). The smoothness part of the Jacobian \(J\) is the constant \(\sqrt{w_s}K\), and the obstacle part has one row per active waypoint: \(-\sqrt{w_o}\nabla d(\mathbf q_k)^\top\). One GN / LM step solves \((J^\top J+\lambda D)\boldsymbol\delta=-J^\top\mathbf r\), where \(J^\top J\) is a banded matrix of \(2\times2\) blocks (waypoints couple only to neighbours), which is why FIG 2's blue curve needs only a few steps.

    带动力学的转录Transcription with dynamics

    配点法的一个常用形式（梯形）。这里动力学写成连续时间的 \(\dot{\mathbf x}=f_c(\mathbf x,\mathbf u)\)（前面的 \(f\) 是离散一步的映射），在每个区间要求

    A common collocation form (trapezoidal). Here the dynamics are written in continuous time, \(\dot{\mathbf x}=f_c(\mathbf x,\mathbf u)\) (the \(f\) above was the discrete one-step map); on every interval require

    $$ \mathbf c_k=\mathbf x_{k+1}-\mathbf x_k-\frac h2\Big(f_c(\mathbf x_k,\mathbf u_k)+f_c(\mathbf x_{k+1},\mathbf u_{k+1})\Big)=\mathbf 0,\qquad k=0,\dots,N-1 $$

    问题成为"代价 + 一堆等式 / 不等式约束"的非线性规划，用 6.7 节的 Lagrange 乘子和 KKT 条件、再加上 SQP 或内点法求解。每个 \(\mathbf c_k\) 只含 \(\mathbf x_k,\mathbf x_{k+1}\)，约束雅可比是带状的（FIG 3 右）。

    The problem becomes a nonlinear program "cost plus equality / inequality constraints", solved with the Lagrange multipliers and KKT conditions of Lesson 6.7 plus SQP or interior-point methods. Each \(\mathbf c_k\) contains only \(\mathbf x_k,\mathbf x_{k+1}\), so the constraint Jacobian is banded (FIG 3, right).

    6机器人中的它：两连杆机械臂绕过障碍物In robotics: a two-link arm around an obstacle

    机械臂的轨迹住在关节空间里：路点 \(\mathbf q_k=(q_{1},q_{2})_k\) 是关节角。平滑项在关节空间里算（关节速度的平方和），障碍项在工作空间里算：沿两个连杆取一串采样点，用正运动学 \(\mathrm{FK}\) 得到它们的位置，再算到障碍物的距离。碰撞项对 \(\mathbf q\) 的梯度要穿过正运动学，等于一次雅可比（3.3、3.4 节）；下面直接用数值微分得到，求解器是 CHOMP 式的预条件下降。

    拖动起点和终点（这是末端执行器的位置，下面用解析逆运动学换成关节角）和障碍物。关节空间里的直线插值会让机械臂的某个连杆扫过障碍物；优化之后，机械臂的中间姿态被"推"开，末端沿一条绕开障碍物的路径运动。淡色的是中间 15 个路点上的姿态，琥珀色的是末端轨迹。

    An arm's trajectory lives in joint space: waypoints \(\mathbf q_k=(q_1,q_2)_k\) are joint angles. The smoothness term is computed in joint space (sum of squared joint velocities), while the obstacle term is computed in workspace: take a string of sample points along both links, get their positions by forward kinematics \(\mathrm{FK}\), and measure their distance to the obstacle. The collision term's gradient with respect to \(\mathbf q\) runs through forward kinematics, i.e. one Jacobian (Lessons 3.3, 3.4); below it is obtained by numerical differentiation, and the solver is CHOMP-style preconditioned descent.

    Drag the start and goal (these are end-effector positions, converted to joint angles by analytic inverse kinematics) and the obstacle. Straight-line interpolation in joint space would sweep one link through the obstacle; after optimization the arm's intermediate poses are "pushed" away and the end effector follows a path around the obstacle. The faint arms are the poses at the 15 intermediate waypoints and the amber curve is the end-effector path.

    FIG 4关节空间的轨迹优化Trajectory optimization in joint space
    关节角 \(q_1,q_2\) 随路点序号joint angles \(q_1,q_2\) vs. waypoint index
    绿色臂是起始姿态，蓝色臂是目标姿态，浅色的是中间 15 个路点的姿态（与障碍物间隙为负的姿态会变红）。障碍项检查两根连杆上共 7 个采样点。右图：两个关节角随路点序号的变化，虚线是初始的线性插值。 The green arm is the start pose, the blue arm the goal pose, and the faint ones are the 15 intermediate waypoints (poses with negative clearance turn red). The obstacle term checks 7 sample points along the two links. Right: the two joint angles vs. waypoint index, dashed lines show the initial linear interpolation.
    • 按"运行"：中间姿态不再与障碍物相交（最小间隙变成非负）。Press "Run": the intermediate poses stop intersecting the obstacle (minimum clearance becomes nonnegative).
    • 把障碍物拖到别处再运行：同样的算法给出另一条轨迹。Drag the obstacle elsewhere and run again: the same algorithm gives another trajectory.
    • 把障碍半径加大到 0.8 以上再运行：机械臂的中间姿态被推得更远，仍然可以找到无碰撞的轨迹，只是代价（平滑项）更高。Raise the obstacle radius above 0.8 and run: the intermediate poses are pushed farther, a collision-free trajectory is still found, only at a higher (smoothness) cost.
    真实系统里In real systems

    MoveIt 里的 CHOMP、TrajOpt（序贯凸化，每步把非凸的碰撞约束线性化后解一个 QP，6.8 节）和 STOMP（用 6.11 节的采样法，无需梯度）都是这类求解器；它们的输入通常是 RRT 给的粗糙路径，作为初值。实际中还要加：关节位置、速度、加速度限制（不等式约束）、用有符号距离场 (SDF) 代替解析的圆做碰撞查询、对时间也做优化（时间最优参数化）。对双足、四足机器人，状态里有接触力，用配点法把接触序列和动力学一并写成约束（如 Drake 的 DIRCON），就成了接触隐式轨迹优化。

    CHOMP and TrajOpt in MoveIt (sequential convexification: linearize the nonconvex collision constraint at each step and solve a QP, Lesson 6.8) and STOMP (using the sampling of Lesson 6.11, no gradients) are all solvers of this kind; their inputs are usually a rough RRT path used as the initial guess. In practice one adds joint position, velocity and acceleration limits (inequality constraints), uses a signed distance field (SDF) instead of an analytic circle for collision queries, and optimizes time as well (time-optimal parameterization). For biped and quadruped robots the state contains contact forces, and collocation writes the contact sequence together with the dynamics as constraints (e.g. DIRCON in Drake), giving contact-implicit trajectory optimization.

    7常见误区Common pitfalls

    误区 1：忽视局部最小，不管初值Pitfall 1: ignoring local minima and the initial guess

    FIG 1 的任务 3 已经展示：初值从障碍物哪一边出发，就收敛到哪一边。梯度法不会"翻越"障碍物。常见做法：用 RRT 等全局规划器给出一条无碰撞的路径作初值，再用优化把它变好；或者用多个不同的初值并行优化，取最好的（也可以让 6.11 节的采样法来探索拓扑）。

    Task 3 of FIG 1 already showed it: whichever side of the obstacle the initialization starts on is the side it converges to. Gradient methods do not "hop over" obstacles. The usual remedy: initialize from a collision-free path given by a global planner such as RRT and let optimization improve it; or run several different initializations in parallel and keep the best (or let the sampling of Lesson 6.11 explore the topology).

    误区 2：只在路点上检查碰撞（"穿墙"）Pitfall 2: checking collisions only at waypoints ("tunnelling")

    相邻路点都在障碍物外面，不等于它们之间的连线也在外面：薄障碍物或大步长时轨迹会从中间穿过去。对策：加密路点；对每一段做连续碰撞检测（扫掠体积）；把障碍物的安全边距 \(\varepsilon\) 设得比"路点间距"大。

    Both waypoints being outside an obstacle does not mean the segment between them is: with thin obstacles or large spacing the trajectory tunnels right through. Remedies: use more waypoints; do continuous collision checking per segment (swept volumes); make the safety margin \(\varepsilon\) larger than the waypoint spacing.

    误区 3：权重比例失衡Pitfall 3: unbalanced weights

    \(w_o\) 太小，障碍项被平滑项压过，轨迹穿过障碍物（FIG 2 的任务 3）；太大则问题变得病态，梯度法几乎不动，GN 的 \(J^\top J\) 条件数变差。更稳妥的做法是把障碍当作约束而不是罚函数（SQP / 内点法 / 增广 Lagrangian，6.7 节），这样碰撞与否是硬性的。

    With \(w_o\) too small the smoothness term overwhelms the obstacle term and the trajectory runs through the obstacle (task 3 of FIG 2); too large and the problem is ill-conditioned: gradient methods barely move and the condition number of GN's \(J^\top J\) worsens. A sturdier approach treats obstacles as constraints rather than penalties (SQP / interior point / augmented Lagrangian, Lesson 6.7), so collision-free is a hard requirement.

    误区 4：轨迹不满足动力学，却当成可执行Pitfall 4: calling a kinematic trajectory executable

    本课 FIG 1、FIG 2 和 FIG 4 的"轨迹"只有几何。要执行，必须按机器人的速度、加速度、力矩限制重新参数化时间，或者在优化里直接加入动力学约束（FIG 3 的转录）。不稳定系统（倒立摆、四旋翼）还要用反馈（LQR、MPC）去跟踪优化出的轨迹，而不是开环播放。

    The "trajectories" in FIG 1, FIG 2 and FIG 4 are only geometric. To execute one must re-time it to the robot's velocity, acceleration and torque limits, or put the dynamics directly into the optimization (the transcription of FIG 3). Unstable systems (inverted pendulum, quadrotor) also need feedback (LQR, MPC) to track the optimized trajectory rather than replaying it open loop.

    8小练习Check yourself

    1. 二维平面轨迹，起点与终点固定，中间 29 个路点（\(N=30\) 段）。优化问题有多少个变量？1. A planar 2D trajectory with fixed start and goal and 29 intermediate waypoints (\(N=30\) segments). How many variables does the optimization have?
    每个路点有 \(x,y\) 两个坐标，\(29\times2=58\)。起点和终点是固定的，不是变量。Each waypoint has two coordinates \(x,y\): \(29\times2=58\). The start and goal are fixed, not variables.
    2. 只有"速度平方和"这个平滑项、没有障碍物，固定端点的最优轨迹是什么？2. With only the "sum of squared velocities" smoothness term and no obstacles, what is the optimal trajectory for fixed endpoints?
    令平滑项的梯度为零：每个路点位于邻居的中点，即 \(\mathbf q_{k+1}-\mathbf q_k\) 对所有 \(k\) 相等，这正是等间距的直线（也是一个严格凸二次问题，所以唯一）。Zero the gradient of the smoothness term: each waypoint is at its neighbours' midpoint, i.e. \(\mathbf q_{k+1}-\mathbf q_k\) is the same for all \(k\), which is the equally spaced straight line (a strictly convex quadratic, hence unique).
    3. 系统 \(x_{k+1}=1.2\,x_k+u_k\)，用打靶法优化 \(u_0,\dots,u_{19}\)。改变 \(u_0\) 一个微小量 \(\delta\)，对 \(x_{20}\) 的影响约是？3. The system \(x_{k+1}=1.2\,x_k+u_k\) is optimized by shooting over \(u_0,\dots,u_{19}\). A tiny change \(\delta\) in \(u_0\) changes \(x_{20}\) by about?
    \(x_1=1.2x_0+u_0\)，之后 \(x_{20}=1.2^{19}x_1+\cdots\)，所以 \(\partial x_{20}/\partial u_0=1.2^{19}\approx31.9\)。这就是打靶法的雅可比里出现 \(a^{N-1}\) 的原因：长时域上不稳定系统的灵敏度指数增长。\(x_1=1.2x_0+u_0\), and then \(x_{20}=1.2^{19}x_1+\cdots\), so \(\partial x_{20}/\partial u_0=1.2^{19}\approx31.9\). That is why \(a^{N-1}\) appears in shooting's Jacobian: on a long horizon an unstable system's sensitivity grows exponentially.
    4. CHOMP 把梯度乘上 \(A^{-1}\)（\(A=K^\top K\) 是平滑项的 Hessian），为什么通常比普通梯度下降快得多？4. CHOMP multiplies the gradient by \(A^{-1}\) (\(A=K^\top K\), the Hessian of the smoothness term). Why is that usually far faster than plain gradient descent?
    平滑项的 Hessian 就是 \(w_sA\)，条件数随 \(N\) 增大像 \(N^2\)；\(A^{-1}\nabla F\) 是预条件（对平滑项就是 Newton 步），所以信息不必一格一格地沿轨迹传播。它仍然是局部方法，不能避开局部最小；变量数不变；仍然需要梯度。The Hessian of the smoothness term is \(w_sA\) with a condition number growing like \(N^2\); \(A^{-1}\nabla F\) is preconditioning (a Newton step for the smoothness part), so information need not crawl cell by cell along the trajectory. It is still a local method (does not avoid local minima), the variable count is unchanged, and it still needs the gradient.
    5. 只在路点上检查与障碍物的距离，会有什么风险？5. What is the risk of checking distance to obstacles only at the waypoints?
    真实轨迹在路点之间是连续的，而代价只"看"到了离散的点。需要更密的路点、扫掠体积的连续碰撞检测，或更大的安全边距。The real trajectory is continuous between waypoints while the cost only "sees" discrete points. Use denser waypoints, swept-volume continuous collision checking, or a larger safety margin.

    9延伸阅读Further reading

    • M. Kelly, An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation, SIAM Review, 2017 — 打靶与配点的最佳入门，带小例子the best entry point to shooting and collocation, with small worked examples
    • J. T. Betts, Practical Methods for Optimal Control and Estimation Using Nonlinear Programming, 2nd ed., SIAM, 2010 — 直接转录的权威专著the authoritative monograph on direct transcription
    • N. Ratliff, M. Zucker, J. A. Bagnell & S. Srinivasa, CHOMP: Gradient Optimization Techniques for Efficient Motion Planning, ICRA 2009 — FIG 2、FIG 4 中预条件下降的出处the source of the preconditioned descent in FIG 2 and FIG 4
    • J. Schulman et al., Motion Planning with Sequential Convex Optimization and Convex Collision Checking, International Journal of Robotics Research, 2014 — TrajOptTrajOpt
    • M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor & S. Schaal, STOMP: Stochastic Trajectory Optimization for Motion Planning, ICRA 2011 — 用采样替代梯度的轨迹优化trajectory optimization with sampling instead of gradients
    • J. Nocedal & S. J. Wright, Numerical Optimization, 2nd ed., Springer, 2006 — SQP 与内点法，约束轨迹优化的求解器基础SQP and interior-point methods: the solver foundations for constrained trajectory optimization