🔢 mathematics

Square Root Gauss-Newton iLQR

本文提出了一种数值鲁棒且算法简单的平方根高斯-牛顿型 iLQR 公式，该公式利用了代价函数的加权最小二乘结构，从而能够基于单次 QR 分解进行后向传递，进而提升了条件数和稳定性，特别是在用于约束轨迹优化的增广拉格朗日框架内。

原作者： Maximilian Haas-Heger, Jur van den Berg

发布于 2026-09-21

📖 1 分钟阅读🧠 深度阅读

原作者： Maximilian Haas-Heger, Jur van den Berg

原始论文采用 CC BY 4.0 许可（http://creativecommons.org/licenses/by/4.0/）。 ✨ 这是对下方论文的AI生成解释。它不是由作者撰写或认可的。如需技术准确性，请参阅原始论文。阅读完整免责声明

机器人、自动驾驶汽车和航天器都面临着一个共同的基本挑战：它们必须在遵守物理定律并避开障碍物的前提下，从一个地方移动到另一个地方。为了实现这一目标，它们依赖于一种被称为“轨迹优化”的数学过程，这就像是一个功能强大的导航员。这个导航员通过测试无数种速度和方向的变化，不断计算出最佳路径，寻找消耗能量或时间最少的路线。当世界简单且可预测时，标准的数学工具可以完美地解决这个谜题。但现实世界是混乱的。机器人有复杂的关节，汽车会在湿滑的道路上打滑，而航天器则必须躲避碎片。在这些情况下，数学变得非线性且难以处理，需要计算机进行一系列有根据的猜测、进行改进并不断重复这一过程，直到找到一个可行的解。这种迭代过程是现代运动规划的支柱，使机器能够执行诸如组装零件或在其他行星上着陆等精细任务。

然而，随着这些机器变得越来越强大，它们被要求在更严格的规则下运行。例如，机械臂可能需要避免撞到人的手，或者无人机可能需要保持在狭窄的通道内。为了强制执行这些规则，计算机会在路径接近违反约束时，在其计算中加入沉重的惩罚项。虽然这迫使机器人遵守规则，但也创造了一个隐藏的危险：计算机用于寻找路径的数值可能会变得差异巨大，以至于机器失去了区分它们的能力。这就像是试图在站在一座大山旁边时测量一根头发的宽度；微小的细节会淹没在巨大的规模噪声之中。当这种情况发生时，计算机的计算会变得不稳定，机器人可能无法找到路径，或者更糟的是，计算出一个在纸面上看起来很完美但在物理上却无法实现的路径。

在最近的一项研究中，来自 Waabi Innovation Inc. 的研究人员 Maximilian Haas-Heger 和 Jur van den Berg 解决了这种数值不稳定的问题。他们专注于一种被称为“迭代线性二次型调节器”（iLQR）的特定算法，该算法广泛用于规划这些复杂的路径。团队发现，通过改变算法处理其内部数学的方式，可以使其在不降低速度的情况下显著提高鲁棒性。他们的工作围绕着一种称为“平方根”形式的技术展开。在标准计算中，算法会追踪一张大型表格，代表每一种可能的移动成本。当约束变得紧密时，这张表格就变得难以读取。研究人员意识到，与其追踪完整的表格，不如追踪一个更简单的三角形版本，这就像是只保留食谱中的核心原料，而不是整个储藏室。

他们成功的关键在于简化问题的方式。他们将机器人的目标视为误差的加权和，类似于摄影师通过调整特定设置来尽量减少照片模糊程度。通过使用一种被称为“高斯-牛顿法”（Gauss-Newton）的方法（该方法用直线近似复杂的曲线），他们确保了问题的数学结构保持稳定。这使得他们可以用一种更轻量、更精确的操作——称为“QR 分解”的操作，来取代沉重且不稳定的计算。可以将这种操作想象成一种将杂乱的数据堆整理成整齐有序的堆叠的方法，其中每一部分都完美契合，没有任何间隙或重叠。这种新方法允许计算机即使在违反规则的惩罚变得极其巨大时，也能保持其精度。

