数值优化库的 C++ 集成生态¶

本章定位：把“会调用一个求解器”升级为“能为规控问题选择、封装、验证和上线求解器”。规控工程里的优化不是孤立算法，而是变量所有权、约束表达、稀疏结构、热启动、实时边界和故障诊断共同组成的工程系统。

前置自测¶

答不出两题以上时，先回顾线性代数、最小二乘、C++ 所有权和基础构建系统。

写出二次规划的标准形式，并解释 Hessian 半正定与凸性的关系。
Ceres 的残差块为什么天然适合最小二乘，但不适合通用不等式约束？
Eigen::Map<const Eigen::VectorXd> 与拷贝成 Eigen::VectorXd 在所有权上有什么区别？
稀疏矩阵的压缩列格式为什么比二维 std::vector 更适合求解器接口？
warm start 对 MPC 有什么价值？什么时候 warm start 反而会伤害收敛？

本章目标¶

学完本章后，你应该能把 SLAM 中熟悉的 Ceres/g2o 思维迁移到规控中的 QP/NLP/代码生成生态。你应该能写出一个最小但边界清晰的 C++ 求解器适配层，知道哪些数据归调用者所有，哪些数据归求解器缓存所有。你还应该能根据问题规模、约束结构和实时预算判断 OSQP、ProxSuite、IPOPT、CasADi、acados 等工具该放在哪一层。

知识树¶

数值优化库集成
├── 最小二乘 vs 约束优化 ⭐（SLAM 残差 vs 控制硬约束）
├── QP 求解器生态 ⭐⭐
│   ├── OSQP（ADMM，通用稀疏）
│   ├── ProxSuite（近端增强拉格朗日，Eigen 原生）
│   ├── qpOASES（活动集，遗留稠密）
│   ├── HPIPM/acados（内点法，OCP 结构）
│   └── 热启动策略与局部重置
├── QP 算法族深度对比 ⭐⭐⭐
│   ├── ADMM vs 活动集 vs 内点法 vs 近端法
│   └── 收敛精度与迭代行为
├── NLP 与自动微分 ⭐⭐⭐
│   ├── IPOPT（通用 NLP 内点法）
│   ├── CasADi（符号建模与代码生成）
│   ├── acados（实时 NMPC 代码生成）
│   └── 导数验证（有限差分）
├── 求解器适配层设计 ⭐⭐
│   ├── 四层分离：建模/标准问题/适配/策略
│   ├── 状态码转换与恢复策略
│   └── 实时边界与日志
├── 代码验证 ⭐⭐（KKT 残差，回归测试）
└── 选型与数值稳定性 ⭐⭐⭐
    ├── 问题族识别
    ├── 稀疏性与结构利用
    ├── 变量标度化
    └── 预算表与尾延迟

1.1 从最小二乘到带约束优化 ⭐¶

这一节解决的问题是：为什么 SLAM 工程师不能把 Ceres 经验原样搬到规控工程。

动机：轨迹规划多出来的不是“几个条件”¶

回顾 SLAM 后端：常见问题是最小化一组残差的平方和。

\[ \min_x \frac{1}{2}\sum_i \|r_i(x)\|^2 \]

这里的核心对象是残差。每个边、每个观测、每个 IMU 预积分都可以写成一个残差项。只要残差可微，Gauss-Newton 或 Levenberg-Marquardt 就能利用 $J^T J$ 结构快速求解。

规控问题的外形很像优化，但内核不同。控制器不仅要“尽量接近期望轨迹”，还必须满足硬约束。

约束来源	数学形式	违反后果	能否当成普通残差
关节限位	$q_{\min} \le q \le q_{\max}$	硬件撞限位	不应只靠罚函数
摩擦锥	$\sqrt{f_x^2+f_y^2}\le \mu f_z$	脚底打滑	不应只靠罚函数
动力学方程	$M(q)\ddot q+h= S^T\tau+J^T\lambda$	轨迹不可执行	必须等式满足
碰撞距离	$d(q)\ge d_{\min}$	撞到环境或自身	通常要硬约束
电机能力	$\lvert\tau_i\rvert\le \tau_{i,\max}$	电机饱和发热	必须显式处理

如果把这些约束都塞进残差，优化器会尝试“折中”。但机器人摔倒、碰撞或力矩饱和不是可折中的误差。这就是规控优化和 SLAM 后端的第一条分界线。

反面失败：罚函数看起来收敛，机器人却不可执行¶

假设用 Ceres 做避障轨迹优化，把障碍距离写成如下罚函数：

\[ \rho(d)=\max(0, d_{\min}-d)^2 \]

当权重很小时，轨迹可能穿过障碍，因为节省控制能量更便宜。当权重很大时，Hessian 变得病态，求解器步长很小，收敛速度下降。当障碍很密时，罚函数仍然不能保证最终解满足 $d\ge d_{\min}$。

这类似用“扣分”代替“红线”管理实验室安全。扣分机制可以鼓励好行为，但不能替代“高压电源不得裸露”的硬规则。规控里的许多约束就是这样的硬规则。

本质洞察：Ceres 的强项不是“优化”，而是“非线性最小二乘残差组织”。规控的强项不是“多加几个残差”，而是“把物理边界写成等式和不等式，并让求解器在边界内寻找最优折中”。

历史脉络：为什么会形成两套生态¶

视觉 SLAM 的核心数据来自观测误差，因此 g2o、Ceres、GTSAM 都围绕因子图和最小二乘发展。 Gauss（1801 年）最早用最小二乘法拟合谷神星轨道，从那以后最小二乘就是"从噪声数据中提取信号"的核心工具。机器人控制的核心数据来自物理可行性，因此控制领域长期围绕 QP、SQP、内点法和最优控制发展。 Pontryagin 最优控制原理（1956 年）和 Bellman 动态规划（1957 年）奠定了控制优化的理论基础。现代 MPC 起源于工业过程控制（1970s-1980s），随着计算能力提升才逐步进入机器人领域。

两条路线在数学上都属于数值优化，但工程接口完全不同。这种分野的根本原因不是数学偏好，而是问题结构不同。观测误差是统计量——它需要最小化而不需要严格为零。物理约束是边界——它必须满足否则系统不可行。这两种需求导致了两种截然不同的求解器生态。

领域	主变量	核心结构	常见库	典型输出
SLAM 后端	位姿、地图点、偏置	残差块和稀疏正定近似 Hessian	Ceres, g2o, GTSAM	估计状态
WBC	加速度、接触力、力矩	线性等式和不等式 QP	OSQP, ProxQP, qpOASES	控制量
MPC	轨迹状态和输入	多阶段动态约束	acados, HPIPM, OCS2	未来输入序列
运动规划	路径点、时间、控制	非线性约束和碰撞距离	IPOPT, SNOPT, NLopt	轨迹

从 Ceres 到 QP 的思维跳跃¶

让这个迁移变得困难的不是数学形式，而是思维模型的转变。 Ceres 用户习惯"加残差"。每个因子、每个观测、每个 IMU 约束都是一条残差。残差越多，估计越准。这像投票——每一票都指向正确状态的概率上升。

QP/NLP 用户习惯"加约束"。每条约束划出一个可行域的边界。约束越紧，可行域越小。这像画围栏——围栏越多，允许活动的范围越明确。

两种思维的关键差异在于"违反的后果"。残差可以违反——它只是让代价变大。硬约束不能违反——违反意味着物理上不可行。

思维模型	"违反"的后果	求解器做什么	类比
残差/代价	代价增大	找代价最小的折中点	扣分制
硬约束	解不可行	必须在可行域内寻找	红线制
软约束	代价增大但有惩罚	优先满足但允许小违反	扣分但重罚

机器人控制中，动力学方程是硬等式约束——违反它意味着轨迹不可执行。关节限位是硬不等式约束——违反它意味着硬件损坏。跟踪误差是代价——它可以在约束内最小化。

理论：QP 与 NLP 的最小公共骨架¶

二次规划写作：

\[ \begin{aligned} \min_x \quad & \frac{1}{2}x^T Hx + g^T x \\ \text{s.t.}\quad & A_{\mathrm{eq}}x=b_{\mathrm{eq}} \\ & l \le Cx \le u \end{aligned} \]

如果 $H\succeq 0$，约束是线性的，那么这是凸 QP。凸 QP 的局部最优就是全局最优。这对控制回路非常重要，因为实时系统没有时间分辨“局部收敛是否足够好”。

非线性规划写作：

\[ \begin{aligned} \min_x \quad & f(x) \\ \text{s.t.}\quad & g(x)=0 \\ & h_{\min}\le h(x)\le h_{\max} \end{aligned} \]

NLP 更表达力强，但实时边界更难。轨迹优化和碰撞规避常常需要 NLP。 1 kHz WBC 通常不应把大型 NLP 放进实时线程。

NLP 和 QP 的关系可以进一步理解。 SQP 方法的每一步都会线性化非线性约束，把当前迭代的子问题变成 QP。因此 QP 求解器是 NLP 求解器的核心组件。 IPOPT 内部用障碍函数把不等式转化后做 Newton 步。 acados 的 SQP-RTI 显式地生成 QP 交给 HPIPM 或 OSQP。理解这种嵌套关系，有助于判断"我的问题需要 QP 层还是 NLP 层"。

如果不做这个区分，常见误用是：用 IPOPT 解一个本质上是线性约束 QP 的问题——浪费了非线性能力并引入不必要的复杂性。或者用 OSQP 解一个本质上有非线性摩擦锥的问题——线性化近似太粗导致约束违反。

工程规则：先定问题族，再选求解器¶

不要从库开始选型。先问问题的结构。

初学者常犯的错误是：先听说某个库很好，再想办法把自己的问题塞进去。正确的顺序恰好相反：先清楚问题是什么数学形式，再找匹配的求解器。就像先量脚，再买鞋；不是先买鞋，再让脚适应。

问题结构的关键维度包括：

维度	判断问题	例子
代价函数类型	二次、非线性、非凸？	跟踪误差通常是二次的
约束类型	线性、非线性、等式、不等式？	动力学是线性等式，摩擦锥是非线性
变量规模	几十、几百、几千？	WBC 通常几十，MPC 展开后几百
调用频率	1 kHz、100 Hz、离线？	WBC 高频，轨迹优化离线
稀疏结构	是否有块状或带状模式？	MPC 展开有块三对角
热启动需求	相邻调用是否相似？	连续控制周期通常很相似

把这六个维度回答清楚后，选型变得明确。

优化问题选型流程
输入变量维度 < 100 且约束少？
├── 是：原型阶段可用 NLopt 或小规模 IPOPT
└── 否：是否二次代价 + 线性约束？
    ├── 是：优先 QP 求解器
    │   ├── 通用稀疏：OSQP / ProxSuite
    │   ├── 稠密小规模：ProxSuite dense / qpOASES 遗留项目
    │   └── OCP 多阶段结构：HPIPM / acados
    └── 否：是否能用 SQP 逐步线性化？
        ├── 是：IPOPT / acados / OCS2
        └── 否：重新审视建模，避免把不可实时问题塞进控制环

C++ 变量所有权：求解器接口最容易错的地方¶

优化库接口通常接收矩阵、向量和稀疏结构。这些数据的生命周期必须比求解器调用更清楚。错误的所有权会导致悬垂引用、隐式拷贝、重复分配和热启动失效。

变量所有权问题在 Eigen 和求解器交界处特别容易出现。 Eigen 有三种常见数据形态，它们的所有权完全不同。

Eigen 类型	是否拥有内存	常见用途	生命周期风险
`MatrixXd` / `VectorXd`	是（堆）	存储计算结果	低，自管理
`Map<const VectorXd>`	否（视图）	映射外部数组	高，原始数组可能先释放
`Ref<const VectorXd>`	可能否	函数参数	中，可能生成临时

在求解器适配层中，应明确每个矩阵参数是"借用"还是"拥有"。如果是借用（view），文档和变量名应说明生命周期限制。如果是拥有（owned），适配层负责分配和释放。

反事实地看，如果不区分 view 和 owned，可能出现一种隐蔽错误：求解器内部把传入的 Ref 缓存起来，下一帧调用时原始数据已经被覆盖。结果是求解器用旧 Hessian 解新问题，得到看似合理但物理错误的控制量。这类 bug 在多数测试中不会暴露，只有在特定状态序列下才出现。

#include <Eigen/Dense>
#include <memory>

