Part I · 第 6 章 优化 · 6.13

轨迹优化初探

规划器找到一条能走的路，轨迹优化把它变成一条好走的路。把整条轨迹拆成一串路点，当作一个长长的向量，再写一个代价：平滑、避开障碍、到达目标，剩下的就是前面学过的优化：梯度、预条件、Gauss-Newton。机械臂绕过障碍物、四足机器人的步态、无人机的穿越飞行，都从这个朴素的想法出发。

约 30 分钟 前置：6.3 梯度下降 · 6.6 Newton、Gauss-Newton 与 LM · 6.9 非线性最小二乘 4 个交互

    1为什么机器人需要它

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

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

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

    2直觉：一条橡皮筋，被障碍物推开

    在起点 \(\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\)）。合力为零的轨迹，就是最优轨迹。

    FIG 1橡皮筋：平滑的拉力与障碍的推力
    红色圆盘是障碍物（可拖动），虚线圈是安全边距。小圆点是可以拖动的路点，起点（左）与终点（右）也可以拖。"下降"按钮用小步长的梯度下降让整条轨迹沿合力方向流动：它流到哪里，合力就缩到零。
    试一试
    • 把一个路点拖进障碍物的边距里：红色推力出现，它的大小和 \((\varepsilon-d)\) 成正比。
    • 按"下降"，让轨迹自己找到平衡位置；读出合力的最大值（应该很小）。
    • 先把所有路点手动拖到障碍物上方，下降一次；再重置，把它们拖到下方，再下降。两次得到不同的轨迹，代价也不同：这就是局部最优。
    核心洞见

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

    3动手玩：三种求解器绕障碍物

    把路点增加到 \(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 用阻尼的接受 / 拒绝。拖动障碍物、起点、终点，改变权重，看三者的反应。

    FIG 2轨迹优化的三种求解器
    总代价 \(F\)（对数）与迭代次数
    灰色虚线是初始的直线轨迹。三条彩色曲线是三种求解器当前的轨迹（红：梯度下降，绿：预条件，蓝：Gauss-Newton）。读数里每种方法给出代价的两个部分（平滑 / 障碍）和轨迹到障碍物的最小间隙（负数表示进入了障碍物）。三种方法不一定收敛到同一个局部最优。
    • 按"运行"：预条件和 Gauss-Newton 在几次迭代里就让代价降到很低，而 梯度下降 还在慢慢爬。
    • 运行结束后，把一个障碍物拖到当前轨迹上，再按"运行（保留轨迹）"：轨迹从现在的位置出发重新避让（热启动）。
    • 把障碍权重 \(w_o\) 调到很小：轨迹径直穿过障碍物（最小间隙为负）。再调大：轨迹被推得越来越远。

    4加上动力学：打靶法与配点法

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

    打靶法 (shooting)配点 / 多重打靶 (collocation)
    变量只有控制 \(\mathbf u_0,\dots,\mathbf u_{N-1}\)，状态由前向仿真算出状态和控制一起 \((\mathbf x_k,\mathbf u_k)\)
    动力学自动满足（隐含在仿真里）作为等式约束 \(\mathbf x_{k+1}-f(\mathbf x_k,\mathbf u_k)=\mathbf 0\)（"缺口 defect"）
    矩阵结构变量少但稠密：每个状态依赖之前所有控制变量多但稀疏（带状）：约束只连接相邻时刻
    长时域不稳定系统上误差指数放大，条件很差每个约束只涉及一步，不放大；初值可以直接给"想去的状态序列"

    用一个最简单的不稳定系统 \(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 条对角线）。

    FIG 3打靶的指数放大 vs. 配点的带状结构
    矩阵里每个小方格的颜色浓淡（不透明度）与元素绝对值的对数成正比（越淡越接近 0，空白为零）。可以看到打靶法的元素范围从 1 一直到 \(a^{N-1}\)，而配点法的所有非零元素都是 \(1\)、\(-a\)、\(-1\) 这样的数量级。右下读数里的条件数是真实算出来的：打靶法的 \(\Gamma^\top\Gamma\)（\(\Gamma=\partial\mathbf x/\partial\mathbf u\) 是左边的矩阵；把 \(\mathbf x\) 当作要跟踪的目标时的 Hessian），和配点法约束雅可比 \(C\) 的 \(CC^\top\)。
    • 把 \(a\) 拖到 1.2、\(N\) 拖到 30：扇形张开到很大，打靶法的条件数爆炸，而配点法的条件数几乎不变。
    • 把 \(a\) 拖到 0.9（稳定系统）：扇形收拢。打靶法对稳定系统的问题要小得多，这就是为什么打靶法在短时域、稳定系统上依然很常用。
    和动态规划的关系

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

    5再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 中对应的力会被高亮。

    从连续轨迹到有限维向量

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

    $$ \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\) 吸收进权重里）：

    $$ 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\)）。

    梯度与 CHOMP 的预条件

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

    $$ \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\) 下的二次罚"最小：

    $$ \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)\)（三对角求解）。

    残差形式与 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 的蓝线只需几步。

    带动力学的转录

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

    $$ \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 右）。

    6机器人中的它：两连杆机械臂绕过障碍物

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

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

    FIG 4关节空间的轨迹优化
    关节角 \(q_1,q_2\) 随路点序号
    绿色臂是起始姿态，蓝色臂是目标姿态，浅色的是中间 15 个路点的姿态（与障碍物间隙为负的姿态会变红）。障碍项检查两根连杆上共 7 个采样点。右图：两个关节角随路点序号的变化，虚线是初始的线性插值。
    • 按"运行"：中间姿态不再与障碍物相交（最小间隙变成非负）。
    • 把障碍物拖到别处再运行：同样的算法给出另一条轨迹。
    • 把障碍半径加大到 0.8 以上再运行：机械臂的中间姿态被推得更远，仍然可以找到无碰撞的轨迹，只是代价（平滑项）更高。
    真实系统里

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

    7常见误区

    误区 1：忽视局部最小，不管初值

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

    误区 2：只在路点上检查碰撞（"穿墙"）

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

    误区 3：权重比例失衡

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

    误区 4：轨迹不满足动力学，却当成可执行

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

    8小练习

    1. 二维平面轨迹，起点与终点固定，中间 29 个路点（\(N=30\) 段）。优化问题有多少个变量？
    每个路点有 \(x,y\) 两个坐标，\(29\times2=58\)。起点和终点是固定的，不是变量。
    2. 只有"速度平方和"这个平滑项、没有障碍物，固定端点的最优轨迹是什么？
    令平滑项的梯度为零：每个路点位于邻居的中点，即 \(\mathbf q_{k+1}-\mathbf q_k\) 对所有 \(k\) 相等，这正是等间距的直线（也是一个严格凸二次问题，所以唯一）。
    3. 系统 \(x_{k+1}=1.2\,x_k+u_k\)，用打靶法优化 \(u_0,\dots,u_{19}\)。改变 \(u_0\) 一个微小量 \(\delta\)，对 \(x_{20}\) 的影响约是？
    \(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}\) 的原因：长时域上不稳定系统的灵敏度指数增长。
    4. CHOMP 把梯度乘上 \(A^{-1}\)（\(A=K^\top K\) 是平滑项的 Hessian），为什么通常比普通梯度下降快得多？
    平滑项的 Hessian 就是 \(w_sA\)，条件数随 \(N\) 增大像 \(N^2\)；\(A^{-1}\nabla F\) 是预条件（对平滑项就是 Newton 步），所以信息不必一格一格地沿轨迹传播。它仍然是局部方法，不能避开局部最小；变量数不变；仍然需要梯度。
    5. 只在路点上检查与障碍物的距离，会有什么风险？
    真实轨迹在路点之间是连续的，而代价只"看"到了离散的点。需要更密的路点、扫掠体积的连续碰撞检测，或更大的安全边距。

    9延伸阅读

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