140_最优控制C++工程实践

博士前数学路线图 · 第三批 · 专题 3.14：最优控制与 MPC 的 C++ 工程实践——从手写算法到生产级部署¶

导言：把理论浇筑成可以在真机上跑的代码¶

本专题是第三批（最优控制与 MPC）的收官工程专题，目标是将 3.1–3.13 的全部数学理论——从 LQR 的代数 Riccati 方程到 iLQR 的二阶贝尔曼展开、从 NMPC 的 SQP/RTI 到 Tube MPC 的 mRPI 收紧、从 CBF 的李导数约束到多接触 DDP——全部用 C++ 代码落地，形成"数学公式 → 手写 Eigen 算法 → 生产级框架 → 嵌入式部署 → 真实机器人"的完整工程闭环。写这门课的真正理由只有一个：最优控制的论文公式和能在 Jetson Orin 上以 500 Hz 无抖动运行的代码之间，横亘着整整一个博士阶段的工程知识。许多研究者能在 Python 里让 MPC "work",但一旦要求换 aarch64、加 PREEMPT_RT、和 ros2_control 对接、和 RL 策略串成安全滤波器，就会在对齐崩溃、链接顺序、BLASFEO target 失配、SIGILL 非法指令、QP warm-start 失效、实时线程被 ROS pub/sub 抢占等问题上逐一踩坑。本专题按照"先手写后框架、先仿真后实机、先单机后嵌入式"的顺序，用一份系统化的学习大纲把这些坑铺成台阶。

读者画像：已完成 3.1–3.13（掌握 Riccati、iLQR/DDP、NMPC、Tube MPC、CBF、Pinocchio 李群动力学等全部理论），有中等以上 C++ 经验，目标是把数学转成可在桌面仿真与 ARM 实机上跑的生产级 stack。完成后你应能：(i) 不依赖任何框架从零写出 LQR/iLQR 并跑通 cart-pole swing-up；(ii) 以 acados+HPIPM+BLASFEO 为核心写一个 13 维四旋翼 NMPC，在 Jetson Orin 上 200 Hz 稳定运行；(iii) 把 Crocoddyl/OCS2/Pinocchio 作为高阶工具集成到四足机器人的 MPC+WBC 双层栈；(iv) 用 CBF-QP 做 RL 策略的安全滤波；(v) 写出 PREEMPT_RT + ros2_control + Iceoryx 零拷贝的完整实时节点。总预算 32–50 周（8–12 个月全职），分成 A/B/C/D/E 五部分。

Part A：C++ 工程基础设施（§3.14.1–§3.14.3）¶

§3.14.1 控制算法的 C++ 工具链搭建¶

严格定义：一个生产级控制项目的"工具链"指的是把源代码变成可部署二进制的全过程——编译器（g++/clang++）、构建系统（CMake）、包管理（apt/vcpkg/FetchContent）、测试框架（GoogleTest/Catch2）、基准框架（Google Benchmark）、静态分析（clang-tidy/cppcheck）、CI（GitHub Actions）的有机组合。工具链的每一环都不能拖累实时性：所有带隐式堆分配的 C++ 标准构造（std::string 拼接、std::vector::resize、std::shared_ptr 拷贝、异常栈展开）都不得出现在热路径。

CMake 现代实践：核心口号是 target-based——每个库/可执行都是 target，依赖通过 target_link_libraries 以 PUBLIC/PRIVATE/INTERFACE 三种可见性传递。PUBLIC 表示"自身使用并传递给下游"，PRIVATE 表示"只自身用"，INTERFACE 表示"自身不用只传递"。这取代了十年前的 include_directories()/link_libraries() 全局污染模式。find_package 有 MODULE 和 CONFIG 两种解析模式：前者靠 CMake 自带或项目提供的 Find<Pkg>.cmake 脚本，后者靠包作者提供的 <Pkg>Config.cmake。Eigen3、Pinocchio、acados、ProxSuite 均提供 CONFIG 模式，是首选；CMAKE_FIND_PACKAGE_PREFER_CONFIG=ON 可让 CONFIG 优先。CMake 3.14+ 的 FetchContent_Declare + FetchContent_MakeAvailable 在 configure 时下载并 add_subdirectory，适合把 Pinocchio、GoogleTest 这类 header-dominant 或标准 CMake 项目绑定到工程里。

典型 CMakeLists.txt 三层结构（库 + 测试 + 可执行）：

cmake_minimum_required(VERSION 3.20)
project(mpc_ctrl LANGUAGES CXX)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