struct QPDataView {
    // 这里只保存引用：调用者必须保证 H 和 g 在 solve() 期间仍然存活。
    // 这种设计适合“单次同步求解”，不适合把 view 缓存在异步线程中。
    Eigen::Ref<const Eigen::MatrixXd> H;
    Eigen::Ref<const Eigen::VectorXd> g;
};

struct QPDataOwned {
    // 这里拥有数据：适合跨线程、跨周期缓存，但每次更新可能产生拷贝成本。
    Eigen::MatrixXd H;
    Eigen::VectorXd g;
};

class SolverAdapter {
public:
    bool solve(const QPDataView& view, Eigen::VectorXd* x) {
        // x 由调用者拥有，适配层只写入结果。
        // 这样控制器可以预分配 x，避免实时循环里反复分配。
        if (x == nullptr || x->size() != view.g.size()) {
            return false;
        }
        // 示例只展示所有权边界，真实代码在这里调用具体求解器。
        x->setZero();
        return true;
    }
};

错误写法通常很隐蔽。

#include <Eigen/Dense>
#include <functional>

std::function<double()> makeBadCallback() {
    Eigen::VectorXd local = Eigen::VectorXd::Ones(10);
    // 错误：lambda 捕获了局部对象引用。
    // local 在函数返回后析构，后续调用会读悬垂引用。
    return [&local]() { return local.sum(); };
}

正确做法是让生命周期显式化。

#include <Eigen/Dense>
#include <functional>
#include <memory>

std::function<double()> makeSafeCallback() {
    auto data = std::make_shared<Eigen::VectorXd>(Eigen::VectorXd::Ones(10));
    // shared_ptr 明确延长数据生命周期。
    // 实时线程中不应创建 shared_ptr；这里适合非实时配置阶段。
    return [data]() { return data->sum(); };
}

常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
概念	用残差代替硬约束	解看似平滑但穿障碍	罚函数不保证可行性	显式建模不等式
编程	适配层缓存 `Eigen::Ref`	偶发崩溃或随机结果	引用对象生命周期结束	只在同步调用内使用 view
工程	每周期重新分配稀疏结构	1 kHz 中出现延迟尖峰	symbolic structure 没缓存	固定 sparsity，只更新数值
思维	只比较平均求解时间	压力下偶发超时	控制看最坏延迟	记录 p99 和最大值

练习¶

把一个二维避障问题分别写成罚函数形式和不等式约束形式，解释两者在边界附近的 KKT 条件差异。
为 SolverAdapter 增加 init() 和 update() 两阶段接口，要求 init() 分配内存，update() 只写入已有缓冲。
设计一个测试，故意让 Eigen::Ref 指向已释放对象，观察 ASan 能否报告问题。

1.2 QP 求解器生态：OSQP、ProxSuite、qpOASES、HPIPM、PIQP ⭐⭐¶

这一节解决的问题是：同样是 QP，为什么不同库在机器人里承担不同角色。

动机：QP 是控制器里的”内层循环”¶

WBC 和线性 MPC 经常把问题化为 QP。在足式机器人中，一次 QP 可能每 1 ms 或每 5 ms 调用一次。这时 API 优雅程度、热启动质量、内存分配行为和数值鲁棒性都直接影响机器人表现。

QP 求解器可以类比为数据库索引。同一条查询语句，索引结构不匹配时仍能跑，只是延迟不可接受。同一个 QP，用错求解器也可能给出正确答案，但在控制频率下无法上线。

QP 在机器人控制中的地位非常特殊。它是”足够通用但又能高效求解”的甜蜜点。比 QP 简单的（线性规划）表达力不够——控制代价通常是二次的。比 QP 复杂的（NLP）实时性不够——非线性约束的求解时间难以预测。因此 QP 成了实时控制的核心工具：WBC 用它分配接触力和关节力矩，线性 MPC 用它优化未来输入序列，甚至碰撞规避也常线性化成 QP 子问题。

反事实推理：如果 QP 的求解时间像 NLP 一样不可预测，机器人控制会怎样？答案是：每个控制周期都必须预留 NLP 最坏耗时的安全余量。对于 1 kHz 控制，如果 NLP 最坏耗时是 10 ms，那就无法在 1 ms 内完成。这就是为什么 QP（或结构化 NLP 如 SQP-RTI）在实时控制中不可替代。

主流库对比¶

求解器	算法族	C++ 体验	稀疏支持	热启动	适合场景	主要边界
OSQP	ADMM	通过包装层较顺手	强	强	稀疏通用 QP、鲁棒原型	高精度可能慢
ProxSuite ProxQP	近端增强拉格朗日	Eigen 原生	稠密和稀疏	强	WBC、小中规模高精度 QP	生态小于 OSQP
qpOASES	活动集	老式 C++	主要稠密	很强	遗留 MPC、小规模稠密	维护活跃度较低
HPIPM	内点法 + OCP 结构	C API	OCP 结构强	强	多阶段 MPC	直接集成成本高
PIQP	近端内点法	现代 C++	稠密和稀疏	强	allocation-aware 实时原型	项目采用量较小

反面失败：只看 benchmark 排名会误导选型¶

一个求解器在某组稀疏随机 QP 上很快，不代表它适合你的 WBC。 WBC 的 Hessian 常常半正定、约束行有冗余、接触切换会改变活动集。 MPC 的矩阵有阶段结构，通用稀疏求解器未必利用 Riccati 结构。

如果不区分问题结构，就像给所有螺丝都用同一把扳手。能拧动不代表不会损坏螺纹。

典型的 benchmark 陷阱包括：

陷阱	为什么误导	正确做法
随机生成 QP	结构与你的问题不同	用你的真实 QP 做测试
只报平均耗时	控制关心最坏情况	记录 p99 和 max
不含初始化开销	某些库初始化很慢	分别统计 init 和 solve
冷启动测试	忽略热启动优势	测连续调用的累计时间
不报精度	更宽松的容差自然更快	统一 KKT 容差后比较

反事实地看，如果某个 benchmark 显示 OSQP 比 ProxQP 慢 3 倍，但你的 WBC 只需要 $10^{-4}$ 精度而 benchmark 用了 $10^{-8}$，实际你的场景里 OSQP 可能更快。 benchmark 只回答"在那个设定下谁快"，不回答"在你的问题上谁合适"。

OSQP 集成骨架¶

OSQP 的优点是稳健、社区大、稀疏接口清楚。工程中要把初始化和数值更新分开。

#include <Eigen/Sparse>
#include <OsqpEigen/OsqpEigen.h>

class OsqpWbcSolver {
public:
    bool init(int n_var, int n_con,
              const Eigen::SparseMatrix<double>& H_pattern,
              const Eigen::SparseMatrix<double>& A_pattern) {
        solver_.settings()->setWarmStart(true);
        solver_.settings()->setVerbosity(false);
        solver_.data()->setNumberOfVariables(n_var);
        solver_.data()->setNumberOfConstraints(n_con);
        solver_.data()->setHessianMatrix(H_pattern);
        solver_.data()->setLinearConstraintsMatrix(A_pattern);
        solver_.data()->setGradient(Eigen::VectorXd::Zero(n_var));
        solver_.data()->setLowerBound(Eigen::VectorXd::Constant(n_con, -1e20));
        solver_.data()->setUpperBound(Eigen::VectorXd::Constant(n_con, 1e20));
        return solver_.initSolver();
    }

    bool solve(const Eigen::SparseMatrix<double>& H_values,
               const Eigen::VectorXd& g,
               const Eigen::SparseMatrix<double>& A_values,
               const Eigen::VectorXd& l,
               const Eigen::VectorXd& u,
               Eigen::VectorXd* x) {
        // 实时边界：这里假设稀疏结构不变，只更新数值。
        // 如果非零模式变化，OSQP 需要更昂贵的结构更新。
        solver_.updateHessianMatrix(H_values);
        solver_.updateGradient(g);
        solver_.updateLinearConstraintsMatrix(A_values);
        solver_.updateBounds(l, u);
        if (solver_.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) {
            return false;
        }
        *x = solver_.getSolution();
        return true;
    }

private:
    OsqpEigen::Solver solver_;
};

ProxSuite 集成骨架¶

ProxSuite 的教学价值在于它让 QP 的数学形式直接映射为 Eigen 参数。变量所有权仍要讲清楚：QP 对象拥有内部工作区，传入矩阵用于初始化或更新。

#include <Eigen/Dense>
#include <proxsuite/proxqp/dense/dense.hpp>

class ProxQPDenseAdapter {
public:
    ProxQPDenseAdapter(int n, int n_eq, int n_in)
        : qp_(n, n_eq, n_in) {
        qp_.settings.eps_abs = 1e-8;
        qp_.settings.eps_rel = 1e-8;
    }

    bool init(const Eigen::MatrixXd& H,
              const Eigen::VectorXd& g,
              const Eigen::MatrixXd& A,
              const Eigen::VectorXd& b,
              const Eigen::MatrixXd& C,
              const Eigen::VectorXd& l,
              const Eigen::VectorXd& u) {
        // 注意 ProxSuite 的 init 不只是建立内部缓存：它会执行首次分解并求解，
        // 因此这里读取 results.info.status 反映的是这次"初始化即求解"的结果，而非单纯的装载是否成功。
        // 之后每周期优先 update + solve，避免重复初始化（见下方 updateAndSolve）。
        qp_.init(H, g, A, b, C, l, u);
        return qp_.results.info.status ==
               proxsuite::proxqp::QPSolverOutput::PROXQP_SOLVED;
    }

    bool updateAndSolve(const Eigen::VectorXd& g,
                        const Eigen::VectorXd& l,
                        const Eigen::VectorXd& u,
                        Eigen::VectorXd* x) {
        // 示例只更新线性项和不等式边界。
        // 接触相位切换时，常把失效脚的力上下界设为 0，而不是改变变量维度。
        qp_.update(std::nullopt, g, std::nullopt, std::nullopt,
                   std::nullopt, l, u);
        qp_.solve();
        if (qp_.results.info.status !=
            proxsuite::proxqp::QPSolverOutput::PROXQP_SOLVED) {
            return false;
        }
        *x = qp_.results.x;
        return true;
    }

private:
    proxsuite::proxqp::dense::QP<double> qp_;
};

结构化 MPC：为什么 HPIPM 常通过 acados 间接使用¶

HPIPM 的优势不是”又一个 QP 求解器”，而是利用最优控制问题的阶段结构。 MPC 的动力学约束把每个时刻连接起来，KKT 矩阵呈现块三对角或箭头结构。 Riccati 递推可以避免把整个大矩阵当普通稀疏矩阵处理。

如果把一个 20 阶段、状态维度 12、输入维度 6 的 MPC 展开成一个大 QP：变量总数是 $20 \times 12 + 20 \times 6 = 360$。约束矩阵大约 $240 \times 360$。通用稀疏求解器把它当成一个 $360 \times 360$ 的大问题。 HPIPM 把它当成 20 个小问题，用 Riccati 递推从最后一步递归回第一步。利用阶段结构后，Riccati 递推的复杂度是 $O(N \cdot n^3)$（每个阶段做一次 $n \times n$ 量级的分解），其中 $N$ 是阶段数，$n$ 是状态维度；而把同一问题当成通用稠密 QP 直接分解约为 $O((Nn)^3)$。这对 $N \geq 10$ 的 MPC 非常有价值。

但 HPIPM 直接暴露 C 接口和 BLASFEO 数据结构。 BLASFEO 是一个为嵌入式优化设计的线性代数库，它使用面板式存储来提高缓存效率。这意味着直接使用 HPIPM 需要理解它的数据打包方式，不能简单传入 Eigen 矩阵。教学和生产中常通过 acados 使用它。 acados 负责把 CasADi 模型转换成 HPIPM 需要的数据格式。这样学生先学习模型、约束和求解流程，再逐步理解底层结构优化。

工程边界：QP 不是永远可解¶

QP 失败不一定是求解器错。常见原因是模型不可行。理解 QP 失败的根因比换求解器更重要。

一个常见场景是：四足机器人在摆动腿切换时，有一帧的接触状态不一致。程序标记某只脚为"摆动"，但约束矩阵仍要求该脚提供支撑力。这时 QP 要求 $f_z = 0$（摆动脚不接触）和 $mg = \sum f_z$（总支撑力等于重力）同时成立。如果只剩两条腿提供支撑，可能不够。

这不是求解器的问题。这是状态估计和步态逻辑的同步问题。求解器只是忠实地报告了"你给我的约束无法同时满足"。