研究人员在一个具有挑战性的场景中测试了他们的新方法，涉及两个协作搬运刚性物体的机械臂。这两个机械臂必须将物体从起点移动到终点，同时保持它们的抓取距离固定，这一约束要求计算机平衡多种相互竞争的需求。他们运行了模拟实验，将违反距离规则的惩罚增加到了极端水平，远超标准计算机通常能处理的范围。在这些测试中，传统方法开始失效，随着惩罚的增长，它逐渐失去精度并最终产生荒谬的结果。然而，新的平方根方法却稳住了阵脚。即使在惩罚如此之大以至于标准方法已经完全崩溃的情况下，它仍能持续产生准确的路径。事实上，在标准计算机算术中，当旧方法几乎失去所有精度时，新方法仍能保持六到七位有效数字的精度。

这种改进不仅仅是理论上的胜利，它直接转化为现实世界机器的可靠性。研究表明，新方法可以驱动优化过程走得更远，使机器人能够以一种以前无法实现的紧密程度来满足约束。如果说标准方法在被要求达到百万分之一的精度时可能会放弃，那么新方法可以将这种精度推向百亿分之一。研究人员指出，这种精度的提升几乎没有额外的计算成本，增加的求解总时间不到 3%。这意味着工程师可以将新方法作为旧方法的“掉入式替换”（drop-in replacement），瞬间让他们的机器人能够处理更困难、更高风险的任务。

论文还探讨了为什么这种特定的改进此前未能得到广泛应用。虽然类似的“平方根”技术在卡尔曼滤波（Kalman filter）等导航系统中已使用了数十年，但由于其数学逻辑更为复杂，很难应用于轨迹优化。以往尝试创建 iLQR 平方根版本的尝试要么过于复杂难以使用，要么在数值表现上较差。作者认为，他们的方法之所以不同，是因为他们利用了问题的特定结构来简化数学，将每一步简化为一个优雅的单一操作。他们认为，对于任何在严格约束下规划路径的机器人来说，没有理由不使用这种更稳定的方法，就像工程师不会选择一个不稳定的导航系统给航天器使用一样。

最终，这项工作证明了机器人的运动极限往往不是由物理定律定义的，而是由计算机计算它们的能力极限定义的。通过改进这些计算的执行方式，研究人员消除了一个重要的障碍，使机器能够以更高的信心在更复杂、更危险的环境中导航。这项研究证实，借助正确的数学工具，我们可以推向自主系统所能达到的边界，确保它们即使在风险最高的情况下也能保持精准与可靠。

技术摘要：平方根高斯-牛顿 iLQR

问题表述

本文研究了离散时间动力系统的非线性轨迹优化问题。其目标是寻找一系列控制输入 $u_0, \dots, u_{\ell-1}$ ，在最小化总代价函数的同时，将系统状态 $x_{t+1} = f_t(x_t, u_t)$ 从初始状态引导至目标状态。作者特别关注可以表示为加权平方残差和（高斯-牛顿结构）的代价函数：
$c_t(x_t, u_t) = \frac{1}{2} r_t(x_t, u_t)^T W_t r_t(x_t, u_t)$
其中 $r_t$ 是残差向量， $W_t$ 是正定权重矩阵。这种形式在机器人技术和自动驾驶领域非常常见，特别是在通过增广拉格朗日框架处理约束时。在这种框架下，使用巨大的惩罚参数 $\mu$ 来强制执行约束，这会显著降低优化问题的条件数，导致标准求解器出现数值不稳定。

方法论

标准高斯-牛顿 iLQR

迭代线性二次调节器（iLQR）通过围绕名义轨迹迭代地对动力学进行线性化并对代价函数进行二次化来解决此问题。在标准的高斯-牛顿变体中，代价函数的处理方式是对残差向量 $r_t$ 进行线性化，而不是对代价函数进行完整的二阶展开。这产生了一个增广海森矩阵（Hessian matrix） $C_t = R_t^T W_t R_t$ ，根据构造，该矩阵是半正定的（PSD）。算法通过后向传递（计算最优反馈增益和代价函数随动矩阵）和前向传递（使用新策略模拟系统）进行。

平方根表述

本文的核心贡献是一种平方根高斯-牛顿 iLQR算法。该算法不再传播完整的代价函数随动海森矩阵 $S_t$ ，而是传播其上三角 Cholesky 分解因子 $\sqrt{S_t}$ （其中 $S_t = \sqrt{S_t}^T \sqrt{S_t}$ ）。

其推导基于两个关键见解：

