各种优化器

本文以做motion planner/IK solver的视角写作。

优化变量 $q$张成的空间上，由cost function定义出十分崎岖的landscape，我们如何在上面行走，不要被困在局部极值点，直抵全局最优的洼地？这是优化器要解决的问题。 更可怕的是，在这个landscape上一片漆黑，我们看不到四面八方别的地方的地形是高是低，只能感受到脚下的坡度（局部梯度）。

走动的策略：优化方法

有很多中优化方法，适用于不同的问题。我们从最简单的开始考虑。 最好的情形莫过于我们预先知道一些关于这片崎岖landscape的信息，并且这信息告诉我们，这片landscape具有某种全局性质，当你到达一个局部极值点，就一定到达了全局最优点！也就是说，你再也不用担心自己掉进局部最优了！

这是怎么一种地形呢？我们称之为“凸”的。简单来说，当cost function是凸的，你可以确信，任意局部极值点一定是全局最优解，如果严格凸，全局最优解还是唯一的。 更爽的情况是，这个地形还具有某种“刚性”，即，它的全局性质由某些局部性质所唯一确定。那就再也不用害怕一片漆黑了，看到了局部的性质，就可以推导出整张地形图，然后一步到位走到最优点！

具有这种刚性的一种地形是 二次函数。 在二次函数所构造的地形上找最优点，这种最最简单最最基础的问题，称为 Quadratic Planning(QP, 二次规划)问题。

现在我们已经学会了一种最简单的步伐，下面可以走向更复杂的地形了。

首先到来的是一种看起来和 quadratic funcion很像的地形：非线性最小二乘。 其优化的目标函数通常定义为残差向量 $r(q)$ 的二范数平方： \(E(q) = \frac{1}{2} \Vert{}r(q)\Vert{}^2\) 注意到，目标函数依然是某个东西的二范数平方（所谓二次函数），但是某个东西却是关于我们的优化变量非线性的，这就好你原本自己亲自在这个二次函数大盆地里漫步，现在变成操纵一个复杂的机械结构在盆地上走【】 好消息是（你最好希望这样），你知道当你给出一个小小的控制信号的时候，调皮小车不会一下跑太远，也就是说 $\frac{\partial r}{\partial q}$ 不会特别大，也就意味着，你可以小心地让小车一点点蹭着走，虽然会慢一点，但你清楚，二次大盆地总是能够下到底的，唯一要重新考虑的就是调皮小车的动作跟控制信号的关系。 你总是可以通过小小的扰动知道在当前信号附近，小车的近似线性的行动方式 $\delta_r=J\delta_q$, 但是当控制信号变化大时，它会怎么动你可就不知道了。 关于如何选择控制小车的策略，有一些好方法。

梯度下降法

SQP

sequential Quadratic Planning 有约束非线性优化。

高斯-牛顿法

Levenberg-Marquardt (LM)

它通过引入阻尼因子 $\lambda$，在梯度下降法（收敛稳健但速度慢）与高斯-牛顿法（局部收敛极快但易发散）之间进行动态插值。

• 步骤 1–3：近似 Hessian 矩阵并求解增量 $\boldsymbol{\delta}$

  利用高斯-牛顿近似，取 Hessian 矩阵 $H \approx J_k^T J_k$。加入阻尼项 $\lambda_k I$ 后，求解正规方程（Normal Equation） \((J_k^T J_k + \lambda_k I) \boldsymbol{\delta} = -g_k\)

  • 当 $\lambda_k \to 0$ 时，方程退化为高斯-牛顿法；
  • 当 $\lambda_k \to \infty$ 时，方程趋近于步长为 $\frac{1}{\lambda_k}$ 的最速下降法。
• 步骤 4–5：应用增量与评估新状态 尝试更新参数 $q^+ = q_k + \boldsymbol{\delta}$，并重新计算新状态下的残差 $r^+$、能量/误差 $E^+$、雅可比矩阵 $J^+$ 及梯度 $g^+$。

• 步骤 6–7：信赖域比例（Trust-Region Ratio）评估 通过信赖域因子 $\rho_{\text{trust}}$ 衡量二次模型对真实误差下降量的预测精度： \(\rho_{\text{pred}} = \frac{1}{2} \boldsymbol{\delta}^T (\lambda_k \boldsymbol{\delta} - g_k)\)

  \[\rho_{\text{trust}} = \frac{E_k - E^+}{\rho_{\text{pred}} + \epsilon}\]

  分子 $E_k - E^+$ 为实际下降量，分母为二阶模型预测的下降量（$\epsilon$ 用于防止除零操作）。

• 步骤 8–13：状态更新与阻尼调节

  • 接受更新 ($\rho_{\text{trust}} \ge \rho_{\text{min}}$)：预测准确，说明当前近似模型可靠。接受新位置 $q_{k+1} = q^+$，并通过缩小阻尼系数 $\lambda_{k+1} = \text{clamp}(\lambda_k / \gamma, \lambda_{\min}, \lambda_{\max})$ 使下一次迭代更偏向加速收敛的高斯-牛顿方向。

  • 拒绝更新 ($\rho_{\text{trust}} < \rho_{\text{min}}$)：预测偏差较大或误差增大。拒绝更新，保持 $q_{k+1} = q_k$，并通过增大阻尼系数 $\lambda_{k+1} = \text{clamp}(\lambda_k \cdot \gamma, \lambda_{\min}, \lambda_{\max})$ 转向步长更保守的最速下降方向。

