工具与仿真 · G7
机器人数学与优化库
机器人圈不重复造轮子:运动学用 Pinocchio、优化用 CasADi、实时 MPC 用 acados、WBC 用 QP 求解器。会用这些库,等于借用别人十年的实现。
课程正文 · 人话版
机器人数学库(Eigen/Pinocchio/CasADi)是算法与代码之间的翻译层。理解它们的取舍——稠密还是稀疏、解析导数还是自动微分——决定你写的算法是 1ms 还是 100ms。
病态问题:条件数决定一切
看动图拖 κ(长短轴比):椭圆越扁(病态越重),梯度下降爬得越慢,L-BFGS 依然飞快。ICP 配准、BA 优化、逆运动学解算——底层全是这个对比。
工程启示:优化慢先算条件数(雅可比奇异值),病态就预处理(归一化/正则化)或换二阶方法(LM/GN)。硬调步长是南辕北辙。
动图 · 拖一拖,亲眼看
三件套的分工
Eigen:矩阵运算底座(稠密为主),表达式模板让运算零临时对象——所有其他库的地基。Pinocchio:刚体动力学(M/C/g、雅可比、RNEA)解析实现,1kHz WBC 的算力来源。CasADi:自动微分+非线性优化(IPOPT 前端),MPC/轨迹优化的主力。
选择逻辑:算动力学用 Pinocchio,构造优化问题用 CasADi,它们底下都站 Eigen。三者拼起来就是现代机器人算法栈。
解析导数 vs 自动微分
解析导数(手推雅可比):最快、可审查,但推导费时易错。自动微分(CasADi/autodiff):写一遍前向,导数自动出,速度略慢但省命。工程默认:热路径用解析(Pinocchio 已给你),原型用自动微分。
稀疏性是隐藏的大腿:BA 的雅可比稀疏率>99%,稀疏求解(SparseLU/Ceres)比稠密快百倍。数据量大了以后,「会不会用稀疏求解器」就是会不会做 SLAM 后端的分界线。
学到什么程度
会用 Eigen 写出无临时对象的表达式;会用 Pinocchio 算 M/C/g 和雅可比;会用 CasADi 定义一个 QP/NLP 并解出。这三件做到,F1 仿真里的动力学你都能自己算。
三道题,测你真懂了
1. 优化迭代很慢,先该检查?
- A学习率
- BCPU 温度
- C问题的条件数(雅可比奇异值)
看答案
C。病态问题是一切慢的根源。预处理或换二阶方法,别死调步长。
2. 1kHz 全身控制(WBC)的动力学计算靠?
- APinocchio 的解析刚体动力学
- B数值差分
- CGPU
看答案
A。RNEA/ABA 解析实现微秒级。数值差分慢两个数量级且噪。
3. BA 大规模优化必须用?
- A稠密 LU
- B稀疏求解器(雅可比稀疏率极高)
- C梯度下降
看答案
B。BA 雅可比稀疏率>99%。稀疏 vs 稠密差百倍——这是 SLAM 后端的入门门槛。
在本站继续深入
正文讲完。下面是这个领域的完整大纲、深度要求与常见错误——查漏和面试准备时用。
要学什么
- Eigen + Sophus:SE(3)/SO(3) 的指数映射、李代数求导、四元数插值
- Pinocchio:URDF 解析、RNEA(逆动力学)、CRBA(质量矩阵)、ABA(正动力学)、解析雅可比、自动微分
- KDL:老牌但接口绕、速度慢;什么时候还在用(MoveIt 默认 IK 就是它)
- CasADi:符号建模 + 自动微分 + IPOPT,MPC 的标准配方;暖启动与求解器选项比模型本身更影响实时性
- 实时最优控制:acados(C 代码生成,毫秒级)、OCS2(腿足 MPC + WBC)、Crocoddyl(DDP/iLQR)
- QP 求解器:OSQP、qpOASES、ProxQP、HPIPM;1kHz 解 30 维 QP 的现实边界
- Drake:带接触的多体动力学与优化,验证与教学最好用
这个领域的主流软件
| 软件 / 库 | 是什么 | 什么时候用 | 优先级 |
|---|---|---|---|
| Eigen / Sophus | C++ 线性代数与李群 | 所有 C++ 机器人代码 | 必装 |
| Pinocchio | 刚体动力学与运动学,速度快、接口现代 | 取代自己写的运动学 | 必装 |
| CasADi + IPOPT | 符号优化与非线性求解 | MPC、轨迹优化原型 | 必装 |
| acados | 生成 C 代码的实时 MPC 求解器 | 要把 MPC 压进 1–5ms | 进阶 |
| OCS2 / Crocoddyl | 腿足与全身控制的 MPC/WBC 实现 | 四足、人形 | 进阶 |
| OSQP / ProxQP / qpOASES | QP 求解器,各有速度与稳定性取舍 | WBC 里 1kHz 解 QP | 进阶 |
| Drake | 带接触的动力学与优化工具箱 | 验证算法、教学 | 备选 |
四条路线需要多深
「精通」= 能从零推导 / 能手写实现 / 能判断别人的方案对不对;「用过」= 能调起来、能改参数;「知道」= 能听懂、能提问。
怎么算学会了
用 Pinocchio 算一台 6 轴臂的逆动力学,与 Gazebo 里同轨迹的实际力矩对上(误差 <10%);用 CasADi 写一个 20 步 horizon 的 MPC,单次求解 <2ms。
常见错误
- 自己推雅可比,符号错一处,控制器只在奇异点附近炸,查两周查不出来
- 用 IPOPT 做 1kHz MPC,单次求解 8ms,只能降到 50Hz,然后说「MPC 不适合这个场景」
- 每周期重建 QP 矩阵,不预分配,1kHz 里 60% 时间在分配内存