失败模式	例子	根因	处理
动力学不可行	单脚支撑却要求巨大横向加速度	参考轨迹超出物理能力	放松参考、调整步态
约束互相冲突	法向力上限太低又要求支撑重力	状态和约束不同步	检查单位和接触状态
Hessian 病态	状态权重差十几个数量级	标度缺失	标度化变量和代价
热启动误导	接触切换后沿用上一相位解	活动集剧变	切换时重置相关变量
约束行冗余	多条约束线性相关	建模重复	检查约束矩阵秩
变量维度变化	摆动脚时删除力变量	稀疏结构改变	固定维度，用边界关闭

本质洞察：QP 不可行不是"出了 bug"，而是"系统告诉你物理约束有矛盾"。正确的响应是理解矛盾来源并修复上游逻辑，而不是放松约束让不可行解"通过"。

练习¶

对同一个 12 变量 WBC QP，分别写 OSQP 和 ProxSuite 的适配层接口，比较初始化、更新、求解三个阶段的内存边界。
设计一个不可行 QP：要求 $x\ge 1$ 且 $x\le 0$，观察不同求解器的状态码，并把状态码转换为控制器可理解的错误枚举。
对一个接触力 QP，保持变量维度固定，通过上下界关闭摆动腿接触力，解释为什么这比删除变量更适合实时循环。

1.2b QP 求解器深度对比与热启动策略 ⭐⭐⭐¶

这一节解决的问题是：同一类 QP 求解器之间，算法族、收敛特性和热启动行为的差异如何映射到工程选择。

动机：表面看都是 QP，但算法假设完全不同¶

QP 求解器的 API 常常"长得很像"：传入 Hessian、梯度、约束矩阵、上下界，返回最优解和状态码。但不同求解器背后的算法族完全不同。算法族决定了迭代策略、对病态的容忍度、热启动的有效程度和最坏迭代次数。

这可以类比交通工具。飞机和高铁都能从北京到上海，但成本结构、准时性、对天气的敏感度、行李限制和出发条件完全不同。只看"北京到上海 4 小时"不足以选交通方式。同样，只看 benchmark 上的平均求解时间不足以选 QP 求解器。

四大算法族¶

算法族	代表库	核心思想	迭代行为	对病态的容忍	热启动质量
ADMM（交替方向乘子法）	OSQP	把原问题拆成可分子问题交替求解	每步便宜但可能需要较多步	较宽容	好，但高精度可能需要很多迭代
近端增强拉格朗日	ProxSuite ProxQP	用近端项增强拉格朗日函数	每步较稳定	好	非常好
活动集法	qpOASES	维护活动约束集合，每步增减一条约束	步数少但每步可能昂贵	中等	极好（参数式求解）
内点法	HPIPM、PIQP	沿中心路径逼近 KKT 点	步数少但每步需要线性系统	好	需要特殊处理

这四种方法解决的是同一个数学问题，但走的路径不同。 ADMM 像把一个大任务拆成很多人分头做再汇总。活动集法像排除法，逐步确定哪些约束真正紧。内点法像从问题内部向边界靠近。近端增强拉格朗日像给问题加了阻尼，让振荡收敛更快。

本质洞察：QP 求解器的"快慢"不是绝对的。一个求解器在某类问题上快，是因为它的算法假设恰好匹配问题结构。选型不是选"最新"或"最引用"，而是让算法假设和问题特征对齐。

OSQP vs ProxQP 的工程差异¶

OSQP 和 ProxQP 是机器人 WBC 和 MPC 中最常对比的两个库。它们的 API 表面相似，但底层行为有重要差异。

维度	OSQP	ProxQP
算法	ADMM	近端增强拉格朗日
收敛精度	中等精度快，高精度可能慢	高精度也较快
稀疏接口	CSC 格式为主	Eigen 原生 dense/sparse
热启动	支持，从上一帧初始化	支持，且更新 API 更轻量
结构更新	区分 symbolic 和 numeric	init/update 分离清晰
社区生态	大，文档多，Python 绑定成熟	较小但增长快，C++ 体验好
典型适用	中到大稀疏 QP、鲁棒原型	WBC、小中规模、高精度场景

如果不区分这些差异，工程上容易犯两种错误。第一种是认为 OSQP 对所有 QP 都足够好，忽略了高精度场景下 ADMM 收敛慢的事实。第二种是认为 ProxQP 总是更好，忽略了 OSQP 在大型稀疏问题上的生态优势和工程成熟度。

反事实地看，如果控制器对 KKT 残差要求极低（例如 $10^{-10}$），OSQP 的 ADMM 可能需要上千次迭代。这时 ProxQP 或内点法更合适。但如果只需要中等精度（$10^{-4}$ 到 $10^{-6}$），OSQP 的每步极低成本反而更有吸引力。

qpOASES 与活动集法的历史定位¶

qpOASES 是机器人控制领域使用最早也最广泛的 QP 求解器之一。它基于参数式活动集法（parametric active-set strategy），最初由 Hans Joachim Ferreau 在 ACADO Toolkit 中开发。它的核心优势是对小到中规模稠密 QP 的极好热启动：当活动集变化很小时（例如 MPC 相邻周期只有少数约束切换），qpOASES 可以从上一帧解出发，只做少量活动集更新。

但 qpOASES 在现代生态中面临几个边界：

优势	边界
参数式热启动极好	只支持稠密 QP，大规模问题内存和计算爆炸
接口简单直接	C++ 风格偏老，与现代 Eigen 集成需要适配
学术引用和工程验证充分	维护活跃度低于 OSQP 和 ProxSuite
对活动集不频繁变化的 MPC 非常高效	活动集大幅变化时性能下降

如果你的项目有遗留 qpOASES 代码且工作稳定，不必急于迁移。如果是新项目，通常优先考虑 OSQP 或 ProxSuite，除非有明确的小规模参数式热启动需求。

HPIPM 与多阶段 OCP 结构¶

HPIPM 不是通用 QP 求解器。它是专门为最优控制问题（OCP）的多阶段结构设计的内点法求解器。 MPC 的动力学约束把问题分成若干个时间步，每个时间步有独立的状态和输入变量，通过动力学等式 $x_{k+1} = A_k x_k + B_k u_k$ 连接。

这种结构让 KKT 系统呈现块三对角或箭头矩阵形状。 HPIPM 用 Riccati 递推代替通用稠密分解：递推复杂度是 $O(N \cdot n^3)$（$N$ 是阶段数，$n$ 是状态维度），而把同一问题当成通用稠密 QP 直接分解约为 $O((Nn)^3)$。这对 horizon 较长的 MPC 非常有价值。

但 HPIPM 直接使用需要了解 BLASFEO 数据结构和 C API。因此大多数教学和生产项目通过 acados 间接使用 HPIPM。 acados 在 CasADi 模型之上生成完整的 SQP-RTI 或 iSQP 求解器，底层调用 HPIPM。

热启动：不只是"从旧解出发"¶

热启动（warm start）的工程含义远比"把上一帧解拿来用"更丰富。好的热启动策略需要回答三个问题：

什么时候热启动有效？ 当相邻周期的 QP 变化不大时，旧解接近新解，热启动能减少迭代。
什么时候热启动有害？ 接触切换、步态转换或大幅参考变化后，旧解可能处于新问题的不可行区域，导致求解器先花迭代走回可行域。
如何局部重置？ 不一定要全部重置。可以只重置接触力相关变量，保留关节变量的热启动。

场景	热启动策略	原因
稳态站立	全量热启动	状态几乎不变
稳态行走中段	全量热启动	步态变化缓慢
接触切换瞬间	局部重置摆腿力变量	活动集剧变
速度指令大幅变化	可选重置或保留	取决于问题灵敏度
求解器上一周期失败	完全重置	旧解不可信

#include <Eigen/Dense>

struct WarmStartManager {
    Eigen::VectorXd primal;   // 变量初值
    Eigen::VectorXd dual;     // 对偶变量初值
    bool valid = false;       // 上一帧是否成功

    void resetContactForces(int force_start, int force_count) {
        // 只重置接触力变量的热启动，保留关节加速度和力矩的旧值。
        // 这比完全重置更精细，比完全保留更安全。
        if (valid && force_start + force_count <= primal.size()) {
            primal.segment(force_start, force_count).setZero();
            dual.segment(force_start, force_count).setZero();
        }
    }

    void fullReset() {
        // 上一帧失败或问题结构大幅变化时，完全放弃旧解。
        valid = false;
        primal.setZero();
        dual.setZero();
    }
};

MPC 中的 QP 调用流程¶

线性 MPC 的每个周期通常遵循以下流程。理解这个流程有助于判断哪些步骤属于实时路径，哪些可以离线或低频完成。

MPC 单周期调用流程

1. 读取当前状态 x0（实时路径开始）
2. 线性化模型：A_k, B_k（可能从上一帧缓存或重新计算）
3. 组装 QP：
   - Hessian H = diag(Q, Q, ..., Q, R, R, ..., R) 或带交叉项
   - 梯度 g = 依赖 x0 和参考轨迹
   - 等式约束 A_eq：动力学 x_{k+1} = A_k x_k + B_k u_k
   - 不等式约束 A_ineq：力矩限幅、状态限幅、摩擦锥
4. 更新求解器数值（不改变稀疏结构）
5. 热启动（从上一帧解或局部重置）
6. 求解 QP
7. 检查状态码和 KKT 残差
8. 提取第一个时间步输入 u0
9. 保存解用于下一帧热启动
10. 发送 u0 到底层控制器（实时路径结束）

步骤 1-3 中，模型线性化可能涉及 Pinocchio 或 CasADi 的函数调用。如果线性化成本高，可以降低频率或在慢线程中完成。步骤 4-6 是求解器的核心路径。步骤 7 是安全检查，不应跳过。步骤 8-10 是控制器输出。

SNOPT 与学术前沿¶

SNOPT 是另一个重要的 NLP 求解器，广泛用于航天和机器人轨迹优化。它基于稀疏 SQP 方法，对大规模稀疏问题有很好的性能。但 SNOPT 是商业软件，学术版本有使用限制。在开源生态中，IPOPT 是最常见的替代。 Drake（MIT）框架同时支持 SNOPT 和 IPOPT 后端，允许用户根据许可和性能需求选择。

求解器	算法	许可	典型用途
IPOPT	内点法	开源（EPL）	学术和开源轨迹优化
SNOPT	稀疏 SQP	商业	航天、Drake、高精度规划
NLopt	多种局部/全局	开源	小规模、原型验证
CasADi + IPOPT	符号 + 内点法	开源	建模友好的 NLP
acados	SQP-RTI + HPIPM	开源	实时 NMPC
Drake (TrajOpt)	多种后端	开源	机械臂和操作规划

Drake 值得特别提及，因为它同时支持 IPOPT 和 SNOPT 后端，并提供了一套 C++ 数学规划 API（MathematicalProgram）。如果你的项目主要面向机械臂操作、抓取规划或多体接触优化，Drake 提供了从建模到求解的一站式方案。但 Drake 的构建系统（Bazel）和运行时依赖较重，集成到现有 ROS2 或 CMake 项目需要额外工作。

对足式机器人控制，acados 和 OCS2 是目前更常见的选择。 Drake 更多出现在机械臂和操作领域。选择时应考虑团队已有依赖、部署平台和问题形式。

⚠️ 常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
概念	认为所有 QP 求解器内部算法一样	换库后行为大变	算法族不同	理解 ADMM/活动集/内点法差异
编程	接触切换后不重置热启动	求解器迭代突增	旧活动集不再有效	局部重置接触力变量
工程	用通用 QP 求解器解 OCP	比 HPIPM 慢很多	没有利用阶段结构	用 acados/HPIPM
思维	只看 benchmark 排名	自己的问题上结果不同	问题结构不匹配	用自己的 QP 做对比
数值	对 ADMM 求解器要求 $10^{-10}$ 精度	迭代次数爆炸	ADMM 不擅长高精度	放松精度或换算法族

练习¶

对同一个 WBC QP，分别用 OSQP 和 ProxQP 求解，比较迭代次数、KKT 残差和最大耗时，解释差异的算法来源。
设计一个接触切换场景的热启动实验：完全热启动 vs 局部重置 vs 完全重置，记录每种策略的迭代次数。
用 CasADi 生成一个简单二轮车 MPC 的 QP 矩阵，把它分别传给 OSQP 和 ProxQP，验证两者给出一致的最优代价值。
解释为什么 HPIPM 对 horizon=50 的 MPC 比 OSQP 更有优势，但对 horizon=5 的 WBC 可能没有优势。

1.3 NLP、自动微分与代码生成 ⭐⭐⭐¶