• 步骤 14–15：收敛判定

  检查参数更新量 $\Vert{}\boldsymbol{\delta}\Vert{}$、梯度大小或位姿误差（如位置与方向容差）是否达到要求的阀值，满足条件则结束迭代，输出最终状态 $S_{k+1}$。

L-BFGS

是一种拟牛顿法。适用于无约束非线性优化。（或者L-BFGS-B可以处理盒式约束）

如何选择策略：对比

SQP和L-BFGS: 非线性约束近似KKT还是无约束软Penalty

之前做机器人轨迹优化，一般是选用SQP，因为可以处理无碰撞约束（非线性的）和关节限幅约束，而且考虑机器人关节自由度的话优化变量维度也就几到几十维，不高。但是curobo却选择了L-BFGS, 核心是出于GPU大规模并行的需要：

1. SQP 每一步都需要求解一个 QP 子问题（通常依赖 Active-Set 激活集法或 Interior-Point 内点法）。这类 QP 算法具有高度动态的循环终止条件、矩阵分解（如 $LDL^T$ / $LU$）以及频繁的分支跳转。在 GPU 上批量并行运行上千个 QP 求解器时，会导致严重的线程束分化，GPU 算力利用率极低。而相比之下，L-BFGS 的核心仅是矩阵-向量乘法、内积以及规约操作（Two-loop recursion）。这些操作非常规则（Regular Execution Flow），可以轻松编译为高度优化的 Tensor/CUDA Kernels，并在数千个种子轨迹（Batch Seeds）上实现极高吞吐的完全并行计算。
2. 此外，SQP 的优势在于精确处理“硬约束”，但机械臂规划中的大部分物理与环境约束在 cuRobo 中被软约束化/可微损失化：

\(\min_{\theta_{1:T}} C_{\text{goal}}(X_g, \theta_T) + \sum_{t=1}^{T} \Bigl[ C_{\text{smooth}}(\theta_t) + C_{\text{self-collision}}(\theta_t) + C_{\text{world-collision}}(\theta_t) \Bigr]\) 具体来说：cuRobo 将机器人抽象为多球体碰撞模型（Sphere approximation），利用可微 Signed Distance Field (SDF) 构造连续可微的平滑碰撞惩罚函数（如 Log-Cosh 或分段二次函数）。将避障、平滑度（Jerk 最小化）和末端位姿误差均转化为目标函数中的 Penalty/Barrier 项后，轨迹优化问题退化为带简单关节极限盒式约束的大规模非线性优化问题，这正是 L-BFGS(-B) 的最佳发挥场景。

1. 最后，在全轨迹motion planning场景下，设轨迹维度为 $N = T \times \text{DoF}$（如 64 个时间步 × 7 自由度 = 448 维），问题的优化变量维度随轨迹长度线性升高，SQP需要维护和分解 $N \times N$ 的稠密/稀疏拉格朗日 Hessian 矩阵与约束雅可比矩阵，当 Batch 达到几百到几千时，显存带宽（HBM Bandwidth）和显存占用会迅速成为瓶颈。而显存占用仅为 $\mathcal{O}(mN)$（$m$ 通常设为 $5 \sim 12$），即使同时优化数千条候选轨迹，显存开销也仅有几兆字节，完全能放入 GPU 缓存与片上共享内存（Shared Memory）。

   不同优化器的分工：全局寻路和局部优化

   可以结合使用不同的优化器。比如，CuRobov2在做collision-free IK的时候，就结合了LM和L-BFGS：LM first converges to the target pose without collision constraints, then L-BFGS refines the solution under self-collision and environment constraints. LM provides a good seed near the goal, so L-BFGS only needs small adjustments to resolve collisions rather than searching from scratch. 在强非凸的复杂避障空间中，单纯靠局部梯度下降（无论是 SQP 还是 L-BFGS）都极易陷入局部极小值。cuRobo 采用混合架构绕开了局部极值问题：

2. 粗探索（Exploration）： 先通过 MPPI（模型预测路径积分，零阶采样法）或并行几何图规划器（Graph Planner）在 GPU 上快速探索出有希望的无碰撞通道。

3. 精修敛（Refinement）： 将 MPPI / 几何路径的解作为初值 Seed 喂给 L-BFGS，配合 cuRobo 定制的 Parallel Line Search 进行亚毫秒级的快速梯度收敛与轨迹平滑。

这种“全局采样初值 + GPU 批量 L-BFGS 快速打光”的范式，比单靠高开销的 SQP 求解在端到端耗时上快了 1~2 个数量级（实现整体 20~50ms 内出解）。

Trust Region信任域

damping