💻 computer science

Hippo: High-performance Interior-Point and Projection-based Solver for Generic Constrained Trajectory Optimization

本文提出了一种名为 Hippo 的高性能求解器，它通过结合自适应障碍更新策略的内点法与投影技术，有效解决了现有轨迹优化器在处理复杂硬约束时效率低、灵活性差及收敛性不佳的问题，为机器人运动规划提供了鲁棒且高效的解决方案。

原作者： Haizhou Zhao, Ludovic Righetti, Majid Khadiv

发布于 2026-03-03

📖 1 分钟阅读☕ 轻松阅读

原作者： Haizhou Zhao, Ludovic Righetti, Majid Khadiv

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

这篇论文介绍了一个名为 Hippo（河马）的新软件工具，它是专门为机器人设计的“超级导航员”。

为了让你轻松理解，我们可以把机器人运动规划想象成在复杂迷宫中开车，而Hippo就是那个能帮你找到最快、最安全路线的天才导航员。

1. 以前的导航员遇到了什么麻烦？

在 Hippo 出现之前，机器人主要依赖两种导航策略（就像两种老式导航仪）：

SQP 和 DDP 方法：它们像是一个循规蹈矩的司机。遇到路障（约束条件）时，它们要么绕路太慢（计算效率低），要么因为太死板而直接撞墙（无法处理复杂的硬约束）。
ALM 方法：这像是一个喜欢走捷径的司机。它试图通过“罚款”来让司机避开障碍。但在复杂的路况下，这种司机容易陷入死胡同（局部最优解），或者因为罚款设置不当而彻底迷路。

痛点：现有的导航员在处理像“机械臂抓取物体”或“四足机器人跳跃”这种高难度、多障碍、必须精准到达的任务时，要么算得太慢，要么根本算不出路。

2. Hippo 是怎么工作的？（核心魔法）

Hippo 结合了两种最聪明的策略，就像给导航员装上了**“透视眼”和“弹簧腿”**：

A. 内点法 (IPM) —— 像“在气球里开车”

想象你在一个巨大的气球里开车，气球壁就是不等式约束（比如：不能撞墙、不能超出关节极限）。

以前的做法：司机小心翼翼地贴着气球壁开，一旦贴太近就慌了。
Hippo 的做法：它使用一种自适应的“气球调节术”。它不会让你真的撞到气球壁，而是通过一种数学上的“屏障”，让你始终在安全区域内平滑行驶。如果离墙壁太近，它会自动调整策略，温柔地把车推回安全区，而不是生硬地刹车。这让它在处理复杂限制时非常稳健。

B. 投影法 (Projection) —— 像“在墙上弹跳”

有些约束是硬性的等式（比如：脚必须精准踩在某个点上，不能偏一毫米）。

Hippo 的做法：它使用投影技术。想象你开车时，如果路线偏离了必须踩的“点”，Hippo 会瞬间把你“弹”回那条正确的线上。这就像在光滑的地板上滑行，一旦偏离，立刻被修正回轨道，保证了极高的精准度。

C. 并行计算 —— 像“多人同时修路”

以前的导航员是单线程的，像是一个人拿着图纸，一步一步地算。

Hippo 的做法：它把整个路线拆分成很多段，同时让很多人（多核 CPU）一起计算。这就像修路时，不是一个人从头修到尾，而是几十个人同时在不同路段施工，速度瞬间提升。

3. 它有多厉害？（实战表现）

论文里做了两个著名的测试：

测试一：UR5 机械臂抓东西
- 场景：让机械臂随机去抓空中的物体，要求必须精准到达，且不能碰到任何奇怪的限制。
- 结果：Hippo 成功解决了绝大多数难题，而且速度很快。其他导航员要么算不出来，要么算得慢吞吞。特别是当任务变得极其困难（比如数学上的“病态”问题）时，Hippo 依然能稳如泰山。
测试二：Go2 机器狗走路
- 场景：让机器狗在复杂地形上跳跃和行走，脚必须精准落地，不能打滑。
- 结果：在“简单模式”下，大家都能跑；但在“困难模式”（比如要求机器狗大跨步跳跃）时，其他导航员纷纷“死机”或陷入死循环，只有 Hippo 和它的“内点法”版本能成功跑完全程。

4. 为什么叫 Hippo（河马）？

虽然论文没明说，但我们可以想象：