这一节解决的问题是：什么时候必须离开 QP，以及如何避免手写复杂导数。

动机：真实规控问题经常是非线性的¶

碰撞距离 $d(q)$、旋转流形、非线性动力学、时间最优目标都不是线性函数。把它们强行线性化成一次 QP 可能只在局部有效。更通用的做法是用 NLP 或 SQP。

为什么不能”用更好的线性化”解决所有非线性问题？因为线性化只在一个点附近有效。如果工作点距离最优解很远（例如大幅避障），线性化 QP 的解可能完全不在正确方向。这时需要多次迭代线性化，或者直接用 NLP 在非线性可行域内搜索。

NLP 可以类比为”直接求山路上的最短路”。 QP 像在当前位置铺一小块平面地图，然后走一步再重画地图。 SQP 的思想正是反复构造局部 QP。

方法	思想	每步成本	收敛域	适合
单次 QP	当前点线性化，解一个 QP	低	很小	WBC、线性 MPC
SQP	反复线性化并解 QP	中	较大	NMPC
内点法	沿中心路径逼近 KKT	高但步数少	大	离线轨迹优化
SQP-RTI	每周期只做一步 SQP	低	需要状态变化慢	实时 NMPC

从 QP 到 NLP 的跳跃不只是”约束变成非线性”。它还意味着：问题可能非凸，局部最优不等于全局最优；求解器的迭代次数不可预测；线搜索和信赖域策略增加了实现复杂度。因此在规控中，能用 QP 的场景应优先 QP，只有确实需要非线性建模时才升级到 NLP。

IPOPT 的接口思维¶

IPOPT 要求用户提供目标、梯度、约束、Jacobian，通常还需要 Hessian 或近似 Hessian。它不像 Ceres 那样围绕残差块组织，而是围绕整个 NLP 的函数接口组织。

// 头文件路径依安装方式而定：交给 CMake 的 find_package(Ipopt) 处理包含目录，
// 这样源码里直接写 <IpTNLP.hpp> 即可，无需硬编码 coin/ 或 coin-or/ 前缀。
#include <IpTNLP.hpp>

class TrajectoryNLP final : public Ipopt::TNLP {
public:
    bool eval_f(Ipopt::Index n,
                const Ipopt::Number* x,
                bool new_x,
                Ipopt::Number& obj_value) override {
        // x 的内存由 IPOPT 拥有，只在回调期间有效。
        // 不要把 x 指针保存到类成员中。
        obj_value = 0.0;
        for (Ipopt::Index i = 0; i < n; ++i) {
            obj_value += x[i] * x[i];
        }
        return true;
    }

    bool eval_grad_f(Ipopt::Index n,
                     const Ipopt::Number* x,
                     bool new_x,
                     Ipopt::Number* grad_f) override {
        // grad_f 的存储由 IPOPT 提供，回调只负责填数值。
        for (Ipopt::Index i = 0; i < n; ++i) {
            grad_f[i] = 2.0 * x[i];
        }
        return true;
    }
};

这段代码的核心不是目标函数本身，而是生命周期。 IPOPT 调用回调时把指针借给用户。用户不能长期保存这些裸指针。

CasADi 与 acados 的分工¶

CasADi 是符号表达和自动微分框架。它能生成目标、约束、Jacobian、Hessian 的 C 代码。它不是完整嵌入式控制栈。

acados 在 CasADi 模型之上生成实时 MPC 求解器代码。它包含 SQP-RTI、QP 后端和线性代数后端。如果目标是把 MPC 放入 C++ 控制器，acados 通常比“CasADi + IPOPT 直接在线求解”更接近生产形态。

工具	主要产物	适合阶段	不适合
CasADi	可微函数和代码生成	建模、导数验证、原型	单独承担完整实时栈
IPOPT	NLP 数值解	离线轨迹优化、非实时规划	1 kHz 控制回调
acados	自包含 MPC 求解器	实时 NMPC	随意变维的复杂问题
CppAD	C++ 自动微分类型	模板化模型、内嵌导数	快速交互建模

NLP 求解器对比：IPOPT vs CasADi vs acados¶

三个工具的定位不同，不应当作同一层级比较。

工具	层级	核心能力	不适合
IPOPT	NLP 求解器	通用非线性约束内点法求解	硬实时控制回路
CasADi	符号建模和自动微分框架	模型表达、导数生成、代码生成	单独作为在线求解器
acados	嵌入式 OCP 求解器生成工具	从 CasADi 模型生成实时 NMPC 代码	随意变维的复杂问题

它们之间的关系像 CAD 软件、材料库和 CNC 机床。 CasADi 是 CAD——用来建模和生成图纸。 IPOPT 是通用加工中心——能处理各种零件，但每次加工都要装夹。 acados 是专用 CNC 线——针对特定零件生成高效加工程序。

典型工作流是：用 CasADi 建模（定义状态、输入、动力学、约束），用 IPOPT 做离线验证（确认模型可行），用 acados 生成实时 C 代码（部署到控制器）。

# CasADi 建模示例（Python 端建模，生成 C 代码后在 C++ 端调用）
import casadi as ca

# 定义符号变量
x = ca.MX.sym('x', 4)   # 状态：位置、速度
u = ca.MX.sym('u', 2)   # 输入：加速度

# 定义离散动力学（欧拉积分）
dt = 0.05
x_next = ca.vertcat(
    x[0] + dt * x[2],
    x[1] + dt * x[3],
    x[2] + dt * u[0],
    x[3] + dt * u[1]
)

# 生成 C 函数，供 C++ 控制器调用
f_discrete = ca.Function('f_discrete', [x, u], [x_next])
f_discrete.generate('discrete_dynamics.c')

这段 Python 代码不是控制器本身。它是离线建模工具，输出的 C 代码才进入实时路径。 C++ 控制器加载生成的函数，在每个 MPC 周期中调用它计算离散动力学和 Jacobian。

IPOPT 内部流程与收敛行为¶

IPOPT 使用障碍函数法（barrier method）把不等式约束转化为目标函数中的对数罚项，然后在一系列递减的障碍参数 $\mu$ 下求解 KKT 系统。每次固定 $\mu$ 后，IPOPT 用 Newton 步求解近似 KKT 系统，再通过线搜索或 filter 策略保证全局收敛。

这个过程可以类比登山时的雾中搜索。障碍函数像一面墙，离约束边界越近墙越高。随着 $\mu$ 减小，墙变矮，解逐渐移向最优的约束边界。如果不做障碍，Newton 步可能直接跳到约束外面。

IPOPT 的 KKT 线性系统通常是对称不定的。它需要一个线性求解器来分解这个系统。常见选择包括 MA57、MA27（HSL 库，学术免费）和 MUMPS（开源但性能可能较低）。线性求解器的选择直接影响 IPOPT 的性能。

线性求解器	特点	获取方式
MA57	稀疏对称不定分解，性能好	HSL 学术许可
MA27	旧版，仍可用	HSL 学术许可
MUMPS	开源，但不定系统处理较弱	apt/源码
Pardiso	Intel MKL 内置	商业或学术

本质洞察：IPOPT 的实时边界不仅来自非线性迭代，还来自每次迭代内的线性求解。如果线性系统很大或很病态，单次迭代就可能超时。这就是为什么 IPOPT 通常不放在 1 kHz 控制线程中。

SQP-RTI：让 NLP 接近实时¶

SQP-RTI（Sequential Quadratic Programming - Real-Time Iteration）是 Moritz Diehl 团队提出的策略。核心思想是：不等 NLP 收敛到最优解，而是每个控制周期只做一步 SQP 迭代。如果系统运动缓慢（相对于控制频率），一步 SQP 的近似解就足够好。

acados 实现了 SQP-RTI。它把 NLP 分解成线性化（preparation）和 QP 求解（feedback）两个阶段。 preparation 可以在等待新状态时完成。 feedback 在收到新状态后只做一步 QP。

SQP-RTI 时间线

MPC 周期 k:
  |-- preparation: 线性化模型、组装 QP (可提前) --|
  |                                 收到新状态 x0 |
  |-- feedback: 更新 x0, 求解一步 QP, 输出 u0 --|

MPC 周期 k+1:
  |-- preparation: 基于 k 的解做下一步线性化 ------|

这种设计让 NMPC 的计算时间变得更可预测。它不是"更快地求解 NLP"，而是"把完整 NLP 分摊到多个控制周期中"。

反面失败：手写导数没有验证就上线¶

轨迹优化中一个符号错号可能不会立刻崩溃。求解器只是变慢、收敛到奇怪轨迹，或者在某些场景失败。最危险的是“多数测试通过，极端场景失效”。

因此导数必须验证。最小验证方式是有限差分。

#include <Eigen/Dense>
#include <cmath>

template <typename Fun>
Eigen::VectorXd finiteDifferenceGradient(Fun&& f,
                                         const Eigen::VectorXd& x,
                                         double eps) {
    Eigen::VectorXd grad(x.size());
    for (int i = 0; i < x.size(); ++i) {
        Eigen::VectorXd xp = x;
        Eigen::VectorXd xm = x;
        xp(i) += eps;
        xm(i) -= eps;
        // 中心差分比前向差分误差低，但 eps 不能过小，否则被浮点舍入支配。
        grad(i) = (f(xp) - f(xm)) / (2.0 * eps);
    }
    return grad;
}

变量标度化¶

NLP 的变量可能同时包含米、弧度、牛顿、秒。如果不做标度化，求解器看到的是不同数量级的变量。线搜索和收敛判据会变得难以解释。

变量	原始量级	建议标度	标度后量级
位置	0.1 到 10 m	1 m	0.1 到 10
角度	0.01 到 3 rad	1 rad	0.01 到 3
力	10 到 1000 N	体重 $mg$	0.01 到 2
时间	0.001 到 10 s	1 s 或步长	接近 1

本质洞察：标度化不是数值装饰，而是告诉求解器”不同物理量在优化空间里应该被同等认真对待”。没有标度化，代价权重会同时承担控制偏好和单位换算两种职责，调参会变得不可解释。

有限差分的 eps 选择¶

eps 太大时，截断误差主导，中心差分近似的精度下降。 eps 太小时，浮点舍入误差主导，差分结果变成噪声。最佳 eps 大约在 $\sqrt[3]{\epsilon_{\text{machine}}}$ 附近，对 double 约为 $10^{-5}$ 到 $10^{-6}$。

eps 值	截断误差	舍入误差	总误差
$10^{-2}$	较大	很小	截断主导
$10^{-5}$	很小	中等	接近最优
$10^{-8}$	极小	较大	舍入主导
$10^{-15}$	几乎零	很大	差分结果不可靠

实际验证中，可以扫描 eps 从 $10^{-2}$ 到 $10^{-12}$，画出误差随 eps 的变化曲线。曲线先下降再上升的拐点区域就是最佳 eps 范围。

反事实地看，如果不做导数验证就上线，一个雅可比中的符号错误可能只在某些关节配置下造成力矩异常。这类 bug 在随机测试中可能数千次都不暴露，却在真机上的特定动作中突然出现。有限差分验证的成本很低（离线测试），但防护价值极高。

⚠️ 常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
编程	把 IPOPT 回调指针保存到类成员	下次迭代崩溃	指针只在回调期有效	限定指针作用域
概念	用 CasADi 在线求解 NLP	延迟不可控	CasADi 不是实时求解器	用 acados 生成代码
数值	eps 选太小做有限差分	验证误报”导数错误”	浮点舍入主导	扫描 eps 找最佳范围
思维	手写导数”看起来对”就跳过验证	某些场景力矩异常	符号或索引错误	离线有限差分对比
工程	不做标度化直接调权重	调参像猜数字	物理量级差异过大	先归一化变量再设权重

练习¶

用有限差分验证一个圆形障碍距离函数的梯度，改变 eps，观察截断误差和舍入误差的折中，画出误差曲线。
把一个力变量从牛顿改成体重归一化，比较 IPOPT 迭代次数和 KKT 残差。
用 CasADi 生成一个二轮车模型的离散动力学函数，再在 C++ 中调用生成的 C 函数，说明输入输出数组的所有权。
解释 SQP-RTI 的 preparation/feedback 分离如何降低 NMPC 的延迟不确定性。

1.3b acados 工作流与实时 NMPC 部署 ⭐⭐⭐¶

这一节解决的问题是：从 CasADi 模型到嵌入式 MPC C 代码的完整路径是什么样的。

动机：从"能离线求解 NLP"到"能在控制器中 30 Hz 在线运行"¶