find_package(Eigen3 3.4 REQUIRED NO_MODULE)
include(FetchContent)
FetchContent_Declare(pinocchio GIT_REPOSITORY https://github.com/stack-of-tasks/pinocchio.git GIT_TAG v3.2.0)
FetchContent_MakeAvailable(pinocchio)

add_library(mpc_core src/mpc_core.cpp src/riccati.cpp)
target_include_directories(mpc_core PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_link_libraries(mpc_core PUBLIC Eigen3::Eigen pinocchio::pinocchio)
target_compile_features(mpc_core PUBLIC cxx_std_17)
target_compile_options(mpc_core PRIVATE
  $<$<CONFIG:Release>:-O3 -march=native -DNDEBUG>
  $<$<CONFIG:Debug>:-O0 -g -fsanitize=address>)

Eigen3 线性代数核心：Eigen 是本专题所有手写算法的数值基础，三个认知必须立刻植入——(1) 固定大小（Matrix<double,N,N>）vs 动态（MatrixXd）：固定大小矩阵在栈上存储、默认构造不做堆分配，循环中反复构造安全；MatrixXd 总是堆分配。13 维四旋翼 NMPC 的状态量、Riccati 递推的 $P_k\in\mathbb{R}^{n\times n}$ 矩阵等一切维度在编译期已知的对象，必须**用固定大小。(2) **SIMD 对齐：SSE/NEON 需 16 字节、AVX 32 字节、AVX-512 64 字节对齐；Vector4d/Matrix4d/Quaterniond 等"fixed-size vectorizable"对象如果放进被 new 分配的类里，必须加 EIGEN_MAKE_ALIGNED_OPERATOR_NEW，放进 STL 容器里必须用 std::vector<Vector4d, Eigen::aligned_allocator<Vector4d>>，否则运行时断言失败或段错误。C++17 起 over-aligned new 使这个宏在大多数情况变为空，但为兼容性仍要写。(3) solve() 而非 inverse()：Riccati 反向递推中求 $(R+B^\top P B)^{-1}B^\top P A$ 必须用 Cholesky（.llt().solve(...) 或 .ldlt().solve(...)），绝不可 inverse() 再乘——后者既慢 2–3 倍又数值不稳。LDLT 不开根号比 LLT 更稳更快。

关键代码——Riccati 一步递推的正确写法：

\[K_k = (R + B^\top P_{k+1} B)^{-1} B^\top P_{k+1} A, \quad P_k = Q + A^\top P_{k+1} A - A^\top P_{k+1} B K_k\]

Eigen::Matrix<double, N, N> P, A, Q; Eigen::Matrix<double, N, M> B;
Eigen::Matrix<double, M, M> R; Eigen::Matrix<double, M, N> K;
auto S = R + B.transpose() * P * B;                 // SelfAdjoint
K.noalias() = S.llt().solve(B.transpose() * P * A); // 单次 Cholesky + 回代
P.noalias() = Q + A.transpose()*P*A - A.transpose()*P*B*K;

性能基准（-O3 -march=native）：Matrix<double,13,13> GEMM ≈150 ns，MatrixXd(13,13) GEMM ≈400–700 ns（含分配），LLT::solve(12×12) ≈1 μs，典型快 2–5×。

常见 bug 清单：(i) auto C = A*B; 得到的是 Product<> 表达式模板而非 MatrixXd，在循环里重复求值或悬垂引用，修法是 .eval() 或显式写类型；(ii) M = M.transpose(); 自别名，要用 M.transposeInPlace()；(iii) M.noalias() = A*B 只有当 LHS 与 RHS 真无别名时才合法；(iv) 不同编译单元使用 -mavx 不一致导致对齐崩溃；(v) Debug 构建跑 benchmark 得到的是 assertion-heavy 的慢数字，Eigen 通常慢 10×。

测试与基准：GoogleTest（fixture + 参数化 + mock）与 Catch2（单头文件 + SECTION）二选一；浮点比较用 EXPECT_NEAR(a,b,tol)，典型容差是 LQR 解对比解析值 1e-9，iLQR 对比 MATLAB 1e-6。Google Benchmark 测微秒级算法必须用 benchmark::DoNotOptimize(expr) + benchmark::ClobberMemory() 阻止编译器 DCE，否则测出 0 ns。CI 用 GitHub Actions matrix 覆盖 Ubuntu/macOS、gcc/clang、Debug/Release。

实时安全编码规范：热路径禁止 new/malloc（glibc ptmalloc 持锁且可能 page fault）、禁用异常（-fno-exceptions 去 unwinding）、禁用 RTTI（-fno-rtti 省 dynamic_cast）、禁用热路径 virtual dispatch（阻止内联与 SIMD 展开），替代方案：CRTP、std::variant+std::visit、模板多态。std::vector 启动期 reserve(n) 后只读写下标，严禁循环内 push_back/resize。日志用 fmtlib 写定长 buffer 或 async-log，std::string 拼接一律挪到非 RT 线程。

与 3.1–3.13 理论的映射：本节对应 3.3 的"线性代数与数值稳定性"前置，是把 Riccati、iLQR、NMPC 的所有矩阵公式从 LaTeX 变成 C++ 的词典。桥梁：下一节 §3.14.2 深挖 Eigen 提供的分解原语在数值稳定性上的具体陷阱。

§3.14.2 数值线性代数核心¶

严格定义：控制算法中 99% 的数值计算最终归结为**矩阵分解**。Riccati 递推需要对对称正定矩阵 $R+B^\top P B$ 做 Cholesky；最小二乘（轨迹拟合、系统辨识）需 QR；大规模 QP 需稀疏 LDL$^\top$；奇异系统需正则化；嵌入式需 square-root 形式防止协方差失去对称正定性。

Cholesky 分解 $A = LL^\top$（LLT）与 $A = LDL^\top$（LDLT）：前者要求 $A$ 严格正定，后者允许半正定（$D$ 是对角阵，可含零元）。Riccati 的核心选择是 LDLT——因为 $R+B^\top P B$ 在 $R$ 接近奇异（执行器代价极小）时可能近半正定，LDLT 用 Bunch-Kaufman 主元保持数值稳定且不开根号。官方文档明确指出列主序 + Lower 三角是最优组合，错配可导致 20% 减速。Eigen 用法：Eigen::LDLT<MatrixType> ldlt(A); → x = ldlt.solve(b);。

QR 分解：用于 rank-deficient 最小二乘和 square-root Riccati。后者把协方差 $P$ 的存储改为其 Cholesky 因子 $S$（$P = SS^\top$），整个 Riccati 递推在 $S$ 空间进行，通过 QR 更新保持 $S$ 下三角——即使浮点误差累积，$P$ 也绝不丢失对称正定性。Eigen 提供 HouseholderQR、ColPivHouseholderQR（主元版，rank-revealing）、FullPivHouseholderQR（全主元，最稳最慢）。

稀疏矩阵（Eigen::SparseMatrix）：CSR/CSC 格式，用于大规模 QP（OSQP 的 P、A 矩阵）、接触力学的 KKT 系统。注意 Eigen 稀疏性能远不如专业库（SuiteSparse、CHOLMOD），仅作原型；生产用 OSQP/HPIPM 自带分解。

数值稳定性三板斧：(1) 条件数监控：每次 Riccati 递推后计算 $\kappa(R+B^\top PB)$，超阈值（如 $10^{10}$）报警；(2) Tikhonov 正则化：$S \leftarrow S + \lambda I$，iLQR 中 $\lambda$ 即 Levenberg-Marquardt 参数，失败则 $\lambda \times 10$，成功则 $\lambda / 2$；(3) square-root 形式：Thornton-Bierman 的 UDU$^\top$ Kalman、Morf 的 SR-Riccati 在嵌入式单精度场景是必选。

LAPACK/BLAS vs Eigen vs BLASFEO：Eigen 自带的线代在小矩阵（$\le 30\times30$）上因为模板展开和 SIMD intrinsic 通常**快于 OpenBLAS/MKL**；但矩阵增大到 $100\times100$ 以上，MKL 的 GEMM kernel 明显占优。BLASFEO 是专为 $2\le n \le 300$ 的中小密集矩阵设计的线代库，采用 panel-major 内存布局（矩阵分成高度固定的水平 panel，panel 内列优先），比标准 column-major 更好利用 SIMD 寄存器和 cache，在 HPIPM 的 OCP-QP 中比 Eigen 快 3–10×。三者定位：Eigen = 通用 + 表达式模板 + 易集成；MKL/OpenBLAS = 大矩阵王者；BLASFEO = 小到中矩阵的 MPC/NMPC 专业户。

与理论的映射：本节把 3.3.8（Riccati 解法）、3.5（Kalman 滤波）、3.11（NMPC 的 QP 子问题）用到的所有数值原语串成"分解选择决策树"。桥梁：§3.14.3 进入自动微分——把手写 Jacobian 替换为符号或代码生成的自动 Jacobian。

§3.14.3 自动微分（AD）的 C++ 实现¶

严格定义：AD 是利用链式法则把任意可计算函数的导数精确到机器精度地求出来的技术，分**前向模式**（对每个输入变量传播切向量，适合输入少）和**反向模式**（先正向记录计算图，再反向传播伴随向量，适合输出少）。在最优控制中典型场景：动力学 $f:\mathbb{R}^{n_x+n_u}\to\mathbb{R}^{n_x}$ 的 Jacobian $(\partial f/\partial x, \partial f/\partial u)$，成本 $\ell$ 的梯度和 Hessian。

四种实现途径对比：

途径	模式	精度	30-DoF 动力学梯度	备注
有限差分 $(f(x+\varepsilon e_i)-f(x))/\varepsilon$	—	$\sim 10^{-5}$ 截断误差	$(n_q+1)\cdot T_f$	最简单但最慢最不准
`Eigen::AutoDiffScalar`	前向	机器精度	仅适用 $n\le 10$	纯头文件
CppAD（无 codegen）	反向	机器精度	≈150 μs	运算符重载，tape 开销
CppADCodeGen	反向+codegen	机器精度	≈10 μs	生成 C 并 dlopen，快 ~3 数量级
CasADi codegen	两者	机器精度	μs 级	符号微分 + CCS 稀疏性
Pinocchio 解析导数	专用	机器精度	5–15 μs	RSS 2018，多体链式递推

CppAD 用法：

std::vector<CppAD::AD<double>> X(n); X[0]=1.0; X[1]=2.0;
CppAD::Independent(X);
std::vector<CppAD::AD<double>> Y(m); Y[0] = X[0]*X[0]*exp(X[1]);
CppAD::ADFun<double> f(X,Y);
std::vector<double> x{2,1}, jac = f.Jacobian(x);  // m*n 行主序

CasADi C-API 嵌入：CasADi 生成的函数签名为 int f0(const double** arg, double** res, casadi_int* iw, double* w, int mem)；稀疏性以 CCS 内嵌，前两项 nrow/ncol，接 colind[ncol+1]、row[nnz]。acados 的整个模型函数生态（<model>_expl_ode_fun.c、<model>_cost_y_fun.c 等）都是这个格式。

工程决策：(a) 原型 / 教学：Eigen::AutoDiffScalar 或 CasADi Python 前端；(b) 机器人动力学：永远先试 Pinocchio 的 computeABADerivatives/computeRNEADerivatives，它是线性复杂度的解析链式递推，单次 30-DoF 梯度 ≈15 μs；(c) 通用代价函数 / 约束：CasADi Python 建模，generate C，FetchContent 进 C++ 工程；(d) acados 自带：模型定义直接用 CasADi SX，acados 自动生成所有 derivatives。

与理论映射：把 3.9 的 $f_x, f_u, l_{xx}$ 等符号落到具体库调用。桥梁：有了分解原语与 AD，§3.14.4 就可以完全手写 LQR/DARE。

Part B：从零手写核心算法（§3.14.4–§3.14.7）¶

§3.14.4 LQR/DARE 的 Eigen 实现¶

严格定义：给定离散时不变系统 $x_{k+1} = Ax_k + Bu_k$、二次代价 $J = \sum_{k=0}^\infty (x_k^\top Q x_k + u_k^\top R u_k)$（$Q\succeq 0, R\succ 0, (A,B)$ 可稳定），最优反馈 $u_k = -Kx_k$ 由 Discrete Algebraic Riccati Equation（DARE）$P = A^\top P A - A^\top P B(R+B^\top P B)^{-1}B^\top P A + Q$ 的稳定化解 $P$ 给出，$K = (R+B^\top PB)^{-1}B^\top PA$。

三种求解算法：

(i) 迭代法：从 $P_0 = Q$ 开始反复 $P_{k+1} \leftarrow Q + A^\top P_k A - A^\top P_k B(R+B^\top P_k B)^{-1}B^\top P_k A$，直到 $\|P_{k+1}-P_k\|_\infty < \varepsilon$（如 $10^{-10}$）。线性收敛，~100–500 次迭代。最简单、最易手写。

(ii) Schur 分解法（SciPy solve_discrete_are 默认）：构造辛矩阵 $Z = \begin{bmatrix}A+BR^{-1}B^\top A^{-\top}Q & -BR^{-1}B^\top A^{-\top}\\ -A^{-\top}Q & A^{-\top}\end{bmatrix}$，做实 Schur 分解并按稳定特征值排序，取前 $n$ 列 $[X_1; X_2]$，则 $P = X_2 X_1^{-1}$。精度最高、单次求解，无迭代。Eigen 通过 Eigen::RealSchur 可实现。

(iii) Doubling algorithm（结构化 SDA）：$A_0=A, G_0=BR^{-1}B^\top, H_0=Q$；$W_k=I+G_k H_k$；$A_{k+1}=A_k W_k^{-1}A_k$；$G_{k+1}=G_k+A_k W_k^{-1}G_k A_k^\top$；$H_{k+1}=H_k+A_k^\top H_k W_k^{-1}A_k$。二次收敛，~20 步精度达机器精度，是嵌入式场景首选。

有限时域 Riccati 反向递推（MPC 内部展开）： $$P_N = Q_f,\quad K_k = (R+B^\top P_{k+1}B)^{-1}B^\top P_{k+1}A,\quad P_k = Q + A^\top P_{k+1}A - A^\top P_{k+1}BK_k$$

完整 C++ 工程结构（库 + 头 + 测试）：

lqr/
├── include/lqr/dare.hpp      # 模板头文件：template<int N, int M>
├── src/dare.cpp              # 显式模板实例化
├── tests/test_dare.cpp       # 对比 Octave/Python 解
└── CMakeLists.txt

头文件核心：

template<int N, int M>
class DiscreteLQR {
 public:
  using StateMat = Eigen::Matrix<double,N,N>;
  using InputMat = Eigen::Matrix<double,N,M>;
  using GainMat  = Eigen::Matrix<double,M,N>;
  DiscreteLQR(const StateMat& A, const InputMat& B,
              const StateMat& Q, const Eigen::Matrix<double,M,M>& R);
  GainMat solve_infinite_horizon(double tol=1e-10, int max_iter=1000);
  std::vector<GainMat> solve_finite_horizon(int N_steps, const StateMat& Qf);
 private:
  StateMat A_, Q_, P_; InputMat B_; Eigen::Matrix<double,M,M> R_;
};

与 3.3.8/3.5 理论的逐行映射：DARE 迭代 ↔ 3.3.8 的值迭代证明；Schur 法 ↔ 3.3.8 关于辛结构的讨论；finite-horizon Riccati ↔ 3.5 LQR 的动态规划推导。每一行 C++ 都可以在讲义里找到对应公式。

常见 bug：(i) 未验证 $(A,B)$ 可控/可稳定导致迭代不收敛；(ii) $Q$ 取非对称矩阵 → Cholesky 失败，应 $Q \leftarrow (Q+Q^\top)/2$；(iii) 浮点累积使 $P$ 失去对称性，每次迭代强制 $P\leftarrow(P+P^\top)/2$；(iv) 闭环 $A-BK$ 特征值未检查是否在单位圆内，应加单元测试。

桥梁：LQR 是线性世界的封闭解；非线性世界的"局部 LQR"就是 iLQR，§3.14.5 将此推广。

§3.14.5 iLQR/DDP 的 Eigen 实现¶

严格定义：对非线性系统 $x_{k+1}=f(x_k,u_k)$ 和代价 $J=\sum\ell(x_k,u_k)+\ell_N(x_N)$，iLQR 在标称轨迹 $(\bar x,\bar u)$ 附近做二阶展开，反向递推贝尔曼方程的二阶近似得到局部最优反馈 $u_k = \bar u_k + \alpha k_k + K_k(x_k-\bar x_k)$。iLQR = DDP 去掉 $V_x\cdot f_{xx}$ 等二阶 tensor 项（Gauss-Newton 近似），工程上无差别。

Backward pass 公式（对 $k=N-1,\dots,0$）： $$Q_x = \ell_x + f_x^\top V_x',\quad Q_u = \ell_u + f_u^\top V_x'$$ $$Q_{xx} = \ell_{xx} + f_x^\top V_{xx}' f_x,\quad Q_{uu} = \ell_{uu} + f_u^\top V_{xx}' f_u,\quad Q_{ux} = \ell_{ux} + f_u^\top V_{xx}' f_x$$ $$k_k = -Q_{uu}^{-1}Q_u,\quad K_k = -Q_{uu}^{-1}Q_{ux}$$ $$V_x = Q_x - K_k^\top Q_{uu}k_k,\quad V_{xx} = Q_{xx} - K_k^\top Q_{uu}K_k$$

Forward pass：对 $k=0,\dots,N-1$，$u_k = \bar u_k + \alpha k_k + K_k(x_k-\bar x_k)$、$x_{k+1}=f(x_k,u_k)$。Armijo line search：从 $\alpha=1$ 按 $\alpha\leftarrow 0.5\alpha$ 回退直到 $J_{\mathrm{new}} \le J_{\mathrm{old}} + c_1\alpha\cdot\Delta J_{\mathrm{expected}}$（$c_1\approx 10^{-4}$）。若 $\alpha<\alpha_{\min}$ 仍失败，则增大正则化。

Levenberg-Marquardt 正则化：$Q_{uu}^{\mathrm{reg}} = Q_{uu} + \lambda I$。若 LDLT 失败（非正定）$\lambda \times 10$ 并重试；成功且 forward pass 接受 $\lambda / 2$。典型 $\lambda_0=10^{-6}$，$\lambda_{\max}=10^{10}$。

Box 约束（Tassa 2014 Control-Limited DDP）：把 $k_k$ 的求解从无约束 Newton 换成 boxQP：$\min\frac12 k^\top Q_{uu}k + Q_u^\top k$ s.t. $u_{\mathrm{lb}}\le\bar u_k+k\le u_{\mathrm{ub}}$，用 Projected Newton：迭代维护 free/clamped 活动集，free 子空间做 Newton step，clamped 分量投影到边界；对应的 $K_k$ 行 clamped 置零，保证反馈不违反 box。

完整 cart-pole swing-up 示例：状态 $[x,\theta,\dot x,\dot\theta]$，动力学 $$\ddot x = \frac{u + m_p\sin\theta(l\dot\theta^2 + g\cos\theta)}{m_c+m_p\sin^2\theta}$$ $$\ddot\theta = \frac{-u\cos\theta - m_pl\dot\theta^2\sin\theta\cos\theta - (m_c+m_p)g\sin\theta}{l(m_c+m_p\sin^2\theta)}$$

代价 $\ell = 10^{-2}u^2$，$\ell_N = 10(x-x^*)^\top(x-x^*)$，$x^* = (0,\pi,0,0)^\top$；$N=200$、$dt=0.02$、$u_{\mathrm{max}}=\pm 10$ N。典型收敛：~50 次迭代，总代价从 $\sim 10^5$ 降到 $\sim 10^1$。

工程结构：

ilqr/
├── include/ilqr/{ilqr.hpp, dynamics.hpp, cost.hpp}
├── examples/cartpole/{model.cpp, main.cpp}
└── CMakeLists.txt

ilqr.hpp 接口关键函数：backward_pass() 返回 std::pair<std::vector<k_vec>, std::vector<K_mat>>，forward_pass(alpha) 返回 $J_{\mathrm{new}}$，solve(max_iter, tol) 驱动迭代。

与 3.9/3.10 理论的逐行映射：backward pass 的 $Q_{xx}, Q_{uu}, Q_{ux}$ 公式一一对应 3.9 贝尔曼展开；Armijo + LM 对应 3.10 的 trust-region 讨论；Tassa boxQP 对应 3.10 控制约束的 Projected Newton 推导。

开源参考：kazuotani14/iLQR（~100+ stars）、joruof/ilqr（OpenMP 单头）、Drake vincekurtz/drake_ddp。

常见 bug：(i) 忘记 Hessian 的对称化 → LDLT 偶发失败；(ii) 数值微分步长与动力学尺度不匹配；(iii) line search 失败后没有递增 $\lambda$ 陷入死循环；(iv) boxQP 中 free/clamped 集反复跳动 → 加 working-set 改变次数上限。

桥梁：手写 iLQR 解决单次轨迹优化；滚动执行并加约束处理 → NMPC。

§3.14.6 NMPC 控制器的 C++ 实现（acados C-API）¶

严格定义：NMPC 在每个采样时刻求解 OCP $\min\sum_{k=0}^{N-1}\ell_k(x_k,u_k)+\ell_N(x_N)$ s.t. $x_{k+1}=f(x_k,u_k)$, $h(x_k,u_k)\le 0$, $x_0=\hat x$，取 $u_0^*$ 执行，下一时刻滚动。Real-Time Iteration（RTI）**是 Diehl 2002 提出的关键技巧：每个采样时刻只做**一次 SQP，拆成**preparation phase**（无需新测量，做积分+线性化+QP 矩阵装配）和 feedback phase（测量到达后仅解 QP + primal/dual 更新），反馈延迟最小化。

acados 五大 C 结构： - ocp_nlp_config：vtable，记录求解器/QP/dynamics/cost/constraints 模块的函数指针。 - ocp_nlp_dims：每 stage 的 $n_x, n_u, n_z, n_s, n_p, n_g, n_h, n_{bx}, n_{bu}$ 维度。 - ocp_nlp_in：数值数据（$W$、$y_{\mathrm{ref}}$、$l_{bx}$、$l_{bu}$、外部函数指针），BLASFEO panel-major 存储。 - ocp_nlp_out：求解结果（$u_x, \pi, \lambda, t, z$），通过 ocp_nlp_out_get(..., "u", buf) column-major 拷出。 - ocp_nlp_opts：RTI phase、tol、max_iter、qp_solver 选项。

生命周期（由 Python acados_template 生成的 <model>_acados_create/solve/free）：

capsule = model_acados_create_capsule();
model_acados_create(capsule);
ocp_nlp_config*  cfg = model_acados_get_nlp_config(capsule);
ocp_nlp_dims*    dims= model_acados_get_nlp_dims(capsule);
ocp_nlp_in*      in  = model_acados_get_nlp_in(capsule);
ocp_nlp_out*     out = model_acados_get_nlp_out(capsule);

SQP_RTI 拆分精确时序：

int phase = 1; ocp_nlp_solver_opts_set(cfg, opts, "rti_phase", &phase);
model_acados_solve(capsule);                       // preparation
// ... 等待新测量 x_meas ...
ocp_nlp_constraints_model_set(cfg, dims, in, 0, "lbx", x_meas);
ocp_nlp_constraints_model_set(cfg, dims, in, 0, "ubx", x_meas);
phase = 2; ocp_nlp_solver_opts_set(cfg, opts, "rti_phase", &phase);
model_acados_solve(capsule);                       // feedback
ocp_nlp_out_get(cfg, dims, out, 0, "u", u0);

HPIPM QP 后端：默认 PARTIAL_CONDENSING_HPIPM。关键选项：qp_solver_cond_N（partial-condensed horizon $N_2<N$，常设 5–10）、nlp_solver_max_iter（RTI 恒为 1）、regularize_method = CONVEXIFY/MIRROR/PROJECT、qp_solver_warm_start = 0/1/2。hessian_approx = GAUSS_NEWTON 在最小二乘型代价（$\ell = \|y-y_{\mathrm{ref}}\|^2_W$）上用 $J^\top WJ$ 近似 Hessian，稳定且快。

四旋翼 NMPC 完整示例（13 维状态）：$x = [p_{xyz},q_{wxyz},v_{xyz},\omega_{xyz}]$，$u = [T,\omega_{x,\mathrm{cmd}},\omega_{y,\mathrm{cmd}},\omega_{z,\mathrm{cmd}}]$ 或四个转子推力。动力学：$\dot p = v$、$\dot q = \frac12 q\otimes(\omega,0)$、$\dot v = R(q)(0,0,T/m) - (0,0,g)$、$\dot\omega = J^{-1}(\tau - \omega\times J\omega)$。CasADi Python 建模，acados 生成 C，C++ wrapper 调用。典型配置：$N=20$, $dt=50\text{ms}$, 求解时间 Orin 上 0.5–2 ms/次，可稳定 200–500 Hz 反馈。

C++ Wrapper 模板（按 acados 社区最佳实践）：

template<int NX, int NU, int N>
class AcadosMpc {
 public:
  AcadosMpc();
  ~AcadosMpc();
  int prepare();                                       // phase=1
  int feedback(const std::array<double,NX>& x_meas,
               std::array<double,NU>& u0);             // phase=2
  void set_reference(int stage, const double* yref);
 private:
  mymodel_solver_capsule* capsule_{};
  ocp_nlp_config* cfg_{}; ocp_nlp_dims* dims_{};
  ocp_nlp_in* in_{}; ocp_nlp_out* out_{};
  void* opts_{};
};

ROS2 集成：MPC solver 实例**不是线程安全**，必须绑定单一 CallbackGroupType::MutuallyExclusive。单 timer 模式：一个 100 Hz timer 顺序做 prepare + feedback，简单但频率受限。双线程模式：背景线程连续 prepare，高频 timer 接收状态后立刻 feedback，吞吐率更高。

链接顺序陷阱：g++ ... -lacados -lhpipm -lblasfeo -lm -lpthread——顺序固定，违反会报 undefined reference to blasfeo_dgemm_nt。

与 3.11/3.12 理论的映射：acados 的 SQP 步与 3.11.5 的 SQP 公式一一对应；RTI 拆分对应 3.11.7 的 RTI 推导；condensing 对应 3.12.4 的 QP 压缩。

桥梁：NMPC 的名义控制器+安全层是现代机器人控制的双层架构——§3.14.7 补齐安全层。

§3.14.7 Tube MPC / CBF-QP 安全层的 C++ 实现¶

严格定义：当系统含有界扰动 $w\in\mathcal{W}$ 时，Tube MPC（Mayne 2005）把真实约束 $X, U$ 在名义问题中收紧为 $X\ominus\mathcal{Z}, U\ominus K\mathcal{Z}$，其中 $\mathcal{Z}$ 是 mRPI（minimal Robust Positively Invariant）集合，确保真实状态始终落在名义轨迹 $\bar x_k$ 的 tube $\bar x_k\oplus\mathcal{Z}$ 内；CBF-QP（Ames 2017）则把"状态保持在安全集 $\mathcal{C}=\{x:h(x)\ge 0\}$"直接编成单步 QP 投影：$\min\|u-u_{\mathrm{nom}}\|^2$ s.t. $L_fh+L_ghu+\alpha(h)\ge 0$。

mRPI 的离线计算（Raković 2005 算法）： - 步 1：求最小 $s\ge 1$ 与 $\alpha\in[0,1)$ 使 $A_K^s\mathcal{W}\subseteq\alpha\mathcal{W}$（每行支撑函数 LP）。 - 步 2：$\mathcal{F}_s = \bigoplus_{k=0}^{s-1}A_K^k\mathcal{W}$。 - 步 3：$\mathcal{F}(\alpha,s) = (1-\alpha)^{-1}\mathcal{F}_s$，外近似 mRPI 且 $\mathcal{F}_\infty\subseteq\mathcal{F}(\alpha,s)\subseteq\mathcal{F}_\infty\oplus B_\varepsilon$。

多面体运算的 C++ 实现：V-rep↔H-rep 需 cddlib 或 Parma Polyhedra Library（无纯 Eigen 实现）；Minkowski 和（V-rep）= 顶点两两相加 + 凸包（qhull）；Pontryagin 差（H-rep 最实用形式）：$X\ominus\mathcal{Z} = \{x:Ax\le b - h_{\mathcal{Z}}(A^\top)\}$，$h_{\mathcal{Z}}$ 是 $\mathcal{Z}$ 的支撑函数，按行求 LP（或 $\mathcal{Z}$ 为 zonotope 时闭式）。工程实践：MATLAB MPT3 离线算出 $\mathcal{Z}$ 的 H-rep，导出 CSV，C++ 中 Eigen 加载为 MatrixXd。

约束收紧的在线实现：只需一次减法 b_tightened = b_X - supportVec(Z, A_X.transpose())；真实控制 $u = \bar u + K(x-\bar x)$。

CBF-QP 标准形式： $$\min_{u,\delta} \|u-u_{\mathrm{nom}}\|^2 + k\delta^2\quad\text{s.t.}\quad L_gh\cdot u \ge -\alpha(h) - L_fh - \delta,\ u_{\min}\le u\le u_{\max},\ \delta\ge 0$$

$\delta$ 软化松弛保证 CBF 与 $u$ box 约束冲突时仍可行。

HOCBF（相对度 $r>1$）：递归 $\psi_0=h$、$\psi_k=\dot\psi_{k-1}+\alpha_k(\psi_{k-1})$，要求 $\dot\psi_{r-1}+\alpha_r(\psi_{r-1})\ge 0$。控制输入 $u$ 只出现在 $\psi_r$ 中，约束仍仿射于 $u$ 故仍是 QP。

QP 求解器选择（CBF-QP 规模小，$n_u\le 12$，$m\le 20$）： - OSQP：CSC 稀疏，ADMM，eps_abs 默认 1e-3 对 CBF 不够必须设 1e-6 或开 polish，典型 50–200 μs；osqp_update_data_vec/_mat 在 sparsity 不变时极快。 - qpOASES：稠密 active-set，MPC 后的 condensed QP 上 10–40 μs 稳态。 - ProxQP（proxsuite）：primal-dual PMM 方法，对退化 QP 鲁棒，可给"最近可行解"——当多个 CBF 约束同时 active 时不 crash，是 CBF-QP 的理想后端，dense 后端 20–80 μs。 - HPIPM dense：无 OCP 结构时 CBF-QP 也可用。

Safety filter 架构：

RL/NN policy π(s) → u_nom → [CBF-QP 投影] → u_safe → actuator
                                    ↑ state x, SDF, joint limits, HOCBF

CBF 构造示例：(i) SDF 避障：$h(x)=\mathrm{SDF}(p(x),\mathcal{O})-r_{\mathrm{safe}}$，双积分器 $\ddot p=u$ 下相对度 2，用 HOCBF。(ii) 关节限位：$h_{\mathrm{pos}}=q-q_{\min}$ 对扭矩 $u$ 相对度 2，用位置-速度双级 CBF 串联：$\psi_0=h_{\mathrm{pos}}$, $\psi_1=\dot q+\alpha_1(q-q_{\min})$，要求 $\dot\psi_1+\alpha_2\psi_1\ge 0$。

完整 C++ 伪代码（Eigen + OSQP）：

VectorXd CBFQPFilter::filter(const VectorXd& x, const VectorXd& u_nom) {
  MatrixXd P = MatrixXd::Identity(nu_,nu_); VectorXd q = -u_nom;
  MatrixXd A(m_+nu_, nu_); VectorXd l(m_+nu_), u_ub(m_+nu_);
  for (int i=0; i<m_; ++i) {
    const auto& cbf = cbfs_[i];
    auto dhdx = cbf.dh(x); double Lfh = dhdx*f_(x);
    auto Lgh = dhdx*g_(x);  // 1×nu
    A.row(i) = Lgh; l(i) = -Lfh - cbf.alpha(cbf.h(x)); u_ub(i) = INF;
  }
  A.bottomRows(nu_) = MatrixXd::Identity(nu_,nu_);
  l.tail(nu_) = u_min_; u_ub.tail(nu_) = u_max_;
  osqp_update_data_vec(solver_, q.data(), l.data(), u_ub.data());
  osqp_warm_start(solver_, u_prev_.data(), y_prev_.data());
  osqp_solve(solver_);
  return Eigen::Map<VectorXd>(solver_->solution->x, nu_);
}

Tube MPC vs CBF-QP 对比：Tube 需要离线重预计算 mRPI、在线只解一个 nominal OCP（ms 级），递归可行性强，天然鲁棒；CBF 无预测、单步贪心（μs 级），但对高相对度需手调多个 class-K，退化时需回退控制器。两者互补：外层 Tube MPC 处理长期可行性，内层 CBF-QP 处理快速变化约束（如动态障碍）。

与 3.8/3.11.8/3.13 映射：mRPI 对应 3.8 鲁棒控制的 RPI 集合；约束收紧对应 3.11.8 Tube MPC；CBF 对应 3.13 安全控制。

桥梁：手写四层全覆盖；接下来 Part C 把手写知识迁移到生产级框架。

Part C：生产级框架集成（§3.14.8–§3.14.11）¶

§3.14.8 Pinocchio + Crocoddyl 的 C++ 集成¶

Pinocchio 是多体动力学的事实标准 C++ 库，Model 存常量参数（质量、惯量、几何、关节拓扑），Data 存算法可变缓存（oMi、tau、M）。多线程并行时每条线程必须拥有独立 Data，但共享一个 Model。URDF 加载：pinocchio::urdf::buildModel(urdf, pinocchio::JointModelFreeFlyer(), model) 一行搞定浮基机器人。三大算法：rnea(model,data,q,v,a)（逆动力学 O(n)）、aba(model,data,q,v,tau)（正向动力学 Featherstone）、crba(model,data,q)（质量矩阵 → data.M）。解析导数：computeRNEADerivatives(model,data,q,v,a) 填 data.dtau_dq/dv、data.M；computeABADerivatives 同理得 $\partial a/\partial q, \partial a/\partial v, M^{-1}$。RSS 2018 论文证明两者共用同一递推核，ABA 导数仅多一个 $M^{-1}$，完整评估仅约函数本身 3× 耗时——比 CppAD 快 3–10×。

SE(3) 李代数：pinocchio::SE3（4×4 齐次）、Motion（6D 空间速度）、Force（对偶 6D 力旋量），exp6/log6、SE3::act/actInv、Motion::cross。与 hpp-fcl 集成做碰撞：buildGeom(model, urdf, COLLISION, geomModel, pkgDir)→updateGeometryPlacements→computeCollisions/Distances。

Crocoddyl 是 LAAS/Edinburgh/Heriot-Watt 的多接触 DDP 框架，基本构件 ActionModelAbstract 派生类实现 calc(data,x,u) 和 calcDiff(data,x,u)，后者填 $F_x, F_u, L_x, L_u, L_{xx}, L_{xu}, L_{uu}$。预置模型：DifferentialActionModelFreeFwdDynamics（无接触 ABA）、DifferentialActionModelContactFwdDynamics（接触约束）、配合 IntegratedActionModelEuler/RK4 离散化。代价：CostModelSum 聚合多个 CostModelResidual（ResidualModelFramePlacement、State、Control、CoMPosition、ContactForce），ActivationModel* 定义 $\rho(r)$。

求解器：SolverDDP（经典）、SolverFDDP（Mastalli 2020 Feasibility-driven，多接触问题稳定）、SolverBoxFDDP/BoxDDP（box 约束）、SolverIntro（新版等式约束 DDP）。主循环：

auto state = boost::make_shared<StateMultibody>(rmodel);
auto act = boost::make_shared<ActuationModelFloatingBase>(state);
auto running = boost::make_shared<IntegratedActionModelEuler>(
    boost::make_shared<DifferentialActionModelContactFwdDynamics>(
        state, act, contacts, costs), dt);
ShootingProblem problem(x0, {running×T}, terminal);
SolverFDDP ddp(boost::make_shared<ShootingProblem>(problem));
ddp.solve(xs_init, us_init, 100);

Python vs C++ 决策：Crocoddyl Python 通过 eigenpy 零拷贝 Eigen↔NumPy，不覆盖 calc/calcDiff 时差异 <10%；覆盖走 Python 慢 2–5×。用 Python 原型 → C++ 翻译。

§3.14.9 OCS2 框架的 C++ 集成¶

OCS2（Optimal Control for Switched Systems） 由 ETH Leggedrobotics 维护，核心抽象：SystemDynamicsBase、CostFunctionBase/StateInputCost、StateInputConstraint、PreComputation。求解器：SLQ（连续时域约束 DDP）、iLQR（离散时域）、SQP（multiple-shooting + HPIPM，legged_robot 默认）、IPM（多段 shooting + 非线性内点法）、SLP（PIPG 顺序线性规划）；约束通过 Augmented Lagrangian 或 Relaxed Barrier 处理。

Pinocchio 集成（ocs2_pinocchio meta-pkg）：PinocchioInterface、PinocchioEndEffectorKinematics（解析导数）、PinocchioEndEffectorKinematicsCppAd（CppAD + codegen）、CentroidalModelPinocchioMapping、ocs2_self_collision（hpp-fcl）。

ROS MPC/MRT 分离：MPC node 跑 SqpMpc/GaussNewtonDDP_MPC，接收 TargetTrajectories，异步解 OCP，把 policy 发到 mpc_policy topic；MRT（Model Reference Tracking）node 订阅 policy，在高频控制线程里插值滚动，输出 $x_{\mathrm{ref}}, u_{\mathrm{ref}}$。分离允许 MPC 50–200 Hz 而 MRT 400–1000 Hz。

legged_control 源码深度解读（qiayuanl，~1.6k stars，基于 OCS2）：

legged_control/
├── legged_common/          # hardware_interface 扩展
├── legged_controllers/     # ros_control 插件 + MPC 线程 + WBC 调用
├── legged_estimation/      # 线性 Kalman：IMU+编码器+足端接触 → base 位姿
├── legged_interface/       # LeggedRobotInterface + task.info + URDF
├── legged_wbc/             # 分层加权最小二乘 (WbcBase + HoQp)
├── legged_gazebo/          # Gazebo 同名 HW
├── legged_examples/legged_unitree/legged_unitree_hw/   # A1/Go1 SDK
└── qpoases_catkin/

MPC 层：状态 $x=[h_{\mathrm{com}};q_b;q_j]$（质心动量 + 浮基位姿 + 关节位置），输入 $u=[f_c;v_j]$（四足接触力 + 关节速度）；centroidal dynamics + 摩擦锥 + 站立足零速 + 摆腿轨迹约束；multiple-shooting + SQP + HPIPM，NUC 11 代 ≈200 Hz。WBC 层：决策变量 $[\ddot q;f_c;\tau]$，按优先级（浮基动力学→接触无滑移→摩擦锥→跟踪 MPC $\ddot q,f_c$→关节限位）分层 QP，位于高优先级零空间最小化低优先级松弛。输出 $\tau$ 作前馈 + 低增益 PD 送电机。硬件接口：UnitreeHW::read() 解析 UDP、write() 下发 $\tau_{\mathrm{ff}}+k_p(q_d-q)+k_d(v_d-v)$。Sim↔实机桥：legged_gazebo 实现同名 HW，controller 插件二进制完全一致，只换 HW 包。

§3.14.10 acados 的完整 C++ 工程模板¶

标准工作流：(1) Python AcadosOcp+CasADi SX 定义模型+代价+约束；(2) AcadosOcpSolver(ocp) 生成 c_generated_code/，含 acados_solver_<name>.c/.h、<name>_model/*.c（CasADi 导出的动力学/Jacobian/Hessian）、<name>_cost/*.c、Makefile、CMakeLists.txt、acados_ocp_nlp.json；(3) 把 .c/.h 纳入主工程 CMake；(4) C++ wrapper class 封装 solver。

生成物绝对路径陷阱：Python 默认生成绝对路径的 CMake/Make 文件，拷到 Jetson 前要 sed -i 's|/host/path|/jetson/path|g' 或在生成时设 ocp.code_export_directory = "c_generated_code" 相对路径。

多线程安全：acados solver 不是线程安全的，每线程一个 solver 实例——生成时 unique name 或运行时加 mutex。

集成模式三选一：(i) Simulink S-function：适合车厂流程；(ii) ROS2 rclcpp timer callback：如前述；(iii) 裸机：static link libacados.a libhpipm.a libblasfeo.a，主循环 while(1){read; solve; write; sleep_until_next_period;}。

§3.14.11 QP 求解器的 C++ 接口对比¶

求解器	算法	适用规模	冷启动	热启动	特色
OSQP	ADMM（CSC 稀疏）	100–5000	1–3 ms	0.2–0.8 ms	warm-start 2.6–7× 提速，eps 默认低
qpOASES	Active-set（稠密）	<100	0.5–2 ms	50–200 μs	MPC hotstart nWSR <5，condensed QP 最佳
HPIPM	IPM（OCP-structured）	OCP 结构	0.3–1 ms	0.2–0.5 ms	partial condensing 稳定 <1 ms
ProxQP	PMM（稠密/稀疏）	通用	0.5–2 ms	100–400 μs	退化鲁棒，eps=1e-6 仍稳定

OSQP C-API 关键：osqp_setup 做一次 LDL$^\top$ 因式分解，osqp_update_data_vec(solver, q, l, u) 只更新向量（矩阵 sparsity 不变时不重因式），warm-start 靠 osqp_warm_start(solver, x, y)。eps_abs 默认 1e-3 对 MPC 危险（Google Groups 案例显示控制信号可错 3 个数量级），CBF-QP 至少 1e-6 或开 polish。

qpOASES SQProblem：opts.setToMPC() 关闭打印、调紧精度、禁正则化；典型 MPC 稳态 nWSR 每步 1–3 次。维度变化必须重新 init()。

HPIPM 数据：BLASFEO panel-major 内部存储，d_ocp_qp_set_all 自动 pack，用户侧只传 column-major。三种 IPM 模式：SPEED_ABS（最快）、SPEED、ROBUST（迭代精化 + 对角正则化）。

ProxQP 在 CBF-QP：proximal ALM 保证即使 Hessian 不正定、约束线性相关、不可行也能收敛到"最近可行"解，是 CBF 多约束冲突场景最佳；RSS 2022 基准比 OSQP 快 2–10×。

工程决策树：(a) 小稠密 CBF-QP → ProxQP dense；(b) MPC 条件稠密 QP（$n_u\cdot N<100$）→ qpOASES；(c) acados OCP 结构 → HPIPM（已集成）；(d) 大规模稀疏 QP（>1000 变量）→ OSQP；(e) 退化 QP、需可行性证书 → ProxQP。

Part D：嵌入式部署与实时性（§3.14.12–§3.14.14）¶

§3.14.12 交叉编译全流程¶

toolchain-aarch64-linux-gnu.cmake 模板：

set(CMAKE_SYSTEM_NAME Linux)
set(CMAKE_SYSTEM_PROCESSOR aarch64)
set(CMAKE_C_COMPILER   aarch64-linux-gnu-gcc)
set(CMAKE_CXX_COMPILER aarch64-linux-gnu-g++)
set(CMAKE_C_FLAGS_INIT   "-march=armv8.2-a+crypto+fp16+rcpc+dotprod")
set(CMAKE_CXX_FLAGS_INIT "-march=armv8.2-a+crypto+fp16+rcpc+dotprod")
set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY)

BLASFEO TARGET 选型表： - ARMV8A_ARM_CORTEX_A76：Jetson Orin（A78AE 属 A78 家族、同世代兼容 A76 kernel）—首选； - ARMV8A_ARM_CORTEX_A57：Jetson Xavier（Carmel 核，无专用 kernel，社区论坛首选 A57）、Jetson Nano/TX1； - ARMV8A_ARM_CORTEX_A53：低端嵌入式； - ARMV8A_APPLE_M1：Apple Silicon； - GENERIC：兜底纯 C。

TARGET 错配 → 运行时 SIGILL 非法指令，这是最常见坑。LA=HIGH_PERFORMANCE + MF=PANELMAJ 是手写汇编 kernel 的最优组合。

构建顺序（BLASFEO → HPIPM → acados）：

export TC=$PWD/toolchain-aarch64-linux-gnu.cmake
export INSTALL=$HOME/install-aarch64

# 1. BLASFEO
cmake -S blasfeo -B build/blasfeo -DCMAKE_TOOLCHAIN_FILE=$TC \
      -DTARGET=ARMV8A_ARM_CORTEX_A76 -DLA=HIGH_PERFORMANCE \
      -DCMAKE_INSTALL_PREFIX=$INSTALL
cmake --build build/blasfeo -j && cmake --install build/blasfeo

# 2. HPIPM
cmake -S hpipm -B build/hpipm -DCMAKE_TOOLCHAIN_FILE=$TC \
      -DTARGET=ARMV8A_ARM_CORTEX_A76 -DBLASFEO_PATH=$INSTALL \
      -DCMAKE_INSTALL_PREFIX=$INSTALL
cmake --build build/hpipm -j && cmake --install build/hpipm

# 3. acados
cmake -S acados -B build/acados -DCMAKE_TOOLCHAIN_FILE=$TC \
      -DBLASFEO_TARGET=ARMV8A_ARM_CORTEX_A76 \
      -DHPIPM_TARGET=ARMV8A_ARM_CORTEX_A76 \
      -DACADOS_PYTHON=OFF -DBUILD_SHARED_LIBS=ON \
      -DCMAKE_INSTALL_PREFIX=$INSTALL
cmake --build build/acados -j && cmake --install build/acados

Pinocchio + hpp-fcl 交叉编译：robotpkg apt 源**不提供 aarch64 二进制**，必须源码编译，依赖 Eigen3、Boost、urdfdom、hpp-fcl、eigenpy（去掉 Python 绑定可 -DBUILD_PYTHON_INTERFACE=OFF）。常见做法：qemu-chroot 或直接在 Jetson 上原生编译（Orin 上 <10 min）。

静态 vs 动态链接：嵌入式推荐**静态链接**，单二进制、无 LD_LIBRARY_PATH 问题、启动快；代价是二进制大（BLASFEO 汇编库 3–5 MB）、不可热更新。

Docker 交叉编译镜像：nvcr.io/nvidia/l4t-jetpack:r36.3.0（JetPack Cross Compilation container）、docker run --platform=linux/arm64/v8 arm64v8/ubuntu:22.04 + qemu-user-static 直接在 x86 上原生编译。

t_renderer 缺失：Python 生成 C 代码依赖 t_renderer 可执行，Jetson 上需下载 aarch64 版放 <acados>/bin/ 并 chmod +x。

§3.14.13 实时性工程¶

PREEMPT_RT 内核：把内核 spinlock 转 rtmutex、硬/软中断放可抢占内核线程、hrtimer 替代 jiffies 时钟，从 Linux 6.12 起部分合入主线。Ubuntu 22.04/24.04 启用：sudo pro attach <token>→sudo pro enable realtime-kernel→sudo reboot→uname -a 应含 PREEMPT RT。cyclictest 指标：工业硬实时 Max <50 μs 良好，<100 μs 可接受。Jetson Orin 通过 nvidia-l4t-rt-kernel Debian 包启用，但 GPU/ISP/BPMP 闭源 OOT 模块与 RT 语义冲突、BPMP 对频率/电压的异步调节带来不可控抖动；Orin 不是硬实时平台，工程实践是 Orin 跑感知/规划/MPC（软实时 100–500 Hz），外挂 STM32/TI AM62x 跑 1–10 kHz 伺服，CAN-FD/EtherCAT/DDS 通信。

SCHED_FIFO vs SCHED_RR：两者皆优先级 1–99 高于 OTHER。FIFO 获得 CPU 后运行到阻塞或被抢占，控制回路独一线程用 FIFO 优先级 80（留 90–99 给内核 IRQ 线程）；同优先级多子任务用 RR 避免饿死。典型设置：

struct sched_param p = { .sched_priority = 80 };
sched_setscheduler(0, SCHED_FIFO, &p);
mlockall(MCL_CURRENT | MCL_FUTURE);

CPU pinning 与隔离：内核启动参数 isolcpus=2,3 nohz_full=2,3 rcu_nocbs=2,3 摘核、停 tick、搬 RCU 回调。推荐更现代的 cset shield --cpu=2,3 --kthread=on + cset shield --exec -- chrt -f 80 ./app。禁用 irqbalance、把所有 IRQ 亲和到 CPU0：for i in /proc/irq/*/smp_affinity; do echo 1 > $i; done。关闭 CPU freq scaling：cpupower frequency-set -g performance。解除 RT 时间预算：echo -1 > /proc/sys/kernel/sched_rt_runtime_us。

实时安全编码规范（复用 §3.14.1 精要）：热路径禁 new/malloc / std::vector::push_back（无 reserve）/ std::string 拼接 / std::shared_ptr 拷贝 / printf / RTTI；用 lock-free SPSC 队列（realtime_tools::LockFreeQueue、boost::lockfree::spsc_queue）跨线程传递数据，同核 push/pop ~10 ns、跨核 ~50–100 ns；pthread_mutex 用 PTHREAD_PRIO_INHERIT 防优先级翻转；Docker 运行时 --ulimit memlock=-1 --ulimit rtprio=99 --cap-add SYS_NICE。

WCET 保证五件套：(1) 静态内存 + mlockall + 栈 prefault；(2) -fno-exceptions -fno-rtti；(3) solver max_iter 限制（acados nlp_solver_max_iter=10），超时退回上一次解；(4) performance governor 锁频、关 Turbo/SMT；(5) 压测：stress-ng --cpu --hdd --vm 同时跑 cyclictest。

p99/p99.9 延迟统计：每周期 clock_gettime(CLOCK_MONOTONIC) 差值写环形缓冲，HdrHistogram（HdrHistogram/HdrHistogram_c）RT 线程累积、非 RT 线程 10 Hz 读分位发布。目标：1 kHz 控制环 p99 <200 μs、p99.9 <500 μs、Max <1 ms。rtla timerlat/rtla osnoise 在线检测内核噪声源。

§3.14.14 ROS2 实时控制节点模板¶

ros2_control 架构：ros2_control_node 主循环运行在实时线程顺序 read → controllers.update → write；另一非实时线程处理 pub/sub/service。Controller 与 Hardware 都是 pluginlib 动态库，controller_manager 按 URDF <ros2_control> 标签加载。

SystemInterface 骨架：

class MyRobot : public hardware_interface::SystemInterface {
  CallbackReturn on_init(const HardwareInfo& info) override;  // 非RT
  CallbackReturn on_configure(...) override;                  // 打开 CAN
  CallbackReturn on_activate(...)  override;                  // 使能电机
  return_type read (const rclcpp::Time&, const rclcpp::Duration&) override; // RT
  return_type write(const rclcpp::Time&, const rclcpp::Duration&) override; // RT
};
PLUGINLIB_EXPORT_CLASS(my_pkg::MyRobot, hardware_interface::SystemInterface)

生命周期：UNCONFIGURED → INACTIVE（可读）→ ACTIVE（可写）。read/write/update 与 perform_command_mode_switch 属 RT 路径；on_init/on_configure 非 RT。

realtime_tools 关键：RealtimePublisher<T> 包一层 publisher，RT 线程 trylock()→写→unlockAndPublish()，实际 IO 在后台非 RT 线程；RealtimeBuffer<T> 单生产单消费 lock-free 双缓冲，RT 侧 readFromRT()，非 RT 侧 writeFromNonRT()。

executor 选型：(i) SingleThreadedExecutor 每轮重新遍历 entities 非 RT 友好；(ii) StaticSingleThreadedExecutor 预构建 WaitSet 无分配（但在 Rolling 已标 deprecated，将被重构 SingleThreaded 吸收）；(iii) MultiThreadedExecutor + callback groups；(iv) EventsExecutor（iRobot 贡献）基于"事件驱动 + timers manager"，低 CPU 低延迟，已合入 rclcpp。

MPC + WBC 双层架构：MPC node 50–200 Hz（MultiThreadedExecutor + MutuallyExclusive callback group），WBC node 400–1000 Hz（ros2_control 插件，RT executor）。两层之间用 RealtimeBuffer 或 Iceoryx 共享内存。

零拷贝通信：Iceoryx（eclipse-iceoryx/iceoryx，ros2/rmw_iceoryx）基于 POSIX shm + lock-free 队列，端到端零拷贝，启动 iox-roudi 守护，RMW_IMPLEMENTATION=rmw_iceoryx_cpp；FastDDS data-sharing 通过共享内存段零拷贝，需 XML profile + 定长消息，borrow_loaned_message() + publish(std::move(loaned))。变长 ROS 消息无法 loan，用 ros2_shm_msgs 或自定义定长 msg。

QoS 关键：RT 命令 BEST_EFFORT + depth=1（丢旧数据），传感器 SensorDataQoS，命令话题 ReliableQoS(depth=1)。Deadline 设为控制周期，违反触发回调监控卡顿。

诊断话题（10–50 Hz）：

diag_msg.solve_time_ms = dt_us/1000.0;
diag_msg.qp_iterations = ocp.get_stat("sqp_iter");
diag_msg.constraint_violation = ocp.get_stat("res_ineq");
diag_msg.cpu_cycle_latency_us = cycle_latency_us;
diag_pub->try_publish(diag_msg);

监控工具：ros2 topic hz/bw/delay、rqt_plot、离线 perf record -F 999 -g -p $(pgrep controller_manager) -- sleep 30 + flamegraph；topic_statistics 自动发 mean/stddev/max；Grafana + Prometheus + ros2-prometheus-exporter 长期留痕。

Part E：完整项目案例（§3.14.15–§3.14.16）¶

§3.14.15 项目一：Cart-Pole 全栈（入门级）¶

目标：单一系统上跑通 LQR 稳定 → iLQR swing-up → NMPC 跟踪的三控制器对比。状态 $[x,\theta,\dot x,\dot\theta]$，参数 $m_c=1, m_p=0.1, l=0.5, g=9.81$。

动力学 Eigen 实现：单文件 cartpole_dynamics.hpp 实现 dx = f(x,u) 和解析 Jacobian Fx, Fu，用 Eigen::Matrix<double,4,4> 和 Eigen::Matrix<double,4,1>。

仿真器：RK4 积分，dt=0.01。用 Gazebo/MuJoCo 替代：Gazebo cartpole.urdf + ros2_control effort_controllers/JointEffortController；MuJoCo cartpole.xml 连续力矩。

LQR 稳定：在 $(0,\pi,0,0)$ 附近线性化得 $A, B$，DARE 解 $P$，$K = (R+B^\top PB)^{-1}B^\top PA$。工程测试：初始扰动 $\theta_0 = \pi+0.3$，闭环 2 s 内稳定到 $\pi\pm 0.01$。

iLQR swing-up：从 $(0,0,0,0)$ 到 $(0,\pi,0,0)$，$N=200$、dt=0.02、$\ell=0.01u^2$、$\ell_N=10(x-x^*)^\top(x-x^*)$、$u_{\max}=\pm 10$ N。迭代 ~50 次收敛，总代价从 $10^5$ 降到 $10^1$。

NMPC 跟踪：用 iLQR 输出的 $(\bar x, \bar u)$ 作参考轨迹，acados 滚动求解 $N=20$ horizon 的跟踪 NMPC，box 约束 $u\in[-10,10]$。在 Gazebo 实时跟踪 200 Hz。

完整 CMake 项目结构：

cartpole_fullstack/
├── cartpole_dynamics/        # 共享 dynamics 库
├── cartpole_lqr/             # LQR 控制器
├── cartpole_ilqr/            # iLQR 轨迹优化
├── cartpole_nmpc_acados/     # NMPC（含 c_generated_code/）
├── cartpole_ros2_control/    # ros2_control plugin
├── cartpole_gazebo/          # URDF + world + launch
├── tests/                    # GoogleTest 单元测试
├── benchmarks/               # Google Benchmark
└── CMakeLists.txt (top)

关键里程碑：(i) 动力学单元测试通过（对比 scipy）；(ii) LQR 闭环稳定（cyclictest 抖动 <50 μs）；(iii) iLQR swing-up 成功；(iv) NMPC Gazebo 跟踪 200 Hz；(v) ros2_control plugin 实机 / MuJoCo 可运行。预计耗时 3–4 周。

§3.14.16 项目二：四足 / 四旋翼 MPC（进阶级）¶

路线 A：四旋翼 rpg_mpc 复现 + acados 迁移（推荐入门）。13 维状态 $[p,q,v,\omega]$，参考 github.com/uzh-rpg/rpg_mpc 的三层分离架构（mpc_solver + mpc_wrapper + mpc_controller），把 ACADO 替换为 acados。CasADi Python 建模 → AcadosOcpSolver 生成 C → C++ wrapper → ROS2 节点。PX4 集成：通过 px4_ros_com 发 setpoint_raw/attitude（body-rate + thrust）。Jetson Orin 部署：BLASFEO TARGET=ARMV8A_ARM_CORTEX_A76，200–500 Hz 反馈。状态估计用 VIO（如 VINS-Fusion）或 OptiTrack。预计耗时 5–8 周。

路线 B：四足 legged_control 迁移（进阶）。从 qiayuanl/legged_control 开始，URDF 替换为目标机器人（A1 → Go1 → 自研），修改 legged_interface/task.info 的权重与约束，legged_estimation 对接新 IMU/编码器，legged_unitree_hw 改写为目标 SDK。CBF-QP 安全层：在 WBC 输出后加一层 ProxQP 的 CBF-QP，处理自碰撞（hpp-fcl 查 SDF 导数）和关节限位。预计耗时 8–12 周。

Jetson 部署清单： 1. JetPack 6.2、nvpmodel -m 0 + jetson_clocks； 2. 启用 nvidia-l4t-rt-kernel（如实机验证 RT 必要）； 3. isolcpus=6,7 + taskset -c 6 ./mpc_node； 4. 静态链接 libacados.a libhpipm.a libblasfeo.a； 5. Iceoryx 共享内存通信（rmw_iceoryx_cpp）； 6. 诊断话题 /diag/solve_time_ms、/diag/qp_iter 每 10 Hz 发布； 7. 记录 rosbag + HdrHistogram 离线分析 p99。

性能调优循环：(i) perf record -g + flamegraph 找热点；(ii) 若 CasADi 生成代码占用高 → 开 codegen_with_mex=True 或重建更紧凑模型；(iii) 若 HPIPM status=3（min_step reached）→ 调 qp_solver_cond_N、加 levenberg_marquardt 正则、改 warm-start 策略；(iv) 若 ROS2 pub/sub 阻塞 RT → 切 Iceoryx 或把 publisher 挪到 callback group。

常见陷阱速查（15 条）¶

(1) Eigen 对齐：结构体含 Vector4d 成员被 new 分配 → 段错误；修法 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 或 C++17 over-aligned new。(2) 模板编译时间爆炸：Eigen 表达式 + Pinocchio + Crocoddyl 叠加可让单 TU 编译 >60 s；修法 pimpl、显式实例化、precompiled headers、ccache。(3) acados codegen 绝对路径：生成的 CMake/Makefile 含 host 绝对路径 → Jetson 上编译失败；修法 ocp.code_export_directory 相对路径或 sed 替换。(4) BLASFEO TARGET 不匹配：主机编 X64_INTEL_HASWELL 拷到 Jetson → SIGILL；修法目标架构上重编三件套。(5) HPIPM status=3：min_step reached 多因初值差/scaling 不当；修法 qp_solver_cond_N 设 5 或原 N、加 LM 正则、切 FULL_CONDENSING_HPIPM 调试。(6) t_renderer 缺失：Python 生成失败；下载 aarch64 二进制放 <acados>/bin/ 并 chmod +x。(7) ROS2 多线程并发调用 solver：随机数值错或崩溃；solver 实例绑单一 MutuallyExclusive callback group 或加 mutex。(8) QP warm-start 失效：A 的 sparsity 改变或维度变化；修法固定 sparsity pattern、维度动态时用 softmax 约束或重 init()。(9) ROS2 实时陷阱：RCLCPP_INFO 在 RT update 里 malloc；改 RealtimePublisher + diagnostics 话题。(10) OSQP 默认 eps 1e-3 太松：MPC/CBF 闭环可能发散；设 eps_abs/rel 1e-6 或开 polish。(11) mlockall 缺失：首次访问栈/堆页触发 major page fault → 毫秒级尖峰；mlockall(MCL_CURRENT|MCL_FUTURE) + 栈 prefault。(12) CPU freq scaling：周期性 50–200 μs 尖峰；cpupower frequency-set -g performance。(13) Pinocchio URDF 浮基缺失：buildModel 不带 JointModelFreeFlyer → 固定基；腿足机器人必须加。(14) Crocoddyl Python 覆盖虚函数：慢 2–5×；把 calc/calcDiff 迁到 C++。(15) 链接顺序：-lacados -lhpipm -lblasfeo 顺序固定；违反报 undefined reference。

⚠️ 深度陷阱专栏（详细展开）¶

⚠️ 编程陷阱：实时循环中使用动态内存分配

错误做法：在 1kHz WBC 或 500Hz MPC 的 update 回调中使用 std::vector::push_back、std::string 拼接、new/delete、甚至 ROS2 RCLCPP_INFO（内部会 malloc）。

现象：系统在 99.9% 的循环中正常运行（平均 0.5ms），但每隔 5-30 秒出现一次 2-10ms 的尖峰（worst-case jitter）。用 perf record 或 cyclictest 能看到尖峰频率与 GC/malloc 争用周期吻合。

根本原因：glibc malloc 内部有锁（多线程）和 mmap/brk 系统调用（首次分配或大块分配触发 page fault）。即使分配量很小（<64B 走 fast bins），在高频循环中累积的锁竞争和 page fault 会导致非确定性延迟。PREEMPT_RT 内核能降低中位数延迟，但**不能消除 malloc 引入的尾延迟**。

正确做法：(1) 所有控制循环中的内存在初始化时预分配（reserve + resize）；(2) 用 mlockall(MCL_CURRENT|MCL_FUTURE) 锁住所有页面；(3) 用 stack prefault（在初始化时写满整个线程栈）消除首次访问的 page fault；(4) 日志/诊断走 RealtimePublisher（lock-free ring buffer + 低优先级发送线程）。

自检方法：cyclictest -p90 -t1 -n -l 100000 看 max latency，目标 < 50us。若 > 100us，用 ftrace 的 function_graph tracer 定位 malloc 调用栈。

⚠️ 编程陷阱：acados RTI 模式下忽略 QP 求解器状态码

错误做法：调用 ocp_solver.solve() 后直接读取 get_u(0) 作为控制输入，不检查返回的 status 码。

现象：正常运行时一切正常。但当状态突然大幅跳变（如四足机器人被踢、传感器故障）时，QP 不可行（status=2）或达到最大迭代（status=1），此时 get_u(0) 返回的是上次 warm-start 的残余值——可能导致关节扭矩飞到硬件极限。

根本原因：RTI 模式只做一次 SQP 迭代，不保证收敛。在大扰动下，线性化点与真实状态差太大，QP 的可行集可能为空。acados 在 status != 0 时仍然返回某个 u 值（最后一次 iterate），但该值无物理意义。

正确做法：(1) 每次调用后检查 status；(2) status != 0 时切换到降级控制律（LQR 或 PD）；(3) 部署前用 worst-case 初始化测试覆盖所有 status 分支；(4) 考虑增加 nlp_solver_max_iter 从 1 到 3 并设 early-termination（KKT 残差 < tol）作为折衷。

🧠 思维陷阱：认为"Python 原型 → C++ 部署"只是简单的语言翻译

新手想法："Python 里 10 行 NumPy 代码能跑通，翻译成等价的 Eigen C++ 就能部署了。"

现象：Python 原型 50ms 完成一次 MPC 求解（离线测试完美），翻译成 C++ 后变成 5ms（好了 10x！），但部署到实时系统后发现有不可接受的尾延迟，且行为偶尔与 Python 不一致。

根本原因：Python→C++ 的翻译忽略了至少三个层面的差异：(a) 数值精度：NumPy 默认 float64 + 某些操作走 LAPACK 的特定 routine，C++ 中换了不同的分解方法（如 LLT vs LDLT）在 ill-conditioned 情况下结果不同；(b) 内存模型：Python 的 GIL + numpy 底层的列存/行存约定 vs Eigen 的表达式模板和 aliasing 规则；(c) 实时性约束：Python 中的"偶尔慢 100ms"无关紧要（有 GIL/GC），C++ 实时循环中同样的延迟意味着控制器跳帧。

正确做法：翻译过程必须分三步：(i) 功能等价性验证（相同输入下 $\|y_{\text{py}} - y_{\text{cpp}}\| < 10^{-10}$）；(ii) 数值鲁棒性测试（用 ill-conditioned 输入验证行为一致）；(iii) 实时性 profiling（确认无动态分配、无系统调用、无未绑核线程）。

工具链速查表与依赖图¶

工具链速查：编译器 g++-11/clang++-14；构建 cmake>=3.20 + ninja；线代 Eigen 3.4；AD CasADi 3.6 / CppAD / Pinocchio 解析；NMPC acados 0.3 + HPIPM + BLASFEO；机器人 Pinocchio 3.2 + hpp-fcl + Crocoddyl 2.1 / OCS2；QP OSQP 1.0 / qpOASES 3.2 / ProxQP 0.6；测试 GoogleTest 1.14 + Google Benchmark 1.8；ROS ROS2 Humble/Jazzy + ros2_control 4.x + realtime_tools；通信 Iceoryx 2.0 / FastDDS 2.x；交叉编译 gcc-aarch64-linux-gnu + qemu-user-static；嵌入式 JetPack 6.2 + PREEMPT_RT patch；监控 HdrHistogram + perf + Prometheus。

依赖关系图：

Eigen3 ─┬─► Pinocchio ─┬─► Crocoddyl
        │              │
        │              └─► OCS2 ─► legged_control
        │
        ├─► CppAD / CppADCodeGen
        │
        └─► OSQP / ProxQP（稀疏线代）

CasADi ─► acados codegen ─► acados ─► HPIPM ─► BLASFEO
                                                    ▲
                                                    │
                                     Jetson Orin 目标架构专用 kernel

ROS2 rclcpp ─► ros2_control ─► hardware_interface ─► 实机驱动
         └──► realtime_tools ─► RealtimePublisher / LockFreeQueue
         └──► Iceoryx rmw ─► 零拷贝共享内存

推荐项目结构模板（通用）：

project_root/
├── cmake/                  # toolchain 文件、common helpers
├── include/<pkg>/          # 公开头文件
├── src/                    # 实现
├── apps/                   # 可执行入口
├── tests/                  # GoogleTest 单元测试
├── benchmarks/             # Google Benchmark
├── third_party/            # FetchContent 下载或 submodule
├── ros2_ws/                # ROS2 工作区（可选）
├── scripts/                # Docker / cross-compile / deploy
├── docs/                   # 设计文档、API
└── CMakeLists.txt

中文资源与推荐¶

知乎专栏：Tomcattiger《CasADi/ACADOS 学习》（含 C++ + Eigen + mobile_robot 示例）、《MPC practice：模型预测控制在 Linux C++ 上的实践分享》、《iterative LQR (iLQR) 推导》**系列（3 篇）、《OCS2 学习记录 (1) — Tutorial》、《Pinocchio 安装教程》与《基本应用》、《控制障碍函数 CBF》、《MPC-CBF》、《基于 CBF 的 MPC》**、"忠厚老实的老王"的控制/MPC/CBF 系列。

B 站：DR_CAN MPC / 最优控制**系列（数学推导最清楚）、**小彭老师 C++/Eigen、古月居 ROS/控制；搜关键词"OCS2 四足"、"legged_control 部署"、"ROS2 实时控制"、"acados 中文"。

GitHub 中文项目：ShuoYangRobotics/A1-QP-MPC-Controller（MIT Cheetah 3 复现）、qiayuanl/legged_control（OCS2 + ros-control，中文作者）、Technician13/MIT-Cheetah-Note（ConvexMPCLocomotion 逐函数中文注释）、tomcattigerkkk/ACADOS_Example（中文注释 acados C++ 模板）。

论文硬核：Tassa 2014 "Control-Limited DDP"、Howell 2019 ALTRO、Mastalli 2020 Crocoddyl、Frison & Diehl 2020 HPIPM、Ames 2017 CBF-QP、Xiao & Belta 2019 HOCBF、Raković 2005 mRPI、Carpentier & Mansard 2018 Pinocchio derivatives（RSS）。

时间预算¶

Part	内容	全职周数
A	C++ 基础设施（Eigen、CMake、AD、实时规范）	4–6 周
B	手写 LQR + iLQR + NMPC + CBF-QP	8–12 周
C	Pinocchio + Crocoddyl + OCS2 + acados + QP 求解器	6–10 周
D	交叉编译 + PREEMPT_RT + ros2_control + Jetson 部署	4–6 周
E	Cart-pole 全栈 + 四旋翼/四足 MPC + CBF 安全层 + 实机	10–16 周
合计		32–50 周（8–12 个月全职）

若仅做到"会用 acados/OCS2 调参 + 小改 cost"的应用级水平，Part A+B 缩为 6 周、C 为 4 周，约 10 周可入门。若只追求会部署一个现成框架而不理解内部，2–3 周即可。路线图默认选**系统博士级深度**：能在不依赖任何第三方框架的情况下复现所有数学公式，并能在实机上排除 SIGILL、PREEMPT_RT 兼容、HPIPM status 异常、acados codegen 路径等工程故障。

总结：从数学到机器人的完整闭环¶

本专题把"最优控制论文里漂亮的递推公式"和"在 Jetson Orin 上以 500 Hz 无抖动跑的 C++ 二进制"之间的距离，按工程逻辑切成五段。Part A 教会你写任何生产控制代码前的基础设施语言——现代 CMake 的 target 粒度、Eigen 固定大小矩阵的对齐 ABI、LDLT 替代 inverse、AD 与 Pinocchio 解析导数的取舍、实时安全规范。Part B 让你用 Eigen 一行不少地复现 LQR/iLQR/NMPC/CBF-QP，代码每一行都映射到 3.1–3.13 的具体公式；"从零手写"不是炫技，而是让你下次遇到 HPIPM status=3 时能真正 debug 而非盲猜。Part C 把手写经验迁移到生产级框架——Pinocchio 提供多体李群动力学、Crocoddyl 提供多接触 DDP、OCS2 提供腿足机器人全栈、acados 提供嵌入式 RTI-NMPC，四者的定位、选型、集成全部明确。Part D 解决"从 x86 仿真到 ARM 实机"的全部工程摩擦——交叉编译 toolchain、BLASFEO TARGET 匹配、PREEMPT_RT 配置、ros2_control 的 RT executor、Iceoryx 零拷贝通信，以及 Jetson 作为"软实时 + 外挂 MCU 硬实时"的工程真相。Part E 用两个完整项目把前四部分全部串起来——cart-pole 让你第一次体会完整 stack，四旋翼/四足让你在真实机器人上验证 200–500 Hz 的控制闭环。

Part F：CasADi 完整工作流详解（§3.14.17-§3.14.19）¶

§3.14.17 CasADi 符号建模与代码生成¶

CasADi 的定位：CasADi 不是求解器——它是一个**符号自动微分框架**，负责把数学表达式转化为高效的 C 代码。它是 acados、do-mpc、FORCES Pro 等 NMPC 框架的共同后端。

三层抽象： 1. SX（标量符号）：最底层，每个标量是一个节点，适合小系统或需要极致优化时 2. MX（矩阵符号）：中间层，支持矩阵运算和条件分支，适合大多数场景 3. Function：封装 SX/MX 表达式为可调用函数，支持 AD、codegen、序列化

完整四旋翼动力学建模示例：

import casadi as ca
import numpy as np

# ====== CasADi SX 符号建模 ======
# 状态: [px, py, pz, qw, qx, qy, qz, vx, vy, vz, wx, wy, wz]
x = ca.SX.sym('x', 13)
u = ca.SX.sym('u', 4)  # 四个转子推力

# 解包状态
p = x[0:3]       # 位置
q = x[3:7]       # 四元数 [w, x, y, z]
v = x[7:10]      # 线速度 (world frame)
omega = x[10:13] # 角速度 (body frame)

# 物理参数
m = 1.5          # 质量 [kg]
g = 9.81         # 重力
J = ca.diag(ca.SX([0.029, 0.029, 0.055]))  # 惯性矩阵
L = 0.175        # 臂长 [m]
kf = 1.0         # 力系数
km = 0.01        # 力矩系数

# 四元数→旋转矩阵
def quat2rot(q):
    """四元数到旋转矩阵 (Hamilton convention, scalar first)"""
    w, x, y, z = q[0], q[1], q[2], q[3]
    R = ca.vertcat(
        ca.horzcat(1-2*(y**2+z**2), 2*(x*y-w*z), 2*(x*z+w*y)),
        ca.horzcat(2*(x*y+w*z), 1-2*(x**2+z**2), 2*(y*z-w*x)),
        ca.horzcat(2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x**2+y**2))
    )
    return R

# 四元数导数: dq/dt = 0.5 * q ⊗ [0, omega]
def quat_derivative(q, omega):
    """四元数运动学"""
    Omega = ca.vertcat(
        ca.horzcat(0, -omega[0], -omega[1], -omega[2]),
        ca.horzcat(omega[0], 0, omega[2], -omega[1]),
        ca.horzcat(omega[1], -omega[2], 0, omega[0]),
        ca.horzcat(omega[2], omega[1], -omega[0], 0)
    )
    return 0.5 * Omega @ q

# 分配矩阵: 转子力→总力/力矩
# 十字型配置
alloc = ca.vertcat(
    ca.horzcat(1, 1, 1, 1),                    # Fz
    ca.horzcat(0, -L, 0, L),                   # Mx
    ca.horzcat(L, 0, -L, 0),                   # My
    ca.horzcat(-km/kf, km/kf, -km/kf, km/kf)  # Mz
)

# 总推力和力矩
thrust_torque = alloc @ u  # [Fz, Mx, My, Mz]
F_body = ca.vertcat(0, 0, thrust_torque[0])  # body frame 推力
M_body = thrust_torque[1:4]                   # body frame 力矩

# 动力学方程
R = quat2rot(q)
dp = v
dq = quat_derivative(q, omega)
dv = R @ F_body / m - ca.vertcat(0, 0, g)  # Newton
domega = ca.inv(J) @ (M_body - ca.cross(omega, J @ omega))  # Euler

# 组装完整状态导数
xdot = ca.vertcat(dp, dq, dv, domega)

# ====== 创建 CasADi Function ======
f_continuous = ca.Function('f', [x, u], [xdot], ['x', 'u'], ['xdot'])

# ====== RK4 离散化 ======
dt = ca.SX.sym('dt')
k1 = f_continuous(x, u)
k2 = f_continuous(x + dt/2*k1, u)
k3 = f_continuous(x + dt/2*k2, u)
k4 = f_continuous(x + dt*k3, u)
x_next = x + dt/6 * (k1 + 2*k2 + 2*k3 + k4)

# 四元数归一化 (数值稳定性关键!)
x_next[3:7] = x_next[3:7] / ca.norm_2(x_next[3:7])

f_discrete = ca.Function('f_disc', [x, u, dt], [x_next])

# ====== 代码生成 ======
# 生成 C 代码供 acados 使用
f_continuous.generate('quadrotor_dynamics.c', {'with_header': True})
print("生成 quadrotor_dynamics.c 完成")

# 验证: 悬停状态下 xdot 应为零
x0_hover = np.array([0,0,1, 1,0,0,0, 0,0,0, 0,0,0])
u0_hover = np.array([m*g/4]*4)  # 每个转子承担 1/4 重力
print(f"悬停 xdot = {f_continuous(x0_hover, u0_hover)}")
# 应输出接近零向量

代码生成的工程要点：

选项	说明	推荐值
`with_header`	生成 .h 头文件	True
`mex`	MATLAB MEX 兼容	False（纯 C++ 项目）
`main`	包含 main 测试函数	True（调试时）
`unroll_args`	展开参数为数组	True（与 acados 兼容）
`indent`	代码缩进	False（减小文件）

§3.14.18 acados Python → C 生成的完整流程¶

from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
import casadi as ca

# 1. 定义 acados 模型
model = AcadosModel()
model.name = 'quadrotor'
model.x = x       # 上面定义的 CasADi 状态符号
model.u = u       # 控制输入符号
model.f_expl_expr = xdot  # 显式 ODE 右端
model.x_dot = ca.SX.sym('xdot', 13)  # 隐式用
model.f_impl_expr = model.x_dot - xdot

# 2. 定义 OCP
ocp = AcadosOcp()
ocp.model = model

# 维度
N = 20  # 预测时域
ocp.dims.N = N

# 代价 (最小二乘型: ||y - y_ref||^2_W)
ny = 13 + 4  # 状态 + 输入
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.Vx = np.eye(13, 13)  # y 中包含完整状态
ocp.cost.Vu = np.zeros((13, 4))
ocp.cost.W = np.diag([100,100,100,  # 位置
                       10,10,10,10,  # 四元数
                       1,1,1,        # 速度
                       0.1,0.1,0.1,  # 角速度
                       0.01,0.01,0.01,0.01])  # 输入
ocp.cost.yref = np.zeros(ny)

# 约束
ocp.constraints.lbu = np.array([0, 0, 0, 0])        # 推力 >= 0
ocp.constraints.ubu = np.array([10, 10, 10, 10])     # 推力上限
ocp.constraints.idxbu = np.arange(4)
ocp.constraints.x0 = np.array([0,0,1, 1,0,0,0, 0,0,0, 0,0,0])

# 求解器选项
ocp.solver_options.tf = N * 0.05  # 终端时间 = N * dt
ocp.solver_options.integrator_type = 'ERK'  # 显式 RK
ocp.solver_options.sim_method_num_stages = 4  # RK4
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'

# 3. 生成 C 代码
ocp.code_export_directory = 'c_generated_code'
solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')

# 4. Python 测试
solver.set(0, 'lbx', x0_hover)
solver.set(0, 'ubx', x0_hover)
for i in range(N):
    solver.set(i, 'yref', np.concatenate([x0_hover, u0_hover]))
status = solver.solve()
print(f"求解状态: {status}, 用时: {solver.get_stats('time_tot')*1000:.2f} ms")
u_opt = solver.get(0, 'u')
print(f"第一步最优控制: {u_opt}")

生成物目录结构：

c_generated_code/
├── acados_solver_quadrotor.c       # 主求解器封装
├── acados_solver_quadrotor.h       # C API 头文件
├── quadrotor_model/
│   ├── quadrotor_expl_ode_fun.c    # 显式 ODE (CasADi 生成)
│   ├── quadrotor_expl_vde_forw.c   # VDE 前向灵敏度
│   └── quadrotor_expl_ode_hess.c   # Hessian (GN 时不用)
├── quadrotor_cost/
│   └── quadrotor_cost_y_fun.c      # 代价函数 y(x,u)
├── CMakeLists.txt                  # 可直接编译
├── Makefile                        # 备选编译系统
└── acados_ocp_nlp.json             # 配置序列化

§3.14.18b CasADi NLP 构造的工程深度¶

为什么理解 NLP 构造过程很重要？ acados 的 AcadosOcpSolver 封装了 NLP 的完整构造链——从模型到 NLP 到 QP 子问题到 KKT 系统。当 solver 不收敛或性能不达标时，你需要深入理解这个链条才能有效排查。把 acados 当黑盒用，迟早会在 HPIPM status=3 或 NaN 面前束手无策。

从 OCP 到 NLP 的离散化：acados 支持三种积分方式：

积分方式	名称	精度	速度	适用场景
`ERK`	显式 Runge-Kutta	$O(h^p)$, $p$ 由 stages 决定	最快	非刚性 ODE (四旋翼/车辆)
`IRK`	隐式 Runge-Kutta (Gauss-Legendre)	$O(h^{2s})$ (超收敛)	慢 2–5×	刚性系统 (接触/柔性)
`GNSF`	Generalized Nonlinear Static Feedback	介于两者	中等	可分离系统 (部分线性)

ERK 的 stages 选择：sim_method_num_stages = 4（RK4）是默认选择，4 阶精度对 50 ms 步长通常足够。但如果动力学变化剧烈（如接触切换），需要减小 dt 或增加 stages 到 6–8。IRK 的核心优势在于 A-stability——即使步长很大也不会数值发散，但每步需要解一个非线性方程组（Newton 迭代），计算量增加 2–5 倍。

从 NLP 到 QP 的线性化（SQP 的一步）：NMPC 的每次 SQP 迭代中，非线性动力学 $x_{k+1} = f(x_k, u_k)$ 在当前工作点 $(\bar x_k, \bar u_k)$ 处线性化为

\[\delta x_{k+1} = A_k \delta x_k + B_k \delta u_k + c_k\]

其中 $A_k = \partial f/\partial x|_{(\bar x_k, \bar u_k)}$，$B_k = \partial f/\partial u|_{(\bar x_k, \bar u_k)}$，$c_k = f(\bar x_k, \bar u_k) - A_k \bar x_k - B_k \bar u_k$。这些 Jacobian 正是 CasADi 代码生成的核心产物——<model>_expl_vde_forw.c 计算前向灵敏度（$A_k, B_k$），<model>_expl_ode_fun.c 计算 $f$ 本身。

Gauss-Newton Hessian 近似的工程含义：对最小二乘型代价 $\ell = \|y(x,u) - y_{\text{ref}}\|_W^2$，完整 Hessian 为 $\nabla^2 \ell = J_y^\top W J_y + \sum_i r_i \nabla^2 y_i$。Gauss-Newton 近似丢弃第二项（$r_i \nabla^2 y_i$），只保留 $J_y^\top W J_y$——这保证了 Hessian 近似是**半正定的**（而完整 Hessian 可能不定），使 QP 子问题始终凸。工程代价是：当残差 $r_i$ 很大（远离参考）时，近似精度下降，SQP 收敛变慢。此时应增加 RTI 的 SQP 步数（从 1 增到 2–3），或用 EXACT Hessian（hessian_approx = 'EXACT'）。

Condensing 与 Partial Condensing：OCP-QP 的决策变量是 $(x_0, u_0, x_1, u_1, \ldots, x_N)$，总维度 $(N+1)n_x + Nn_u$。Full condensing 利用动力学等式消去所有 $x_k$（$x_k$ 可以用 $x_0$ 和 $u_{0:k-1}$ 线性表示），将 QP 压缩到仅含 $Nn_u$ 个变量。这对 $n_u$ 小的系统（如四旋翼 $n_u=4$）极快，但对 $n_u$ 大的系统（如人形 $n_u=30$），condensed Hessian 变得稠密且维度 $30 \times 20 = 600$。Partial condensing 是折中：将 $N$ 步压缩成 $N_2 < N$ 个"超级步"（qp_solver_cond_N），每个超级步包含 $N/N_2$ 个原始步。HPIPM 内部对这些超级步使用 Riccati 递推。经验法则：$N_2 \in [3, 10]$，通常 $N_2 = 5$ 是一个好的起点。

Soft Constraint 的工程实现：当约束可能暂时不可行（如外部推力导致）时，硬约束会使 QP 不可行（HPIPM status=4）。acados 通过**松弛惩罚**处理：对约束 $g(x,u) \le 0$，引入松弛变量 $s \ge 0$：

\[g(x,u) - s \le 0, \quad \text{代价中加 } Z_l s + Z_u s^2\]

其中 $Z_l$（L1 惩罚）和 $Z_u$（L2 惩罚）控制违反代价。工程建议：

摩擦锥约束：软化（$Z_l = 100, Z_u = 1000$），因为突发滑移不应让 QP 崩溃
关节限位：硬约束，因为物理限位不可违反
碰撞距离：**软化**但 $Z$ 值很大，接近硬约束

# acados 软约束配置示例
ocp.constraints.lsh = np.zeros(n_friction_constraints)  # 松弛变量下界
ocp.constraints.ush = np.zeros(n_friction_constraints)   # 上界
ocp.constraints.idxsh = np.arange(n_friction_constraints)

# L1 + L2 惩罚权重
ocp.cost.zl = 100 * np.ones(n_friction_constraints)
ocp.cost.zu = 100 * np.ones(n_friction_constraints)
ocp.cost.Zl = 1000 * np.ones(n_friction_constraints)
ocp.cost.Zu = 1000 * np.ones(n_friction_constraints)

acados 求解器生命周期管理：在 C++ 中，acados solver 的生命周期必须严格管理——create 分配内部 BLASFEO 矩阵，free 释放。常见的 RAII wrapper：

class AcadosSolverRAII {
 public:
    AcadosSolverRAII() {
        capsule_ = model_acados_create_capsule();
        int status = model_acados_create(capsule_);
        if (status != 0) throw std::runtime_error("acados create failed");
        // 缓存常用指针
        cfg_ = model_acados_get_nlp_config(capsule_);
        dims_ = model_acados_get_nlp_dims(capsule_);
        in_ = model_acados_get_nlp_in(capsule_);
        out_ = model_acados_get_nlp_out(capsule_);
    }

    ~AcadosSolverRAII() {
        model_acados_free(capsule_);
        model_acados_free_capsule(capsule_);
    }

    // 禁止拷贝 (solver 不可拷贝)
    AcadosSolverRAII(const AcadosSolverRAII&) = delete;
    AcadosSolverRAII& operator=(const AcadosSolverRAII&) = delete;

    // 允许移动
    AcadosSolverRAII(AcadosSolverRAII&& other) noexcept
        : capsule_(other.capsule_) {
        other.capsule_ = nullptr;
    }

    int solve() { return model_acados_solve(capsule_); }

    void set_initial_state(const double* x0) {
        ocp_nlp_constraints_model_set(cfg_, dims_, in_, 0, "lbx", x0);
        ocp_nlp_constraints_model_set(cfg_, dims_, in_, 0, "ubx", x0);
    }

    void get_u0(double* u0) {
        ocp_nlp_out_get(cfg_, dims_, out_, 0, "u", u0);
    }

 private:
    model_solver_capsule* capsule_ = nullptr;
    ocp_nlp_config* cfg_ = nullptr;
    ocp_nlp_dims* dims_ = nullptr;
    ocp_nlp_in* in_ = nullptr;
    ocp_nlp_out* out_ = nullptr;
};

Warm-Start 策略的性能影响：RTI 的核心依赖是 warm-start——用上一次的最优解初始化下一次。标准策略是"shift by one step"：

\[\bar x_k^{\text{new}} = x_{k+1}^{\text{old}}, \quad \bar u_k^{\text{new}} = u_{k+1}^{\text{old}}, \quad k = 0, \ldots, N-2\]

最后一步用终端值复制：$\bar x_{N}^{\text{new}} = x_N^{\text{old}}$，$\bar u_{N-1}^{\text{new}} = u_{N-1}^{\text{old}}$。

// acados warm-start: shift + 终端复制
void warm_start_shift(AcadosSolverRAII& solver, int N, int nx, int nu) {
    std::vector<double> x_buf(nx), u_buf(nu);
    for (int k = 0; k < N - 1; ++k) {
        ocp_nlp_out_get(solver.cfg(), solver.dims(), solver.out(), k + 1, "x", x_buf.data());
        ocp_nlp_out_set(solver.cfg(), solver.dims(), solver.out(), k, "x", x_buf.data());
        ocp_nlp_out_get(solver.cfg(), solver.dims(), solver.out(), k + 1, "u", u_buf.data());
        ocp_nlp_out_set(solver.cfg(), solver.dims(), solver.out(), k, "u", u_buf.data());
    }
    // 最后一步: 复制
    ocp_nlp_out_get(solver.cfg(), solver.dims(), solver.out(), N - 1, "x", x_buf.data());
    ocp_nlp_out_set(solver.cfg(), solver.dims(), solver.out(), N, "x", x_buf.data());
}

实测数据：对 13 维四旋翼 NMPC（$N=20$），warm-start 使 HPIPM 的平均 QP 迭代从 8–12 次降到 1–3 次，求解时间从 1.5 ms 降到 0.3–0.5 ms——提速 3–5 倍。

反事实推理：如果不做 warm-start 会怎样？每次从零开始，HPIPM 需要从头搜索活动集——QP 迭代次数增加 3–5 倍。在 500 Hz 控制循环中，这意味着从"稳定 < 1 ms"变成"偶尔 > 2 ms 超时"。warm-start 不是"锦上添花"——对 RTI 来说，它是**必需品**。

§3.14.19 Drake C++ API 快速入门¶

Drake（MIT/TRI）是另一个主要的机器人控制框架，与 acados/Pinocchio 生态互补：

#include <drake/systems/framework/diagram_builder.h>
#include <drake/multibody/plant/multibody_plant.h>
#include <drake/multibody/parsing/parser.h>
#include <drake/systems/controllers/inverse_dynamics_controller.h>
#include <drake/systems/controllers/linear_quadratic_regulator.h>

using namespace drake;

// 1. 加载模型
systems::DiagramBuilder<double> builder;
auto [plant, scene_graph] = 
    multibody::AddMultibodyPlantSceneGraph(&builder, 0.001);
multibody::Parser parser(&plant);
parser.AddModels("quadrotor.urdf");
plant.Finalize();

// 2. 计算线性化 (在悬停点)
auto context = plant.CreateDefaultContext();
plant.get_actuation_input_port().FixValue(context.get(), u_hover);
auto linearization = systems::Linearize(plant, *context);

// 3. LQR 设计
Eigen::MatrixXd Q = 100 * Eigen::MatrixXd::Identity(nx, nx);
Eigen::MatrixXd R = Eigen::MatrixXd::Identity(nu, nu);
auto lqr_result = systems::controllers::LinearQuadraticRegulator(
    linearization->A(), linearization->B(), Q, R);

// 4. 仿真
auto diagram = builder.Build();
systems::Simulator<double> simulator(*diagram);
simulator.AdvanceTo(10.0);

Drake vs acados/Pinocchio 选型：

维度	Drake	acados + Pinocchio
定位	通用机器人仿真+控制	专业 NMPC 部署
语言	C++ (Bazel 构建)	C (CMake 构建)
多体动力学	自带 MultibodyPlant	Pinocchio
MPC 求解	SNOPT/IPOPT (通用)	HPIPM (OCP 专用)
嵌入式部署	困难 (Bazel 生态)	原生支持 (静态链接)
可视化	Meshcat (优秀)	需外部 (rviz/MuJoCo)
适合场景	研究/原型/仿真	生产/部署/实机

Drake MathematicalProgram 的独特优势：Drake 的 MathematicalProgram 是一个通用优化建模接口，它不限于 OCP 结构——你可以添加任意非线性约束、互补约束（LCP）、二次约束（SOCP）。这使得 Drake 在处理**接触隐含动力学**（接触力作为互补约束而非显式力）方面独树一帜。典型应用：操作臂的抓取规划、灵巧手的接触序列优化。

// Drake MathematicalProgram 示例: 带接触约束的轨迹优化
#include <drake/solvers/mathematical_program.h>
#include <drake/solvers/solve.h>

using namespace drake::solvers;

MathematicalProgram prog;

// 决策变量
auto x = prog.NewContinuousVariables(N * nx, "x");  // 状态
auto u = prog.NewContinuousVariables(N * nu, "u");  // 控制
auto lambda = prog.NewContinuousVariables(N * nc, "lambda");  // 接触力

// 动力学约束 (通过 MultibodyPlant 的 CalcTimeDerivatives)
for (int k = 0; k < N - 1; ++k) {
    // 隐式 Euler: x_{k+1} = x_k + dt * f(x_{k+1}, u_k, lambda_k)
    // 这是一个非线性等式约束
    prog.AddConstraint(
        std::make_shared<DynamicsConstraint>(plant, dt),
        {x.segment(k*nx, nx), x.segment((k+1)*nx, nx), 
         u.segment(k*nu, nu), lambda.segment(k*nc, nc)}
    );
}

// 互补约束 (接触不穿透 + 法向力非负)
// phi(q) >= 0 (不穿透), lambda_n >= 0 (法向力), phi * lambda_n = 0 (互补)
for (int k = 0; k < N; ++k) {
    auto phi_k = /* 距离函数 */;
    auto lambda_n_k = lambda.segment(k*nc, nc);
    prog.AddConstraint(phi_k >= 0);
    prog.AddConstraint(lambda_n_k >= 0);
    // 松弛互补约束: phi * lambda_n <= epsilon
    prog.AddConstraint(phi_k.cwiseProduct(lambda_n_k) <= 1e-4);
}

// 求解
auto result = Solve(prog);
std::cout << "求解状态: " << result.get_solution_result() << std::endl;

Drake 在 sim-to-real 验证中的角色：即使最终部署用 acados + Pinocchio，Drake 仍然在研发流程中扮演重要角色——它的 MultibodyPlant + SceneGraph 提供了**高保真的接触仿真**（基于 convex contact model 或 hydroelastic contact），可以在部署前验证 MPC 策略在复杂接触场景下的行为。典型工作流：

CasADi 建模 → acados 生成 C → C++ MPC 控制器
                                     ↕
Drake MultibodyPlant + SceneGraph → 高保真仿真验证
                                     ↕
                              通过? → 部署到 Jetson
                              失败? → 调整模型/约束

Drake 的 systems::controllers 工具箱：Drake 提供了一系列预置控制器，适合快速原型验证：

控制器	类名	适用场景
逆动力学	`InverseDynamicsController`	关节空间跟踪
LQR	`LinearQuadraticRegulator`	平衡点稳定
PID	`PidController`	简单位置控制
阻抗控制	需自行实现	力控/柔顺操作

跨领域类比：Drake 之于机器人控制，就像 MATLAB/Simulink 之于工业控制——功能全面但部署困难。acados 之于机器人控制，就像 PLC 编程之于工业控制——功能专一但部署高效。两者的关系不是替代，而是"研发阶段用 Drake，部署阶段用 acados"。

反事实推理：如果不用 Drake 做仿真验证会怎样？你可能在 MuJoCo 中测试通过（因为 MuJoCo 的接触模型相对简单），但在真实场景中遇到 Drake 能发现的问题——比如 hydroelastic contact 下的力分布与点接触模型不同。特别是在灵巧操作、双臂协作等需要精确接触力的场景中，Drake 的高保真接触模型是不可替代的验证工具。

Part G：性能优化与调试技巧（§3.14.20-§3.14.22）¶

§3.14.20 Flamegraph 性能分析实战¶

当 MPC 控制器在 Jetson 上跑不到目标频率时，第一步是**找到热点**：

# 1. 采集性能数据 (30 秒)
perf record -F 999 -g -p $(pgrep mpc_node) -- sleep 30

# 2. 生成 flamegraph
perf script | stackcollapse-perf.pl | flamegraph.pl > flame.svg

# 3. 在浏览器中打开 flame.svg 分析

常见热点及解决方案：

热点函数	占比	原因	解决方案
`blasfeo_dgemm_nt`	40-60%	HPIPM 矩阵乘法	确认 BLASFEO TARGET 正确
`malloc/free`	10-20%	动态分配	用 pre-allocated buffer
`casadi_f0`	15-30%	CasADi 模型函数	优化模型表达式
`__clone`	5-10%	线程创建	线程池 + thread pinning
`ros2` 相关	5-15%	pub/sub 序列化	切 Iceoryx 零拷贝

§3.14.21 HPIPM 状态码排查¶

状态码	含义	常见原因	解决方案
0	成功	-	-
1	达到最大迭代	QP 起点太差	增加 max_iter 或改 warm-start
2	数值问题	Hessian 不正定	加 `levenberg_marquardt` 正则化
3	min_step reached	步长太小	调 `qp_solver_cond_N`，切 `FULL_CONDENSING` 调试
4	不可行	约束矛盾	检查约束交集非空，加 soft constraint

HPIPM 调参流程图：

status != 0?
├── status == 1 (max_iter)
│   ├── warm_start 关了? → 开启 warm_start = 2
│   └── 初始猜测太差? → 用上一次解 shift + 终端复制
├── status == 2 (数值)
│   ├── Hessian 检查 → 加 lm_regularization = 1e-4
│   └── 约束 scaling → 归一化约束量纲
├── status == 3 (min_step)
│   ├── partial condensing N2 不合适? → 试 N2=5 或 N2=N
│   └── 约束过紧? → 放松或加 soft margin
└── status == 4 (infeasible)
    ├── 初始状态不在可行集? → 检查 x0 约束
    └── 约束互相矛盾? → 用 penalty method 替代硬约束

§3.14.22 实时控制的延迟预算分析¶

在 500 Hz 控制循环中，每个周期只有 2 ms 的时间预算：

┌─────────────────────────────── 2.0 ms 总预算 ──────────────────────────────┐
│                                                                             │
│  读传感器   MPC 准备    MPC 求解    WBC/CBF   写执行器  余量（jitter 容忍）  │
│  0.1 ms    0.3 ms     0.8 ms     0.4 ms    0.1 ms      0.3 ms            │
│                                                                             │
└─────────────────────────────────────────────────────────────────────────────┘

预算超支时的降级策略： 1. 等级 1：跳过本次 MPC 求解，复用上一次解（shift by 1 step） 2. 等级 2：切换到预计算的 LQR 反馈（零延迟但次优） 3. 等级 3：紧急制动（安全优先）

// 延迟监控与自动降级
auto t_start = std::chrono::steady_clock::now();
int status = mpc_solver.solve();
auto t_end = std::chrono::steady_clock::now();
double solve_ms = std::chrono::duration<double, std::milli>(t_end - t_start).count();

if (solve_ms > budget_ms_ * 0.8) {
    // 预警: 接近超时
    diagnostics_.warn_count++;
}
if (status != 0 || solve_ms > budget_ms_) {
    // 降级: 使用备份控制律
    u_apply = K_lqr_ * (x_current - x_ref_);
    diagnostics_.fallback_count++;
} else {
    u_apply = mpc_solver.get_u0();
}

Part H：Crocoddyl / Pinocchio 协作深度详解（§3.14.23–§3.14.25）¶

§3.14.23 Pinocchio 动力学建模的完整链路¶

为什么 Pinocchio 是机器人最优控制的基石？ 回顾 Part B 的手写算法：iLQR 需要动力学函数 $f(x,u)$ 及其 Jacobian $f_x,f_u$。对简单系统（cart-pole）可以手推解析形式，但对 30-DOF 人形机器人，手写 Jacobian 既不现实也不可维护。Pinocchio 的核心价值在于：它把多体动力学的全部计算——正/逆运动学、正/逆动力学、及其**解析导数**——封装成 $O(n)$ 复杂度的递推算法，同时保证机器精度。从 Riccati 到 DDP 到 NMPC，所有算法中出现的 $M(q), C(q,\dot q), g(q)$ 及其偏导都由 Pinocchio 一站式提供。

Model-Data 分离架构的设计哲学：Pinocchio 把所有不可变参数（连杆质量、惯性张量、关节拓扑、几何形状）存在 Model 中，而所有随算法变化的中间量（关节空间位置、雅可比、质量矩阵分解结果）存在 Data 中。这种分离有三个工程优势：(1) 同一 Model 可以被多个 Data 共享，天然支持多线程——MPC 线程和 WBC 线程各自持有独立的 Data，但共享同一个 Model，避免内存拷贝；(2) Data 的内存布局在 createData() 时一次性分配，后续所有算法调用都是**零分配**的——这对实时控制至关重要；(3) Model 可以序列化/反序列化（pinocchio::serialize），便于跨进程传递或持久化存储。

浮基机器人的关键陷阱：加载四足或人形机器人的 URDF 时，必须显式指定浮基类型。Pinocchio 提供四种关节模型供选择：

浮基类型	配置空间	切空间	适用场景
`JointModelFreeFlyer`	$\mathbb{R}^3 \times SO(3)$ (7D)	$\mathbb{R}^6$ (6D)	四足/人形/四旋翼
`JointModelPlanar`	$\mathbb{R}^2 \times SO(2)$ (4D)	$\mathbb{R}^3$ (3D)	平面移动机器人
无（默认）	仅关节	仅关节	固定基座机械臂

注意 $n_q \ne n_v$ 的根本原因：FreeFlyer 用四元数表示姿态（$n_q = 7$），但切空间用角速度（$n_v = 6$）。所有接受速度/加速度的 API（rnea, aba, computeABADerivatives）的维度是 $n_v$，不是 $n_q$。忽视这一点是浮基机器人编程中最常见的维度不匹配 bug。

解析导数的性能优势：RSS 2018 论文（Carpentier & Mansard）证明了 RNEA 和 ABA 的解析导数可以与正向计算共享递推核——对 30-DOF 机器人，完整的 computeABADerivatives（包含 $\partial \ddot q/\partial q$、$\partial \ddot q/\partial \dot q$、$M^{-1}$）仅需正向 ABA 调用时间的约 3 倍，典型值 15 μs。相比之下，CppAD tape 模式需要约 150 μs，有限差分需要 $(n_q+n_v+n_u)\times T_f \approx 30 \times 15 = 450$ μs。对 400 Hz MPC（每周期 2.5 ms 预算），这个差距直接决定了能否实时。

// 完整的 Pinocchio 动力学+导数评估链路
#include <pinocchio/algorithm/aba.hpp>
#include <pinocchio/algorithm/aba-derivatives.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>

// 正向动力学 (给定 q, v, tau → 计算 a)
pinocchio::aba(model, data, q, v, tau);
// data.ddq 现在包含加速度

// 正向动力学 + 全部解析导数
pinocchio::computeABADerivatives(model, data, q, v, tau);
// data.ddq_dq   : ∂a/∂q   (nv × nv)
// data.ddq_dv   : ∂a/∂v   (nv × nv)
// data.Minv     : M^{-1}  (nv × nv) —— 注意这是副产品！

// 在 iLQR/DDP 中的使用方式:
// f_x = [∂x_next/∂x] 需要组装:
//   对连续时间 x = [q, v], f = [v, M^{-1}(tau - h(q,v))]
//   ∂f/∂q = [0, I; ddq_dq]  (2*nv × 2*nv 的上半三角为 0 和 I)
//   ∂f/∂v = [I, 0; ddq_dv]
// 离散化后通过 RK4 的链式法则得到离散 Jacobian

碰撞检测集成（hpp-fcl）：Pinocchio 通过 GeometryModel + GeometryData 封装 hpp-fcl 的碰撞/距离查询。工作流：buildGeom(model, urdf, COLLISION, geomModel, pkgDir) 加载碰撞几何→ addCollisionPair 指定需检测的对→ updateGeometryPlacements(model, data, geomModel, geomData, q) 更新几何位姿→ computeCollisions(geomModel, geomData) 返回碰撞 flag→ computeDistances(geomModel, geomData) 返回最近点距离。在 CBF-QP 中，SDF 的梯度 $\partial d/\partial q$ 可通过 computeDistancesDerivatives 获取——这是构造关节空间 CBF $h(q) = d(q) - d_{\min}$ 的关键。

§3.14.24 Crocoddyl 多接触 DDP 完整实战¶

Crocoddyl 的设计理念：Crocoddyl（Contact Robot Control by Differential Dynamic Programming Library）由 LAAS-CNRS/Edinburgh/Heriot-Watt 联合开发，设计目标是让研究者能以"搭积木"的方式组装多接触最优控制问题。与 acados 的核心区别在于：acados 面向嵌入式 NMPC（RTI、代码生成、HPIPM），而 Crocoddyl 面向轨迹优化（多接触 DDP、FDDP、free-time 优化）。两者在四足机器人控制栈中通常互补：Crocoddyl 离线生成参考轨迹，acados 在线跟踪。

ActionModel 层次结构的深度理解：

Crocoddyl 的核心抽象是 ActionModelAbstract，每个 action model 代表一个时间步的"动力学+代价"。理解这个层次结构对正确组装问题至关重要：

ActionModelAbstract
├── IntegratedActionModelEuler       # Euler 离散化容器
│   └── DifferentialActionModel*     # 连续时间动力学+代价
│       ├── DifferentialActionModelFreeFwdDynamics    # 无接触自由浮动
│       └── DifferentialActionModelContactFwdDynamics # 有接触（关键！）
├── IntegratedActionModelRK          # RK4 离散化容器
└── ActionModelNumDiff               # 数值微分包装器

每一层的职责：DifferentialActionModel* 定义连续时间的 $\dot x = f(x,u)$ 和 $\ell(x,u)$，通过 calc() 计算 data.xnext（下一状态）和 data.cost（代价），通过 calcDiff() 计算 $F_x, F_u, L_x, L_u, L_{xx}, L_{xu}, L_{uu}$（DDP 需要的全部导数）。IntegratedActionModel* 把连续时间包装成离散时间 $x_{k+1} = F(x_k, u_k)$。

接触模型的核心机制：DifferentialActionModelContactFwdDynamics 在 ABA 正向动力学的基础上，加入接触约束。接触力 $\lambda$ 通过满足以下条件计算：

\[M\ddot q + h = S^\top \tau + J_c^\top \lambda, \quad \text{s.t. } J_c \ddot q + \dot J_c \dot q = 0\]

其中 $J_c$ 是接触雅可比。Pinocchio 内部通过 Baumgarte 稳定化或直接 KKT 求解。代价函数中可以包含接触力正则项 ResidualModelContactForce——这对避免接触力尖峰至关重要。

完整的四足站立到行走 DDP 示例：

import crocoddyl
import pinocchio as pin
import numpy as np

# ====== 1. 加载模型 ======
rmodel = pin.buildModelFromUrdf("a1.urdf", pin.JointModelFreeFlyer())
rdata = rmodel.createData()
state = crocoddyl.StateMultibody(rmodel)
actuation = crocoddyl.ActuationModelFloatingBase(state)
# 注意: FloatingBase 意味着前6维(浮基)不被驱动, 只有关节力矩

nu = actuation.nu  # 驱动维度 = nv - 6 = 12 (四足每腿3关节)

# ====== 2. 定义接触模型 (四足站立, 4个接触点) ======
contact_model = crocoddyl.ContactModelMultiple(state, nu)
for foot_name in ['FL_foot', 'FR_foot', 'RL_foot', 'RR_foot']:
    frame_id = rmodel.getFrameId(foot_name)
    # Contact3D: 只约束位置(3D), 不约束姿态
    contact = crocoddyl.ContactModel3D(
        state, frame_id,
        pin.SE3.Identity().translation,  # 参考接触位置
        nu, np.array([0, 50])  # Baumgarte gains [位置, 速度]
    )
    contact_model.addContact(foot_name, contact)

# ====== 3. 定义代价函数 ======
cost_model = crocoddyl.CostModelSum(state, nu)

# 3a. 质心高度跟踪 (保持站立高度)
com_ref = np.array([0, 0, 0.35])  # 目标质心高度 35 cm
com_residual = crocoddyl.ResidualModelCoMPosition(state, com_ref, nu)
com_activation = crocoddyl.ActivationModelWeightedQuad(
    np.array([0, 0, 100])  # 只惩罚 z 方向偏差
)
cost_model.addCost("com_track",
    crocoddyl.CostModelResidual(state, com_activation, com_residual), 1e2)

# 3b. 浮基姿态跟踪 (保持直立)
base_residual = crocoddyl.ResidualModelFramePlacement(
    state, rmodel.getFrameId("base_link"),
    pin.SE3(np.eye(3), com_ref), nu
)
cost_model.addCost("base_pose",
    crocoddyl.CostModelResidual(state, base_residual), 1e1)

# 3c. 关节力矩正则 (节省能量)
ctrl_residual = crocoddyl.ResidualModelControl(state, nu)
cost_model.addCost("ctrl_reg",
    crocoddyl.CostModelResidual(state, ctrl_residual), 1e-2)

# 3d. 接触力正则 (避免力尖峰, 关键!)
for foot_name in ['FL_foot', 'FR_foot', 'RL_foot', 'RR_foot']:
    force_residual = crocoddyl.ResidualModelContactForce(
        state, frame_id, pin.Force.Zero(), 3, nu
    )
    cost_model.addCost(f"{foot_name}_force",
        crocoddyl.CostModelResidual(state, force_residual), 1e-4)

# ====== 4. 组装连续时间动力学 + 代价 → 离散化 ======
diff_model = crocoddyl.DifferentialActionModelContactFwdDynamics(
    state, actuation, contact_model, cost_model, 0, True
)
# 0 = 摩擦锥内无约束(仅正则化), True = 使能 Baumgarte 稳定化
dt = 0.01  # 10 ms
running_model = crocoddyl.IntegratedActionModelEuler(diff_model, dt)

# ====== 5. 终端代价 (无控制项) ======
terminal_cost = crocoddyl.CostModelSum(state, nu)
terminal_cost.addCost("com_final",
    crocoddyl.CostModelResidual(state, com_activation, com_residual), 1e4)
terminal_diff = crocoddyl.DifferentialActionModelContactFwdDynamics(
    state, actuation, contact_model, terminal_cost, 0, True
)
terminal_model = crocoddyl.IntegratedActionModelEuler(terminal_diff, 0)

# ====== 6. 构建并求解 OCP ======
T = 100  # 100 步, 共 1 秒
x0 = np.concatenate([
    pin.neutral(rmodel),  # 名义配置 (nq=19)
    np.zeros(rmodel.nv)    # 零速度 (nv=18)
])

problem = crocoddyl.ShootingProblem(
    x0,
    [running_model] * T,
    terminal_model
)

# FDDP 求解器 (Feasibility-driven, 多接触稳定)
solver = crocoddyl.SolverFDDP(problem)
solver.setCallbacks([crocoddyl.CallbackVerbose()])

# 初始猜测: 全零控制 + 前向模拟
xs_init = [x0] * (T + 1)
us_init = [np.zeros(nu)] * T

converged = solver.solve(xs_init, us_init, maxiter=200, isFeasible=False)
print(f"收敛: {converged}, 迭代: {solver.iter}, 代价: {solver.cost:.4f}")

Python vs C++ 性能决策的量化分析：上面的 Python 代码通过 eigenpy 调用的是 C++ 后端——calc() 和 calcDiff() 的核心计算在 C++ 中执行，Python 只做调度。性能分析显示：如果**不覆盖** ActionModel 的虚函数（即使用预置模型），Python 和 C++ 的总求解时间差异 <10%。但如果你在 Python 中自定义了 calc/calcDiff——例如添加自定义障碍物代价——每次虚函数调用都要穿越 Python/C++ 边界（eigenpy 的 bp::override），开销约 2–5 μs/次。对 100 步 OCP、每步 2 次调用（calc+calcDiff），总开销约 $100 \times 2 \times 5 = 1$ ms——在离线轨迹优化中可忽略，但在 MPC（10 ms 预算）中会吃掉 10% 预算。

工程建议：原型阶段全用 Python——迭代速度快、debug 方便；生产部署把自定义代价用 C++ 实现（继承 CostModelAbstract），然后整个 solver 在 C++ 中运行。Crocoddyl 的 examples/ 目录下有完整的 C++ 示例。

§3.14.25 Pinocchio + Crocoddyl + MuJoCo/Gepetto-Viewer 可视化链¶

三层可视化方案：

层级	工具	用途	延迟
开发调试	Gepetto-Viewer	轨迹逐帧回放、关节极限可视化	实时
仿真验证	MuJoCo + mujoco-python-viewer	物理仿真 + 控制闭环	实时
论文图表	Meshcat (Drake)	浏览器内 3D 渲染、可嵌入 Jupyter	~100 ms

Gepetto-Viewer 完整工作流：

from pinocchio.visualize import GepettoVisualizer

# 创建可视化器
viz = GepettoVisualizer(rmodel, geom_model_col, geom_model_vis)
viz.initViewer(open=True)
viz.loadViewerModel("robot")

# 回放 Crocoddyl 求解结果
import time
for i, xs in enumerate(solver.xs):
    q = xs[:rmodel.nq]
    viz.display(q)
    time.sleep(dt)

MuJoCo 闭环验证的核心思路：Crocoddyl 求解的最优轨迹 $(x_k^*, u_k^*)$ 需要在物理引擎中验证——因为 Crocoddyl 使用的是解析动力学（Pinocchio ABA），而真实世界和高保真仿真器（MuJoCo）有接触模型差异。验证方法：用 $(x_k^*, u_k^*)$ 作为 MPC 的参考轨迹，在 MuJoCo 中闭环跟踪，观察跟踪误差是否可接受。

Part I：跨框架选型与迁移指南（§3.14.26–§3.14.28）¶

§3.14.26 四大框架深度对比¶

在 2026 年的机器人最优控制生态中，CasADi/acados、Crocoddyl、OCS2 和 Drake 是四大主流框架。正确选型需要理解它们的**设计理念、技术定位和社区生态**——不仅仅是功能对比表。

设计哲学的根本差异：

CasADi + acados 的核心理念是"符号微分 + 代码生成 + 嵌入式部署"。你用 CasADi 的 Python API 写出数学表达式（动力学、代价、约束），CasADi 自动对这些表达式做符号微分，然后生成高度优化的 C 代码。acados 把这些 C 函数嵌入到 HPIPM QP 求解器的 SQP/RTI 框架中。最终产物是一个纯 C 的 solver，可以在任何嵌入式平台上运行。这条路径的优势是**部署性**——从 Jetson 到 STM32，只要有 C 编译器就能跑。代价是：你必须用 CasADi 的符号语言描述问题，不能直接调用 Pinocchio 的 C++ API——需要通过 casadi_kin_dyn（Pinocchio 的 CasADi 绑定）或手动把动力学写成 CasADi SX/MX 表达式。

Crocoddyl 的核心理念是"多接触 DDP + 研究灵活性"。它直接使用 Pinocchio 的 C++ API 作为动力学后端，通过虚函数接口让用户以最小代码量组装自定义 OCP。Crocoddyl 的求解器（FDDP、BoxDDP、Intro）都是 DDP 变体——不像 acados 基于 SQP+QP，而是直接做二阶近似+线搜索。这使得 Crocoddyl 在处理**自由终端时间**、多接触切换、**空间约束**等非标准问题时更灵活。但 Crocoddyl 没有代码生成，不支持嵌入式部署——它是一个研究工具，不是生产部署工具。

OCS2 的核心理念是"腿足机器人全栈 MPC"。它由 ETH RSL（Robotic Systems Lab）维护，深度集成了 Pinocchio、hpp-fcl、CppAD/CppADCodeGen，提供 SLQ/iLQR/SQP/IPM 等多种求解器，并通过 ROS MPC/MRT（Model Reference Tracking）分离架构支持异步 MPC。OCS2 的独特之处是它**内置了腿足机器人特有的抽象**——gait schedule、swing leg trajectory、centroidal dynamics mapping——使得在 ANYmal/Go1/Unitree 上部署 MPC 只需修改 task.info 配置文件和 URDF，而不需要从头写求解器。

Drake 的核心理念是"通用机器人仿真+控制研究平台"。它由 MIT/TRI 维护，内置 MultibodyPlant（自研多体动力学引擎）、SceneGraph（碰撞检测和可视化）、MathematicalProgram（通用优化接口）。Drake 的 MPC/优化走的是通用 NLP 路径——通过 SNOPT 或 IPOPT 求解，不像 acados/HPIPM 那样利用 OCP 结构。这使得 Drake 在处理**非标准约束**（如 LCP 接触、complementarity）时非常强大，但嵌入式部署困难（Bazel 构建系统、庞大的依赖树）。

量化选型决策树：

你的控制周期是多少？
├── ≤ 2 ms (≥ 500 Hz)
│   ├── 需要嵌入式部署? → acados + HPIPM
│   └── 仅 x86 工作站? → acados 或 OCS2
├── 2–20 ms (50–500 Hz)
│   ├── 腿足机器人? → OCS2 (全栈最快)
│   ├── 操作臂/四旋翼? → acados (代码生成)
│   └── 研究新算法? → Crocoddyl (灵活)
└── > 20 ms (离线规划)
    ├── 多接触运动规划? → Crocoddyl 或 Drake
    ├── 需要 LCP/complementarity? → Drake
    └── 通用轨迹优化? → CasADi Opti + IPOPT

§3.14.27 框架间迁移的工程实践¶

从 Crocoddyl 到 acados 的迁移：这是最常见的迁移路径——研究阶段用 Crocoddyl 验证算法，生产阶段用 acados 部署。核心挑战是：Crocoddyl 的代价函数用 Pinocchio C++ API 计算（如 ResidualModelFramePlacement），而 acados 需要 CasADi 符号表达式。迁移步骤：

动力学迁移：用 casadi_kin_dyn（或手动）把 Pinocchio 动力学写成 CasADi SX/MX。对四旋翼等简单系统，直接手写（如 Part F 的示例）；对多体机器人，casadi_kin_dyn.CasadiKinDyn(urdf) 提供 .aba(), .rnea() 等 CasADi 兼容接口。
代价函数迁移：Crocoddyl 的 ResidualModel* 对应 acados 的 y(x,u) - y_ref 最小二乘形式。将所有 residual 拼接成 y 向量，权重矩阵对应 acados 的 W。
约束迁移：Crocoddyl 通过 ConstraintModel* 或 Augmented Lagrangian 处理约束；acados 直接支持 box、polytopic、nonlinear 约束。需要重新组装。
验证：在同一初始条件下，比较两个求解器的最优轨迹和代价。典型偏差应 <1%——如果超过，检查离散化方法（Crocoddyl Euler vs acados RK4）和约束处理方式。

从 OCS2 到独立部署的迁移：OCS2 深度绑定 ROS——如果目标平台不用 ROS（如嵌入式 Linux + CAN 直连），需要剥离 ROS 层。OCS2 的核心求解器（ocs2_sqp、ocs2_ipm）是纯 C++ 的，理论上可以独立使用。但 MPC/MRT 分离依赖 ROS topic，需要替换为自定义 IPC（共享内存/管道）。

§3.14.28 框架特性详细对比表¶

维度	CasADi + acados	Crocoddyl	OCS2	Drake
动力学后端	CasADi SX/MX	Pinocchio (直接)	Pinocchio + CppAD	MultibodyPlant
求解算法	SQP/RTI + HPIPM	DDP/FDDP/BoxDDP	SLQ/iLQR/SQP/IPM	SNOPT/IPOPT/SCS
约束处理	原生 (box/polytopic/NL)	Augmented Lagrangian	Aug-Lag/Relaxed Barrier	原生 (通用 NLP)
代码生成	核心功能	无	CppADCodeGen	无
嵌入式部署	原生支持	不支持	ROS 依赖	困难
多接触	手动建模	原生支持	原生 (centroidal)	LCP 原生
自由终端时间	需手动	原生支持	需自定义	MathematicalProgram
可视化	无 (外部)	Gepetto-Viewer	RViz	Meshcat (优秀)
构建系统	CMake	CMake	catkin/CMake	Bazel
社区活跃度	高 (学术+工业)	中 (学术)	中 (腿足社区)	高 (MIT/TRI)
典型求解时间 (30-DOF)	0.5–2 ms (RTI)	5–50 ms	1–10 ms	10–100 ms
Stars (2026)	~2k (acados)	~1.5k	~1k	~8k
机器人案例	四旋翼/车辆/通用	ANYmal/Talos/Solo	ANYmal/A1/Go1	Spot/通用

本质洞察：四个框架不是竞争关系——它们解决的是最优控制工作流中的不同环节。一个成熟的四足机器人控制栈通常**同时使用多个框架**：Crocoddyl 做离线轨迹库生成 → OCS2 做在线 MPC → acados 做低延迟 CBF-QP 安全层 → Drake 做高保真仿真验证。理解每个工具的最佳适用场景，比精通任何单一工具都更有价值。

Part J：高级性能优化技巧（§3.14.29–§3.14.31）¶

§3.14.29 SIMD 与并行化优化¶

为什么 SIMD 对最优控制至关重要？ 回顾 iLQR 的 backward pass：对每个时间步 $k$，核心计算是若干矩阵乘法和 Cholesky 分解。这些操作的矩阵维度通常在 4–30 之间——太小以至于 BLAS 的启动开销不可忽略，太大以至于不能用标量循环。SIMD（AVX2/NEON）在这个"甜蜜区间"提供了 2–8 倍的加速。

Eigen 的 SIMD 利用机制：Eigen 在编译时通过 intrinsic 检测（-mavx2 / -mfma / -march=native）自动向量化固定大小矩阵的操作。Matrix<double,4,4> 的 GEMM 在 AVX2 下使用 _mm256_fmadd_pd，一次处理 4 个 double。但**自动向量化的前提**是矩阵大小在编译期已知——MatrixXd 走的是运行时分派路径，不会触发 intrinsic 优化。

BLASFEO 的 panel-major 布局为什么快？ 传统 column-major 布局在做 GEMM 时，行方向上的元素不连续——每次 SIMD load 只能取到同一列的几个元素。BLASFEO 的 panel-major 把矩阵切成高度固定（通常 4 或 8，与 SIMD 宽度匹配）的水平 panel，panel 内部 column-major。这样每次 SIMD load 恰好取到一个 panel 行的全部元素，消除了行方向的缓存浪费。对 HPIPM 中的 Riccati 递推（大量 $12\times 12$ 到 $30\times 30$ 的矩阵操作），BLASFEO 比 Eigen 快 3–10 倍。

并行化策略：

层级	技术	适用场景	加速比
指令级	SIMD (AVX2/NEON)	单矩阵运算	2–4×
数据级	OpenMP parallel for	MPC 的多 scenario 并行	$N_{\text{core}}$×
任务级	std::async / 线程池	MPC preparation vs feedback	2× (流水线)
节点级	ROS2 多节点	MPC + WBC + 估计器分离	降低单核负载

OpenMP 在 Scenario MPC 中的应用：如果使用 scenario approach（回顾 §3.13.6），每个 scenario 的前向积分和线性化是独立的——天然适合 #pragma omp parallel for：

// Scenario MPC: N_s 个 scenario 并行线性化
#pragma omp parallel for schedule(dynamic)
for (int s = 0; s < N_s; ++s) {
    for (int k = 0; k < N; ++k) {
        // 每个 scenario 使用独立的 Pinocchio Data!
        pinocchio::computeABADerivatives(model, thread_data[s], 
                                          q_scenario[s][k], v_scenario[s][k], tau[k]);
        // 填充 scenario s 的 Jacobian 矩阵
        fill_jacobians(s, k, thread_data[s]);
    }
}

关键陷阱：Pinocchio Data 不是线程安全的——每个线程必须持有独立的 pinocchio::Data 实例。共享 Data 会导致数据竞争和不确定性 bug。

§3.14.30 内存布局与缓存优化¶

为什么缓存友好性在 MPC 中如此重要？ 现代 CPU 的 L1 缓存延迟约 1 ns，主存延迟约 100 ns——差距 100 倍。iLQR 的 backward pass 对每个时间步需要访问 $Q_{xx}, Q_{uu}, Q_{ux}, V_{xx}, k, K$ 等多个矩阵。如果这些矩阵在内存中不连续，每步都会触发缓存未命中。

预分配连续内存的策略：

// 不好的做法: std::vector<MatrixXd> 每个元素独立分配
std::vector<Eigen::MatrixXd> P_history(N);  // N 次 heap alloc!

// 好的做法: 一次分配, 使用 Eigen::Map 视图
constexpr int NX = 13;
std::vector<double> P_storage(N * NX * NX);  // 单次分配
auto P = [&](int k) -> Eigen::Map<Eigen::Matrix<double, NX, NX>> {
    return Eigen::Map<Eigen::Matrix<double, NX, NX>>(
        P_storage.data() + k * NX * NX
    );
};
// P(k) 返回第 k 步的 P 矩阵视图, 内存连续

struct-of-arrays vs array-of-structs：

// AoS (差): 不同步骤的同类型矩阵在内存中不连续
struct TimeStep {
    Eigen::Matrix<double,NX,NX> Qxx, Vxx;
    Eigen::Matrix<double,NU,NU> Quu;
    Eigen::Matrix<double,NU,NX> Qux, K;
    Eigen::Matrix<double,NU,1> k;
};
std::vector<TimeStep> steps(N);  // 步骤内连续, 步骤间跳跃

// SoA (好): 同类型矩阵跨步骤连续——backward pass 扫描时缓存友好
struct RiccatiStorage {
    Eigen::Matrix<double, NX*NX, Eigen::Dynamic> Vxx_all;  // 列=步骤
    Eigen::Matrix<double, NU*NU, Eigen::Dynamic> Quu_all;
    // ... 每个 backward step 顺序访问列, 缓存预取高效
};

实测效果：在 13 维四旋翼 MPC（$N=20$）中，SoA 布局的 backward pass 比 AoS 快约 15–20%——主要来自 L1 缓存命中率的提升（从约 85% 到约 95%）。

§3.14.31 实时性验证的完整方法论¶

实时性不是"跑得快"——而是"每次都跑得一样快"。 平均求解时间 0.5 ms 但 p99 是 5 ms 的控制器，不如平均 1.2 ms 但 p99 是 1.5 ms 的控制器。在 500 Hz 控制循环中，一次 5 ms 的尖峰意味着跳过 2–3 个控制周期——足以导致机器人摔倒。

五层验证方法论：

层级	验证内容	工具	合格标准
L1: 微观	单次 solver 调用时间	Google Benchmark	mean < 预算的 50%
L2: 周期	单个控制周期端到端延迟	`clock_gettime` + HdrHistogram	p99 < 预算的 80%
L3: 系统	内核调度抖动	cyclictest / rtla timerlat	max < 100 μs
L4: 压力	高负载下的延迟	stress-ng + L2	p99.9 < 预算
L5: 长期	24h+ 运行的延迟分布	Prometheus + Grafana	无延迟漂移

HdrHistogram 的集成方式：

#include <hdr/hdr_histogram.h>

class LatencyMonitor {
 public:
    LatencyMonitor() {
        // 记录 1 μs 到 100 ms 的延迟, 3 位有效数字
        hdr_init(1, 100000, 3, &histogram_);
    }

    void record(int64_t latency_us) {
        hdr_record_value(histogram_, latency_us);
    }

    void report() const {
        printf("p50: %ld μs\n", hdr_value_at_percentile(histogram_, 50.0));
        printf("p99: %ld μs\n", hdr_value_at_percentile(histogram_, 99.0));
        printf("p99.9: %ld μs\n", hdr_value_at_percentile(histogram_, 99.9));
        printf("max: %ld μs\n", hdr_max(histogram_));
    }

 private:
    struct hdr_histogram* histogram_;
};

// 在控制循环中使用
LatencyMonitor lat_monitor;
while (running) {
    auto t0 = steady_clock::now();
    // ... read + solve + write ...
    auto t1 = steady_clock::now();
    int64_t dt_us = duration_cast<microseconds>(t1 - t0).count();
    lat_monitor.record(dt_us);
}

延迟尖峰的系统级排查流程：

p99 > 预算?
├── 用 perf 确认热点来源
│   ├── solver 本身慢 → 检查 warm-start / 正则化 / 模型复杂度
│   ├── malloc → mlockall + pre-allocate + 检查 STL 容器
│   └── 内核中断 → isolcpus + irq affinity + RT kernel
├── 尖峰是否周期性?
│   ├── 是 (~100 Hz) → CPU freq scaling → performance governor
│   ├── 是 (~1 Hz) → RCU/workqueue 回调 → rcu_nocbs
│   └── 随机 → page fault → mlockall + stack prefault
└── 仅在多线程时出现?
    ├── priority inversion → PTHREAD_PRIO_INHERIT
    ├── false sharing → 对齐到 cache line (64B)
    └── lock contention → lock-free queue / SPSC buffer

从 §3.14.13 的理论到这里的实践映射：§3.14.13 讲了 PREEMPT_RT、SCHED_FIFO、CPU 隔离的原理；本节把这些原理编织成一个可执行的验证流程。两者的关系是"为什么"和"怎么验证"——没有验证的实时配置只是心理安慰。

故障排查手册¶

症状	可能原因	排查步骤	相关节
编译时 `SIGILL`	BLASFEO TARGET 不匹配	1.确认目标架构；2.重编三件套	§3.14.12
acados 生成代码编译失败	绝对路径问题	1.检查 CMakeLists.txt 路径；2.sed 替换	§3.14.10
HPIPM status=3	min_step / scaling	1.调 qp_solver_cond_N；2.加 LM 正则	§3.14.21
ROS2 控制器延迟尖峰	malloc / GC / IRQ	1.perf + flamegraph；2.mlockall；3.isolcpus	§3.14.13
Pinocchio URDF 加载崩溃	缺少 FreeFlyer joint	加 JointModelFreeFlyer	§3.14.8
Eigen 段错误	SIMD 对齐问题	EIGEN_MAKE_ALIGNED_OPERATOR_NEW	§3.14.1
MPC 频率不稳定	CPU freq scaling	cpupower performance mode	§3.14.13
CBF-QP 输出 NaN	OSQP eps 太松	设 eps_abs=1e-6 + polish	§3.14.11
Crocoddyl FDDP 不收敛	初始猜测不可行	设 `isFeasible=False`，加 cost 正则	§3.14.24
OCS2 MPC/MRT 频率不匹配	policy 插值延迟	检查 `mpc_policy` topic QoS	§3.14.9
多线程 Pinocchio 崩溃	共享 Data 对象	每线程独立 `createData()`	§3.14.23
CasADi codegen 文件过大	模型表达式未简化	使用 `cse=True` 公共子表达式消除	§3.14.17

前置自测¶

#	自测题	前置
Q1	DARE 的标准形式是什么？它与 LQR 的关系？	3.3/3.5
Q2	iLQR 的 backward pass 中 $Q_{uu}$ 的物理含义？	3.9
Q3	NMPC 的 SQP 步与完整 NLP 的关系？RTI 为什么只做一步？	3.11
Q4	CBF 的李导数约束如何转化为 QP？	3.8
Q5	什么是 Cholesky 分解？为什么 LDLT 比 LLT 更稳定？	线性代数基础

本章常见误解汇总¶

误解	正确理解
Eigen 的动态矩阵（`MatrixXd`）和固定矩阵（`Matrix3d`）性能差不多	固定大小矩阵在栈上分配且编译器可做 SIMD 向量化，比动态矩阵快 10-100 倍；在 1 kHz 控制循环中这个差距是致命的
CasADi 的符号微分和有限差分精度相同	CasADi 的 AD（自动微分）给出机器精度的导数，而有限差分有 $O(h) + O(\epsilon/h)$ 的截断/舍入误差权衡；AD 对约束 Jacobian 的精度至关重要
Pinocchio 和 RBDL 功能等价，选哪个都行	Pinocchio 提供解析的动力学导数（$\partial \text{RNEA}/\partial q, \dot{q}$）且支持 Lie 群操作，RBDL 不提供解析导数；对 DDP/MPC 类算法 Pinocchio 是必选
`Release` 编译就足以获得最佳性能	还需 `-march=native`（启用 AVX/AVX2 指令集）、链接 BLAS/LAPACK（MKL 或 OpenBLAS）、消除堆分配（预分配 workspace）；这些工程细节的累积效果可达 5-10 倍
acados 和 CasADi 是竞争关系	acados 使用 CasADi 作为前端（问题建模+代码生成），后端用 HPIPM/BLASFEO 求解——两者是互补的上下游关系
只要算法正确，C++ 和 Python 实现性能差距不大	纯 Python 的矩阵运算通过 NumPy（底层 BLAS）确实快，但控制循环中的分支逻辑、内存分配、GC 暂停使 Python 在 100 Hz 以上不可用；C++ 是实时控制的唯一选择
交叉编译到嵌入式平台只需改编译器路径	嵌入式部署还需：静态链接所有依赖、消除动态内存分配、验证 WCET（最坏执行时间）、配置 PREEMPT_RT 实时调度、设计超时 fallback 策略

本章小结¶

Part	核心能力	关键工具	验证标准
A	C++ 工具链 + 数值基础	CMake, Eigen, AD	Riccati 一步 < 1 μs
B	从零手写 LQR/iLQR/NMPC/CBF	Eigen + LDLT	cart-pole swing-up 收敛
C	生产级框架集成	Pinocchio/Crocoddyl/OCS2/acados	legged_control 编译通过
D	嵌入式部署	BLASFEO/HPIPM/PREEMPT_RT	Jetson 上 200 Hz 无抖动
E	完整项目	全栈	实机闭环
F	CasADi 代码生成	CasADi + acados template	四旋翼 NMPC 求解 < 2 ms
G	性能优化与调试	perf/flamegraph/HPIPM debug	p99 延迟 < 目标的 80%
H	Crocoddyl/Pinocchio 深度	多接触 DDP + 解析导数	四足站立 DDP 收敛
I	跨框架选型	四大框架对比 + 迁移	能为项目选择正确框架
J	高级性能优化	SIMD/缓存/RT验证	p99 < 预算 80%

练习题汇编¶

本节汇集全章各 Part 的综合练习，按难度分级。

Part A/B 基础练习¶

练习 A.1 ⭐：编写一个 CMakeLists.txt，使用 FetchContent 下载 Eigen3 和 GoogleTest，创建一个测试 target 验证 Matrix3d 的 LLT 分解。确保 Release 模式开启 -O3 -march=native，Debug 模式开启 -fsanitize=address。

练习 A.2 ⭐⭐：实现一个简单的 benchmark，比较 Eigen::Matrix<double,13,13> 和 Eigen::MatrixXd(13,13) 的 GEMM 性能差异。用 Google Benchmark 框架，记录均值和标准差。解释为什么固定大小更快。

练习 B.1 ⭐⭐：手写 DARE 迭代法和 Doubling algorithm，对比二者在 cart-pole 系统上的收敛速度（迭代次数 vs 精度）。画出 $\|P_{k+1} - P_k\|_\infty$ 随迭代次数的曲线。

练习 B.2 ⭐⭐⭐：实现完整的 iLQR 求解 cart-pole swing-up。要求包含：(a) Levenberg-Marquardt 正则化；(b) Armijo line search；(c) box 约束处理（Tassa 2014 boxQP）。比较有无 box 约束时的收敛行为。

练习 B.3 ⭐⭐⭐：在 B.2 的基础上，实现 CBF-QP 安全层。定义 $h(x) = x_{\max} - |x_{\text{cart}}|$（小车不出轨道范围），用 ProxQP 求解。测试当 iLQR 策略试图让小车冲出边界时，CBF-QP 如何修正。

Part C/F 框架集成练习¶

练习 C.1 ⭐⭐：用 Pinocchio 加载一个 7-DOF 机械臂（如 Franka Panda）的 URDF，计算 RNEA 和 ABA 及其导数。验证 data.M 的对称正定性（所有特征值 > 0），并检查 data.dtau_dq 与有限差分 Jacobian 的差异（应 < 1e-6）。

练习 C.2 ⭐⭐⭐：用 CasADi 建模一个双积分器 $\ddot x = u$，完成 acados 的完整工作流：(a) Python 定义 AcadosOcp；(b) 生成 C 代码；(c) 写 C++ wrapper；(d) 用 GoogleTest 验证跟踪性能。对比 RTI 和 full SQP 的求解时间。

练习 C.3 ⭐⭐⭐：复现 §3.14.24 的四足站立 Crocoddyl 示例。修改代价权重，观察以下行为变化：(a) 增大质心高度跟踪权重 → 站得更稳但力矩更大；(b) 增大接触力正则权重 → 力矩更平滑但跟踪变差；(c) 去掉接触力正则 → 接触力出现尖峰。

Part D/G 部署与优化练习¶

练习 D.1 ⭐⭐：编写 toolchain-aarch64-linux-gnu.cmake，交叉编译 BLASFEO（TARGET=GENERIC，避免 SIGILL）。在 QEMU 下验证编译产物可运行。

练习 D.2 ⭐⭐⭐：在 Ubuntu 上启用 PREEMPT_RT 内核（或使用 Docker + --cap-add SYS_NICE），运行 cyclictest 测量基线延迟。然后启动你的 MPC 节点，再次测量——对比有无 MPC 负载时的 max latency。

练习 G.1 ⭐⭐：对 §3.14.20 的 flamegraph 分析方法，在一个简单的 MPC 节点上实际执行。找到至少两个热点函数，提出优化方案。

练习 G.2 ⭐⭐⭐：实现 §3.14.30 中的 SoA 内存布局优化。用 perf stat 比较 AoS 和 SoA 两种布局的 L1 cache miss 率。画出随 OCP horizon $N$ 增大时两种布局性能差距的曲线。

Part H/I 跨框架练习¶

练习 H.1 ⭐⭐⭐：用 Crocoddyl 和 acados 分别求解同一个四旋翼悬停稳定问题（相同动力学、代价、约束）。比较：(a) 最优代价差异；(b) 最优轨迹差异；(c) 求解时间差异。解释差异来源（离散化方法、QP 求解器、Hessian 近似）。

练习 H.2 ⭐⭐⭐⭐：设计一个完整的 Crocoddyl→acados 迁移实验。用 Crocoddyl 对一个简单机械臂做轨迹优化（100 步），然后把动力学和代价迁移到 acados，用 RTI 做 MPC 跟踪。测量跟踪误差是否在可接受范围内（< 5%）。

跨章综合练习¶

综合练习 1（数值稳定性 + 工程实践） ⭐⭐⭐

回顾 §3.14.2 的数值线性代数核心和 §3.14.4 的 DARE 实现。设计一个实验： (a) 构造一个"病态"的 DARE 问题（$R$ 接近奇异，条件数 > $10^8$）； (b) 分别用 LLT、LDLT 和 QR 分解求解，记录精度； (c) 验证 Tikhonov 正则化 $R \leftarrow R + \lambda I$ 对精度的影响； (d) 回顾 §3.13（鲁棒 MPC）：这种数值敏感性对 Tube MPC 的约束收紧有什么影响？

综合练习 2（AD + 框架选型 + 部署） ⭐⭐⭐

设计一个完整的"从 Python 原型到 Jetson 部署"流水线： (a) 用 CasADi Python 建模一个 6-DOF 机械臂动力学（可用 Pinocchio 的 CasADi 绑定）； (b) 用 acados 生成 C 代码； (c) 编写 C++ wrapper + ROS2 节点； (d) 交叉编译到 aarch64（或 QEMU 验证）； (e) 用 HdrHistogram 记录 1000 次求解的延迟分布，报告 p50/p99/max。

综合练习 3（系统辨识 + MPC 工程实践） ⭐⭐⭐

结合 150 号文件（系统辨识）和本章： (a) 用 Pinocchio 的 computeJointTorqueRegressor 生成回归矩阵； (b) 用 OLS 辨识一个仿真机械臂的动力学参数； (c) 把辨识后的参数填入 acados MPC 模型，对比 CAD 参数和辨识参数下的跟踪性能。

常见陷阱速查（扩展版，25 条）¶

本节将 Part A–J 的所有陷阱汇总为快速查阅表。前 15 条已在正文中详述，此处补充 Part H–J 的 10 条新陷阱。

(16) Pinocchio nq != nv：浮基机器人的配置维度（$n_q$，含四元数 7D）与速度维度（$n_v$，角速度 6D）不同。用 $n_q$ 分配速度/加速度向量会导致维度不匹配崩溃。修法：始终用 model.nv 分配速度/加速度相关变量。

(17) Pinocchio integrate vs 直接加法：在流形上更新配置不能用 $q_{\text{new}} = q + \delta q$——四元数部分不满足 $\|q\| = 1$。必须用 pinocchio::integrate(model, q, v*dt) 做李群积分。

(18) Crocoddyl isFeasible 参数：solver.solve(xs, us, maxiter, isFeasible)。如果初始猜测不满足动力学约束（如 $x_{k+1} \ne f(x_k, u_k)$），必须设 isFeasible=False——否则 FDDP 假设初始轨迹可行而跳过 feasibility 驱动机制，导致不收敛或数值错误。

(19) Crocoddyl 接触模型忘记 Baumgarte 稳定化：不设 Baumgarte gains（默认为零）时，数值积分中接触约束会逐步漂移——接触点"穿透"地面。修法：设 gains [0, 50]（位置 gain 0 + 速度 gain 50 是常用经验值）。

(20) OCS2 的 TargetTrajectories 时间戳不连续：MRT 用三次样条插值 policy，如果时间戳有跳变（如 MPC 重新初始化），插值会产生巨大瞬态。修法：重新初始化后发送一个平滑过渡的 target。

(21) Drake Bazel 构建缓存失效：修改任何 BUILD 文件后，Bazel 可能重编大量 target（>30 分钟）。修法：用 --disk_cache 持久化缓存，或用 cmake 分支（Drake 从 1.x 起支持 CMake 安装）。

(22) CasADi SX vs MX 混用：SX 和 MX 是两套独立的符号图，不能混用——SX 表达式不能出现在 MX 函数中。acados 模型要求 SX（因为需要稀疏性分析），而 CasADi 的 integrator 类默认生成 MX。修法：用 SXFunction 或在 acados 中设 model_external_shared_lib_type='casadi'。

(23) SIMD 对齐在不同编译单元间不一致：TU1 用 -mavx2 编译，TU2 用 -msse4.2——链接后 TU1 的 AVX 对齐假设（32B）在 TU2 分配的内存（16B 对齐）上崩溃。修法：整个项目统一 -march 标志，或在接口层用 alignas(32) 显式指定。

(24) 多 scenario 并行中忘记独立 Pinocchio Data：OpenMP 并行循环中共享 pinocchio::Data 导致不确定性数值错误（每次运行结果不同）。修法：#pragma omp parallel 块内 auto data = model.createData()。

(25) 实时线程中使用 std::cout：std::cout 内部持有互斥锁（std::mutex），一旦在 RT 线程中调用，可能被非 RT 线程抢占后持锁等待——经典优先级反转。修法：所有 RT 线程的日志/诊断通过 lock-free 队列传到非 RT 线程输出。

延伸阅读¶

资料	内容	难度
Frison & Diehl 2020 HPIPM 论文	OCP-QP 算法	⭐⭐⭐
Mastalli 2020 Crocoddyl	多接触 DDP	⭐⭐⭐
Carpentier 2018 Pinocchio RSS	解析导数	⭐⭐⭐
acados 官方文档	API reference	⭐⭐
ros2_control 设计文档	架构理解	⭐⭐
Iceoryx 白皮书	零拷贝通信	⭐⭐⭐

累积项目：本章新增模块¶

在"从零构建四旋翼 NMPC"项目中，本章覆盖**全部工程实现**：

阶段	内容	代码量	依赖
1	Eigen 手写 LQR + cart-pole	500 行 C++	Part B
2	iLQR swing-up	800 行 C++	Part B
3	acados NMPC + CasADi 建模	400 行 Python + 生成 C	Part C/F
4	C++ wrapper + ROS2 集成	600 行 C++	Part C/D
5	Jetson 交叉编译 + 部署	脚本 + CMake	Part D
6	CBF-QP 安全层	300 行 C++	Part B
7	性能调优 + 监控	200 行 C++	Part G

总计: ~2800 行 C++ + ~400 行 Python。项目代码位于 projects/quadrotor_mpc/。

术语对照表¶

英文术语	中文	首次出现节
DARE (Discrete Algebraic Riccati Equation)	离散代数 Riccati 方程	§3.14.4
iLQR (iterative LQR)	迭代线性二次调节器	§3.14.5
DDP (Differential Dynamic Programming)	微分动态规划	§3.14.5
NMPC (Nonlinear MPC)	非线性模型预测控制	§3.14.6
RTI (Real-Time Iteration)	实时迭代	§3.14.6
SQP (Sequential Quadratic Programming)	序列二次规划	§3.14.6
CBF (Control Barrier Function)	控制屏障函数	§3.14.7
HOCBF (High-Order CBF)	高阶控制屏障函数	§3.14.7
mRPI (minimal Robust Positively Invariant)	最小鲁棒正不变集	§3.14.7
AD (Automatic Differentiation)	自动微分	§3.14.3
CppAD	C++ 自动微分库	§3.14.3
CppADCodeGen	CppAD 代码生成扩展	§3.14.3
BLASFEO	嵌入式优化线性代数库	§3.14.2
HPIPM	高性能内点法 QP 求解器	§3.14.11
Panel-Major	BLASFEO 的矩阵内存布局	§3.14.2
LDLT	不开根号的 Cholesky 分解	§3.14.2
Square-Root Riccati	Riccati 的平方根形式	§3.14.2
PREEMPT_RT	Linux 实时抢占补丁	§3.14.13
SCHED_FIFO	POSIX 实时调度策略	§3.14.13
CPU Pinning / Isolation	CPU 核心绑定与隔离	§3.14.13
Iceoryx	零拷贝进程间通信中间件	§3.14.14
RealtimePublisher	ros2 实时安全发布器	§3.14.14
Flamegraph	火焰图性能分析	§3.14.20
HdrHistogram	高动态范围直方图	§3.14.31
WCET (Worst-Case Execution Time)	最坏执行时间	§3.14.13
Condensing	QP 变量压缩	§3.14.18b
Partial Condensing	部分变量压缩	§3.14.18b
Gauss-Newton Approximation	高斯-牛顿 Hessian 近似	§3.14.18b
Soft Constraint	软约束（松弛惩罚）	§3.14.18b
Warm-Start	热启动（用上次解初始化）	§3.14.18b
FDDP (Feasibility-Driven DDP)	可行性驱动 DDP	§3.14.24
ActionModel	Crocoddyl 时间步抽象	§3.14.24
Baumgarte Stabilization	接触约束数值稳定化	§3.14.24
MPC/MRT Separation	OCS2 的求解/跟踪分离	§3.14.9
SoA (Struct of Arrays)	数组结构体（缓存友好）	§3.14.30
CRTP (Curiously Recurring Template Pattern)	静态多态模式	§3.14.1

桥梁总览（承前启后）¶

本专题是第三批 MPC 系列的**工程收官**，它与前后章节的关系网络如下：

向前承接：

→ 控制理论/50_LQR_LQG与Riccati方程：Part B 的手写 DARE 直接实现了 50 号文件的理论公式。读过那里的推导后，本章的 C++ 代码每一行都能映射到具体的数学公式。
→ 控制理论/90_DDP_iLQR原理与实现：iLQR 的 backward pass 公式（$Q_{xx}, Q_{uu}, Q_{ux}$）来自 90 号文件的二阶贝尔曼展开。本章把这些公式逐行翻译成 Eigen 代码。
→ 控制理论/110_非线性MPC稳定性：acados 的 RTI 对应 110 号文件的 SQP 框架；终端约束和终端代价的设计准则来自那里的"四条件"定理。
→ 控制理论/120_MPC数值求解与实时实现：HPIPM 的 OCP-QP 结构、partial condensing 策略、QP warm-start 的理论基础在 120 号文件中详述。本章聚焦于"怎么在 C++ 中调用"，而 120 号文件聚焦于"为什么这么设计"。
→ 控制理论/130_鲁棒与随机MPC：Part B 的 Tube MPC / CBF-QP 实现直接对应 130 号文件的鲁棒 MPC 理论。mRPI 的离线计算、约束收紧、反馈律——130 号文件给出数学证明，本章给出 C++ 代码。

向后指向：

→ 控制理论/150_机器人系统辨识：本章的 Pinocchio computeJointTorqueRegressor API 在系统辨识中是核心工具。辨识出的参数直接填入本章的 MPC 模型。综合练习 3 横跨两章。
→ 控制理论/160_HJ可达性分析：HJ 值函数可以作为 CBF 使用（160 号文件 §8 "CBVF"），而本章 §3.14.7 的 CBF-QP 框架是 HJ 值函数在线部署的载体。
→ 运动控制/DDP 家族与 Crocoddyl：本章 Part H 的 Crocoddyl 深度与运动控制教程中的 DDP 应用互补——一个侧重 C++ 工程，一个侧重机器人场景。
→ 运动控制/OCS2 完整栈：本章 §3.14.9 的 OCS2 概述与运动控制教程中 OCS2 的深度教程衔接。这里讲"怎么集成"，那里讲"怎么调参"。

跨批次联系：

第四批（刚体动力学）：Pinocchio 的 ABA/RNEA/解析导数来自刚体动力学的空间向量理论。理解 Featherstone 的递归算法，才能真正理解为什么 Pinocchio 的导数是 $O(n)$ 复杂度。
第五批（SLAM）：状态估计提供 MPC 的 $\hat x$。估计协方差 $\Sigma_{\hat x}$ 驱动 Tube MPC 的 $\mathcal{W}$（回顾 130 号文件 §3.13.1）。
第六批（RL）：RL 策略 + MPC 安全层是当前最成熟的控制架构。本章的 CBF-QP 代码直接服务于 RL 策略的安全部署。

本章目标¶

学完本专题后，你应能：

不依赖任何框架，用纯 Eigen C++ 手写 LQR/iLQR 并跑通 cart-pole swing-up
用 CasADi Python 建模 → acados 生成 C → C++ wrapper → ROS2 节点 完成四旋翼 NMPC 的完整工程链
**用 Pinocchio 的解析导数**为 Crocoddyl DDP 提供高效动力学后端
正确交叉编译 BLASFEO/HPIPM/acados 到 Jetson Orin，避免 SIGILL
配置 PREEMPT_RT + CPU 隔离 + Iceoryx 零拷贝，搭建满足 500 Hz 的实时控制节点
用 flamegraph + HdrHistogram 定位延迟瓶颈并验证实时性
在 CasADi/acados、Crocoddyl、OCS2、Drake 之间做正确的选型，理解每个框架的最佳适用场景
实现 CBF-QP 安全层，为 RL 策略提供在线安全修正
理解 HPIPM 状态码的含义并排查 QP 求解失败
编写满足实时安全编码规范的 C++——零堆分配、无异常、无 RTTI、lock-free 通信

中文参考资源（按使用场景分类）¶

入门学习：

平台	资源	内容	适合阶段
B 站	DR_CAN MPC/最优控制系列	数学推导清晰，中文讲解	Part A/B
B 站	小彭老师 C++/Eigen	C++ 工程基础	Part A
知乎	忠厚老实的老王 MPC/CBF 系列	理论与代码结合	Part B
知乎	Tomcattiger CasADi/ACADOS 学习	含 C++ Eigen 示例	Part F

框架使用：

平台	资源	内容	适合阶段
知乎	OCS2 学习记录系列	Tutorial 逐步讲解	Part C
知乎	Pinocchio 安装+基本应用	环境搭建	Part C/H
CSDN	do-mpc 教程	Python MPC 实现	Part F
GitHub	tomcattigerkkk/ACADOS_Example	中文注释 C++ 模板	Part F

深度工程：

平台	资源	内容	适合阶段
GitHub	ShuoYangRobotics/A1-QP-MPC-Controller	MIT Cheetah 3 中文复现	Part E
GitHub	qiayuanl/legged_control	OCS2 + ros-control 全栈	Part C/E
GitHub	Technician13/MIT-Cheetah-Note	ConvexMPCLocomotion 中文注释	Part C
知乎	MPC practice 实践分享	Linux C++ MPC 部署经验	Part D

论文精读（中文解读资源）：

论文	中文解读	核心内容
Tassa 2014 Control-Limited DDP	知乎 iLQR 推导三篇	boxQP + DDP
Frison 2020 HPIPM	知乎 HPIPM 笔记	OCP-QP 结构利用
Carpentier 2018 Pinocchio RSS	Pinocchio 解析导数原理	多体链式递推
Mastalli 2020 Crocoddyl	Crocoddyl 架构设计	FDDP 可行性驱动

Part K：工程 Anti-Patterns 与设计模式（§3.14.32–§3.14.33）¶

§3.14.32 最优控制工程中的 Anti-Patterns¶

Anti-Pattern 1："先跑通再优化"的陷阱

新手常用 Python 写出一个"能跑"的 MPC，然后试图"优化"到实时。问题在于：Python MPC 的架构设计（动态内存、GIL、解释器开销）从根本上无法"优化"到实时——你需要的是"重写"而非"优化"。正确路径是：Python 原型验证算法正确性 → CasADi 代码生成 → C++ wrapper → 实时验证。这不是"先慢后快"，而是"先验证后部署"——两个阶段解决不同的问题。

Anti-Pattern 2："万能框架"的幻觉

试图用一个框架解决所有问题——比如只用 Drake 做从离线规划到实时控制到嵌入式部署的全部工作。每个框架有其设计定位（Part I 详述），强行越界使用会事倍功半。正确做法是**根据任务选择框架**，必要时在框架间传递数据（如 Crocoddyl 输出轨迹 → acados 输入参考）。

Anti-Pattern 3："调参代替理解"的死循环

遇到 HPIPM status=3 时，反复调 qp_solver_cond_N、levenberg_marquardt、warm_start 参数，而不理解这些参数的数学含义。这会陷入"改一个参数、跑一次测试、看是否好转"的低效循环。正确做法是：先理解 120 号文件的 QP 理论 → 再定位问题类型（不可行？不正定？scaling 差？）→ 最后有针对性地调参。

Anti-Pattern 4："Copy-Paste 驱动开发"

从 GitHub 上找到一个"能跑"的 acados 示例，修改几个参数就声称"移植完成"。但示例的 QP 配置、积分方法、约束类型可能完全不适合你的系统。正确做法是：理解每个配置参数的含义（Part F §3.14.18b 详述），然后根据你的系统特性（状态维度、动力学刚性、约束类型）从头配置。

§3.14.33 推荐的设计模式¶

模式 1：MPC/WBC/Estimator 三层分离

感知层 (10-50 Hz)     →  状态估计器 (100-1000 Hz)  →  MPC (50-200 Hz)
                                                        ↓
                                                    WBC (500-1000 Hz)
                                                        ↓
                                                    电机驱动 (1-10 kHz)

每层运行在独立线程/进程中，通过 lock-free 队列或共享内存通信。MPC 异步求解，WBC 同步执行——这保证了即使 MPC 偶尔超时，WBC 仍然能以高频输出安全扭矩（回退到上一次 MPC 解 + PD 反馈）。

模式 2：Solver-Agnostic 接口

// 抽象接口——不依赖具体 solver
class MpcSolverInterface {
 public:
    virtual ~MpcSolverInterface() = default;
    virtual int solve(const StateVector& x0) = 0;
    virtual ControlVector get_u0() const = 0;
    virtual double get_solve_time_ms() const = 0;
    virtual void set_reference(int stage, const ReferenceVector& ref) = 0;
};

// acados 实现
class AcadosMpcSolver : public MpcSolverInterface { ... };
// Crocoddyl 实现 (用于对比测试)
class CrocoddylMpcSolver : public MpcSolverInterface { ... };

这个模式允许你在不修改上层代码的情况下切换 solver——在开发阶段用 Crocoddyl（debug 友好），在部署阶段切 acados（实时性能）。

模式 3：降级链（Fallback Chain）

MPC 求解成功? → 使用 MPC 输出
       ↓ 失败
LQR 反馈可用? → 使用预计算的 LQR 增益
       ↓ 失败  
PD 反馈可用? → 使用固定增益 PD
       ↓ 失败
紧急制动 → 关节锁定 / 电机断电

每一层的可靠性递增、性能递减。这确保了即使最上层（MPC）出现异常（QP 不可行、超时、NaN），系统仍然有安全的退路。每个控制系统都应该有降级链——单层架构在任何组件失败时就是全局失败。

本质洞察：机器人控制的工程挑战不在于"算法能不能 work"——iLQR 在 Python 里半天就能跑通。真正的挑战在于"算法能不能**每次**都在 2 ms 内 work，连续 24 小时不出错，在 -10°C 到 40°C 的温度范围内、在 WiFi 干扰的电磁环境中、在地形变化的户外环境下"。本专题的全部 10 个 Part，就是为了把"能跑"变成"能部署"。

途径	模式	精度	30-DoF 动力学梯度	备注
有限差分 \((f(x+\varepsilon e_i)-f(x))/\varepsilon\)	—	\(\sim 10^{-5}\) 截断误差	\((n_q+1)\cdot T_f\)	最简单但最慢最不准
`Eigen::AutoDiffScalar`	前向	机器精度	仅适用 \(n\le 10\)	纯头文件
CppAD（无 codegen）	反向	机器精度	≈150 μs	运算符重载，tape 开销
CppADCodeGen	反向+codegen	机器精度	≈10 μs	生成 C 并 dlopen，快 ~3 数量级
CasADi codegen	两者	机器精度	μs 级	符号微分 + CCS 稀疏性
Pinocchio 解析导数	专用	机器精度	5–15 μs	RSS 2018，多体链式递推

积分方式	名称	精度	速度	适用场景
`ERK`	显式 Runge-Kutta	\(O(h^p)\), \(p\) 由 stages 决定	最快	非刚性 ODE (四旋翼/车辆)
`IRK`	隐式 Runge-Kutta (Gauss-Legendre)	\(O(h^{2s})\) (超收敛)	慢 2–5×	刚性系统 (接触/柔性)
`GNSF`	Generalized Nonlinear Static Feedback	介于两者	中等	可分离系统 (部分线性)

浮基类型	配置空间	切空间	适用场景
`JointModelFreeFlyer`	\(\mathbb{R}^3 \times SO(3)\) (7D)	\(\mathbb{R}^6\) (6D)	四足/人形/四旋翼
`JointModelPlanar`	\(\mathbb{R}^2 \times SO(2)\) (4D)	\(\mathbb{R}^3\) (3D)	平面移动机器人
无（默认）	仅关节	仅关节	固定基座机械臂

#	自测题	前置
Q1	DARE 的标准形式是什么？它与 LQR 的关系？	3.3/3.5
Q2	iLQR 的 backward pass 中 \(Q_{uu}\) 的物理含义？	3.9
Q3	NMPC 的 SQP 步与完整 NLP 的关系？RTI 为什么只做一步？	3.11
Q4	CBF 的李导数约束如何转化为 QP？	3.8
Q5	什么是 Cholesky 分解？为什么 LDLT 比 LLT 更稳定？	线性代数基础