体型庞大但灵活：河马看起来笨重，但在水里（复杂约束环境）非常灵活。Hippo 能处理以前被认为“太重、太难”的优化问题。
稳健：河马在自然界中非常强壮，Hippo 在数学上也极其稳健，不容易崩溃。

总结

Hippo 就是一个既快又稳、既聪明又灵活的机器人运动规划器。

它不像以前的工具那样容易“卡死”或“迷路”。
它不需要工程师像调教宠物一样，花大量时间去微调参数（手调参数少）。
它能帮助机器人更快地学会新技能（比如通过模仿学习生成数据），或者在复杂环境中自主完成任务。

简单来说，Hippo 让机器人从“小心翼翼、经常迷路”变成了“身手矫健、指哪打哪”的专家。

1. 研究背景与问题 (Problem)

核心问题：
基于模型的机器人控制和运动规划高度依赖轨迹优化（Trajectory Optimization）。现有的主流求解器（基于序列二次规划 SQP 或微分动态规划 DDP）在处理复杂任务时存在以下局限性：

计算效率低： 难以满足大规模或高难度最优控制问题（OCP）的实时性或数据生成需求。
建模灵活性差： 难以处理隐式动力学（Implicit Dynamics）和复杂的约束。
收敛性差： 在处理包含硬约束（Hard Constraints，如接触动力学、不等式约束）的复杂任务时，容易陷入局部极小值或无法收敛。
现有求解器的不足：
- SQP/ALM 类 (如 acados, aligator, mim solver)： 增广拉格朗日法（ALM）在处理不等式约束时，由于需要显式检测活动集（Active Sets），收敛性较差且容易陷入局部最优。此外，ALM 的惩罚参数调度往往需要人工精细调整。
- 通用 NLP 求解器 (如 IPOPT)： 虽然鲁棒性强，但未利用 OCP 的时间稀疏结构，导致在机器人领域计算过慢。
- 现有专用求解器 (如 fatrop)： 虽然利用了稀疏性，但通常仅限于显式动力学，且缺乏有效的并行化。

目标：
开发一种通用、高效且鲁棒的轨迹优化求解器，能够同时处理隐式动力学、硬等式约束（包括秩亏情况）和不等式约束，适用于步态生成（Locomotion）和操作（Manipulation）等复杂场景。

2. 方法论 (Methodology)

论文提出了 Hippo，一个结合了正则化内点法 (Regularized IPM) 和基于投影的约束 SQP 的求解器。其核心架构如下：

A. 基于投影的 Riccati 递归 (Projection-based Riccati Recursion)

等式约束处理： 针对动力学方程 $f(x,u,y)=0$ 和其他等式约束，Hippo 采用零空间投影 (Null-space Projection) 技术。
核心机制：
- 利用动力学雅可比矩阵的可逆性（如欧拉或 RK4 积分器），构建零空间基矩阵 $Z$ 。
- 将原始 KKT 系统投影到约束的零空间中，从而消除等式约束变量，将问题转化为无约束的 Reduced System。
- 这种方法不仅适用于显式动力学，也天然支持隐式动力学和接触动力学（Contact Dynamics）。
- 通过并行计算和递归更新，高效求解价值函数（Value Function）的导数。

B. 正则化内点法 (Regularized IPM) 处理不等式约束

约束转化： 将不等式约束转化为松弛变量形式，引入障碍参数 $\mu$ 。
正则化策略： 为了防止当松弛变量 $t \to 0$ $t \to 0$ 时 Hessian 矩阵病态，引入了正则化项 $\rho$ $ρ$ 。
- 证明了在 $t, \nu \ge 0$ 的假设下，正则化项能保证 Hessian 修改量的有界性，极大提高了数值稳定性。
自适应障碍更新 (Adaptive Barrier Strategy)：
- 采用 Mehrotra 预测 - 校正 (MP-C) 策略。
- 在预测步计算 $\mu=0$ 的步长，在校正步更新 $\mu$ 。
- 引入了安全机制 (Safeguard)：仅当试验互补残差未增加时才接受校正项，防止步长过大导致发散。
秩亏等式约束处理： 对于可能秩亏的等式约束（如某些接触约束），Hippo 提供了两种模式：
1. 投影法： 直接投影求解（速度快，但对秩亏敏感）。
2. IPM 法： 将等式约束转化为双边箱约束（Box Constraints），利用 IPM 强制松弛变量趋于零（更鲁棒，但迭代次数稍多）。