用 IPOPT 验证轨迹可行性和用 acados 在线运行 MPC 是两个完全不同的工程目标。 IPOPT 允许数秒求解，acados 必须在毫秒级完成。 IPOPT 每次可以从零开始求解，acados 依赖 SQP-RTI 和热启动实现单步 QP 近似。

这个差距类似用 MATLAB 验证滤波器设计和用 FPGA 实时执行滤波。算法相同，工程形态完全不同。

acados 工作流概览¶

acados 部署流程

1. Python 端建模
   ├── 用 CasADi 定义连续时间或离散时间动力学
   ├── 定义代价函数（跟踪误差、控制能量）
   ├── 定义路径约束（力矩限幅、状态限幅、摩擦锥）
   └── 定义终端约束和终端代价

2. Python 端配置 OCP
   ├── 设置 horizon 长度和步数
   ├── 选择积分方法（ERK、IRK）
   ├── 选择 NLP 求解策略（SQP、SQP-RTI）
   ├── 选择 QP 后端（HPIPM、OSQP）
   └── 设置 code generation 选项

3. 代码生成
   ├── acados 生成 C 代码和 Makefile
   ├── 包含求解器、线性代数、模型函数
   └── 不依赖 Python 运行时

4. C++ 端集成
   ├── 编译生成的 C 代码为库
   ├── C++ 控制器调用 acados C API
   ├── 每周期：设置 x0 → 更新参考 → 求解 → 读取 u0
   └── 监控状态码和求解时间

C++ 端调用 acados 的最小骨架¶

// 概念性骨架：真实项目需要包含 acados 生成的头文件。
// 文件名和函数名由 acados code generation 决定。

struct AcadosMpcWrapper {
    // acados 生成的 capsule 持有求解器内部状态和工作区。
    // void* capsule = nullptr;
    int N = 20;  // horizon 步数，与生成时一致。

    bool init() {
        // 创建 capsule 并初始化。
        // capsule = my_model_acados_create_capsule();
        // my_model_acados_create(capsule);
        return true;
    }

    bool solve(const double* x0, double* u0) {
        // 1. 设置当前状态约束 x(0) = x0。
        // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", x0);
        // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", x0);

        // 2. 求解（SQP-RTI 模式下只做一步 QP）。
        // int status = my_model_acados_solve(capsule);

        // 3. 读取第一个输入。
        // ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", u0);

        // 4. 返回状态。
        // return status == ACADOS_SUCCESS;
        return true;
    }

    void shutdown() {
        // 释放 capsule 和工作区。
        // my_model_acados_free_capsule(capsule);
    }
};

这段代码中注释掉的 API 是 acados C 接口的典型调用模式。真实项目中这些函数名由 code generation 步骤决定，通常包含模型名称前缀。关键原则是：C++ 端只做设置、调用和读取，不做模型建模。

acados 与 OCS2 的对比¶

OCS2 是 ETH 团队开发的另一个实时最优控制框架。它和 acados 解决类似问题，但设计哲学不同。

维度	acados	OCS2
建模端	CasADi (Python)	C++ 模板或 CasADi
生成模式	离线生成 C 代码	在线 C++ 编译期或运行时
QP 后端	HPIPM、OSQP	自带 SQP、HPIPM 适配
部署形态	自包含 C 库	C++ 库
适合场景	嵌入式、明确模型、固定结构	研究、灵活模型、ROS2 集成
学习曲线	Python 建模简单，C 接口需适配	C++ 全栈，接口丰富但复杂

两者不是替代关系。如果目标是生成最紧凑的嵌入式 MPC 代码，acados 更自然。如果目标是在 ROS2 中灵活实验不同模型和约束，OCS2 更方便。

⚠️ 常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
编程	在 C++ 端修改 acados 生成代码	重新生成后丢失修改	生成代码是输出不是源码	修改 Python 端模型
概念	认为 acados 可以动态变维	运行时改 horizon 失败	代码生成时维度固定	不同场景生成不同求解器
工程	忽略 acados 返回的状态码	上线后偶发数值错误	求解器可能未收敛	每周期检查状态码
思维	把 CasADi 在线求解当作 acados	延迟不可控	CasADi 不是实时框架	acados 生成代码后离线

练习¶

用 CasADi 建模一个质点 MPC，选择 acados SQP-RTI 和 HPIPM 后端，生成 C 代码并编译。
在 C++ 中封装 acados 求解器，要求初始化和求解分离，状态码转换为枚举。
对比同一模型用 IPOPT 和 acados 的求解时间，解释为什么 acados 快但 IPOPT 更灵活。
设计 acados 求解失败时的降级策略：使用上一帧输入、切换保守控制或急停。

1.4 求解器适配层设计 ⭐⭐¶

这一节解决的问题是：怎样把第三方优化库封装成可维护、可测试、可替换的工程模块。

分层原则¶

不要让控制器业务代码直接散落求解器 API。一个稳健的适配层至少分四层。

层次	责任	允许依赖	不应包含
建模层	组装变量、约束、代价	Eigen、机器人模型	第三方求解器细节
标准问题层	保存 QP/NLP 标准形式	Eigen/Sparse	控制器业务判断
适配层	调用 OSQP/ProxQP/IPOPT	具体库头文件	机器人语义
策略层	失败降级、热启动重置	状态机、日志	矩阵组装细节

这种分层像硬件驱动。上层说“我要读编码器”，不应该知道 SPI 寄存器布局。控制器说“我要解 QP”，也不应该知道求解器内部工作区怎么更新。

状态码转换¶

第三方求解器的状态码通常很细。控制器需要的是少数明确动作。

enum class SolveStatus {
    kOptimal,
    kMaxIteration,
    kPrimalInfeasible,
    kDualInfeasible,
    kNumericalError,
    kInvalidProblem
};

enum class RecoveryAction {
    kUseSolution,
    kUsePreviousCommand,
    kRelaxReference,
    kResetWarmStart,
    kEnterSafeMode
};

RecoveryAction chooseRecovery(SolveStatus status) {
    switch (status) {
        case SolveStatus::kOptimal:
            return RecoveryAction::kUseSolution;
        case SolveStatus::kMaxIteration:
            return RecoveryAction::kUsePreviousCommand;
        case SolveStatus::kPrimalInfeasible:
            return RecoveryAction::kRelaxReference;
        case SolveStatus::kDualInfeasible:
        case SolveStatus::kNumericalError:
            return RecoveryAction::kResetWarmStart;
        case SolveStatus::kInvalidProblem:
            return RecoveryAction::kEnterSafeMode;
    }
    return RecoveryAction::kEnterSafeMode;
}

实时边界¶

求解器适配层必须清楚哪些函数可在实时线程调用。

函数	是否可在实时线程	原因
`configure()`	否	解析参数、分配内存、建立稀疏结构
`resize()`	否	改变变量维度通常重新分配
`updateNumerics()`	是，若已预分配	只写已有数组
`solve()`	视预算而定	必须记录最坏耗时
`getLastStats()`	是	只读固定大小结构

降级策略的层次设计¶

求解器失败后控制器不能只是"停止"。降级策略应分层。

失败严重度	策略	持续时间
精度不足但可行	使用当前解，记录警告	单周期
迭代上限但有中间解	使用中间解，下周期重置热启动	单周期
原始不可行	使用上一周期命令，放松参考	数周期
连续多周期失败	切换到保守控制器（站立、阻尼）	直到恢复
数值异常（NaN/Inf）	输出零力矩或安全保持	直到手动恢复

降级不是承认失败。它是把"求解器不可用"视为正常工况之一。就像汽车 ABS 系统：传感器失效时不是停止刹车，而是切换到保守制动模式。

本质洞察：求解器适配层的核心价值不是让求解更快，而是让失败可控。控制器的安全性不应依赖求解器永远成功。

适配层的测试策略¶

适配层测试不应只测"正常 QP 能解出来"。它应覆盖五类场景。

测试类别	输入特征	断言
解析小问题	手算已知解	解值与手算一致
随机可行 QP	随机正定 Hessian	目标值跨库一致
不可行 QP	矛盾约束	返回不可行状态码
接触切换	上下界从松到紧	热启动后迭代次数合理
病态问题	条件数很大	不崩溃且状态码合理

#include <gtest/gtest.h>
#include <Eigen/Dense>

TEST(SolverAdapterTest, DetectsInfeasibleProblem) {
    // 构造矛盾约束：x >= 1 且 x <= 0。
    // 期望适配层返回不可行，而不是崩溃或默默返回零向量。
    Eigen::MatrixXd H = Eigen::MatrixXd::Identity(1, 1);
    Eigen::VectorXd g = Eigen::VectorXd::Zero(1);
    Eigen::VectorXd l(1), u(1);
    l << 1.0;
    u << 0.0;

    // SolveStatus status = adapter.solve(H, g, l, u, &x);
    // EXPECT_EQ(status, SolveStatus::kPrimalInfeasible);
}

日志与诊断¶

实时线程中不要直接打印。适配层可以写入固定大小统计结构，由非实时线程读取。

#include <array>
#include <atomic>

struct SolverStats {
    double solve_us = 0.0;
    int iter = 0;
    SolveStatus status = SolveStatus::kInvalidProblem;
};

class StatsBuffer {
public:
    void publish(const SolverStats& stats) {
        // 单写单读场景可用更完整的双缓冲。
        // 这里用原子索引说明生命周期：slots_ 永远存活，不在实时线程分配。
        const int next = 1 - index_.load(std::memory_order_relaxed);
        slots_[next] = stats;
        index_.store(next, std::memory_order_release);
    }

    SolverStats read() const {
        const int idx = index_.load(std::memory_order_acquire);
        return slots_[idx];
    }

private:
    std::array<SolverStats, 2> slots_{};
    std::atomic<int> index_{0};
};

练习¶

为 OSQP 和 ProxQP 设计同一个 IQpSolver 接口，要求业务层不包含任何第三方头文件。
给 SolveStatus 增加“解可用但精度不足”的状态，并设计控制器如何降级。
统计 10000 次 QP 求解的平均值、p95、p99、最大值，解释为什么平均值不足以说明实时安全。

1.5 代码验证：从 KKT 到回归测试 ⭐⭐¶

这一节解决的问题是：怎样确认求解器接入正确，而不是只确认“程序能跑”。

为什么"求解器返回成功"不够¶

求解器返回成功只表示它的内部收敛判据满足了。但这不代表你的问题被正确求解。常见的差距包括：

问题组装错了（Hessian 漏了一项、约束行列反了），但错的问题恰好有可行解。
求解器容差对你的应用太宽松。
稀疏结构和数值不匹配（结构指向错误位置）。
对偶变量没有被读取或被误解。

因此业务层应独立检查 KKT 残差。这不是不信任求解器，而是验证"你传给求解器的问题确实是你想解的问题"。

KKT 残差¶

对 QP 来说，求解结果至少应检查原始可行性、对偶可行性和互补性。即使第三方库返回成功，业务层也应该有轻量检查。

\[ r_p = \|Ax-b\|,\quad r_d = \|Hx+g+A^T\lambda\|,\quad r_c = \|\lambda_i(c_i(x)-b_i)\| \]

三个残差分别回答三个问题：

残差	物理含义	过大时说明
$r_p$	等式约束是否满足	动力学方程被违反
$r_d$	梯度条件是否满足	目标或约束组装有误
$r_c$	互补条件是否满足	活动集判断不准确

#include <Eigen/Dense>
#include <cmath>

struct KktResiduals {
    double primal = 0.0;
    double dual = 0.0;
    double complementarity = 0.0;
};

KktResiduals checkQpKkt(const Eigen::MatrixXd& H,
                        const Eigen::VectorXd& g,
                        const Eigen::MatrixXd& A,
                        const Eigen::VectorXd& b,
                        const Eigen::VectorXd& x,
                        const Eigen::VectorXd& lambda) {
    KktResiduals res;
    // 原始可行性：等式约束残差。
    res.primal = (A * x - b).norm();
    // 对偶可行性：拉格朗日梯度条件。
    res.dual = (H * x + g + A.transpose() * lambda).norm();
    // 互补性：这里是"仅等式约束"的简化版（A x = b），等式约束本身可行时该项天然趋于 0。
    // 对前文带上下界的不等式 QP（lb <= C x <= ub），完整互补条件应逐约束检查
    // lambda_i * (C_i x - lb_i) 与 lambda_i * (ub_i - C_i x)，活动约束对偶变量非零、非活动为零。
    res.complementarity = std::abs(lambda.dot(A * x - b));
    return res;
}

这段检查不应放在高频实时路径中（涉及矩阵乘法）。但在调试构建、回归测试和上机初期验证中，它是定位问题的利器。