全局 PSD 性质： 由于高斯-牛顿结构，不仅代价函数随动矩阵的海森块是半正定的，整个增广代价函数随动矩阵 $Q_t$ 也是半正定的。
QR 分解的高效性： 矩阵 $Q_t$ 可以表示为 $M_t^T M_t$ ，其中 $M_t$ 是由权重平方根、残差雅可比矩阵以及下一时刻代价函数随动因子的平方根构建而成的。

后向传递被简化为对矩阵 $M_t$ 进行单次 QR 分解：
$M_t = \begin{bmatrix} \sqrt{W_t} R_t \\ \sqrt{S_{t+1}} F_t \end{bmatrix}$
通过执行 QR 分解（ $M_t = QU$ ）得到上三角因子 $U$ ，该因子直接编码了 $Q_t$ 的 Cholesky 分解因子。反馈增益 $L_t$ 和下一个代价函数随动因子 $\sqrt{S_t}$ 直接从 $U$ 的分块中提取。这种方法避免了显式构建平方矩阵（如 $Q_t$ 或 $S_t$ ），从而防止了舍入误差的放大。

对于使用增广拉格朗日法的约束问题，约束雅可比矩阵和惩罚项只需作为额外的行块附加到 $M_t$ 中，即可保持相同的计算结构。

核心贡献

数值稳定性： 通过传播 Cholesky 因子而非完整矩阵，该算法有效地使可靠有效数字增加了一倍。Cholesky 因子的条件数随 $\sqrt{\mu}$ 缩放，而非随 $\mu$ 缩放，这直接缓解了由约束优化中巨大惩罚参数导致的数值退化。
算法简洁性： 不同于以往复杂的 iLQR 或微分动态规划（DDP）平方根表述（那些方法要么算法复杂，要么需要多次分解和秩一更新），本方法将每次后向传递步骤简化为单次 QR 分解。
计算效率： 平方根表述带来的开销极小。其计算复杂度对于依赖于残差维度的领先项最多增加一个 2 倍的系数（即 QR 分解相对于正规方程的经典代价）。在实际场景中（例如双臂协作操作），总计算时间增加不到 3%。
对奇异性的鲁棒性： 该方法通过构造保证了数值上的半正定性。当海森矩阵因舍入误差变得不定时，标准求解器可能会完全失效，而平方根变体则表现出平滑的降级，允许基于三角因子对角线的数值检查进行健康检测。

实验结果

作者在涉及两个三连杆机械臂协同搬运物体的约束轨迹优化问题上验证了该方法。

精度对比： 在针对高精度参考值（50 位小数）进行测试时，标准 iLQR（使用双精度 f64）每增加一个数量级的惩罚参数 $\mu$ ，大约会损失一位有效数字。在 $\mu = 10^{16}$ 时，标准方法完全失效。相比之下，平方根 f64 变体即使在 $\mu = 10^{20}$ 时仍能保持 6 到 7 位有效数字。
单精度 (f32)： 其优势在单精度下更为显著。标准 f32 求解器在 $\mu = 10^7$ 时失效，而平方根 f32 求解器在 $\mu = 10^{15}$ 时仍能保持功能，提供了额外 5 到 6 个数量级的稳定性。
收敛性： 在完整的增广拉格朗日循环中，平方根求解器成功从 24 条随机初始轨迹收敛至 $10^{-8}$ 的约束容差，而标准求解器在 $10^{-7}$ 时便已失败。平方根方法并未找到“更好”的最优解，但它允许外层循环在数值噪声导致后向传递失效之前，将约束收紧得更严。

意义与主张

论文声称，对于可以表述为加权平方和代价（涵盖许多机器人应用的广泛类别）的轨迹优化问题，平方根高斯-牛顿 iLQR 应该是优于标准 iLQR 的默认选择。作者类比了平方根卡尔曼滤波在估计领域的广泛应用，指出估计与控制的对偶性质意味着类似的收益。

其意义在于优雅与鲁棒性的结合。该方法消除了在标准 iLQR 中通常需要为了维持正定性而采用的启发式正则化技术（如 Levenberg-Marquardt 阻尼）。作者断言，在应用该高斯-牛顿结构的生产系统中，使用这种表述“没有任何缺点”，因为它提供了卓越的数值调节能力，且计算开销微乎其微。论文总结道，只要问题结构允许高斯-牛顿近似，平方根方法在估计领域的成功也应延伸到轨迹优化领域。