C. 全局化策略 (Globalization)

采用固定步长的回溯线搜索 (Backtracking Line Search)。
如果无法找到下降步，求解器会重置障碍参数 $\mu$ 并重新初始化松弛变量，以跳出局部不可行点。

D. 实现细节

基于 C++ 和 Eigen/BLASFEO 库实现。
支持 CasADi 和 Pinocchio 的代码生成管道，用于高效计算导数。
充分利用 OCP 的块稀疏结构，支持并行计算。

3. 主要贡献 (Key Contributions)

Hippo 求解器： 提出了一种极简但高效的求解器实现，统一了内点法（IPM）和基于投影的 SQP，能够处理通用的 OCP 形式（包括隐式动力学和跨时间步的状态约束）。
鲁棒的约束处理：
- 设计了正则化 IPM，解决了传统 IPM 在接近边界时的数值病态问题。
- 提出了基于投影的 Riccati 递归，有效处理了秩亏的等式约束和接触动力学。
自适应与无需调参： 采用了 MP-C 自适应障碍更新策略和简单的全局化机制，使得求解器在大多数情况下无需人工精细调整参数即可收敛。
性能验证： 通过广泛的基准测试，证明了 Hippo 在收敛速度和求解成功率上优于现有最先进求解器（acados, fatrop, aligator, mim solver）。

4. 实验结果 (Results)

论文在两个代表性任务上进行了基准测试：

A. UR5 机械臂随机到达 (UR5 Random Reaching)

任务： 100 个随机目标位姿，包含硬等式约束（终端状态约束）。
结果：
- 成功率： Hippo (使用 IPM 处理等式约束模式) 解决了 65/100 的问题，优于 fatrop (56/100) 和 acados (36/81，且部分解松弛值不达标)。
- 效率： 虽然 Hippo 的每次 QP 迭代时间略长（由于通用投影），但由于其收敛所需的迭代次数更少，且不需要像 ALM 类求解器那样进行昂贵的子问题精确最小化，整体表现优异。
- 结论： 正则化 IPM 处理硬约束比纯投影法更鲁棒，比 ALM 类方法更不易陷入局部不可行点。

B. Go2 四足机器人两步行走 (Go2 2-step Locomotion)

任务： 包含 trot（小跑）和 hopping（跳跃）两种步态，测试在间歇接触和多接触设置下的收敛性。分为“简化 (Reduced)"和“完整 (Full)"约束设置，以及“简单 (Simple)"和“困难 (Hard)"目标。
结果：
- 成功率： Hippo 和 Hippo(i) 在几乎所有设置下都解决了 90% 以上 的问题（除极个别困难设置外）。
- 对比：
  - fatrop： 解决了所有问题，但计算时间极慢（约 24ms/QP，是 Hippo 的 10 倍），主要因为缺乏并行化和高效的函数评估。
  - acados/aligator/mim solver： 在“完整约束 (Full)"设置下，ALM 类求解器（aligator, mim）完全失败（0% 成功率），acados 在跳跃任务中也完全失败。这表明 ALM 在处理严格约束时极易陷入局部极小值或不可行点。
- 速度： 在严格约束的完整设置下，Hippo 展现了最快的速度（约 2.3-2.9ms/QP），同时保持了与 fatrop 相当的鲁棒性。

5. 意义与总结 (Significance)

填补空白： Hippo 填补了现有求解器在“通用性”、“处理硬约束的鲁棒性”和“计算效率”之间的空白。它既不像 IPOPT 那样慢，也不像 acados 那样在处理复杂接触约束时容易失败。
应用价值：
- 行为克隆 (BC) 数据生成： 由于其高成功率和无需大量调参的特性，非常适合用于生成高质量的学习数据。
- 任务与运动规划 (TAMP)： 能够可靠地解决包含复杂接触和硬约束的规划问题。
未来展望： 论文指出未来可以集成更完善的全局化策略（如 IPOPT 的可行性恢复阶段和滤波器线搜索），以进一步提升在极端困难问题上的鲁棒性。

总结： Hippo 是一个高性能、通用的轨迹优化求解器，通过结合正则化内点法和投影技术，成功解决了机器人轨迹优化中常见的收敛性差和计算效率低的问题，特别是在处理硬约束和接触动力学方面表现卓越。