单元测试样例¶

#include <Eigen/Dense>
#include <gtest/gtest.h>

TEST(QPSolverAdapterTest, SolvesOneDimensionalBoxQP) {
    // min 0.5 * x^2 - 2x, s.t. 0 <= x <= 1
    // 无约束最优是 x=2，受上界限制后最优是 x=1。
    Eigen::MatrixXd H(1, 1);
    H << 1.0;
    Eigen::VectorXd g(1);
    g << -2.0;
    Eigen::MatrixXd C(1, 1);
    C << 1.0;
    Eigen::VectorXd l(1), u(1);
    l << 0.0;
    u << 1.0;

    Eigen::VectorXd x(1);
    // 这里调用你的适配层。
    // ASSERT_TRUE(solver.solve(H, g, C, l, u, &x));
    x << 1.0;
    EXPECT_NEAR(x(0), 1.0, 1e-9);
}

回归场景¶

测试	目的	判据
解析小问题	验证符号和约束方向	与手算解一致
随机凸 QP	验证不同库一致性	目标值差小于阈值
不可行问题	验证状态码	不返回可用解
接触切换	验证热启动重置	无大力尖峰
标度变化	验证单位鲁棒性	迭代次数不过度增长

练习¶

手算二维 box QP 的解，并写成 gtest。
构造一个 Hessian 半正定但不正定的问题，观察求解器是否需要正则项。
把同一个 QP 分别传给两个求解器，比较最优目标值而不是逐元素比较解，解释为什么。

1.6 从问题结构到库选型：把优化器放在正确的位置 ⭐⭐⭐¶

这一节解决的问题是：为什么真正的求解器选型不是“哪个库最快”，而是先识别问题结构、接口边界、实时预算和数值条件。

先画变量图，不急着写 API¶

很多优化器接入失败，不是因为库不好，而是因为问题还没有被讲清楚。在机器人规控中，一个优化问题通常同时包含物理变量、辅助变量和诊断变量。如果这些变量在建模时混在一起，后面的库选型会变成凭感觉猜测。

一个稳健的流程是先画出变量图。变量图要回答四个问题：

哪些变量是真正由求解器决策的？
哪些量只是参数，会在每个控制周期更新？
哪些约束改变数值但不改变稀疏结构？
哪些状态跨周期保留，用于热启动和故障恢复？

以站立 WBC QP 为例，决策变量可以组织为：

\[ x = \begin{bmatrix} \ddot{q} \\ \lambda \\ \tau \\ s \end{bmatrix} \]

其中 $\ddot{q}$ 是广义加速度，$\lambda$ 是接触力，$\tau$ 是关节力矩，$s$ 是松弛变量。看起来这只是把变量拼起来，但拼接顺序决定了 Hessian 和约束矩阵的块结构。如果把同一类变量放在连续区间内，调试、切片、稀疏填充和热启动都会更清楚。

变量块	典型维度	物理含义	是否必须每周期存在	常见约束
$\ddot{q}$	$n_v$	全身加速度	是	动力学等式、任务跟踪
$\lambda$	$3n_c$	接触力	是，摆动脚可用上下界关掉	摩擦锥、法向力
$\tau$	$n_a$	执行器力矩	是	电机上下限
$s$	任务维度	软约束误差	可选	松弛非负、权重惩罚

这一步类似机械装配中的爆炸图。爆炸图不是为了制造零件，而是为了让工程师看清每个零件怎样连接。优化变量图也不是求解本身，而是让后续矩阵装配和库接口有共同语言。

本质洞察：求解器选型的第一层不是算法，而是问题的“形状”。变量维度、约束类型、稀疏模式、更新频率和失败策略共同决定了一个库是否合适。如果这些结构没有先被明确，任何 benchmark 数字都可能误导工程判断。

从结构判断问题族¶

同样写成“优化”，背后的数学问题族差别很大。最小二乘、QP、SOCP、NLP、OCP 的求解器接口不同，是因为它们暴露给算法的结构不同。

问题族	标准形态	可利用结构	常见机器人任务	典型库
非线性最小二乘	$\min \sum_i\\|r_i(x)\\|^2$	残差块、Jacobian、近似 Hessian	SLAM、标定、曲线拟合	Ceres
凸 QP	二次代价 + 线性约束	KKT 稀疏、活动集、热启动	WBC、线性 MPC	OSQP、ProxSuite
多阶段 OCP QP	分阶段 QP	Riccati 递推、块三对角	结构化 MPC	HPIPM、acados
一般 NLP	非线性代价 + 非线性约束	KKT、障碍函数、线搜索	离线轨迹优化、NMPC 原型	IPOPT
符号 OCP	可微模型 + 代码生成	自动微分、结构化 QP 后端	实时 NMPC	CasADi、acados

选择流程可以写成更可执行的版本：

先看目标和约束
├── 目标是残差平方和，硬约束很少？
│   ├── 是：Ceres 优先，特别适合 SLAM/标定/拟合
│   └── 否：继续看约束
├── 代价二次、约束线性、需要在线高频？
│   ├── 是：QP 求解器
│   │   ├── 稀疏大问题、精度要求中等：OSQP
│   │   ├── 小中规模、WBC、希望 Eigen 原生接口：ProxSuite
│   │   └── 严格 OCP 阶段结构：acados/HPIPM
│   └── 否：继续看非线性
├── 非线性约束必须保留，频率不高或离线？
│   ├── 是：IPOPT
│   └── 否：考虑 SQP-RTI、模型简化或把问题下放到慢线程

如果不做这一步，最常见的误用是把 Ceres 当成万能优化器。 Ceres 可以通过残差模拟边界惩罚，但它没有把一般不等式约束作为核心接口。当约束是“不能摔倒、不能超力矩、不能穿障碍”时，残差惩罚不应替代硬约束。

稀疏性：省的不是内存，而是重复工作¶

稀疏矩阵不只是”零很多”的矩阵。在优化器里，稀疏性真正的价值是把计算分成两个不同频率的阶段。这和控制系统中的”在线”和”离线”分离思想一致。离线阶段做一次昂贵的结构分析。在线阶段只做必要的数值更新。如果把两个阶段混在一起，每个控制周期都会重复做结构分析，造成不必要的延迟尖峰。

稀疏矩阵的存储格式也很重要。 Eigen 使用压缩列存储（CSC）作为默认格式。 OSQP 也要求 CSC 输入。如果你的矩阵以行主序存储，每次调用前要转换格式，这个转换本身可能成为瓶颈。

稀疏性真正的价值是把重复工作拆成两类：

阶段	做什么	是否经常变化	工程目标
symbolic	分析非零结构、排序、分配工作区	尽量不变	初始化阶段完成
numeric	填入当前数值、分解、迭代	每周期变化	实时路径只做必要更新

例如 MPC 的动力学约束：

\[ x_{k+1} - A_k x_k - B_k u_k = 0 \]

对每个阶段 $k$，它只连接相邻的 $x_k, u_k, x_{k+1}$。因此约束 Jacobian 是块带状矩阵。如果把它当成普通稠密矩阵，求解器会在大量零元素上浪费时间。

多阶段动力学约束的稀疏形状

row 0: [A0  B0  I   .   .   .]
row 1: [.   .   A1  B1  I   .]
row 2: [.   .   .   .   A2  B2  I]

只有相邻阶段相连，远距离阶段没有直接耦合。

这也是为什么 HPIPM/acados 对 OCP MPC 有优势。它们不是“更会算矩阵”，而是知道这些矩阵来自时间展开的动力学系统。通用 QP 求解器看到的是一个大稀疏矩阵；结构化 OCP 求解器看到的是一串由动力学连接的小问题。

在 C++ 适配层里，应该显式记录稀疏结构是否变化。接触切换时，推荐固定变量维度，通过上下界关闭摆动脚的力，而不是删除对应变量。删除变量会改变非零模式，进而触发 symbolic 阶段重做。

#include <Eigen/Sparse>
#include <cstddef>
#include <vector>

struct SparsityFingerprint {
    int rows = 0;
    int cols = 0;
    std::vector<int> outer;
    std::vector<int> inner;

    static SparsityFingerprint fromMatrix(const Eigen::SparseMatrix<double>& mat) {
        SparsityFingerprint fp;
        fp.rows = mat.rows();
        fp.cols = mat.cols();
        fp.outer.assign(mat.outerIndexPtr(), mat.outerIndexPtr() + mat.outerSize() + 1);
        fp.inner.assign(mat.innerIndexPtr(), mat.innerIndexPtr() + mat.nonZeros());
        return fp;
    }

    bool samePattern(const Eigen::SparseMatrix<double>& mat) const {
        // 只比较维度和非零位置，不比较数值。
        // 实时循环可以用这个检查防止误触发昂贵的结构更新。
        if (rows != mat.rows() || cols != mat.cols()) {
            return false;
        }
        if (outer.size() != static_cast<std::size_t>(mat.outerSize() + 1)) {
            return false;
        }
        if (inner.size() != static_cast<std::size_t>(mat.nonZeros())) {
            return false;
        }
        for (int i = 0; i <= mat.outerSize(); ++i) {
            if (outer[i] != mat.outerIndexPtr()[i]) {
                return false;
            }
        }
        for (int k = 0; k < mat.nonZeros(); ++k) {
            if (inner[k] != mat.innerIndexPtr()[k]) {
                return false;
            }
        }
        return true;
    }
};

上面这段代码不求解任何优化问题，但它保护了实时边界。一旦某次装配改变了稀疏模式，控制器就可以在非实时路径重建求解器，或进入保守降级，而不是在 1 kHz 循环中突然重新初始化。

接口边界：Ceres、OSQP、ProxSuite、IPOPT 的“自然语言”¶

每个库都有自己的自然语言。顺着库的自然语言建模，代码会短、错误少、性能稳定。逆着库的自然语言建模，最后会出现大量胶水代码和不可解释的调参。

库	自然语言	最舒服的变量组织	不舒服的建模方式
Ceres	残差块、参数块、局部参数化	位姿图、重投影误差、标定残差	大量硬不等式约束
OSQP	稀疏 CSC 矩阵、上下界约束	固定结构的线性约束 QP	高精度强非线性问题
ProxSuite	Eigen 矩阵、QP 对象工作区	WBC、小中规模 dense/sparse QP	大型符号建模和代码生成
IPOPT	全局目标、约束、Jacobian、Hessian	一般 NLP、离线轨迹优化	高频硬实时内层控制

Ceres 的优势是残差组织。它让每个观测误差像积木一样独立添加。如果问题的核心是“很多观测共同估计状态”，Ceres 很自然。但如果问题的核心是“变量必须满足一批硬边界”，Ceres 就不是最自然的工具。

OSQP 的优势是稀疏凸 QP 的稳健性。 ADMM 对病态问题通常比较宽容，warm start 也实用。它的边界是高精度和极低迭代预算。如果控制器要求非常小的 KKT 残差，OSQP 可能需要更多迭代。

ProxSuite 的优势是机器人控制友好的 QP 接口。 Eigen 原生、dense/sparse 两套接口、状态码清晰，适合 WBC 和小中规模 MPC。它的边界是生态规模和工程项目中已有依赖。如果团队已有成熟 OSQP 监控和调参体系，不一定为了接口更顺手就迁移。

IPOPT 的优势是一般非线性约束。它适合离线轨迹优化、慢频率重规划和验证模型可行性。它的边界是实时确定性。线搜索、障碍参数更新和线性求解器耗时都可能随场景变化。因此 IPOPT 常放在规划线程或工具链中，而不是 1 kHz 控制线程。

本质洞察：库的边界不是“能不能解”，而是“解这个问题时是否顺着它的核心假设”。 Ceres 假设问题可以拆成残差块，OSQP 假设凸 QP 的稀疏结构稳定，ProxSuite 假设 QP 工作区可复用，IPOPT 假设可以为非线性可行性付出更高且变化的计算成本。

实时性：把平均耗时换成预算表¶

求解器上线前，应该把“看起来很快”改写成预算表。预算表不仅记录求解时间，还记录矩阵装配、数据拷贝、状态码转换和诊断发布。

环节	500 Hz WBC 预算示例	30 Hz MPC 预算示例	风险
状态读取与坐标变换	100 us	300 us	传感器锁和拷贝
矩阵装配	250 us	2 ms	稀疏结构变化
求解器更新	80 us	1 ms	隐式分配
solve	700 us	8 ms	迭代次数尖峰
结果检查与降级	100 us	500 us	状态码处理不完整
安全余量	770 us	21 ms	系统负载和中断

预算表的作用不是把每个数写死，而是暴露隐藏成本。很多项目只统计 solve()，却忽略 update() 中的矩阵格式转换。如果每周期把 row-major 稠密矩阵转换成 CSC 稀疏矩阵，真正的成本不在求解器，而在接口胶水。

下面的计时器示例强调两个点：使用单调时钟；记录最大值而不是只记录平均值。

#include <algorithm>
#include <array>
#include <chrono>
#include <cstddef>

class SolveTimeWindow {
public:
    void push(double us) {
        samples_[index_] = us;
        index_ = (index_ + 1) % samples_.size();
        if (count_ < samples_.size()) {
            ++count_;
        }
    }

    double maxUs() const {
        // 最大值用于发现实时尖峰；没有样本时返回 0。
        if (count_ == 0) {
            return 0.0;
        }
        return *std::max_element(samples_.begin(), samples_.begin() + count_);
    }

private:
    std::array<double, 1024> samples_{};
    std::size_t index_ = 0;
    std::size_t count_ = 0;
};

template <typename Fun>
double measureOnceUs(Fun&& fun) {
    const auto t0 = std::chrono::steady_clock::now();
    fun();
    const auto t1 = std::chrono::steady_clock::now();
    return std::chrono::duration<double, std::micro>(t1 - t0).count();
}

如果不记录尾部延迟，会出现一种典型错觉：平均 200 us 的 QP 看起来足够快，但每隔几千次出现一次 3 ms 尖峰。对离线优化来说，这个尖峰无关紧要。对高频控制来说，它就是一次错过周期。

数值稳定性：先处理量纲，再讨论算法¶

数值稳定性经常被误解为”换一个更强的求解器”。实际工程里，很多不稳定来自变量量纲和约束尺度。比如同一个 QP 里同时出现角度 rad、位置 m、力 N、力矩 N·m，数值量级可能相差 $10^3$ 到 $10^6$。

数值问题可以类比为乐队调音。如果一个乐器比另一个响一千倍，调音师的麦克风会被响乐器饱和，安静乐器的信号就丢失了。 KKT 系统里的”调音师”就是浮点运算。变量量级差异过大时，浮点精度会被大变量消耗，小变量的梯度信息被舍入吞没。

为什么这对机器人控制特别重要？因为控制问题天然涉及不同物理量。一个足式机器人的 QP 可能同时包含关节加速度（$\sim 10$ rad/s$^2$）、接触力（$\sim 100$ N）、关节力矩（$\sim 20$ N$\cdot$m）和松弛变量（$\sim 0.01$）。不做标度化时，Hessian 的对角线跨越多个数量级，条件数很大，迭代行为不稳定。

求解器看到的不是“米”和“牛顿”，而是一组浮点数。如果某些列比其他列大很多，KKT 系统就会病态。病态系统会放大舍入误差，导致迭代次数增加、状态码不稳定、解对微小扰动敏感。

现象	数值根因	工程修复
迭代次数随场景大幅波动	约束行尺度不一致	行归一化或变量标度化
接触力出现很小负值	容差与物理阈值混淆	明确求解容差和安全裕度
Hessian 非正定	任务权重写错或少了正则	加 $\epsilon I$ 并检查任务符号
warm start 后更慢	初值落在错误活动集附近	接触切换时局部重置

变量标度化可以写成：

\[ x = S_x \bar{x} \]

其中 $\bar{x}$ 是求解器内部变量，$S_x$ 是把无量纲变量映射回物理量的尺度矩阵。把原始 QP 代入：

\[ \frac{1}{2}x^T Hx + g^T x = \frac{1}{2}\bar{x}^T(S_x^T H S_x)\bar{x} + (S_x^Tg)^T\bar{x} \]

约束也随之变成：

\[ l \le A S_x \bar{x} \le u \]

这说明标度化不是简单改权重，而是对变量坐标系做变换。坐标系变得均衡，求解器的线性代数才更稳定。

#include <Eigen/Dense>

struct ScaledQp {
    Eigen::MatrixXd H_bar;
    Eigen::VectorXd g_bar;
    Eigen::MatrixXd A_bar;
};

ScaledQp scaleQp(const Eigen::MatrixXd& H,
                 const Eigen::VectorXd& g,
                 const Eigen::MatrixXd& A,
                 const Eigen::VectorXd& variable_scale) {
    // variable_scale(i) 表示第 i 个求解器变量对应的物理尺度。
    // 例如力变量可用体重 mg，位置变量可用 1 m。
    const Eigen::MatrixXd S = variable_scale.asDiagonal();
    ScaledQp out;
    out.H_bar = S.transpose() * H * S;
    out.g_bar = S.transpose() * g;
    out.A_bar = A * S;
    return out;
}

标度化以后，代价权重才更像“控制偏好”。没有标度化时，权重同时承担单位换算和控制偏好，调参会变成猜数字。

四个库的工程边界对照¶

最后把选型落到可执行的边界。下面的表不是排名，而是把库放到它更自然的位置。

场景	首选	可选	不推荐的原因
相机-激光外参标定	Ceres	IPOPT	QP 不适合残差块几何
500 Hz WBC 小中规模 QP	ProxSuite	OSQP	IPOPT 非线性能力多余，实时尾部难控
稀疏线性 MPC 原型	OSQP	ProxSuite sparse	Ceres 不表达硬线性约束
结构化 NMPC 代码生成	acados	CasADi + 自定义后端	直接 IPOPT 在线求解尾部延迟大
离线复杂轨迹优化	IPOPT	SNOPT、CasADi	OSQP 只能解线性约束 QP
SLAM 位姿图	Ceres	g2o、GTSAM	QP/NLP 接口会丢失残差块优势

一个实用的团队规则是：慢线程可以使用表达力强的工具，快线程必须使用结构明确的工具。离线轨迹优化可以用 IPOPT 验证可行轨迹。在线 MPC 可以把非线性问题线性化成 QP，再交给 OSQP、ProxSuite 或 acados。最内层 WBC 则应尽量固定变量维度、固定稀疏结构、固定内存布局。

从原型到生产的迁移路径¶

很多机器人项目从原型到生产会经历求解器迁移。理解典型迁移路径可以避免走弯路。

典型迁移路径

原型阶段：
  Python + CasADi + IPOPT
  → 快速验证模型可行性，不关心实时
  → 确认约束、变量、代价函数的正确性

工程验证阶段：
  C++ + OSQP 或 ProxSuite
  → 线性化模型后用 QP 在线求解
  → 验证热启动、稀疏结构和实时预算
  → 记录 p99 和最大耗时

生产阶段：
  C++ + acados（NMPC）或 ProxSuite/OSQP（线性 MPC/WBC）
  → 代码生成或固定适配层
  → 完整降级策略和故障恢复
  → CI 自动测试和回归验证

不要试图跳过原型阶段直接做生产代码。用 IPOPT 验证模型花几小时，可以避免在 C++ 中调试几天。反过来，也不要把原型阶段的 Python IPOPT 直接部署到控制器。两者之间需要明确的工程化过程。

选型决策树的工程版本¶

把前面的讨论总结成一个可执行的决策树：

你的优化问题是什么类型？

1. 核心是残差平方和，约束很少或只有简单边界？
   → Ceres（SLAM、标定、拟合）
   → 退出

2. 核心是二次代价 + 线性约束？
   → 进入 QP 选择：
   a. 变量 < 50，稠密，需要极好热启动？
      → ProxSuite dense 或 qpOASES（遗留）
   b. 变量 > 50，稀疏，中等精度？
      → OSQP
   c. 变量 > 50，稀疏，高精度？
      → ProxSuite sparse
   d. 明确 OCP 阶段结构？
      → acados/HPIPM

3. 需要非线性约束？
   a. 离线或低频（< 10 Hz）？
      → IPOPT（直接或通过 CasADi）
   b. 在线高频（> 20 Hz）？
      → acados SQP-RTI
   c. 问题太复杂无法实时？
      → 重新审视建模：能否简化模型、分层控制或把 NLP 放到慢线程

4. 不确定？
   → 先用 CasADi + IPOPT 离线验证
   → 再根据实时需求选择在线方案

常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
概念	先选库再改问题	API 勉强接上但调参混乱	问题族没有识别清楚	先画变量图和约束图
编程	每周期重建稀疏矩阵结构	求解时间偶发尖峰	symbolic 阶段进入实时路径	固定非零模式，只更新数值
数值	用权重弥补单位混乱	某些场景迭代暴涨	KKT 病态	做变量和约束标度化
思维	把 Ceres 当通用约束优化器	轨迹看似平滑但越界	残差惩罚不是硬约束	硬边界使用 QP/NLP 约束接口
工程	只记录 `solve()` 时间	上线后仍有周期超时	装配和格式转换被忽略	记录完整 pipeline 耗时

练习¶

选一个站立 WBC QP，画出变量块、等式约束块和不等式约束块，标出哪些矩阵块每周期只变数值。
把同一个接触力 QP 分别按“删除摆动脚变量”和“保留变量但上下界设为 0”两种方式装配，比较稀疏模式是否变化。
为一个含位置、角度、接触力的 QP 设计变量尺度矩阵 $S_x$，解释每个尺度的物理含义。
判断下列任务应使用 Ceres、OSQP、ProxSuite 还是 IPOPT：相机标定、站立 WBC、离线绕障轨迹优化、30 Hz 线性 MPC，并说明理由。

本章小结¶

知识点	关键结论	工程动作
最小二乘 vs 约束优化	残差不能替代硬约束	根据物理边界选择 QP/NLP
QP 算法族	ADMM/活动集/内点法/近端法各有假设	按问题精度、规模和结构选族
QP 求解器生态	OSQP 通用稳健，ProxQP 高精度，HPIPM 多阶段	固定变量维度、缓存稀疏结构
热启动	不只是复用旧解，还要局部重置	接触切换时重置力变量
NLP 求解器	IPOPT 通用但非实时，acados 可实时	离线用 IPOPT 验证，在线用 acados
自动微分与代码生成	CasADi 建模，acados 生成实时代码	验证导数、做变量标度化
SQP-RTI	每周期只做一步 SQP 近似	preparation 和 feedback 分离
适配层	业务层不应依赖具体求解器 API	统一状态码和恢复策略
稀疏结构	symbolic 和 numeric 阶段分离	初始化阶段固定稀疏模式
数值稳定性	标度化解决量纲混乱	先归一化变量再设权重
测试	成功返回不等于接入正确	检查 KKT、不可行和回归场景

跨章综合练习¶

结合本章 QP 适配层和第二章实时 Linux 知识：为一个 500 Hz WBC 控制器设计完整的求解器调用路径，要求初始化不在实时线程中，solve() 的最大耗时有预算，热启动和降级策略分层，日志通过无锁队列输出。画出线程、内存和数据流图。
结合本章 NLP/acados 和第四章 ROS2 架构知识：设计一个 MPC 节点，要求模型线性化在慢线程运行，QP 求解在快线程运行，ROS2 参数更新通过双缓冲传入，MPC 轨迹通过三缓冲传给下游 WBC。说明每条数据流的所有权和失败策略。
结合本章变量标度化和第五章 Eigen 高级用法：为一个含位置、角度和接触力的 QP 设计变量尺度矩阵 $S_x$，用 Eigen::Map 零拷贝读取求解器输出，检查对齐和生命周期。

优化库版本与 API 验证¶

机器人项目常使用的优化库版本和关键 API 如下。工程中应固定版本号，避免 API 变更导致编译或运行异常。

库	推荐版本范围	关键 API	已废弃 API 提示
OSQP	0.6.x	`OsqpEigen::Solver` 的 `initSolver`/`solve`	0.5.x 的 `setProblem` 签名不同
ProxSuite	0.6.x+	`proxsuite::proxqp::dense::QP<double>` 的 `init`/`update`/`solve`	早期版本命名空间不同
IPOPT	3.14.x	`Ipopt::TNLP` 回调接口	`eval_h` 签名历史上有变化
CasADi	3.6.x+	`ca.Function`、`generate`	`SX` 和 `MX` 的选择影响生成代码
acados	0.3.x+	`ocp_nlp_*` 系列 C API	版本间 API 变化较大，固定 tag
Ceres	2.x	`ceres::Problem::AddResidualBlock`	1.x 的 `AutoDiffCostFunction` 模板参数不同

本质洞察：优化库不是"装上就不用管"的基础设施。它们的 API、默认参数和行为都会随版本演进。固定版本号是工程可重复性的基本要求，就像固定编译器版本一样。

累积项目：规划控制优化工具箱¶

本章新增模块是 optimization_bridge。它提供一个统一的 QP 问题结构、两个求解器适配层和一组验证测试。

阶段 1：实现 QpProblem，包含 Hessian、gradient、等式、不等式和上下界。阶段 2：实现 OSQP 或 ProxQP 后端，要求初始化阶段分配内存，求解阶段只更新数值。阶段 3：加入状态码转换和失败恢复策略。阶段 4：加入解析小 QP、不可行 QP、接触切换 QP 三类测试。阶段 5：记录每次求解耗时，输出平均值、p99 和最大值。阶段 6：为 OSQP 和 ProxQP 实现同一个 IQpSolver 接口，验证两个后端在相同问题上的一致性。阶段 7：接入 CasADi 生成的离散动力学 C 代码，验证梯度与有限差分一致。阶段 8：设计变量标度化流程，使 QP 的条件数下降并记录迭代次数变化。阶段 9：为降级策略写回归测试：注入不可行约束后验证控制器输出安全命令而非崩溃。阶段 10：整合实时循环（来自第二章）和无锁数据通道（来自第三章），完成一个可度量的 500 Hz WBC 求解闭环。

延伸阅读¶

资料	难度	阅读目的
OSQP 文档与论文（Stellato et al. 2020）	⭐	理解 ADMM、warm start 和状态码
ProxSuite 文档与论文（Bambade et al. 2022）	⭐⭐	理解近端 QP 在机器人控制中的优势
qpOASES 文档（Ferreau et al. 2014）	⭐⭐	理解活动集法和参数式热启动
HPIPM 论文（Frison & Diehl 2020）	⭐⭐⭐	理解多阶段内点法和 Riccati 结构
Nocedal & Wright, Numerical Optimization	⭐⭐⭐	系统理解 KKT、SQP、内点法
acados 文档与教程	⭐⭐⭐	理解代码生成 NMPC 工作流
CasADi 文档	⭐⭐	学习符号建模和自动微分
OCS2 文档和教程	⭐⭐⭐	学习 C++ 实时 OCP 框架
Boyd & Vandenberghe, Convex Optimization	⭐⭐⭐	凸性、对偶性和内点法基础
Di Carlo et al. 2018, MIT Mini Cheetah MPC	⭐⭐	理解 QP MPC 在足式机器人中的实际应用

故障排查手册¶

症状	可能原因	排查步骤	处理
QP 偶发不可行	接触状态与力约束冲突	打印每只脚上下界和接触标志	固定变量维度，用边界关闭摆动脚
求解时间出现尖峰	每周期改变稀疏结构	记录非零模式和初始化次数	初始化结构，只更新数值
解满足代价但违反约束	把硬约束写成罚函数	检查问题标准形式	改成显式等式或不等式
IPOPT 迭代很多	变量尺度差异过大	打印变量和约束量级	做归一化和权重重整
接触切换时力跳变	热启动沿用错误活动集	对比切换前后 active set	切换时重置相关 warm start
C++ 偶发崩溃	缓存了临时矩阵引用	用 ASan 和生命周期审计	适配层拥有数据或限制 view 作用域
OSQP 高精度场景迭代爆炸	ADMM 不擅长高精度	打印 KKT 残差随迭代变化	换 ProxQP 或内点法
acados SQP-RTI 单步不收敛	模型线性化偏差大	打印 NLP 残差和 QP 残差	增加内迭代或检查模型精度
两个求解器给出不同解	多解或约束冗余	比较目标值而非逐元素比较	目标值应一致，解不一定唯一
CasADi 生成代码编译失败	版本不匹配	检查 CasADi 和 acados 版本	固定版本组合
降级策略后控制抖动	降级输出不连续	记录命令跳变幅度	加平滑过渡或限制变化率
Hessian 非正定警告	任务权重写错或正则项不足	打印 Hessian 对角线和特征值	加 $\epsilon I$ 并检查权重符号
稠密矩阵求解器内存溢出	问题太大用了 dense 接口	监控内存分配	切换到 sparse 接口或简化模型
两次调用之间结果突变	未正确热启动或问题结构变化	记录连续帧的目标值	检查稀疏模式是否一致
ProxQP 状态码为 MAX_ITER	精度要求过高或问题病态	放宽容差后观察是否收敛	做标度化或检查约束冗余
IPOPT 线性求解器报错	HSL 库缺失或版本不对	检查链接的线性求解器	确认 MA57/MUMPS 正确安装
warm start 后求解反而变慢	上一帧的活动集与当前帧差异过大	对比 warm start 和 cold start 的迭代次数	在约束结构剧变时重置 warm start
目标函数值正确但约束违反度较大	容差设置不当或约束缩放问题	检查约束残差的绝对值和相对值	收紧约束容差或做约束缩放
实时循环中偶发超时	求解器内部重新分配稀疏结构	监控每帧的内存分配次数	初始化阶段固定稀疏模式

QP 求解器的数值条件化与预处理 ⭐⭐⭐¶

工程问题：条件数差导致迭代爆炸¶

在足式机器人的 WBC 中，QP 的 Hessian 矩阵 $H$ 的条件数 $\kappa(H) = \|H\| \cdot \|H^{-1}\|$ 直接影响求解速度和数值稳定性。当不同任务的权重量级差异过大时（如位置追踪权重 $10^6$、关节正则化权重 $10^{-2}$），$\kappa(H)$ 可以达到 $10^8$ 以上，导致 ADMM 类求解器（如 OSQP）收敛极慢。

条件数差的根本原因是变量和约束的物理量纲不同。关节角度（rad）和接触力（N）的量级可能差三个数量级；笛卡尔位置（m）和角度（rad）的权重也不在同一尺度。

标度化（Scaling）策略¶

标度化的目标是让变量和约束的量级接近统一，从而降低条件数。

变量标度化：引入对角矩阵 $D$，用 $\hat{x} = D^{-1} x$ 替换原变量。变换后的 QP 变为：

\[\min_{\hat{x}} \frac{1}{2} \hat{x}^T (D H D) \hat{x} + (D g)^T \hat{x}$$ $$\text{s.t.} \quad (A D) \hat{x} = b\]

其中 $D$ 的对角元素选为原变量典型量级的倒数。例如，关节角度的典型范围是 $[-\pi, \pi]$，接触力的典型范围是 $[0, 500]$ N，则 $D_{角度} = 1$，$D_{力} = 1/500$。

约束标度化：对等式约束乘以行缩放因子，使每行约束的系数量级接近一致。

// 标度化的工程实现
void scaleQP(QpProblem& qp, const Eigen::VectorXd& var_scale) {
    // var_scale[i] = 变量 i 的典型量级
    Eigen::VectorXd inv_scale = var_scale.cwiseInverse();
    Eigen::MatrixXd D = inv_scale.asDiagonal();

    // 变换 Hessian：H_new = D * H * D
    qp.H = D * qp.H * D;
    // 变换梯度：g_new = D * g
    qp.g = D * qp.g;
    // 变换约束矩阵：A_new = A * D
    qp.A = qp.A * D;

    // 求解后反变换：x = D * x_hat
}

指标	标度化前	标度化后	效果
Hessian 条件数	$10^8$	$10^2 - 10^3$	显著下降
OSQP 迭代次数	200-500	20-50	求解加速
数值精度	约束违反度 $10^{-4}$	约束违反度 $10^{-8}$	精度提升

类比：标度化就像把"用毫米量身高、用千米量地图距离"统一成"都用米"。在统一的量纲下，数值计算不会因为量级差异而丢失有效位数。不做标度化的 QP 就像用同一把尺子量纳米和公里——尺子的精度不够同时覆盖两个尺度。

正则化与 Hessian 修正¶

当 Hessian 矩阵 $H$ 接近奇异或不正定时，求解器可能失败或给出数值垃圾。常见的正则化手段是添加小的对角项：

\[H_{\text{reg}} = H + \epsilon I\]

其中 $\epsilon$ 的选择是一个工程权衡：太小不起作用，太大会扭曲原问题的最优解。经验上，$\epsilon$ 设为 $H$ 最大对角元素的 $10^{-6}$ 到 $10^{-4}$ 倍是合理的起点。

在 WBC 中，如果某个任务的权重为零（如摆动腿不追踪接触力），对应的 Hessian 子块为零矩阵，整个 Hessian 变得奇异。正则化确保即使某些任务权重为零，QP 仍然可解。

⚠️ 概念误区：认为正则化总是安全的

正则化改变了原问题的解。$\epsilon$ 过大时，正则化项会"拉着"解趋向原点（因为 $\epsilon I$ 等价于对所有变量加了 L2 正则惩罚）。在接触力优化中，这可能导致所有脚的力被不必要地拉向零，破坏力分配的物理合理性。正确做法是：先检查 Hessian 的条件数和特征值，只在确实需要时才加正则化，且 $\epsilon$ 取能保证正定性的最小值。

跨章综合练习（补充）¶

[综合数值优化库集成 + 实时Linux与安全编程] 设计一个"优化求解器健康监控"模块：在 1 kHz 实时循环中记录每帧的 QP 求解时间、迭代次数、状态码和目标函数值，通过 SPSC 队列传递到非实时线程进行统计和可视化。要求监控模块本身在实时路径上零分配（预分配环形缓冲区），并在求解器连续 3 帧返回非最优状态码时触发降级策略。
[综合数值优化库集成 + Eigen深入] 用 Eigen 手动组装一个 2-contact 站立 WBC 的 QP 矩阵（12 维决策变量：6 维加速度 + 2×3 维接触力），然后分别用 OSQP 和 ProxQP 求解。对比两个求解器在 Hessian 条件数分别为 $10^2$、$10^5$、$10^8$ 时的迭代次数和求解精度。解释为什么 ProxQP 在高条件数场景下通常比 OSQP 更稳定。

约束来源	数学形式	违反后果	能否当成普通残差
关节限位	\(q_{\min} \le q \le q_{\max}\)	硬件撞限位	不应只靠罚函数
摩擦锥	\(\sqrt{f_x^2+f_y^2}\le \mu f_z\)	脚底打滑	不应只靠罚函数
动力学方程	\(M(q)\ddot q+h= S^T\tau+J^T\lambda\)	轨迹不可执行	必须等式满足
碰撞距离	\(d(q)\ge d_{\min}\)	撞到环境或自身	通常要硬约束
电机能力	\(\lvert\tau_i\rvert\le \tau_{i,\max}\)	电机饱和发热	必须显式处理

eps 值	截断误差	舍入误差	总误差
\(10^{-2}\)	较大	很小	截断主导
\(10^{-5}\)	很小	中等	接近最优
\(10^{-8}\)	极小	较大	舍入主导
\(10^{-15}\)	几乎零	很大	差分结果不可靠

残差	物理含义	过大时说明
\(r_p\)	等式约束是否满足	动力学方程被违反
\(r_d\)	梯度条件是否满足	目标或约束组装有误
\(r_c\)	互补条件是否满足	活动集判断不准确

变量块	典型维度	物理含义	是否必须每周期存在	常见约束
\(\ddot{q}\)	\(n_v\)	全身加速度	是	动力学等式、任务跟踪
\(\lambda\)	\(3n_c\)	接触力	是，摆动脚可用上下界关掉	摩擦锥、法向力
\(\tau\)	\(n_a\)	执行器力矩	是	电机上下限
\(s\)	任务维度	软约束误差	可选	松弛非负、权重惩罚

问题族	标准形态	可利用结构	常见机器人任务	典型库
非线性最小二乘	\(\min \sum_i\\|r_i(x)\\|^2\)	残差块、Jacobian、近似 Hessian	SLAM、标定、曲线拟合	Ceres
凸 QP	二次代价 + 线性约束	KKT 稀疏、活动集、热启动	WBC、线性 MPC	OSQP、ProxSuite
多阶段 OCP QP	分阶段 QP	Riccati 递推、块三对角	结构化 MPC	HPIPM、acados
一般 NLP	非线性代价 + 非线性约束	KKT、障碍函数、线搜索	离线轨迹优化、NMPC 原型	IPOPT
符号 OCP	可微模型 + 代码生成	自动微分、结构化 QP 后端	实时 NMPC	CasADi、acados

指标	标度化前	标度化后	效果
Hessian 条件数	\(10^8\)	\(10^2 - 10^3\)	显著下降
OSQP 迭代次数	200-500	20-50	求解加速
数值精度	约束违反度 \(10^{-4}\)	约束违反度 \(10^{-8}\)	精度提升