本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

互补约束¶

难度: ⭐⭐~⭐⭐⭐ | 前置: 足式/50_空间向量与浮动基座动力学空间向量, 足式/60_QP_NLP建模 QP/NLP, 足式/70_腿足简化模型理论简化模型 | 学时: 20-25 小时

前置自测¶

在开始本章之前,请确认你能回答以下问题。如果任何一题感到陌生,请先回顾对应章节:

空间力(Wrench)的 6 维表达 $\boldsymbol{f} = [\boldsymbol{n}^T, \boldsymbol{f}^T]^T$ 中,力矩和力的排列顺序是什么?(→ 足式/50_空间向量与浮动基座动力学)
QP 问题的标准形式 $\min \frac{1}{2}x^T H x + g^T x$ s.t. $Ax \le b$ 中,不等式约束如何编码"单侧接触"?(→ 足式/60_QP_NLP建模)
Centroidal Dynamics 的 6 维动量 $\boldsymbol{h}_G = [k^T, l^T]^T$ 的物理含义是什么?(→ 足式/70_腿足简化模型理论)
Pinocchio 中 getFrameJacobian() 返回的矩阵维度是什么?LOCAL_WORLD_ALIGNED 参考系的含义?(→ 足式/30_Pinocchio深度精读)
LIPM 假设角动量为零,这在什么情况下会严重失效?(→ 足式/70_腿足简化模型理论)

本章目标¶

学完本章,你应能:

深入理解接触力学三大铁律——法向非负、摩擦锥、互补性——并写出其数学形式
掌握摩擦锥线性化的完整推导——内切/外切近似、误差界、wrench cone 扩展
理解多面体锥的 H-rep 与 V-rep 对偶——并用 GIWC 给出"机器人能否静止站稳"的几何判据
推导刚体动力学到 LCP 的完整链条——Delassus 算子与互补约束的来龙去脉
理解互补约束的三大处理路线——预定义模式/软化/Contact-Implicit TO——及各自的数学基础
推导接触 Jacobian 的对偶关系——从运动空间到力空间的映射
掌握 CCRBA 算法——O(N) 计算 CMM 的完整步骤
理解角动量的守恒与可控性——飞行相位的约束与猫翻身悖论
能把接触约束编码为 QP/NLP 约束——为足式/90_WBC分层优化与TSID WBC 和足式/100_DDP家族与Crocoddyl DDP 打基础

知识导航¶

本章围绕一条主线展开:接触力受什么约束（物理）→ 这些约束如何进入优化器（建模）→ 接触力如何在动力学里传递（桥梁）→ 这套机制在控制、仿真与学习中如何落地（应用）。下表给出全章 10 个大节的定位，括号内是难度与所属层次。

节	主题	在主线中的位置
52.1 三大铁律 ⭐⭐	法向非负 / 摩擦锥 / 互补性	物理：接触力必须满足什么
52.2 摩擦锥线性化 ⭐⭐	多面体近似、误差界、Wrench Cone（52.2.4）、H-rep/V-rep 对偶（52.2.5）、静平衡判据（52.2.6）	建模：把凸的 SOC 约束变成 QP 能吃的线性不等式，并给出"能否站稳"的几何判据
52.3 互补约束三路线 ⭐⭐⭐	刚体动力学→LCP（52.3.0）、预定义模式 / 软化 / Contact-Implicit TO	建模：非光滑的互补性怎么交给求解器
52.4 接触 Jacobian ⭐⭐	对偶映射 $J_c/J_c^T$、漂移项、参考系选择	桥梁：接触力与关节空间互相翻译
52.5 CMM 与 CCRBA ⭐⭐⭐	O(N) 计算质心动量	桥梁：Centroidal 动力学的计算基础
52.6 角动量 ⭐⭐	守恒、可控性、猫翻身、摆臂	应用：理解全身动力学相对 Centroidal 模型的盲区
52.7 非线性 vs 线性化 ⭐⭐⭐	精度-性能权衡、SOC 求解器	应用：工程选型
52.8 软接触 vs 刚体接触 ⭐⭐⭐	LCP vs 弹簧阻尼 / Hydroelastic	应用：仿真器选择
52.9 RL 仿真中的接触 ⭐⭐⭐	仿真器对比、域随机化	应用：连接 RL 训练栈

知识依赖关系：52.1（物理）是全章地基；52.2 与 52.3 是两条并列的"建模"支线（凸的摩擦锥 → 线性化；非光滑的互补性 → 三路线），二者在 52.3.0 处由 LCP 串起来；52.4、52.5 是把接触力接回动力学的两座"桥"；52.6–52.9 都是建立在前面之上的应用层。其中 52.2.5/52.2.6（锥的对偶与静平衡）与 52.6（角动量可控性）共享同一个几何工具——接触力锥张成的可达 wrench 集合。

推荐阅读路径：

速查（2 h）：52.1 三大铁律 → 52.2.1 线性化推导 → 52.4.1 接触 Jacobian 对偶 → 52.10 小结表。只求知道"有哪些约束、怎么进 QP"。
主线（精读 12–15 h）：按 52.1 → 52.2（含 52.2.4–52.2.6）→ 52.3（含 52.3.0）→ 52.4 → 52.5 → 52.6 顺序通读，这是 WBC/MPC 工程师必经之路。
进阶（研究向，+8 h）：在主线基础上深入 52.3.3（Contact-Implicit TO）、52.7–52.9（工程权衡与仿真），以及延伸阅读中的可微接触前沿。

52.1 接触力的物理约束——三大铁律 ⭐⭐¶

动机:为什么需要约束接触力? ⭐¶

本节解决的问题:如果不约束接触力,优化器(QP/NLP)会给出物理上不可能的解——比如地面"吸"住机器人、足底在冰面上产生无穷大摩擦力、或者脚在空中仍然受到地面支撑力。

反面案例:假设你在足式/60_QP_NLP建模的 QP 中忘记加摩擦锥约束,只加了牛顿-欧拉方程作为等式约束。求解器会怎么做?

场景: 四足机器人需要向右加速
QP目标: min ||τ||² (最小化关节扭矩)
等式约束: M*ddq + h = S^T*τ + J_c^T*λ (动力学)

无摩擦锥时的"最优解":
  λ_z = 0 (法向力为零,省力!)
  λ_x = 很大的值 (全靠水平摩擦力推动)
  → 物理上: 摩擦力远超法向力 × μ, 机器人会滑倒!

这就是为什么**每一个**足式机器人控制器都必须包含接触力约束。

历史背景 ⭐¶

接触力学的数学框架可追溯到三位先驱:

年份	人物	贡献
1785	Coulomb	提出库仑摩擦定律 $f_t \le \mu f_n$
1933	Signorini	形式化单边接触问题(Signorini problem)
1963	Fichera	证明 Signorini 问题解的存在唯一性
1988	Moreau	非光滑力学框架,互补约束的现代形式
2013	Orin et al.	Centroidal 动力学与 CCRBA 算法

52.1.1 约束 1:法向非负(Signorini 条件) ⭐⭐¶

物理直觉:地面只能"推"机器人,不能"拉"。这是因为地面和足底之间没有粘性连接(不像胶带)。

数学形式:设接触点 $\boldsymbol{p}_c$ 到地面的**有符号距离**为 $\phi(\boldsymbol{p}_c)$(正为离开地面,零为接触,负为穿透),接触力法向分量为 $\lambda_c^n$,则:

\[\lambda_c^n \ge 0\]

这里 $n$ 是接触面法向方向(指向机器人一侧)。对于水平地面,法向就是 $z$ 轴,即 $\lambda_c^z \ge 0$。

完整 Signorini 条件(Hertz-Signorini-Moreau 条件):

Signorini 条件不仅要求法向力非负,还包含三个联立关系:

\[\boxed{\phi(\boldsymbol{p}_c) \ge 0, \quad \lambda_c^n \ge 0, \quad \phi(\boldsymbol{p}_c) \cdot \lambda_c^n = 0}\]

这三个条件的物理含义:

条件	含义	直觉
$\phi \ge 0$	不穿透	脚不能穿过地面
$\lambda_c^n \ge 0$	法向力非负	地面只推不拉
$\phi \cdot \lambda_c^n = 0$	互补性	离地时无力,有力时在地面上

几何解读:在 $(\phi, \lambda_c^n)$ 平面上,合法的 $(\phi, \lambda_c^n)$ 对只能落在两条射线上——正 $\phi$ 轴(空中,无力)和正 $\lambda_c^n$ 轴(接触,有力)。这构成一个"L 形"可行域。这好比电路中的理想二极管:电压(对应 $\phi$)和电流(对应 $\lambda_c^n$)不能同时为正——要么导通(接触,有力),要么截止(离地,无力)。不同之处在于,二极管的 V-I 关系是标量的,而接触力学还叠加了切向摩擦,使问题从 1D 扩展到 3D。

非平坦地形上的法向方向:

对于非平坦地形,法向 $\boldsymbol{n}$ 不再是简单的 $[0, 0, 1]^T$。设地形高度函数为 $z = h(x, y)$,则:

\[\boldsymbol{n} = \frac{1}{\sqrt{1 + h_x^2 + h_y^2}} \begin{bmatrix} -h_x \\ -h_y \\ 1 \end{bmatrix}\]

其中 $h_x = \partial h / \partial x$, $h_y = \partial h / \partial y$。接触力的法向分量变为:

\[\lambda_c^n = \boldsymbol{\lambda}_c \cdot \boldsymbol{n} \ge 0\]

⚠️ 编程陷阱:法向方向的坐标系

错误: 在倾斜地面上仍然用 $\lambda_c^z \ge 0$ 作为约束。

症状: 机器人在斜坡上滑动,因为约束允许了沿斜面的"推力",但没有正确约束法向力。

根因: 法向非负约束必须在**接触面局部坐标系**中表达,而非世界系。

正确做法: 计算接触面法向 $\boldsymbol{n}$,用 $\boldsymbol{\lambda}_c \cdot \boldsymbol{n} \ge 0$ 代替 $\lambda_c^z \ge 0$。

52.1.2 约束 2:库仑摩擦锥(Coulomb Friction Cone) ⭐⭐¶

物理直觉:摩擦力不能无限大。当你推一个重物,摩擦力最多等于法向力乘以摩擦系数——超过这个极限,物体就滑动了。

数学形式:切向力(摩擦力)的模不能超过法向力乘以摩擦系数 $\mu$:

\[\|\boldsymbol{\lambda}_c^t\|_2 = \sqrt{(\lambda_c^x)^2 + (\lambda_c^y)^2} \le \mu \lambda_c^n\]

其中 $\boldsymbol{\lambda}_c^t$ 是切向(tangential)分量,$\mu$ 是静摩擦系数。

典型摩擦系数:

接触对	$\mu$ 范围	工程常用值
橡胶 — 干燥水泥	0.6 ~ 0.8	0.7
橡胶 — 湿水泥	0.4 ~ 0.5	0.45
橡胶 — 冰	0.05 ~ 0.15	0.1
金属 — 金属	0.3 ~ 0.6	0.4
橡胶 — 草地	0.3 ~ 0.5	0.4

几何含义:在 $(\lambda^x, \lambda^y, \lambda^z)$ 三维空间中,合法的接触力向量必须落在一个**圆锥体**内部:

顶点在原点 $(0, 0, 0)$
轴线沿法向 $z$ 方向
半顶角 $\alpha = \arctan(\mu)$

当 $\mu = 0.7$ 时,$\alpha \approx 35°$;当 $\mu = 0.1$ 时,$\alpha \approx 5.7°$——冰面上的摩擦锥非常"尖",几乎只能产生竖直力。

本质洞察:Signorini 条件的三个联立关系 $\phi \ge 0, \lambda^n \ge 0, \phi \cdot \lambda^n = 0$ 并非三条独立规则的简单叠加,而是**一个统一的物理原理的三个投影**:接触是一种**单边约束(unilateral constraint)**——它只能推、不能拉,且只在几何接触时才产生力。这三个条件中去掉任何一个,都会导致非物理解(穿透、拉力、或悬空力)。

为什么是 SOC(Second-Order Cone)?

摩擦锥约束 $\sqrt{(\lambda^x)^2 + (\lambda^y)^2} \le \mu \lambda^z$ 在数学上是一个**二阶锥约束(SOC)**。SOC 是**凸**的,但不是线性的——这意味着:

如果问题只有 SOC 约束 + 二次目标 → SOCP(Second-Order Cone Program) → 可以用 Mosek/ECOS 求解
如果我们想用 QP 求解器(OSQP/qpOASES) → 必须把 SOC 线性化 → 这就是 52.2 节的内容

推导:为什么摩擦锥是凸的?

设 $\boldsymbol{\lambda}_1, \boldsymbol{\lambda}_2$ 都在锥内,即 $\|\boldsymbol{\lambda}_i^t\| \le \mu \lambda_i^n$, $i = 1, 2$。对任意 $\alpha \in [0, 1]$:

\[\|\alpha \boldsymbol{\lambda}_1^t + (1-\alpha) \boldsymbol{\lambda}_2^t\| \le \alpha \|\boldsymbol{\lambda}_1^t\| + (1-\alpha) \|\boldsymbol{\lambda}_2^t\| \le \alpha \mu \lambda_1^n + (1-\alpha) \mu \lambda_2^n = \mu (\alpha \lambda_1^n + (1-\alpha) \lambda_2^n)\]

第一个不等式用了三角不等式,第二个用了各自的锥约束。因此凸组合仍在锥内,摩擦锥是凸集。$\square$

52.1.3 约束 3:互补性(Complementarity) ⭐⭐⭐¶

物理直觉:一只脚要么在空中(无接触力),要么在地面上(有接触力)——不可能"在空中却受到地面力",也不可能"在地面上却没有支撑力(除非起跳瞬间)"。

数学形式:

\[0 \le \phi(\boldsymbol{p}_c) \perp \lambda_c^n \ge 0\]

符号 $\perp$ 读作"互补",表示 $\phi \cdot \lambda_c^n = 0$——两个非负量的乘积为零,意味着**至少一个为零**。

这是 LCP 的标准形式:线性互补问题(Linear Complementarity Problem)。如果 $\phi$ 是 $q$ 的线性函数(或线性化后),整个接触问题变成 LCP。

速度级别的互补条件:

在实际时间积分中,位置级别的互补条件 $\phi \cdot \lambda^n = 0$ 不够用——我们还需要约束速度。速度级别的 Signorini 条件:

\[\text{接触状态:} \quad \phi = 0, \; \dot{\phi} = 0, \; \lambda^n \ge 0$$ $$\text{分离条件:} \quad \phi = 0, \; \dot{\phi} > 0 \implies \lambda^n = 0$$ $$\text{保持条件:} \quad \phi = 0, \; \dot{\phi} = 0, \; \ddot{\phi} \ge 0, \; \lambda^n \ge 0, \; \ddot{\phi} \cdot \lambda^n = 0\]

为什么互补约束难处理?

互补约束 $\phi \cdot \lambda^n = 0$ 是一个**等式约束**,但它是**非光滑的**——在 $(\phi, \lambda^n) = (0, 0)$ 处(即接触刚好建立/断裂的瞬间)不可微。这相当于优化问题中遇到了一个"棱角"——梯度在棱角处不存在,就像绝对值函数 $|x|$ 在 $x = 0$ 处不可微一样,只是这里的棱角发生在二维空间中。这导致:

标准梯度法(如 SQP)在这一点处 Jacobian 退化
目标函数的梯度包含不连续跳变
传统 QP 求解器无法直接处理

这就是 52.3 节三大路线要解决的核心难题。

💡 概念误区:互补性 vs 碰撞

错误理解: "互补约束就是碰撞检测"。

正确理解: 互补约束描述的是**稳态接触关系**——"接触 ↔ 力"的逻辑耦合。碰撞(impact)是一个**瞬时事件**,需要额外的冲量模型(如恢复系数)。在足式机器人的 MPC/WBC 中,我们通常假设**无碰撞**(soft landing),只处理互补性。

🧠 思维陷阱:三大约束的独立性

陷阱: "法向非负 + 摩擦锥已经够了,互补性是多余的。"

反例: 如果没有互补性,优化器可以在脚离地 10cm 时仍然产生接触力——这在数学上满足 $\lambda^z \ge 0$ 和摩擦锥,但物理上是胡说八道。互补性是连接**几何(距离)**和**力学(力)**的桥梁。

52.1.4 三大约束的统一数学框架 ⭐⭐¶

将三大约束合写为一个紧凑的数学形式:

\[\boxed{\begin{aligned} &\text{(Signorini)} \quad 0 \le \phi(\boldsymbol{p}_c) \perp \lambda_c^n \ge 0 \\ &\text{(Coulomb)} \quad \|\boldsymbol{\lambda}_c^t\|_2 \le \mu \lambda_c^n \\ &\text{(Maximum Dissipation)} \quad \dot{\boldsymbol{p}}_c^t \ne 0 \implies \boldsymbol{\lambda}_c^t = -\mu \lambda_c^n \frac{\dot{\boldsymbol{p}}_c^t}{\|\dot{\boldsymbol{p}}_c^t\|} \end{aligned}}\]

第三个条件是**最大耗散原则**(Maximum Dissipation Principle):如果接触点在滑动,摩擦力方向与滑动方向相反,且大小恰好等于 $\mu \lambda_c^n$。这是 Coulomb 摩擦的完整表述。

在足式机器人控制中的简化:大多数 MPC/WBC 假设**无滑动**(no-slip),即 $\dot{\boldsymbol{p}}_c^t = 0$。此时第三个条件自动满足,只需处理前两个。

52.1.5 补充约束：关节力矩限幅与足端力的耦合 ⭐⭐¶

除了上述三大接触力约束外，工程中还有两类实用约束值得注意：

足端力幅度约束：摆动腿不与地面接触，足端力严格为零（$\mathbf{f}_i = \mathbf{0}$）；支撑腿的法向力存在上限 $f_{z,\max}$，由电机物理极限决定。在 MPC 优化中，这些约束通过步态调度器提供的接触状态变量（$c_i \in \{0, 1\}$）来实现：$c_i = 0$ 时强制 $\mathbf{f}_i = \mathbf{0}$。

关节力矩约束：即便优化器求出了满足摩擦锥的足端力，仍需确保这些力经雅可比转置 $\boldsymbol{\tau} = J_c^T(\mathbf{q}) \cdot \mathbf{f}$ 映射到关节空间后不超过电机力矩极限：

\[|\tau_j| \le \tau_{j,\max}, \quad \forall j\]

由于 $J_c(\mathbf{q})$ 含有正余弦函数，当关节角度 $\mathbf{q}$ 也是决策变量时，力矩约束是非凸的。但在实时 MPC 的常见做法中，$\mathbf{q}$ 由当前状态或参考轨迹确定（而非优化变量），此时 $J_c$ 退化为常数矩阵，力矩约束变为关于足端力的线性约束——可直接纳入 QP 框架。

练习¶

练习 52.1.1 (⭐⭐): 对于一个四足机器人站在 $30°$ 斜坡上,写出每只脚的法向非负约束和摩擦锥约束(用世界系坐标)。如果 $\mu = 0.5$,机器人能否稳定站立?为什么?

练习 52.1.2 (⭐⭐⭐): 证明:当地面是平面时,Signorini 条件 $\phi \ge 0, \lambda^n \ge 0, \phi \lambda^n = 0$ 可以等价写为 $\lambda_c^n \in N_{C}(\phi)$,其中 $N_C$ 是凸集 $C = \{\phi \ge 0\}$ 的法锥(normal cone)。这个变分不等式观点为什么重要?

练习 52.1.3 (⭐⭐): 在 MuJoCo 中,接触模型使用"软接触"(spring-damper)。试分析:软接触是否满足严格的 Signorini 条件?为什么 MuJoCo 选择违反它?

52.2 摩擦锥线性化——从 SOC 到 QP 的关键一步 ⭐⭐¶

动机:为什么要线性化? ⭐⭐¶

本节解决的问题:摩擦锥约束 $\sqrt{(\lambda^x)^2 + (\lambda^y)^2} \le \mu \lambda^z$ 是二阶锥约束(SOC),标准 QP 求解器(OSQP, qpOASES)不支持 SOC。如何在保持问题凸性的前提下,把摩擦锥变成线性不等式?

反面案例:MIT Cheetah 3 的凸 MPC 必须在 1ms 内求解——如果用 SOCP 求解器(Mosek),耗时约 3-5ms,控制频率从 1000Hz 降到 200Hz。线性化后用 QP 求解器,耗时降到 0.8ms。

历史与现状 ⭐¶

年份	方法	代表工作
2005	多面体近似(k=4)	早期 ZMP 步行控制器
2018	k=4 线性化 + 凸 MPC	MIT Cheetah 3 (Di Carlo et al.)
2020	SOC 直接求解	Grandia et al. (ANYmal, Mosek)
2023	混合:MPC 用线性化, WBC 用 SOC	OCS2 + 现代 WBC

52.2.1 多面体近似的数学推导 ⭐⭐¶

基本思想:用 $k$ 条边的正多边形**内切/外切**于圆,把圆锥近似为多面体锥。这与计算机图形学中用多边形逼近曲面的思路一致——正如 3D 渲染用三角面片逼近球体(面数越多越圆),我们用线性半平面逼近圆锥(边数越多越精确)。区别在于,图形学的逼近误差影响视觉效果,而这里的逼近误差影响机器人是否会滑倒。

步骤 1:圆锥的截面是圆

在 $\lambda^z = 1$ 平面上截取摩擦锥,得到半径为 $\mu$ 的圆:

\[(\lambda^x)^2 + (\lambda^y)^2 \le \mu^2\]

步骤 2:用正 $k$ 边形近似圆

在圆周上等距取 $k$ 个点 $(\mu \cos\theta_i, \mu \sin\theta_i)$,其中 $\theta_i = 2\pi i / k$。

连接相邻点构成正 $k$ 边形。每条边对应一个线性不等式。第 $i$ 条边的**外法向**方向是 $(\cos\psi_i, \sin\psi_i)$,其中 $\psi_i = \pi(2i+1)/k$。

步骤 3:写出线性不等式

第 $i$ 条不等式:接触力在第 $i$ 个外法向方向上的投影不超过边到原点的距离:

\[\cos\theta_i \cdot \lambda^x + \sin\theta_i \cdot \lambda^y \le \mu_{\text{eff}} \cdot \lambda^z, \quad i = 0, 1, \dots, k-1\]

其中 $\theta_i = 2\pi i / k$。

关键: $\mu_{\text{eff}}$ 的选取决定了是**内切**还是**外切**近似:

\[\boxed{\mu_{\text{eff}} = \begin{cases} \mu & \text{外切近似(outer approximation)} \\ \mu \cos(\pi/k) & \text{内切近似(inner approximation)} \end{cases}}\]

推导内切近似的 $\mu_{\text{eff}}$:

正 $k$ 边形**内切于**半径为 $\mu$ 的圆时,多边形的**内切圆半径**为:

\[r_{\text{inner}} = \mu \cos(\pi / k)\]

当 $k = 4$ 时,$r_{\text{inner}} = \mu \cos(45°) = \mu / \sqrt{2} \approx 0.707\mu$,损失约 29% 的摩擦能力。

当 $k = 8$ 时,$r_{\text{inner}} = \mu \cos(22.5°) \approx 0.924\mu$,损失约 7.6%。

当 $k = 16$ 时,$r_{\text{inner}} = \mu \cos(11.25°) \approx 0.981\mu$,损失约 1.9%。

52.2.2 近似误差的定量分析 ⭐⭐¶

外切近似误差(多面体比圆锥大多少):

外切正 $k$ 边形的面积与圆的面积之比:

\[\frac{A_{\text{outer}}}{A_{\text{circle}}} = \frac{k \mu^2 \tan(\pi/k)}{\pi \mu^2} = \frac{k \tan(\pi/k)}{\pi}\]

$k$	$\tan(\pi/k)$	面积比	超出百分比
4	1.000	1.273	27.3%
6	0.577	1.103	10.3%
8	0.414	1.055	5.5%
12	0.268	1.023	2.3%
16	0.199	1.013	1.3%

内切近似误差(多面体比圆锥小多少):

\[\frac{A_{\text{inner}}}{A_{\text{circle}}} = \frac{k \sin(2\pi/k)}{2\pi}\]

$k$	面积比	损失百分比
4	0.637	36.3%
6	0.827	17.3%
8	0.900	10.0%
12	0.955	4.5%
16	0.975	2.5%

工程选择指南:

场景	推荐 $k$	近似类型	理由
实时 MPC (1kHz)	4	外切	速度优先,接受 ~27% 过近似
标准 WBC (500Hz)	8	外切	平衡精度与速度
离线轨迹优化	16 或 SOC	内切/精确	精度优先
安全关键	8	内切	保守近似,不允许超出真实锥

💡 概念误区:内切 vs 外切的安全性

错误理解: "外切近似更安全,因为它包含了整个真实锥。"

正确理解: 恰恰相反!内切近似更安全。外切近似允许的力方向**超出**了真实摩擦锥——优化器可能找到一个"合法"解,但实际执行时机器人会滑倒。内切近似**保守**地限制了力方向——宁可少用摩擦力,也不冒滑倒风险。

MIT Cheetah 用外切的原因: 它的 MPC 代价函数已经倾向于产生接近竖直的力(加权矩阵使 $\lambda^z$ 优先),实际解很少触碰锥边界。但如果你的控制器会频繁在锥边界操作(如极端地形),应切换到内切。

52.2.3 完整 C++ 实现与可视化 ⭐⭐¶

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

/**
 * @brief Generate polyhedral friction cone constraints
 * @param mu Friction coefficient
 * @param k Number of edges (4, 6, 8, 16, ...)
 * @param inner If true, use inner approximation (conservative)
 * @return A matrix (k x 3) such that A * [lx, ly, lz]^T <= 0
 *
 * Each row encodes: cos(theta_i)*lx + sin(theta_i)*ly - mu_eff*lz <= 0
 */
Eigen::MatrixXd frictionConeLinearization(double mu, int k = 8, 
                                           bool inner = false) {
  // Effective friction coefficient
  double mu_eff = inner ? mu * std::cos(M_PI / k) : mu;

  Eigen::MatrixXd A(k, 3);
  for (int i = 0; i < k; ++i) {
    double theta = 2.0 * M_PI * i / k;
    A(i, 0) = std::cos(theta);   // lambda^x coefficient
    A(i, 1) = std::sin(theta);   // lambda^y coefficient
    A(i, 2) = -mu_eff;           // lambda^z coefficient
  }
  return A;  // Constraint: A * [lx, ly, lz]^T <= 0
}

/**
 * @brief Compute approximation error metrics
 * @return {area_ratio, max_radial_error}
 */
std::pair<double, double> approximationError(int k, bool inner) {
  double area_ratio, max_error;
  if (inner) {
    // Inner polygon area / circle area
    area_ratio = k * std::sin(2.0 * M_PI / k) / (2.0 * M_PI);
    max_error = 1.0 - std::cos(M_PI / k);  // Relative radial shrinkage
  } else {
    // Outer polygon area / circle area
    area_ratio = k * std::tan(M_PI / k) / M_PI;
    max_error = 1.0 / std::cos(M_PI / k) - 1.0;  // Relative radial expansion
  }
  return {area_ratio, max_error};
}

/**
 * @brief For multiple contacts, stack friction cone constraints
 * @param mu Friction coefficients per contact
 * @param n_contacts Number of contacts
 * @param k Edges per cone
 * @return Block diagonal constraint matrix (n_contacts*k x n_contacts*3)
 */
Eigen::MatrixXd stackedFrictionCones(const std::vector<double>& mu,
                                      int n_contacts, int k = 8) {
  int total_rows = n_contacts * k;
  int total_cols = n_contacts * 3;
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(total_rows, total_cols);

  for (int c = 0; c < n_contacts; ++c) {
    A.block(c * k, c * 3, k, 3) = frictionConeLinearization(mu[c], k);
  }
  return A;
}

可视化代码(Python):

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection

def visualize_friction_cones(mu=0.7, k_values=[4, 6, 8, 16]):
    """Visualize exact cone vs polyhedral approximations."""
    fig = plt.figure(figsize=(16, 4))

    for idx, k in enumerate(k_values):
        ax = fig.add_subplot(1, len(k_values), idx + 1, projection='3d')

        # Exact cone
        theta = np.linspace(0, 2 * np.pi, 100)
        z = np.linspace(0, 1, 50)
        T, Z = np.meshgrid(theta, z)
        X = mu * Z * np.cos(T)
        Y = mu * Z * np.sin(T)
        ax.plot_surface(X, Y, Z, alpha=0.15, color='blue')

        # Polyhedral cone (outer)
        angles = np.linspace(0, 2 * np.pi, k + 1)
        for i in range(k):
            verts = [[0, 0, 0],
                     [mu * np.cos(angles[i]), mu * np.sin(angles[i]), 1],
                     [mu * np.cos(angles[i+1]), mu * np.sin(angles[i+1]), 1]]
            ax.add_collection3d(Poly3DCollection(
                [verts], alpha=0.3, color='red', edgecolor='red'))

        # Error annotation
        err = (1.0 / np.cos(np.pi / k) - 1.0) * 100
        ax.set_title(f'k={k}, error={err:.1f}%')
        ax.set_xlabel('lx'); ax.set_ylabel('ly'); ax.set_zlabel('lz')

    plt.suptitle('Friction Cone Linearization Comparison')
    plt.tight_layout()
    plt.savefig('friction_cone_comparison.png', dpi=150)
    plt.show()

52.2.4 Wrench Cone:从 3D 点接触到 6D 面接触 ⭐⭐⭐¶

问题:3D 点接触模型假设接触发生在一个几何点上。但实际上,足底是一个有面积的矩形/圆形区域。面接触除了产生力,还会产生**力矩**(绕法向的摩擦力矩)。

6D Contact Wrench(接触旋量):

对于一个矩形面接触(尺寸 $2l_x \times 2l_y$),接触 wrench $\boldsymbol{w}_c = [\boldsymbol{n}_c^T, \boldsymbol{f}_c^T]^T \in \mathbb{R}^6$ 必须满足:

\[\boldsymbol{w}_c \in \mathcal{W}_c = \{U \boldsymbol{w}_c \le 0\}\]

其中 $U$ 是一个约束矩阵,它同时编码了:

法向力非负:$f_c^z \ge 0$
摩擦锥:$\sqrt{(f_c^x)^2 + (f_c^y)^2} \le \mu f_c^z$(线性化后)
绕法向的扭矩约束:$|n_c^z| \le \gamma f_c^z$,其中 $\gamma$ 是"扭转摩擦系数"
CoP(Center of Pressure)约束:$|n_c^x| \le l_y f_c^z$ 且 $|n_c^y| \le l_x f_c^z$

关键区别: 力和力矩不是独立的。如 Caron 等(2015)所指出:固定合力后,力矩的可行范围(moment cone)的形状会随之改变。因此 6D wrench cone 不是 3D 力锥和 3D 力矩锥的简单笛卡尔积——它们之间有耦合约束。

\[U = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & -1 \\ 0 & 0 & 0 & 1 & 0 & -\mu \\ 0 & 0 & 0 & -1 & 0 & -\mu \\ 0 & 0 & 0 & 0 & 1 & -\mu \\ 0 & 0 & 0 & 0 & -1 & -\mu \\ 1 & 0 & 0 & 0 & 0 & -l_y \\ -1 & 0 & 0 & 0 & 0 & -l_y \\ 0 & 1 & 0 & 0 & 0 & -l_x \\ 0 & -1 & 0 & 0 & 0 & -l_x \\ 0 & 0 & 1 & 0 & 0 & -\gamma \\ 0 & 0 & -1 & 0 & 0 & -\gamma \\ \end{bmatrix}\]

对于人形机器人的扁平足底,6D wrench cone 比 3D 点接触更准确——它能捕捉"脚尖翘起"(CoP 移到足底边缘)这类失稳模式。

TSID 中的 6D 接触:

// TSID: 6D contact for flat foot (humanoid)
tsid::contacts::Contact6d contact_RF(
    "contact_RF", robot, "right_foot_frame",
    contact_normal,    // [0, 0, 1]
    mu,                // friction coefficient
    lx, ly,            // half-dimensions of foot
    fMin, fMax,        // min/max normal force
    w_forceReg         // force regularization weight
);

⚠️ 编程陷阱:k=4 的方向选择

错误: 用 $\theta_i = 0, \pi/2, \pi, 3\pi/2$(与 x-y 轴对齐)。

问题: 当 $k = 4$ 且方向对齐坐标轴时,多面体退化为**方形锥**(box cone)。约束变成 $|\lambda^x| \le \mu \lambda^z$ 且 $|\lambda^y| \le \mu \lambda^z$,这过度放松了对角线方向——对角方向上允许 $\sqrt{2}\mu \lambda^z$ 的摩擦力。

解决: 旋转 45 度,使用 $\theta_i = \pi/4, 3\pi/4, 5\pi/4, 7\pi/4$,或直接用 $k = 8$。

🧠 思维陷阱:线性化精度过度追求

陷阱: "必须用 k=16 才能保证安全。"

现实: 摩擦系数 $\mu$ 本身的测量误差通常在 10-20%。用 k=4 的近似误差(~27%)与 $\mu$ 的不确定性是同一量级。在 $\mu$ 测不准的情况下,追求 k=16 是伪精度。更好的策略是:用 k=4 配合保守的 $\mu$ 估计(比如真实 $\mu = 0.7$ 时,控制器用 $\mu = 0.5$)。

52.2.5 两种锥表示——H-rep 与 V-rep 的对偶 ⭐⭐⭐¶

到目前为止,52.2.1–52.2.4 节都把摩擦锥/wrench cone 写成"一堆线性不等式"——$A\boldsymbol{\lambda}\le0$。这叫**半空间表示(half-space representation, H-rep):锥是若干半空间的交。但同一个多面体锥还有**另一种完全等价的表示,而且在很多场景下更好用——生成元表示(vertex/ray representation, V-rep):锥是若干"射线"(生成元 generator)的非负组合。

动机:为什么需要两种表示? 考虑一个具体痛点——你想知道"四足机器人四只脚的接触力,合成到质心后,能产生哪些总的力/力矩?"用 H-rep 很难直接回答,因为每只脚的力是被不等式**约束**的;但用 V-rep 就很自然——把每只脚摩擦锥的生成元(锥的"棱")收集起来,合成到质心,它们张成的锥就是答案。这正是 52.6 节角动量可控性、以及"机器人能否站稳"问题的几何核心。

V-rep 的形式:把 $k$ 边线性化摩擦锥的 $k$ 条棱写成生成元(单位射线)。第 $i$ 条棱方向(在接触局部系,$z$ 为法向)为:

\[\boldsymbol{g}_i = \begin{bmatrix} \mu\cos\theta_i \\ \mu\sin\theta_i \\ 1 \end{bmatrix}, \quad \theta_i = \frac{2\pi i}{k}, \quad i = 0,\dots,k-1\]

任何锥内的接触力都可以写成这些生成元的**非负组合**:

\[\boldsymbol{\lambda} = \sum_{i=0}^{k-1} \beta_i \boldsymbol{g}_i, \quad \beta_i \ge 0\]

这与 H-rep 描述的是**同一个**多面体锥——这是凸分析里的 Minkowski-Weyl 定理:每个多面体锥既可以写成有限多个半空间的交(H-rep),也可以写成有限多条射线的非负包(V-rep),两者可以相互转换。

本质洞察:H-rep 和 V-rep 不是两种不同的锥,而是**同一个锥的"内"与"外"两种视角**。H-rep 从外部"切"——每个不等式砍掉一个半空间,适合**检验**一个给定的力是否合法(逐行代入即可)。V-rep 从内部"撑"——每条生成元从原点向外射,适合**生成/参数化**所有合法的力(给一组 $\beta_i\ge0$ 就得到一个合法力)。优化建模时这个区别是实打实的:WBC 把接触力当决策变量、要施加约束,所以用 H-rep($A\boldsymbol{\lambda}\le0$ 直接进 QP);而可达性分析要枚举所有可能的合力,所以用 V-rep。

Contact Wrench Cone(CWC)的两种算法:52.2.4 节给出的矩阵 $U$ 是单个矩形面接触 wrench cone 的 H-rep。当多个接触点协同时(人形双脚 + 双手,或四足四脚),把每个接触的力锥变换到公共参考系(质心)并求 Minkowski 和,得到的总锥称为 Gravito-Inertial Wrench Cone (GIWC)。计算它有两条路:

路线	输入表示	算法	适用
闭式公式	单个矩形接触	Caron et al. 2015 (ICRA) 给出 CWC 的解析 $U$ 矩阵	单面接触(人形单脚支撑)
双重描述法	多接触力锥的 V-rep	Caron et al. 2015 (RSS) 用 double description 求 Minkowski 和后转回 H-rep	多接触(双脚/多肢)

双重描述法(Double Description, DD) 是 V-rep ↔ H-rep 相互转换的经典算法(Motzkin et al. 1953)。它的作用:先把所有接触锥的生成元(V-rep)合成,得到总锥的 V-rep;再转成 H-rep,这样就能用一组紧凑的不等式 $A_{\text{GIWC}}\boldsymbol{w}_G\le0$ 判断"质心 wrench 是否可达"。pycddlib(C 库 cdd 的 Python 绑定)是常用实现。

import numpy as np
import cdd  # pycddlib: double description method

def friction_cone_generators(mu, k=4):
    """Return V-rep generators (rays) of a linearized friction cone."""
    rays = []
    for i in range(k):
        th = 2 * np.pi * i / k
        rays.append([mu * np.cos(th), mu * np.sin(th), 1.0])
    return np.array(rays)  # shape (k, 3), each row is a ray

def cone_v_to_h(rays):
    """Convert a cone from V-rep (rays) to H-rep (A x <= 0) via DD method.

    cdd format: each generator row is [t, x...]; t=0 marks a ray (not vertex).
    Output inequalities are b + A x >= 0; for a cone b=0, so A x <= 0
    corresponds to the negated cdd rows.
    """
    gens = np.hstack([np.zeros((rays.shape[0], 1)), rays])  # t=0 => ray
    mat = cdd.Matrix(gens, number_type='float')
    mat.rep_type = cdd.RepType.GENERATOR
    poly = cdd.Polyhedron(mat)
    ineq = np.array(poly.get_inequalities())  # rows: [b, a...] meaning b + a.x >= 0
    return ineq  # H-rep of the same cone

⚠️ 编程陷阱:V-rep 与 H-rep 的"组合爆炸"方向相反

错误: 以为"边数 $k$ 越大,两种表示都只是线性增长"。

现象: 对**多接触** Minkowski 和,V-rep 的生成元数是各接触生成元数之**和**($n_c\cdot k$,线性);但转成 H-rep 后的不等式行数可能**指数级**膨胀(最坏 $O(k^{n_c})$),DD 算法在高维退化锥上会变得很慢。

根因: 多面体锥的"面数"和"棱数"没有简单的线性关系——这是计算几何里 H↔V 转换的固有难度(McMullen 上界定理)。

正确做法: 多接触时**离线**预计算 GIWC 的 H-rep(机器人接触几何固定时锥不变),在线只做矩阵乘法判定;或直接在 WBC 里用每接触的 H-rep 而不显式构造 GIWC。

52.2.6 静平衡可行性——"机器人能站稳吗?"的几何判据 ⭐⭐⭐¶

V-rep 不仅是数学换皮,它直接回答一个工程问题:给定一组接触点和摩擦系数,机器人能否静止站立? 这是练习 52.1.1(斜坡站立)和 52.6 节(支撑域)反复用到的判据,这里给出完整的判定路径。

静平衡条件:机器人静止($\dot{v}=0$,所有 link 速度为零)时,Newton-Euler 方程退化为"接触力的合力/合力矩 = 重力的合力/合力矩"。把所有接触力变换到质心 $G$ 处求和,记总接触 wrench 为 $\boldsymbol{w}_G^{\text{contact}}=\sum_c{}^G\boldsymbol{X}_c^*\boldsymbol{\lambda}_c$,则:

\[\boldsymbol{w}_G^{\text{contact}} = -\boldsymbol{w}_G^{\text{gravity}} = \begin{bmatrix} \boldsymbol{0} \\ -m\boldsymbol{g} \end{bmatrix}_G\]

(重力过质心,绕质心力矩为零,所以右边角动量率部分是 $\boldsymbol{0}$。)

判据:机器人能静止站立 $\iff$ 重力 wrench 的相反数落在 GIWC 内部,即:

\[\boxed{-\boldsymbol{w}_G^{\text{gravity}} \in \mathcal{W}_{\text{GIWC}} = \Big\{\sum_c{}^G\boldsymbol{X}_c^*\boldsymbol{\lambda}_c : \boldsymbol{\lambda}_c\in\text{friction cone}_c\Big\}}\]

这正是 V-rep 大显身手的地方:GIWC 是各接触摩擦锥(用生成元描述)变换、Minkowski 求和后张成的锥。判定"重力 wrench 是否在锥内"等价于一个**小的线性可行性问题**(LP):

\[\text{找 } \beta_{c,i}\ge0 \text{ 使得 } \sum_{c,i}\beta_{c,i}\,({}^G\boldsymbol{X}_c^*\boldsymbol{g}_{c,i}) = -\boldsymbol{w}_G^{\text{gravity}}\]

若该 LP 可行,则存在一组合法接触力支撑重力,机器人能站稳;若不可行(LP 无解),无论怎么分配接触力都无法平衡重力——必然滑倒或倾覆。

回到练习 52.1.1 的斜坡:四足站在 $30°$ 斜坡、$\mu=0.5$。单脚摩擦锥半顶角 $\alpha=\arctan(0.5)\approx26.6°$。重力相对**斜面法向**的倾角恰好是斜坡角 $30°$。由于 $30° > 26.6°$,重力方向**落在摩擦锥之外**——LP 无解,机器人无法仅靠静摩擦站稳,会下滑。临界摩擦系数是 $\mu_{\min}=\tan30°\approx0.577$。这就是为什么练习问"$\mu=0.5$ 能否站立"——答案是不能,需要 $\mu\ge0.577$。

本质洞察:静平衡判据把"力学问题"彻底转化成了"几何包含问题"——能不能站稳,等价于**重力这一个向量,是否落在接触摩擦锥张成的那个锥里**。这就是为什么足式机器人领域如此痴迷于各种"区域"(support polygon、ZMP region、CoM static-equilibrium region):它们都是 GIWC 在不同子空间上的**投影**。ZMP 必须落在支撑多边形内,本质上就是"重力+惯性 wrench 落在 GIWC 内"在平地、点接触假设下的退化特例——这条线索把本章的摩擦锥和足式/70_腿足简化模型理论的 ZMP 判据统一了起来。

🧠 思维陷阱:静平衡可行 $\ne$ 动态稳定

陷阱: "GIWC 判据说能站稳,机器人就一定不会倒。"

现实: GIWC 判据只保证存在一组**静态**接触力平衡重力(必要条件),它不考虑(1)关节力矩能否真的产生这组接触力(受 52.1.5 节力矩限幅约束);(2)受扰动后能否恢复(这是动态稳定性,需要 capturability/MPC 分析)。"能摆出一个平衡姿态"和"能稳定维持"是两件事——前者是静力学,后者是控制。

练习¶

练习 52.2.1 (⭐⭐): 用 Python 画出 $k = 4, 8, 16$ 的内切和外切多面体锥截面(在 $\lambda^z = 1$ 平面上),与精确圆对比。标注每种情况的面积误差百分比。

练习 52.2.2 (⭐⭐⭐): 对于一个人形机器人的扁平足底($l_x = 0.1\text{m}$, $l_y = 0.05\text{m}$, $\mu = 0.6$),构造完整的 6D wrench cone 约束矩阵 $U$。用 Python 验证:当 CoP 刚好在足底边缘时($n_c^x = l_y f_c^z$),约束是否恰好取等号?

练习 52.2.3 (⭐⭐): 阅读 OCS2 的 FrictionConeConstraint.cpp 源码,回答:它是把摩擦锥**线性化成多面体棱**,还是保留为**非线性二阶锥(SOC)**?它返回的是每个接触点几条约束还是一条标量约束?约束值表达式中的 gripperForce 和 regularization 各起什么作用?(提示:核心是 coneConstraint() 返回 mu*(Fz + gripperForce) - sqrt(Fx^2 + Fy^2 + regularization),这是单条 SOC 标量约束;OCS2 通过 augmented Lagrangian + 一/二阶近似把它喂给求解器,而非预先切成 $k$ 条直线。)

52.3 互补约束的处理——三大路线 ⭐⭐⭐¶

动机:为什么互补约束是足式控制的核心难题? ⭐⭐¶

本节解决的问题:互补约束 $0 \le \phi \perp \lambda^n \ge 0$ 是非光滑的,传统优化求解器无法直接处理。如何让优化器"知道"何时该有接触力、何时不该有?

反面案例:如果把互补约束直接放进 NLP:

minimize  cost(x, u, lambda)
subject to:
  dynamics: M*ddq + h = S^T*tau + J_c^T*lambda
  complementarity: phi(p_c) * lambda_n = 0
  bounds: phi >= 0, lambda_n >= 0

Ipopt 会怎么做?在 $\phi = 0, \lambda^n = 0$ 附近,约束 Jacobian $[\lambda^n, \phi]$ 的秩不足(LICQ 违反)——Ipopt 要么不收敛,要么收敛到非物理解。

52.3.0 从刚体动力学到 LCP——互补约束的来龙去脉 ⭐⭐⭐¶

在 52.1.3 节我们断言"接触问题是 LCP 的标准形式",但没有展示**刚体动力学方程是怎么一步步变成 LCP 的**。这一步推导是理解后续三大路线的前提——只有看清 LCP 的结构,才能明白为什么"预定义模式"能绕开它、为什么"软化"在做什么近似。本节补上这条推导链。

问题设定:考虑一个半隐式(semi-implicit)时间步格式,把动力学离散在速度级。回顾足式/50_空间向量与浮动基座动力学:浮动基座动力学方程为 $M(q)\dot{v} + h(q,v) = S^T\boldsymbol{\tau} + J_c^T\boldsymbol{\lambda}$,其中 $M$ 是 $n_v\times n_v$ 的对称正定质量矩阵,$h$ 含科氏力与重力,$J_c$ 是接触 Jacobian(法向行记为 $J_n$)。我们想求解一个时间步 $\Delta t$ 后的接触冲量 $\boldsymbol{\lambda}$(这里 $\boldsymbol{\lambda}$ 已吸收 $\Delta t$,量纲是冲量 $\mathrm{N\cdot s}$)。

步骤 1:离散化动力学,解出下一时刻速度

用前向欧拉离散加速度项 $\dot{v}\approx (v^+ - v)/\Delta t$,其中 $v$ 是当前速度、$v^+$ 是步末速度:

\[M(v^+ - v) = \Delta t\,(S^T\boldsymbol{\tau} - h) + J_c^T\boldsymbol{\lambda}\]

把已知量(当前速度对未来速度的贡献、外力)合并成"自由速度" $v^{\text{free}} = v + \Delta t\,M^{-1}(S^T\boldsymbol{\tau} - h)$——这是**假设没有接触力**时机器人在下一步会达到的速度。于是:

\[v^+ = v^{\text{free}} + M^{-1}J_c^T\boldsymbol{\lambda}\]

阶段小结:到这里我们把"未来速度"写成了"自由速度 + 接触力的修正项"。接下来要做的是:把速度级约束(脚不能继续穿透)套在 $v^+$ 上,从而解出未知的 $\boldsymbol{\lambda}$。

步骤 2:写出法向接触点的速度级互补条件

只看法向方向。接触点的法向相对速度为 $\dot{\phi}^+ = J_n v^+$。速度级 Signorini 条件(52.1.3 节给过)要求:接触点要么分离($\dot{\phi}^+ > 0$,此时无力 $\lambda^n = 0$),要么保持($\dot{\phi}^+ = 0$,此时可以有力 $\lambda^n \ge 0$)。两种情形恰好是互补的:

\[0 \le \dot{\phi}^+ \perp \lambda^n \ge 0\]

为了不穿透,还要加一个 Baumgarte 风格的位置修正偏置 $b = \frac{\gamma}{\Delta t}\phi$(把当前的微小穿透 $\phi<0$ 在一步内"推"回零,$\gamma\in(0,1]$ 是修正比例)。于是约束写成 $\dot{\phi}^+ + b \ge 0$。

步骤 3:Schur 补——消去 $v^+$,得到只含 $\lambda^n$ 的 LCP

把步骤 1 的 $v^+$ 代入 $\dot{\phi}^+ = J_n v^+$:

\[\dot{\phi}^+ = J_n v^{\text{free}} + \underbrace{J_n M^{-1} J_n^T}_{=\,\boldsymbol{A}}\,\lambda^n\]

定义 Delassus 算子 $\boldsymbol{A} = J_n M^{-1} J_n^T$(这是接触空间里的"等效逆惯量"——它通过 $M^{-1}$ 这个 Schur 补把全身惯量投影到接触法向方向)和常数项 $\boldsymbol{w} = J_n v^{\text{free}} + b$。代入互补条件,得到标准 LCP:

\[\boxed{0 \le \lambda^n \perp \boldsymbol{A}\lambda^n + \boldsymbol{w} \ge 0}\]

这就是 LCP 的标准形式 $\text{LCP}(\boldsymbol{A}, \boldsymbol{w})$:求 $\lambda^n \ge 0$ 使得 $\boldsymbol{A}\lambda^n + \boldsymbol{w} \ge 0$ 且 $(\lambda^n)^T(\boldsymbol{A}\lambda^n + \boldsymbol{w}) = 0$。

本质洞察:LCP 不是凭空规定的"数学游戏",而是**把"接触力修正速度"(动力学)和"速度决定有无力"(单边约束)这两个互相缠绕的逻辑用 Schur 补解开后的必然结果**。Delassus 算子 $\boldsymbol{A} = J_n M^{-1}J_n^T$ 是这条推导的核心——它把"一个接触点的力如何通过整副身体的惯量影响所有接触点的速度"压缩成一个稠密的 $n_c\times n_c$ 矩阵。正是这个稠密耦合,使得多接触问题不能逐点独立求解。

为什么 $\boldsymbol{A}$ 半正定、LCP 有解? 因为 $M\succ 0$ 所以 $M^{-1}\succ 0$,对任意向量 $x$ 有 $x^T\boldsymbol{A}x = (J_n^T x)^T M^{-1}(J_n^T x) \ge 0$,故 $\boldsymbol{A}\succeq 0$。半正定 LCP 一定有解(Cottle-Pang-Stone 定理),且当 $J_n$ 行满秩时 $\boldsymbol{A}\succ 0$ 解唯一。这解释了 52.4.2 节"JMinvJt_damping 在 $J_c$ 不满秩时保证可逆"的工程做法——它在 $\boldsymbol{A}$ 对角线上加 $\epsilon I$,把半正定退化情形正则化为严格正定。

加入摩擦后:LCP 变成什么? 上面只处理了法向。一旦把线性化摩擦锥(52.2 节的 $k$ 边)也写进来,切向冲量被表示为锥生成元的非负组合,滑动方向引入额外的互补对,整个问题变成一个**更大的混合 LCP**(Stewart-Trinkle 1996 的经典格式)。维度从 $n_c$ 膨胀到约 $n_c(k+2)$。这正是 52.8.1 节 $w = M^{-1}(\cdots)$、$0\le w\perp\lambda\ge0$ 那个公式的展开来源。

💡 概念误区:LCP 是凸优化吗?

错误理解:"LCP 可以写成 QP,所以它总是凸的。"

正确理解:当 $\boldsymbol{A}\succeq 0$(对称半正定,纯法向、无摩擦或关联摩擦模型)时,LCP 等价于一个凸 QP $\min \frac12\lambda^T\boldsymbol{A}\lambda + \boldsymbol{w}^T\lambda$ s.t. $\lambda\ge0$,可用 QP 求解器。但**加入库仑摩擦后** $\boldsymbol{A}$ 通常**非对称**(Stewart-Trinkle 格式),对应的不再是凸 QP,而是需要 Lemke 算法或 PATH 求解器的一般 LCP。这就是为什么"刚体接触 + 真实库仑摩擦"在数学上比"纯法向接触"难一个量级。

52.3.1 路线 1:预定义接触模式(Mode-Scheduled) ⭐⭐¶

看清了 LCP 的结构后,回到本节开头的问题:既然互补约束这么难,工程上怎么绕开它?最直接的一条路就是**根本不让求解器去判断接触状态**——这就是第一条路线。

核心思想:步态(gait)是预先设计的——控制工程师**告诉**求解器每条腿在每个时刻是"接触"还是"摆动"。

数学实现:

设时域 $[0, T]$ 被分成 $M$ 个模式段 $[t_{m-1}, t_m]$,每段有一个**接触模式** $\sigma_m \in \{0, 1\}^{n_c}$,其中 $n_c$ 是接触点数(四足 = 4)。

在第 $m$ 段,对第 $i$ 个接触点:

\[\sigma_m^{(i)} = 1 \implies \begin{cases} \phi_i = 0 & \text{(脚在地面)} \\ \lambda_i^n \ge 0 & \text{(有支撑力)} \\ \dot{\boldsymbol{p}}_{c,i}^t = 0 & \text{(无滑动)} \end{cases}\]

\[\sigma_m^{(i)} = 0 \implies \begin{cases} \phi_i \ge 0 & \text{(脚可能离地)} \\ \lambda_i^n = 0 & \text{(无接触力)} \\ \text{swing trajectory} & \text{(摆腿轨迹)} \end{cases}\]

互补性自动满足:在每个模式段内,$\phi_i$ 和 $\lambda_i^n$ 中恰好一个被固定为零,乘积自然为零。

OCS2 中的实现:

// OCS2: ModeSchedule defines contact sequence
// Example: trot gait for quadruped
ModeSchedule modeSchedule;
modeSchedule.eventTimes = {0.2, 0.4, 0.6, 0.8};  // Phase transitions
modeSchedule.modeSequence = {
  9,   // 0b1001: LF + RH in contact (trot phase 1)
  6,   // 0b0110: RF + LH in contact (trot phase 2)
  9,   // LF + RH again
  6,   // RF + LH again
  9    // LF + RH
};
// Each mode number encodes a bit mask: 
//   bit 0 = LF, bit 1 = RF, bit 2 = LH, bit 3 = RH

优缺点分析:

维度	评价
实时性	每段内是凸 QP/NLP,可实时求解
简单性	实现简单,工业界主流
适应性	接触模式不是优化变量,不能自适应地形
新颖步态	不能发现新步态(如用手支撑)
鲁棒性	依赖步态调度器的正确性

52.3.2 路线 2:软化互补约束(Relaxation) ⭐⭐⭐¶

核心思想:用光滑函数**近似**非光滑的互补条件,使问题变成标准 NLP。

方法 A:松弛(Relaxation)

\[\phi \cdot \lambda^n \le \epsilon, \quad \epsilon > 0\]

允许 $\phi$ 和 $\lambda^n$ 同时为小正数。在优化过程中逐步减小 $\epsilon \to 0$。

方法 B:对数障碍(Log-Barrier)

\[\min_{\phi, \lambda^n} \; \text{cost} - \kappa \left( \ln(\phi) + \ln(\lambda^n) \right) + \frac{1}{2\rho} (\phi \cdot \lambda^n)^2\]

障碍项 $-\kappa \ln(\cdot)$ 保证 $\phi > 0, \lambda^n > 0$;惩罚项 $(\phi \cdot \lambda^n)^2 / (2\rho)$ 推动乘积趋向零。逐步减小 $\kappa$ 和 $\rho$。

方法 C:Fischer-Burmeister (FB) 函数

定义 NCP 函数:

\[\Phi_{\text{FB}}(a, b) = \sqrt{a^2 + b^2} - a - b\]

关键性质:$\Phi_{\text{FB}}(a, b) = 0 \iff a \ge 0, b \ge 0, ab = 0$

证明:

若 $a > 0, b = 0$:$\sqrt{a^2} - a - 0 = |a| - a = 0$ (因为 $a > 0$)
若 $a = 0, b > 0$:$\sqrt{b^2} - 0 - b = |b| - b = 0$ (因为 $b > 0$)
若 $a > 0, b > 0$:$\sqrt{a^2 + b^2} < a + b$(因为 $(a+b)^2 = a^2 + 2ab + b^2 > a^2 + b^2$),所以 $\Phi_{\text{FB}} < 0 \ne 0$。
若 $a < 0$ 或 $b < 0$:$\sqrt{a^2+b^2} \ge |a| > a$(若 $a < 0$),所以 $\Phi_{\text{FB}} > 0 \ne 0$。

因此 $\Phi_{\text{FB}} = 0$ 精确等价于互补条件。$\square$

把互补约束替换为:

\[\Phi_{\text{FB}}(\phi, \lambda^n) = 0\]

这是一个**等式约束**,且在 $a > 0$ 或 $b > 0$ 时可微。不可微点只在 $(0, 0)$,可以用**正则化**处理:

\[\Phi_{\text{FB}}^{\epsilon}(a, b) = \sqrt{a^2 + b^2 + \epsilon^2} - a - b\]

当 $\epsilon > 0$ 时,$\Phi_{\text{FB}}^{\epsilon}$ 处处可微,且 $\epsilon \to 0$ 时逼近原始 FB 函数。

三种软化方法对比:

方法	光滑性	收敛速度	精度	实现复杂度
松弛	光滑	慢(线性)	$O(\epsilon)$	低
Log-Barrier	光滑	中等	$O(\kappa)$	中
Fischer-Burmeister	半光滑	快(超线性)	精确	高

⚠️ 编程陷阱:松弛参数的调度

错误: 一开始就设 $\epsilon = 10^{-8}$。

症状: 优化器不收敛,NaN 爆炸。

根因: $\epsilon$ 太小时问题的条件数极差。

正确做法: 从 $\epsilon = 0.1$ 开始,每次优化收敛后减半,直到 $\epsilon < 10^{-4}$。典型的 continuation schedule: $\epsilon \in \{0.1, 0.05, 0.01, 0.005, 0.001\}$。

💡 概念误区:Fischer-Burmeister 的光滑性

错误理解: "FB 函数是光滑的,可以直接放进 Ipopt。"

正确理解: 原始 FB 函数在 $(0,0)$ 处不可微(半光滑)。需要用正则化版本 $\Phi_{\text{FB}}^{\epsilon}$,或者用专门的半光滑牛顿法。Ipopt 需要二阶导数,而 FB 在 $(0,0)$ 附近的 Hessian 不存在。

52.3.3 路线 3:Contact-Implicit Trajectory Optimization (CITO) ⭐⭐⭐⭐¶

核心思想:把接触位置、时刻和力**全部作为决策变量**,让优化器自主发现最优接触策略。

标准 CITO 公式(Posa et al. 2014):

\[\boxed{\begin{aligned} \min_{x_{0:N}, u_{0:N-1}, \lambda_{0:N}} \quad & \sum_{k=0}^{N-1} l(x_k, u_k) + l_f(x_N) \\ \text{s.t.} \quad & x_{k+1} = f(x_k, u_k, \lambda_k) & \text{(dynamics)} \\ & 0 \le \phi(x_k) \perp \lambda_k^n \ge 0 & \text{(normal complementarity)} \\ & \|\boldsymbol{\lambda}_k^t\| \le \mu \lambda_k^n & \text{(friction cone)} \\ & 0 \le (\mu \lambda_k^n - \|\boldsymbol{\lambda}_k^t\|) \perp \|\dot{\boldsymbol{p}}_c^t\| \ge 0 & \text{(sliding complementarity)} \end{aligned}}\]

最后一条是**滑动互补**:如果摩擦力没有饱和(在锥内部),则接触点不滑动。

最新进展(2024-2026):

论文	核心创新	关键指标
Pang et al. 2023, T-RO	局部光滑化 + quasi-dynamic 模型	首次全局 CITO for manipulation
Kurtz et al. 2024, IJRR	Inverse dynamics CITO	实时 CITO-MPC > 100Hz
STOCS 2025, T-RO	无穷规划框架,动态实例化接触点	高保真几何 3D manipulation
ci-SCvx 2026, arXiv	GPU 加速连续时间逐次凸化	比 SCP SOTA 快 10x+

CITO 的根本困难:

组合爆炸: $n_c$ 个接触点,$N$ 个时间步,最多 $2^{n_c \cdot N}$ 种可能的接触模式序列
非凸非光滑: 互补约束导致可行域非凸
局部极小值: 不同的接触策略对应不同的局部最优
计算代价: 典型 CITO 需要分钟到小时级求解

玩具示例:2D 弹跳球:

// Simplified CITO: 2D ball bouncing on ground
// Decision variables: [x(t), z(t), lambda_n(t)] for t = 0..N
// Dynamics: m*ddz = lambda_n - m*g, m*ddx = 0
// Complementarity: 0 <= z _|_ lambda_n >= 0 (relaxed: z*lambda_n <= eps)

#include <ifopt/problem.h>
#include <ifopt/ipopt_solver.h>

class BouncingBallVariables : public ifopt::VariableSet {
public:
  static constexpr int N = 50;
  static constexpr double dt = 0.02;

  // Variables: z_0..z_N, lambda_0..lambda_N
  BouncingBallVariables() : VariableSet(2 * (N + 1), "ball_vars") {
    // Initialize: free fall trajectory
    for (int k = 0; k <= N; ++k) {
      double t = k * dt;
      x_(k) = 1.0 - 0.5 * 9.81 * t * t;  // z position
      x_(N + 1 + k) = 0.0;                 // lambda_n = 0
    }
  }

  VectorXd GetValues() const override { return x_; }
  void SetVariables(const VectorXd& x) override { x_ = x; }
  VecBound GetBounds() const override {
    VecBound bounds(GetRows());
    for (int k = 0; k <= N; ++k) {
      bounds[k] = ifopt::Bounds(0.0, ifopt::inf);     // z >= 0
      bounds[N+1+k] = ifopt::Bounds(0.0, ifopt::inf); // lambda >= 0
    }
    return bounds;
  }
private:
  VectorXd x_ = VectorXd::Zero(2 * (N + 1));
};

class ComplementarityConstraint : public ifopt::ConstraintSet {
public:
  static constexpr double eps = 1e-3;  // Relaxation
  static constexpr int N = 50;

  ComplementarityConstraint() : ConstraintSet(N + 1, "compl") {}

  VectorXd GetValues() const override {
    auto vars = GetVariables()->GetComponent("ball_vars")->GetValues();
    VectorXd g(N + 1);
    for (int k = 0; k <= N; ++k) {
      g(k) = vars(k) * vars(N + 1 + k);  // z_k * lambda_k
    }
    return g;
  }

  VecBound GetBounds() const override {
    VecBound b(N + 1);
    for (int k = 0; k <= N; ++k)
      b[k] = ifopt::Bounds(-ifopt::inf, eps);  // z*lambda <= eps
    return b;
  }

  void FillJacobianBlock(std::string var_set, 
                          Jacobian& jac) const override {
    if (var_set == "ball_vars") {
      auto vars = GetVariables()->GetComponent("ball_vars")->GetValues();
      for (int k = 0; k <= N; ++k) {
        jac.coeffRef(k, k) = vars(N + 1 + k);       // d(z*l)/dz = l
        jac.coeffRef(k, N + 1 + k) = vars(k);        // d(z*l)/dl = z
      }
    }
  }
};

🧠 思维陷阱:CITO 能替代步态设计吗?

陷阱: "有了 CITO,就不需要人工设计步态了。"

现实: 目前(2026 年)CITO 在腿足机器人上仍然是**离线工具**——用于发现新步态或验证概念。实时控制仍然依赖预定义步态 + MPC/WBC。原因:CITO 的求解时间(秒级)远超实时要求(毫秒级)。Kurtz et al. 2024 首次展示了 100Hz CITO-MPC,但仅限于操作任务(自由度低)。ci-SCvx(2026)在 GPU 上实现了快速 CITO,但离腿足实时部署仍有距离。

练习¶

练习 52.3.1 (⭐⭐⭐): 实现正则化 Fischer-Burmeister 函数 $\Phi_{\text{FB}}^{\epsilon}(a, b)$,计算其梯度 $\partial \Phi / \partial a$ 和 $\partial \Phi / \partial b$。画出 $\epsilon = 0.1, 0.01, 0.001$ 时的函数值热力图(以 $a, b$ 为轴)。

练习 52.3.2 (⭐⭐⭐⭐): 用 Ifopt + Ipopt 实现上述 2D 弹跳球 CITO。初始条件 $z(0) = 1$, $\dot{z}(0) = 0$。优化器能否自动发现"自由落体 → 接触地面 → 产生支撑力 → 停住"的序列?

练习 52.3.3 (⭐⭐): 比较三种软化方法(松弛/log-barrier/FB)在以下指标上的差异:收敛迭代次数、最终互补性违反量 $\max_k \phi_k \cdot \lambda_k^n$、总计算时间。用一个简单的 1D 弹跳球例子。

52.4 接触 Jacobian 深入——从速度到广义力 ⭐⭐¶

前三节解决了"接触力应满足什么约束"以及"如何在优化中处理这些约束"。但还有一个关键环节尚未打通:接触力作用在足端,而动力学方程写在关节空间——两者之间的桥梁就是接触 Jacobian。

动机:为什么接触 Jacobian 是力传递的核心? ⭐⭐¶

本节解决的问题:接触力 $\boldsymbol{\lambda}_c$ 作用在足端,但动力学方程是在关节空间写的。如何把足端力"翻译"成关节空间的广义力?反过来,如何从关节速度得到足端速度?

反面案例:忘记接触 Jacobian 的后果——

场景: 你手动在动力学方程中直接加了 lambda_z 到基座z方向
错误: M*ddq + h = S^T*tau + [0,0,lambda_z,0,...,0]^T

正确: M*ddq + h = S^T*tau + J_c^T * lambda_c
       J_c^T 把力从接触空间映射到关节空间
       它不仅影响基座,还影响接触链上的所有关节!

52.4.1 接触 Jacobian 的推导 ⭐⭐¶

运动学链:接触点 $\boldsymbol{p}_c$ 是广义坐标 $q$ 的函数:

\[\boldsymbol{p}_c = \boldsymbol{p}_c(q) \in \mathbb{R}^3\]

对时间求导(链式法则):

\[\dot{\boldsymbol{p}}_c = \frac{\partial \boldsymbol{p}_c}{\partial q} \dot{q} = \boldsymbol{J}_c(q) \dot{q}\]

其中 $\boldsymbol{J}_c(q) \in \mathbb{R}^{3 \times n_v}$ 就是接触 Jacobian(线速度部分)。

完整 6D Jacobian:如果我们关心接触点的角速度(例如面接触),则用 6D Jacobian:

\[\begin{bmatrix} \boldsymbol{\omega}_c \\ \dot{\boldsymbol{p}}_c \end{bmatrix} = \boldsymbol{J}_c^{6D}(q) \dot{q}, \quad \boldsymbol{J}_c^{6D} \in \mathbb{R}^{6 \times n_v}\]

对偶关系(Virtual Work Principle):

由虚功原理,接触力 $\boldsymbol{\lambda}_c$ 在虚位移 $\delta \boldsymbol{p}_c$ 上做的虚功等于对应广义力在虚广义位移 $\delta q$ 上做的虚功:

\[\boldsymbol{\lambda}_c^T \delta \boldsymbol{p}_c = \boldsymbol{\lambda}_c^T \boldsymbol{J}_c \delta q = (\boldsymbol{J}_c^T \boldsymbol{\lambda}_c)^T \delta q\]

因此,接触力在广义坐标空间中等价于:

\[\boxed{\boldsymbol{\tau}_{\text{contact}} = \boldsymbol{J}_c^T \boldsymbol{\lambda}_c}\]

这就是动力学方程 $M \ddot{q} + h = S^T \tau + \boldsymbol{J}_c^T \boldsymbol{\lambda}_c$ 中接触力项的来源。

本质洞察:$J_c$ 和 $J_c^T$ 构成一对**对偶映射**——$J_c$ 把关节速度"翻译"成接触点速度(运动方向),而 $J_c^T$ 把接触力"翻译"成关节广义力(力方向)。这不是巧合,而是**虚功原理的必然结果**:能量守恒要求运动和力的传递使用同一个线性映射的转置。这个对偶关系贯穿整个机器人学——从末端执行器力控到 WBC 的约束力分配,都依赖于此。

52.4.2 接触 Jacobian 的时间导数与漂移项 ⭐⭐⭐¶

为什么需要 $\dot{\boldsymbol{J}}_c$?

在约束接触(脚固定在地面)的情况下,加速度级约束为:

\[\ddot{\boldsymbol{p}}_c = \boldsymbol{J}_c \ddot{q} + \dot{\boldsymbol{J}}_c \dot{q} = 0\]

$\dot{\boldsymbol{J}}_c \dot{q}$ 称为**漂移项(drift term / bias acceleration)**。如果忽略它,约束加速度 $\ddot{\boldsymbol{p}}_c \ne 0$,接触点会逐渐漂离地面(constraint drift)。

Pinocchio 的漂移计算:

// Method 1: Use getFrameClassicalAcceleration (includes drift)
pinocchio::forwardKinematics(model, data, q, v, 
                              Eigen::VectorXd::Zero(model.nv));
pinocchio::updateFramePlacements(model, data);

auto drift = pinocchio::getFrameClassicalAcceleration(
    model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED);
// drift.linear() = dJ * dq (when joint accelerations are zero)

// Method 2: Explicit computation of dJ
pinocchio::computeJointJacobiansTimeVariation(model, data, q, v);
Eigen::MatrixXd dJ(6, model.nv);
dJ.setZero();
pinocchio::getFrameJacobianTimeVariation(
    model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED, dJ);
Eigen::VectorXd drift_term = dJ * v;  // 6x1 bias acceleration

Baumgarte 稳定化:

即使正确计算了漂移项,数值积分仍可能导致微小的约束漂移。Baumgarte 稳定化在加速度约束中加入位置和速度修正:

\[\boldsymbol{J}_c \ddot{q} + \dot{\boldsymbol{J}}_c \dot{q} + 2\alpha \dot{\boldsymbol{p}}_c + \beta^2 (\boldsymbol{p}_c - \boldsymbol{p}_c^{\text{ref}}) = 0\]

其中 $\alpha, \beta > 0$ 是稳定化增益。典型值:$\alpha = \beta = 50$。

⚠️ 编程陷阱:忘记漂移项

错误: 在 WBC 中写接触约束为 $J_c \ddot{q} = 0$。

症状: 仿真中脚缓慢滑动,数秒后偏移数厘米。

根因: 缺少 $\dot{J}_c \dot{q}$ 项。正确的约束是 $J_c \ddot{q} = -\dot{J}_c \dot{q}$。

进一步: 即使加了漂移项,仍需 Baumgarte 稳定化来抑制数值积分累积误差。

💡 概念误区:Crocoddyl 的隐式约束处理

问题: Crocoddyl 的 DifferentialActionModelContactFwdDynamics 看起来没有显式的 $J_c \ddot{q} = -\dot{J}_c \dot{q}$ 约束,它是怎么处理的?

答案: Crocoddyl 调用 pinocchio::forwardDynamics(),该函数内部求解 KKT 系统: $$\begin{bmatrix} M & J_c^T \\ J_c & 0 \end{bmatrix} \begin{bmatrix} \ddot{q} \\ -\lambda \end{bmatrix} = \begin{bmatrix} \tau - h \\ -\dot{J}_c \dot{q} \end{bmatrix}$$ 约束 $J_c \ddot{q} = -\dot{J}_c \dot{q}$ 被**隐式**嵌入在 KKT 系统中。a0(drift acceleration)就是 $-\dot{J}_c \dot{q}$。添加 JMinvJt_damping 可以在 $J_c$ 不满秩时保证矩阵可逆。

52.4.3 参考系选择:LOCAL vs WORLD vs LOCAL_WORLD_ALIGNED ⭐⭐¶

Pinocchio 提供三种参考系选项,它们对接触 Jacobian 的计算至关重要:

参考系	数学定义	Jacobian 结构	摩擦锥自然性
`LOCAL`	速度在 frame 自身坐标系下表达	稀疏(只含关节链)	摩擦锥方向随 frame 旋转
`WORLD`	速度在世界系下表达,参考点在世界原点	包含额外的叉乘项	但有远距离叉乘误差
`LOCAL_WORLD_ALIGNED` (LWA)	速度在世界系朝向下表达,参考点在 frame 处	兼顾稀疏性和直觉性	推荐用于接触力学

为什么 LWA 最适合接触力学?

摩擦锥 $\|\lambda^{xy}\| \le \mu \lambda^z$ 假设 $z$ 轴是接触面法向。在世界系中(水平地面),法向就是 $z$ 轴——直接匹配。如果用 LOCAL 参考系,法向方向随 frame 旋转,每次都要旋转摩擦锥约束矩阵,增加复杂性且容易出错。

代码示例:对比三种参考系:

// Compare Jacobians in different reference frames
Eigen::MatrixXd J_local(6, model.nv), J_world(6, model.nv), J_lwa(6, model.nv);
J_local.setZero(); J_world.setZero(); J_lwa.setZero();

pinocchio::computeJointJacobians(model, data, q);
pinocchio::updateFramePlacements(model, data);

pinocchio::getFrameJacobian(model, data, foot_frame_id,
                             pinocchio::LOCAL, J_local);
pinocchio::getFrameJacobian(model, data, foot_frame_id,
                             pinocchio::WORLD, J_world);
pinocchio::getFrameJacobian(model, data, foot_frame_id,
                             pinocchio::LOCAL_WORLD_ALIGNED, J_lwa);

// Relationship: J_lwa = [R_wf, 0; 0, R_wf] * J_local
// where R_wf is the frame orientation in world frame
// J_world involves additional cross-product terms due to 
// the reference point being at the world origin

🧠 思维陷阱:WORLD 参考系的"远距离"问题

陷阱: "WORLD 参考系最自然,应该总是用它。"

问题: WORLD 参考系的参考点在世界原点。当接触点远离原点时,角速度分量会引入大量叉乘项 $\omega \times r$,导致 Jacobian 数值条件差,且 $J^T \lambda$ 的物理含义不直观。

推荐: 使用 LOCAL_WORLD_ALIGNED——参考点在接触 frame 处(避免远距离叉乘),方向用世界系(摩擦锥自然表达)。

练习¶

练习 52.4.1 (⭐⭐): 对一个 3-DOF 平面机械臂(基座固定,2 个旋转关节 + 末端),手动推导接触 Jacobian $J_c(q)$。验证 $J_c^T \lambda$ 给出的关节力矩与你用力矩平衡直接计算的结果一致。

练习 52.4.2 (⭐⭐⭐): 用 Pinocchio 计算 Go2 四足机器人前左脚的 Jacobian,分别用三种参考系。验证关系:$J_{\text{LWA}} = \text{diag}(R_{wf}, R_{wf}) \cdot J_{\text{LOCAL}}$。

练习 52.4.3 (⭐⭐): 解释为什么 Crocoddyl 的 DifferentialActionModelContactFwdDynamics 构造时需要 JMinvJt_damping 参数。当 $J_c$ 满秩时,该参数设为 0 会有问题吗?

52.5 Centroidal Momentum Matrix (CMM) 与 CCRBA ⭐⭐⭐¶

动机:为什么需要 CMM? ⭐⭐¶

本节解决的问题:Centroidal Dynamics($\dot{h}_G = \sum \text{external wrenches}$)中的 $h_G$——系统在质心处的 6 维动量——如何从关节状态 $(q, \dot{q})$ 高效计算?朴素方法需要遍历所有 link 逐个累加,复杂度 O(N^2)。CMM 给出一个线性映射,且 CCRBA 算法实现了 O(N) 计算。

反面案例:如果你用朴素方法计算 Centroidal Momentum:

# WRONG: O(N^2) brute force
h_G = np.zeros(6)
for link in robot.links:
    # Get link CoM velocity in world frame - O(N) per link
    v_link = get_link_com_velocity(link, q, dq)
    # Get link spatial inertia
    I_link = link.spatial_inertia
    # Transform to centroidal frame and accumulate
    h_G += transform_to_com(I_link @ v_link, com)

# Total: O(N) per link x N links = O(N^2)
# For a humanoid with 30+ links, this is wasteful

52.5.1 CMM 的数学定义 ⭐⭐¶

Centroidal Momentum $\boldsymbol{h}_G \in \mathbb{R}^6$ 是系统所有 link 在质心 $G$ 处的空间动量之和:

\[\boldsymbol{h}_G = \begin{bmatrix} \boldsymbol{k}_G \\ \boldsymbol{l}_G \end{bmatrix} = \sum_{i=1}^{N_b} {}^G\boldsymbol{X}_i^* \boldsymbol{I}_i \boldsymbol{v}_i\]

其中: - $\boldsymbol{k}_G \in \mathbb{R}^3$:绕质心的角动量 - $\boldsymbol{l}_G \in \mathbb{R}^3$:线动量($= m \dot{\boldsymbol{p}}_G$) - ${}^G\boldsymbol{X}_i^*$:从 link $i$ 到质心 $G$ 的空间力变换 - $\boldsymbol{I}_i$:link $i$ 的空间惯量 - $\boldsymbol{v}_i$:link $i$ 的空间速度

CMM 的线性性:由于 $\boldsymbol{v}_i$ 是 $\dot{q}$ 的线性函数(通过运动学链),整个 $\boldsymbol{h}_G$ 也是 $\dot{q}$ 的线性函数:

\[\boxed{\boldsymbol{h}_G = \boldsymbol{A}_G(q) \dot{q}}\]

$\boldsymbol{A}_G(q) \in \mathbb{R}^{6 \times n_v}$ 就是 Centroidal Momentum Matrix (CMM)。它依赖构型 $q$,但不依赖速度 $\dot{q}$。

52.5.2 CMM 的分块结构与物理含义 ⭐⭐¶

将 CMM 按基座和关节分块:

\[\boldsymbol{A}_G = \begin{bmatrix} \boldsymbol{A}_{G,\text{base}}^{6 \times 6} & \boldsymbol{A}_{G,\text{joints}}^{6 \times (n_v - 6)} \end{bmatrix}\]

进一步分解基座部分:

\[\boldsymbol{A}_{G,\text{base}} = \begin{bmatrix} \boldsymbol{A}_{G,\omega\omega} & \boldsymbol{A}_{G,\omega v} \\ \boldsymbol{A}_{G,v\omega} & \boldsymbol{A}_{G,vv} \end{bmatrix}\]

关键性质 1: 线动量子块

线动量 $\boldsymbol{l}_G = m \dot{\boldsymbol{p}}_G$。由于质心速度对基座线速度的偏导数是 $m \boldsymbol{I}_3$(总质量乘以单位阵),CMM 的对应子块**总是** $m \boldsymbol{I}_3$——与构型 $q$ 无关:

\[\frac{\partial \boldsymbol{l}_G}{\partial \dot{\boldsymbol{p}}_{\text{base}}} = m \boldsymbol{I}_3\]

关键性质 2: Centroidal 惯量矩阵 $\boldsymbol{I}_G$ (CCRBI)

\[\boldsymbol{I}_G = \boldsymbol{A}_G M^{-1} \boldsymbol{A}_G^T \in \mathbb{R}^{6 \times 6}\]

$\boldsymbol{I}_G$ 称为 Centroidal Composite Rigid Body Inertia (CCRBI)——它描述了"如果整个机器人被焊成一个刚体,该刚体在质心处的空间惯量"。CCRBI 随构型 $q$ 变化(因为 link 之间的相对位置改变了)。

关键性质 3: CMM 与全身质量矩阵 $M$ 的关系

\[\boldsymbol{A}_G(q) = \sum_{i=1}^{N_b} {}^G\boldsymbol{X}_i^* \boldsymbol{I}_i \boldsymbol{J}_i(q)\]

其中 $\boldsymbol{J}_i$ 是 link $i$ 质心的空间 Jacobian。注意 $A_G$ 的维度是 $6 \times n_v$(从关节空间到 6D 动量空间),而 $M$ 的维度是 $n_v \times n_v$(关节空间到关节空间)。

52.5.3 CCRBA 算法详解 ⭐⭐⭐¶

CCRBA (Centroidal Composite Rigid Body Algorithm) 是 Orin et al. 2013 提出的 O(N) 算法。它的核心思想是**从叶子到根递推复合惯量,然后从根到叶子计算 CMM 的列**。这好比计算一棵企业组织树的"总薪资":先从基层员工向上逐级汇总(叶到根),得到每个部门的总人力成本;再从总部向下分配预算(根到叶)。CCRBA 用同样的递推逻辑汇总空间惯量,避免了逐个 link 独立计算带来的 O(N^2) 重复遍历。

算法步骤(伪代码):

Algorithm: CCRBA (Orin et al. 2013)
Input:  model (kinematic tree), q (configuration), v (velocity)
Output: A_G (CMM), h_G (centroidal momentum), I_G (CCRBI)

// Phase 1: Forward Kinematics (root to leaves)
for i = 1 to N_b do
    Compute joint transform X_J(q_i)
    X_i = X_J(q_i) * X_T(i)         // Frame transform for link i
    v_i = X_i * v_{parent(i)} + S_i * dq_i  // Spatial velocity
end

// Phase 2: Composite Inertia (leaves to root) - same as CRBA
for i = N_b downto 1 do
    I_c(i) = I(i)                     // Start with own inertia
    for each child j of i do
        I_c(i) += X_j^* * I_c(j) * X_j^{-1}  // Accumulate children
    end
end

// Phase 3: CoM and Centroidal Transform
p_G = (1/m) * sum_i m_i * p_i        // Center of mass
X_G = spatial_transform(R=I, p=p_G)  // Transform to centroidal frame

// Phase 4: CMM Columns (root to leaves)
I_G = X_G^* * I_c(root) * X_G^{-1}  // CCRBI
for i = 1 to N_b do
    // Column of A_G corresponding to joint i
    A_G(:, joint_i) = X_G^* * I_c(i) * S_i
end

// Phase 5: Centroidal Momentum
h_G = A_G * v

复杂度分析:

步骤	操作	复杂度
Phase 1	正向运动学	O(N)
Phase 2	复合惯量	O(N)
Phase 3	质心计算	O(N)
Phase 4	CMM 列	O(N)
Phase 5	矩阵-向量乘法	O(N * nv)
总计		*O(N nv)**

对比朴素方法的 O(N^2 * nv),当 N = 30(nv ~ 36)时加速约 30 倍。

Pinocchio 调用:

#include <pinocchio/algorithm/centroidal.hpp>

// Compute CMM, centroidal momentum, and CCRBI
pinocchio::ccrba(model, data, q, v);

// Results:
Eigen::MatrixXd A_G = data.Ag;   // CMM: 6 x nv
Eigen::Matrix<double, 6, 1> h_G = data.hg;  // Centroidal momentum
Eigen::Vector3d com = data.com[0]; // Center of mass position
Eigen::Matrix<double, 6, 6> I_G = data.Ig;  // CCRBI: 6 x 6

// Verify: h_G should equal A_G * v
assert((h_G - A_G * v).norm() < 1e-10);

// Also available: time derivative of CMM
pinocchio::dccrba(model, data, q, v);
Eigen::MatrixXd dA_G = data.dAg;  // dA_G/dt: 6 x nv
// hdot_G = dA_G * v + A_G * a  (where a = ddq)

⚠️ 编程陷阱:ccrba() vs computeCentroidalMomentum()

错误: 混用 ccrba() 和 computeCentroidalMomentum()。

区别: ccrba() 计算 CMM 矩阵 $A_G$ + 动量 $h_G$;computeCentroidalMomentum() 只计算 $h_G$(不构造 $A_G$),更快但无法用于 WBC 约束公式。如果你需要 $A_G$(比如在 MPC 中约束角动量率),必须调用 ccrba()。

💡 概念误区:CMM 与质量矩阵 M 的混淆

错误理解: "CMM 就是质量矩阵 M 的前 6 行。"

正确理解: $A_G$ 把 $\dot{q}$ 映射到质心处的 6D 动量;$M$ 把 $\ddot{q}$ 映射到关节空间的广义力。两者维度不同($A_G$: $6 \times n_v$, $M$: $n_v \times n_v$)。$A_G$ 涉及到质心的坐标变换,不是简单地截取 $M$ 的某些行。

🧠 思维陷阱:CCRBI 是常数?

陷阱: "系统的空间惯量不随构型变化。"

反例: 当四足机器人蜷缩四肢时,绕质心的转动惯量远小于四肢伸展时——这就是 CCRBI $I_G$ 依赖 $q$ 的原因。花样滑冰运动员收臂加速旋转就是这个原理。

练习¶

练习 52.5.1 (⭐⭐): 用 Pinocchio 对 Go2 计算 ccrba()。验证 $A_G$ 的线动量子块确实接近 $m I_3$($m$ 是总质量)。精确等于还是近似等于?为什么?

练习 52.5.2 (⭐⭐⭐): 计算 $I_G$(CCRBI)在不同构型下的变化。当四条腿完全伸直 vs 完全蜷缩时,$I_G$ 的角动量子块(左上 $3 \times 3$)差异有多大?物理解释是什么?

练习 52.5.3 (⭐⭐⭐): 验证 Centroidal 动力学恒等式:$\dot{h}_G = \sum_c {}^G X_c^* \lambda_c + m [0^T, g^T]^T$。用 Pinocchio 的 ABA(前向动力学)计算 $\ddot{q}$,再用 $dA_G \dot{q} + A_G \ddot{q}$ 验证左边等于右边。

52.6 角动量的特殊地位——守恒与可控性 ⭐⭐¶

上一节展示了如何通过 CMM 高效计算系统的 6 维 Centroidal 动量。其中线动量 $l_G = m\dot{p}_G$ 的行为相对直观,但角动量 $k_G$ 蕴含着更加微妙且对控制至关重要的物理性质,值得专门展开讨论。

动机:为什么角动量值得单独讨论? ⭐⭐¶

本节解决的问题:线动量 $\boldsymbol{l}_G = m \dot{\boldsymbol{p}}_G$ 的规律简单——外力之和改变线动量。但角动量 $\boldsymbol{k}_G$ 有微妙得多的性质:它在飞行相位守恒,在接触相位只能通过"力的力臂"改变。理解这些性质对跳跃、翻滚、步行摆臂都至关重要。

52.6.1 角动量动力学 ⭐⭐¶

线动量与角动量的演化方程:

\[\dot{\boldsymbol{l}}_G = m \boldsymbol{g} + \sum_c \boldsymbol{f}_c\]

\[\dot{\boldsymbol{k}}_G = \sum_c (\boldsymbol{p}_c - \boldsymbol{p}_G) \times \boldsymbol{f}_c\]

关键区别:

特征	线动量 $\boldsymbol{l}_G$	角动量 $\boldsymbol{k}_G$
外力影响	所有外力直接叠加	只有相对质心有"力臂"的力才产生力矩
重力影响	$m\boldsymbol{g}$ 直接改变	零! 重力过质心,力臂为零
无接触时	$\dot{l}_G = mg$ (自由落体)	$\dot{k}_G = 0$ (守恒!)
单点接触	可自由改变(改变力大小)	力矩方向受限(只能绕接触点旋转)

52.6.2 飞行相位的角动量守恒 ⭐⭐¶

定理:当机器人完全离开地面(飞行相位)时,重力绕质心的力矩为零,因此:

\[\dot{\boldsymbol{k}}_G = 0 \implies \boldsymbol{k}_G = \text{const}\]

推论 1:跳跃中不能"转身"

如果起跳时 $k_G^z = 0$(无偏航角动量),那么整个飞行阶段 $k_G^z = 0$——机器人不可能在空中改变偏航角。

这意味着:如果跳跃目标需要落地时朝向不同方向,必须在**起跳前**就注入足够的角动量。

推论 2:翻跟斗需要精确的起跳角动量

后空翻(backflip)需要起跳时提供精确的 $k_G^y$(绕俯仰轴),使得在飞行时间 $T$ 内恰好旋转 $2\pi$:

\[k_G^y = I_{yy} \cdot \frac{2\pi}{T}\]

$T$ 由起跳竖直速度决定($T = 2v_z / g$),而 $I_{yy}$ 在飞行中可以通过蜷缩/伸展改变——这就是体操运动员的技巧。

52.6.3 猫翻身悖论:零角动量下的姿态改变 ⭐⭐⭐¶

悖论:猫从倒置位置自由落下,能在空中翻转 180 度平稳落地。但飞行中角动量守恒,初始 $k_G = 0$,怎么可能旋转?

解答:猫**不是作为刚体旋转**,而是通过**关节内部运动**改变姿态:

前半身蜷缩(减小转动惯量),后半身伸展(增大转动惯量)
前半身快速旋转 $\Delta\theta_1$(角动量 $I_1 \dot{\theta}_1$)
后半身反向慢速旋转 $\Delta\theta_2$(角动量 $-I_2 \dot{\theta}_2$)
由于 $I_1 < I_2$ 但 $I_1 \dot{\theta}_1 = I_2 \dot{\theta}_2$,所以 $\Delta\theta_1 > \Delta\theta_2$
重复多次,净旋转逐步累积

数学表达:

\[k_G = I_{\text{front}} \dot{\theta}_{\text{front}} + I_{\text{rear}} \dot{\theta}_{\text{rear}} = 0\]

\[\implies \dot{\theta}_{\text{rear}} = -\frac{I_{\text{front}}}{I_{\text{rear}}} \dot{\theta}_{\text{front}}\]

由于 $I_{\text{front}} \ne I_{\text{rear}}$(不对称蜷缩),$|\dot{\theta}_{\text{front}}| \ne |\dot{\theta}_{\text{rear}}|$,净旋转角非零。

这是 Centroidal 模型的盲区:Centroidal 模型只看总角动量 $k_G = 0$,无法捕捉内部角动量重分配。需要全身动力学模型。

52.6.4 工程应用:摆臂稳定化与 Flywheel 模型 ⭐⭐¶

人形机器人行走中的角动量问题:

行走时腿的摆动产生角动量:左腿前摆产生正向角动量;右腿前摆也产生正向角动量。如果不补偿,躯干会逐渐向后旋转。

解决方案 1:摆臂(Arm Swing)

手臂反向摆动产生反向角动量,抵消腿的角动量:

\[k_G^y \approx k_{\text{legs}}^y + k_{\text{arms}}^y = 0\]

解决方案 2:Flywheel 模型

在 LIPM 的基础上增加一个"飞轮"——躯干的角动量自由度:

\[\dot{k}_G^y = \sum_c (\boldsymbol{p}_c - \boldsymbol{p}_G) \times \boldsymbol{f}_c \cdot \hat{y}\]

Flywheel 模型保留了 LIPM 的简单性,同时能捕捉角动量的变化——这对快速行走和跑步至关重要。

角动量在 MPC 中的约束形式:

// In OCS2/WBC: constrain angular momentum rate
// h_dot_angular = sum_c (p_c - p_G) x f_c
// Often soft-constrained: minimize ||h_dot_angular||^2
// This encourages "zero angular momentum rate" = smooth motion

// Hard constraint version (bound total angular momentum):
// |k_G| <= k_max
// This prevents excessive spinning

// Centroidal MPC cost term:
Eigen::Matrix<double, 6, 6> Q_centroidal;
Q_centroidal.setZero();
Q_centroidal.topLeftCorner<3, 3>() = w_angular * Eigen::Matrix3d::Identity();
// Weight angular momentum tracking (usually toward zero)

💡 概念误区:LIPM 假设 $k_G = 0$ 的限制

LIPM 假设角动量恒为零:$k_G = 0$。

成立条件: 慢速行走(< 0.5 m/s),双足支撑时间长,腿的摆动角动量小。

严重失效条件: 快速行走(> 1 m/s)、跑步、跳跃、在不平地形上行走。此时必须升级到 Centroidal 模型(保留角动量自由度)或 Flywheel 模型。

🧠 思维陷阱:角动量"越小越好"

陷阱: 在 MPC 中强制 $k_G = 0$。

反例: 跳跃前需要累积角动量;在扰动恢复时,适量角动量帮助保持平衡(摆臂效应)。正确做法:软约束 $\min \|k_G\|^2$,允许必要时偏离零。

练习¶

练习 52.6.1 (⭐⭐): 一个四足机器人从 0.5m 高度自由落体。起跳时 $k_G = 0$。落地前身体绕俯仰轴偏转了 5 度。这是否违反角动量守恒?解释原因(提示:考虑空气阻力和内部关节运动)。

练习 52.6.2 (⭐⭐⭐): 用 Pinocchio 仿真:一个简化的"猫"模型(2-body planar robot,通过关节连接)。初始角动量为零,通过控制关节角度实现 180 度翻转。提示:交替改变两个 body 的惯量(通过改变关节角)。

练习 52.6.3 (⭐⭐): 解释为什么人形机器人 Atlas (Boston Dynamics) 在跑步时摆臂幅度很大,而四足机器人 Spot 不需要摆臂。从角动量的角度分析。

52.7 非线性摩擦锥 vs 线性化——精度与性能的工程权衡 ⭐⭐⭐¶

动机:线性化够好了吗? ⭐⭐¶

本节解决的问题:52.2 节展示了如何线性化摩擦锥。但在实际 WBC/MPC 中,线性化带来多大误差?什么时候必须用精确 SOC 锥?

52.7.1 系统性精度对比 ⭐⭐¶

实验设置:四足机器人 WBC 力分配问题,四脚接触,$\mu = 0.6$,目标:用最小力抗重力。

约束类型	求解器	求解时间	最大摩擦违反	力分配均匀度	约束条数
线性锥 $k=4$	OSQP	0.3 ms	7.6% 可能超出	95.2%	4x4=16
线性锥 $k=4$ (内切)	OSQP	0.3 ms	0% (保守)	93.1%	16
线性锥 $k=8$	OSQP	0.5 ms	1.1%	97.8%	4x8=32
线性锥 $k=16$	OSQP	0.8 ms	0.15%	99.2%	4x16=64
精确 SOC	Mosek	2.5 ms	0%	100%	4 (SOC)
精确 SOC	ECOS	3.2 ms	0%	100%	4 (SOC)
精确 SOC	ProxQP	1.8 ms	0%	99.9%	4 (SOC)

关键发现:

$k=4$ 外切的 7.6% 误差意味着:在摩擦极限附近操作时(如冰面),可能超出真实摩擦锥,导致滑倒
$k=8$ 是实践中的甜蜜点:1.1% 误差 vs 0.5ms(足够 2kHz WBC)
ProxQP 的 SOC 求解比 Mosek 快——Simple Robotics 团队优化了 proximal point 迭代

52.7.2 计算复杂度分析 ⭐⭐⭐¶

QP 的约束数与求解时间的关系:

对于 $n_c$ 个接触点,每个用 $k$ 条边近似,加上法向非负约束:

总不等式约束数:$n_c \times (k + 1)$
四足($n_c = 4$),$k = 8$:$4 \times 9 = 36$ 个不等式
人形($n_c = 2$,6D wrench),$k = 8$ + CoP + 扭矩:$2 \times 16 \approx 32$ 个不等式

QP 求解时间通常与约束数**超线性**增长(对于 active-set 方法约 $O(m^2)$,对于 interior-point 约 $O(m^{1.5})$)。

SOC 求解的替代路径——ProxSuite:

ProxSuite 是 Simple Robotics 团队开发的 proximal point 优化库,可以直接处理 SOC 约束:

#include <proxsuite/proxqp/dense/dense.hpp>
using namespace proxsuite::proxqp;

// Setup QP with friction cone constraints
// ProxQP efficiently handles box + equality + inequality constraints
// For direct SOC support, use the conic interface or reformulate

dense::QP<double> qp(n_var, n_eq, n_ineq);
qp.init(H, g, A_eq, b_eq, A_ineq, b_ineq, l_box, u_box);
qp.settings.eps_abs = 1e-6;
qp.settings.max_iter = 100;
qp.solve();

Eigen::VectorXd x_opt = qp.results.x;
// Contact forces extracted from x_opt

⚠️ 编程陷阱:QP warm-start 与接触模式切换

场景: MPC 每步 warm-start 上一步的解。

陷阱: 当接触模式切换(脚从接触变为摆动),active set 完全改变。如果直接 warm-start,OSQP 可能需要更多迭代(旧 active set 完全错误)。

解决: 在模式切换时禁用 warm-start,或用"冷启动 + 更多最大迭代"策略。OCS2 的 SwitchedModelReferenceManager 在模式切换时自动处理这个问题。

练习¶

练习 52.7.1 (⭐⭐): 对同一个力分配问题,分别用 $k = 4, 8, 16$ 和精确 SOC 求解。画出四条腿力向量在摩擦锥截面上的位置。哪种情况的力最接近锥边界?

练习 52.7.2 (⭐⭐⭐): 用 ProxQP 实现一个简单的 WBC QP,比较 OSQP ($k=8$ 线性化) vs ProxQP 的求解时间和解精度。在 100 个随机构型上统计。

52.8 软接触 vs 刚体接触——工程选择与仿真实践 ⭐⭐⭐¶

动机:为什么有两种接触模型? ⭐⭐¶

本节解决的问题:刚体接触(LCP/互补性)假设接触面无变形,接触力瞬间产生。软接触(弹簧-阻尼/体积弹性)假设接触面有弹性变形。两者各有优劣——为什么工业控制器用刚体,而仿真器(MuJoCo)用软接触?

52.8.1 刚体接触模型 ⭐⭐¶

数学表达:

\[\text{LCP: } \quad w = M^{-1}(J_c^T \lambda + \tau_{\text{ext}}) + \text{bias}, \quad 0 \le w \perp \lambda \ge 0\]

其中 $w$ 是接触点的加速度(或速度,取决于时间步格式)。

特点: - 接触力是**约束力**,不是显式函数——由互补条件隐式确定 - 数学上精确(真正的不穿透,不滑移) - 但需要求解 LCP/QP,每一步都是一个优化问题

52.8.2 软接触模型 ⭐⭐¶

弹簧-阻尼(Spring-Damper)模型:

\[\lambda_c^n = \max\!\Big(0,\; k \cdot (-\phi) - d \cdot \dot{\phi}\Big) \cdot \mathbb{1}[\phi \le 0]\]

$k$：接触刚度(N/m)
$d$：接触阻尼(Ns/m)
$\phi$：有符号距离（$\phi < 0$ 表示穿透，$-\phi$ 即穿透深度 $\delta$）
外层 $\max(0, \cdot)$ 防止阻尼项在闭合阶段（$\dot{\phi} < 0$）使法向力变为负值（拉力），保证 $\lambda_c^n \ge 0$

Hertz 接触模型(更符合物理):

\[\lambda_c^n = k_H |\phi|^{3/2} - d_H |\phi|^{3/2} \dot{\phi}\]

$3/2$ 次方来自弹性力学——球体压入平面的接触力与变形量的 $3/2$ 次方成正比(Hertz 1882)。

Drake Hydroelastic 模型(2019+):

Drake 的 Hydroelastic Contact 更进一步——不是点接触,而是**体积压力场接触**:

每个碰撞体被赋予一个**压力场**(pressure field):刚度从表面到内部线性增加
两个物体的压力场在接触区域**叠加**
接触面积、压力分布、合力/合力矩通过积分计算

\[\boldsymbol{f}_c = \int_{\Omega_{\text{contact}}} p(\boldsymbol{x}) \boldsymbol{n}(\boldsymbol{x}) \, dA\]

优势: 自然处理面接触、滚动摩擦、CoP 偏移。

劣势: 需要 mesh 离散化,计算量大,参数(弹性模量)难校准。

MuJoCo 的软接触:

MuJoCo 使用一种**凸近似**的软接触模型——将接触问题转化为一个优化问题,通过 PGS (Projected Gauss-Seidel) 或 Newton 方法求解。核心参数:

<!-- MuJoCo contact parameters -->
<option>
  <flag contact="enable"/>
</option>
<default>
  <geom condim="3" friction="0.7 0.005 0.0001"
        solimp="0.9 0.95 0.001 0.5 2"
        solref="0.02 1"/>
  <!-- solref: [timeconst, dampratio] controls contact dynamics -->
  <!-- solimp: [dmin, dmax, width, midpoint, power] controls impedance -->
</default>

52.8.3 两种模型的详细对比 ⭐⭐¶

维度	刚体接触(LCP)	软接触(Spring-Damper/Hydroelastic)
物理精度	精确的不穿透	允许微小穿透
数学光滑性	非光滑(互补约束)	光滑(连续力函数)
计算方法	LCP/QP 求解	ODE 积分
时间步	可以用大步长	需要小步长(刚性 ODE)
参数需求	只需 $\mu$	需要 $k, d$(难校准)
面接触	需要显式建模	自然支持(Hydroelastic)
控制器适配	控制器也用 LCP 思维	控制器和仿真不匹配
可微性	不可微(需特殊处理)	天然可微(适合 RL)

为什么工业控制器用刚体模型?

Sim-to-Real 一致性: 控制器中的约束(摩擦锥 QP)与刚体接触模型匹配
参数简单: 只需 $\mu$,不需要弹性模量/阻尼比
性能: 不需要小时间步

为什么仿真器(MuJoCo/Isaac)倾向软接触?

数值稳定性: 软接触力是连续函数,ODE 积分更稳定
GPU 并行: 软接触不需要求解全局 QP,适合 GPU 大规模并行
RL 训练: RL 需要可微的仿真器(用于策略梯度),软接触天然可微

🧠 思维陷阱:仿真器选择对控制算法的影响

陷阱: "仿真器无所谓,只要能仿真就行。"

现实: 不同仿真器的接触模型差异会导致 sim-to-real gap。在 MuJoCo 中训练的 RL 策略,如果迁移到 Drake 刚体仿真中验证,行为可能完全不同。2024 年的研究(Humanoid-Gym)发现 MuJoCo 的 dynamics 比 Isaac Gym 更接近真实环境,因此推荐"Isaac Gym 训练 + MuJoCo Sim2Sim 验证"流程。

练习¶

练习 52.8.1 (⭐⭐): 在 MuJoCo 中,调节 solref 参数(接触时间常数)从 0.002 到 0.2。观察:穿透深度如何变化?接触力的振荡如何变化?找到"足够硬但不振荡"的甜蜜点。

练习 52.8.2 (⭐⭐⭐): 如果你要研究"足底压力分布对平衡的影响",应该用刚体接触还是 Hydroelastic?用技术论据说明理由(提示:刚体点接触无法表达压力分布)。

52.9 接触力学在 RL 仿真中的角色 ⭐⭐⭐¶

动机:RL 时代为什么仍需理解接触力学? ⭐⭐¶

本节解决的问题:强化学习(RL)通过试错学习控制策略,似乎不需要显式建模接触力。但仿真器中的接触模型决定了 RL 策略的质量和 sim-to-real 迁移性能。

52.9.1 三大仿真器的接触模型对比 ⭐⭐¶

特性	MuJoCo (3.x)	Drake (Hydroelastic)	Isaac Gym/Sim
接触模型	软接触(凸近似)	Hydroelastic + 刚体	GPU 软接触
求解方法	PGS / Newton	LCP + 压力场积分	并行 PBD/TGS
可微性	MuJoCo MJX (JAX)	部分可微	Warp/Torch
并行度	MJX: TPU ~M FPS	单线程为主	4096 env, ~90k FPS
物理精度	中(可调)	高	低-中
Sim2Real 难度	中	低(高保真)	高(需域随机化)

2024-2025 年的实践趋势:

训练阶段: Isaac Gym (4096 并行环境) --> 策略
验证阶段: MuJoCo (高精度) --> Sim2Sim 验证
部署准备: 域随机化 + 接触参数扫描

为什么不全用 Drake?

Drake 的 Hydroelastic 接触在每步需要 mesh 求交和压力场积分,单环境仿真速度约 100-500 Hz(远低于 RL 需求的 10000+ Hz)。且 GPU 加速有限。

52.9.2 接触参数对 RL 策略的影响 ⭐⭐⭐¶

域随机化(Domain Randomization)中的接触参数:

# Isaac Gym: contact parameter randomization
randomize_config = {
    "friction_range": [0.3, 1.2],      # Friction coefficient
    "restitution_range": [0.0, 0.3],   # Bounce coefficient  
    "contact_stiffness": [1e3, 1e5],   # Contact spring stiffness
    "contact_damping": [10, 1000],     # Contact damping
}

研究发现(2024): - 摩擦系数随机化范围过大 → 策略过于保守,不利用摩擦 - 接触刚度过低 → 策略学会"踩进"地面(利用穿透获得额外支撑) - 最佳实践:先在真实机器人上测量 $\mu$ 范围,再在该范围附近随机化

⚠️ 编程陷阱:RL 策略利用仿真器 bug

症状: 策略在仿真中表现完美,但 sim-to-real 完全失败。

常见原因: 策略学会了利用仿真器的非物理特性,如: - 穿透地面获得额外推力(接触刚度太低) - 在空中仍受到摩擦力(接触检测有延迟) - 利用非对称的接触求解器(PGS 的求解顺序影响结果)

验证方法: 在另一个仿真器(如 MuJoCo -> Drake)中验证策略。如果跨仿真器表现差异大,说明策略可能利用了仿真器特异性。

💡 概念误区:RL 不需要理解物理

错误理解: "RL 策略自动学会物理,不需要工程师理解接触力学。"

正确理解: RL 工程师需要理解接触力学来**设计 reward**、选择仿真器参数、诊断 sim-to-real gap。比如:如果 reward 惩罚足底力太大,策略可能学会用"滑步"代替"踏步"——这需要理解摩擦锥才能诊断。

练习¶

练习 52.9.1 (⭐⭐): 在 MuJoCo 中用同一个策略(如 PPO 训练的四足行走),分别在 $\mu = 0.3$ 和 $\mu = 0.8$ 下测试。策略的步态有何变化?这说明策略是否"理解"了摩擦物理?

练习 52.9.2 (⭐⭐⭐): 设计一个实验:比较 Isaac Gym 和 MuJoCo 中训练的同一个策略在真实机器人上的表现。需要控制哪些变量?预期哪个仿真器训练的策略迁移更好?

52.10 本章小结¶

核心知识点总结表 ⭐¶

知识点	核心公式/概念	工程应用	难度
Signorini 条件	$\phi \ge 0, \lambda^n \ge 0, \phi \cdot \lambda^n = 0$	所有接触约束的基础	⭐⭐
库仑摩擦锥	$\\|\lambda^t\\| \le \mu \lambda^n$ (SOC)	WBC/MPC 的核心约束	⭐⭐
摩擦锥线性化	$k$-边多面体近似,误差 $\sim 1-\cos(\pi/k)$	MIT Cheetah 凸 MPC	⭐⭐
H-rep / V-rep 对偶	锥 = 半空间交 = 生成元非负包(Minkowski-Weyl)	检验力 vs 枚举可达力	⭐⭐⭐
Wrench Cone	6D 力/力矩联合约束	人形机器人面接触	⭐⭐⭐
GIWC + 静平衡判据	$-\boldsymbol{w}_G^{\text{gravity}}\in$ GIWC(双重描述法)	"能否站稳"的几何 LP	⭐⭐⭐
LCP + Delassus 算子	$0\le\lambda^n\perp \boldsymbol{A}\lambda^n+\boldsymbol{w}\ge0$,$\boldsymbol{A}=J_nM^{-1}J_n^T$	刚体接触求解/仿真器	⭐⭐⭐
Fischer-Burmeister	$\Phi(a,b) = \sqrt{a^2+b^2} - a - b = 0$	互补约束光滑化	⭐⭐⭐
Contact-Implicit TO	接触时刻/位置作为决策变量	离线步态发现	⭐⭐⭐⭐
接触 Jacobian 对偶	$\tau_c = J_c^T \lambda_c$	力从接触空间到关节空间	⭐⭐
漂移项	$J_c \ddot{q} + \dot{J}_c \dot{q} = 0$	约束保持(防止足滑)	⭐⭐
CCRBA	O(N) 计算 CMM: $h_G = A_G \dot{q}$	Centroidal MPC/WBC	⭐⭐⭐
角动量守恒	飞行相位 $\dot{k}_G = 0$	跳跃/翻滚规划	⭐⭐
猫翻身悖论	零角动量下通过惯量不对称旋转	理解全身动力学 vs Centroidal	⭐⭐⭐
软 vs 刚体接触	弹簧阻尼 vs LCP	仿真器选择	⭐⭐

三大路线对比总结 ⭐⭐¶

维度	预定义模式	软化互补	Contact-Implicit
实时性	ms 级	秒级	分钟级
适应性	固定步态	需初始猜测	自动发现
工业成熟度	OCS2/legged_control	研究代码	前沿研究
数学难度	低	中	高
代表	MIT Cheetah, ANYmal	Ipopt + NLP	Posa 2014, ci-SCvx

累积项目:四足站立控制器——接触力约束模块¶

项目目标 ⭐⭐¶

在前面章节的"四足站立控制器"基础上,添加**接触力约束模块**,使得 QP 求解器产生物理上合法的接触力。

模块接口 ⭐⭐¶

/**
 * @brief Contact force constraint module for quadruped standing controller
 * 
 * This module adds friction cone and unilateral contact constraints
 * to the WBC QP formulation from 足式/60_QP_NLP建模.
 */
class ContactForceConstraintModule {
public:
  struct Config {
    double mu = 0.6;                // Friction coefficient
    int k = 8;                      // Polygon edges for linearization
    bool use_inner = false;         // Inner approximation (conservative)
    double f_z_min = 10.0;          // Minimum normal force (N)
    double f_z_max = 500.0;         // Maximum normal force (N)
  };

  /**
   * @brief Add friction cone constraints for active contacts
   * @param contact_flags Which feet are in contact (4-element bool vector)
   * @param A_ineq Reference to inequality constraint matrix
   * @param b_ineq Reference to inequality bound vector
   * @param offset Starting row in constraint matrices
   * @return Number of constraints added
   */
  int addFrictionConeConstraints(
      const std::array<bool, 4>& contact_flags,
      Eigen::MatrixXd& A_ineq,
      Eigen::VectorXd& b_ineq,
      int offset);

  /**
   * @brief Zero force constraints for swing legs
   * @return Number of equality constraints added
   */
  int addZeroForceConstraints(
      const std::array<bool, 4>& contact_flags,
      Eigen::MatrixXd& A_eq,
      Eigen::VectorXd& b_eq,
      int offset);

private:
  Config config_;
  Eigen::MatrixXd cone_A_;  // Precomputed cone constraint matrix
};

实现要点 ⭐⭐¶

int ContactForceConstraintModule::addFrictionConeConstraints(
    const std::array<bool, 4>& contact_flags,
    Eigen::MatrixXd& A_ineq,
    Eigen::VectorXd& b_ineq,
    int offset) {

  int n_added = 0;

  for (int leg = 0; leg < 4; ++leg) {
    if (!contact_flags[leg]) continue;  // Skip swing legs

    // Decision variable indices for this leg's contact force
    int col_start = leg * 3;  // lx, ly, lz for this leg

    // Add k friction cone edges
    for (int i = 0; i < config_.k; ++i) {
      double theta = 2.0 * M_PI * i / config_.k;
      double mu_eff = config_.use_inner ? 
          config_.mu * std::cos(M_PI / config_.k) : config_.mu;

      int row = offset + n_added;
      A_ineq(row, col_start)     = std::cos(theta);   // lx
      A_ineq(row, col_start + 1) = std::sin(theta);   // ly
      A_ineq(row, col_start + 2) = -mu_eff;           // -mu*lz
      b_ineq(row) = 0.0;  // cos*lx + sin*ly - mu*lz <= 0
      n_added++;
    }

    // Add normal force bounds: f_z_min <= lz <= f_z_max
    // lz >= f_z_min  -->  -lz <= -f_z_min
    A_ineq(offset + n_added, col_start + 2) = -1.0;
    b_ineq(offset + n_added) = -config_.f_z_min;
    n_added++;

    // lz <= f_z_max
    A_ineq(offset + n_added, col_start + 2) = 1.0;
    b_ineq(offset + n_added) = config_.f_z_max;
    n_added++;
  }

  return n_added;
}

验证方法 ⭐⭐¶

// Test: Standing with symmetric weight distribution
// Expected: each leg bears m*g/4 vertically
// Friction force components should be ~0

void testSymmetricStanding() {
  double m = 12.0;  // Go2 mass (kg)
  double g = 9.81;

  // Solve QP with friction cone constraints
  auto result = solveStandingQP(
      /*contact_flags=*/{true, true, true, true},
      /*mu=*/0.6, /*k=*/8);

  for (int leg = 0; leg < 4; ++leg) {
    double fz = result.forces[leg].z();
    double ft = result.forces[leg].head<2>().norm();

    // Normal force approx mg/4
    EXPECT_NEAR(fz, m * g / 4.0, 1.0);
    // Tangential force approx 0 (symmetric standing)
    EXPECT_NEAR(ft, 0.0, 0.1);
    // Friction cone satisfied
    EXPECT_LE(ft, 0.6 * fz + 1e-6);
  }
}

项目精读指南¶

项目	文件路径	精读重点	难度
OCS2	`ocs2_legged_robot/src/constraint/FrictionConeConstraint.cpp`	摩擦锥约束:SOC 形式,augmented Lagrangian 处理	⭐⭐
OCS2	`ocs2_centroidal_model/src/CentroidalModelInfo.cpp`	Centroidal 模型信息提取	⭐⭐
Pinocchio	`include/pinocchio/algorithm/centroidal.hpp`	CCRBA 实现	⭐⭐⭐
Pinocchio	`include/pinocchio/algorithm/frames.hpp`	接触 Jacobian API	⭐⭐
Crocoddyl	`include/crocoddyl/multibody/actions/contact-fwddyn.hxx`	KKT 系统求解接触动力学	⭐⭐⭐
Crocoddyl	`include/crocoddyl/multibody/friction-cone.hpp`	摩擦锥类实现	⭐⭐
TSID	`include/tsid/contacts/contact-6d.hpp`	6D wrench 接触(人形)	⭐⭐⭐
TSID	`include/tsid/contacts/contact-point.hpp`	3D 点接触(足式)	⭐⭐
Drake	`multibody/plant/contact_model.h`	Hydroelastic 接触模型	⭐⭐⭐⭐

API 速查表¶

本章涉及的核心 API 签名与一句话说明。摩擦锥/wrench cone 多为手写矩阵(见 52.2.3 代码),此处汇总库函数。

Pinocchio——接触 Jacobian 与 Centroidal¶

API	签名要点	作用	关联节
`computeJointJacobians(model, data, q)`	输入 $q$,填充 `data`	计算所有关节 Jacobian(调 `getFrameJacobian` 前必需)	52.4.1
`getFrameJacobian(model, data, frame_id, ref, J)`	`ref` 取 `LOCAL` / `WORLD` / `LOCAL_WORLD_ALIGNED`;`J` 为 $6\times n_v$	取某 frame 的 6D Jacobian	52.4.1/52.4.3
`getFrameJacobianTimeVariation(model, data, frame_id, ref, dJ)`	需先 `computeJointJacobiansTimeVariation`	取 $\dot{J}_c$,用于漂移项 $\dot{J}_c\dot{q}$	52.4.2
`getFrameClassicalAcceleration(model, data, frame_id, ref)`	零关节加速度时返回 $\dot{J}_c\dot{q}$	直接拿 bias acceleration	52.4.2
`ccrba(model, data, q, v)`	填充 `data.Ag`($6\times n_v$ CMM)、`data.hg`、`data.Ig`	O(N) 计算 CMM + 质心动量 + CCRBI	52.5.3
`dccrba(model, data, q, v)`	填充 `data.dAg`	CMM 时间导数,$\dot{h}_G=\dot{A}_G v + A_G a$	52.5.3
`computeCentroidalMomentum(model, data, q, v)`	只返回 `data.hg`	仅要动量、不要 $A_G$ 时更快	52.5.3 陷阱
`forwardDynamics(model, data, q, v, tau, J_c, gamma, damping)`	解 KKT 系统,`gamma`$=-\dot{J}_c\dot{q}$	约束前向动力学(Crocoddyl 内部用)	52.4.2

OCS2 / ProxSuite——约束与求解¶

API	作用	关联节
`FrictionConeConstraint::coneConstraint(F)`	返回单条 SOC 标量 $\mu(F_z+\text{gripper}) - \sqrt{F_x^2+F_y^2+\text{reg}}\ge0$	52.2.3/52.7
`ModeSchedule{eventTimes, modeSequence}`	用位掩码编码每段接触模式(预定义步态)	52.3.1
`proxsuite::proxqp::dense::QP::init(H,g,A_eq,b_eq,A_ineq,...)`	初始化稠密 QP(含 box/等式/不等式)	52.7.2

pycddlib——锥 H-rep ↔ V-rep 转换¶

API	作用	关联节
`cdd.Matrix(rows, number_type)` + `.rep_type = RepType.GENERATOR`	以生成元(V-rep)建锥;首列 $t=0$ 标记射线	52.2.5
`cdd.Polyhedron(mat).get_inequalities()`	双重描述法转出 H-rep($b + Ax\ge0$)	52.2.5

延伸阅读¶

必读经典 ⭐⭐¶

Di Carlo J., Wensing P. M., Katz B., Bledt G., Kim S. (2018) "Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control" — IROS. 凸 MPC 的奠基论文,摩擦锥线性化的工程典范。
Orin D. E., Goswami A., Lee S.-H. (2013) "Centroidal dynamics of a humanoid robot" — Autonomous Robots. CMM 与 CCRBA 算法的完整数学推导。
Stewart D., Trinkle J. (1996) "An implicit time-stepping scheme for rigid body dynamics with inelastic collisions and Coulomb friction" — Int. J. Numer. Meth. Eng. LCP 接触时间积分的经典论文。

Contact-Implicit TO ⭐⭐⭐¶

Posa M., Cantu C., Tedrake R. (2014) "A direct method for trajectory optimization of rigid bodies through contact" — IJRR. CITO 奠基论文。
Manchester Z., Kuindersma S. (2020) "Variational contact-implicit trajectory optimization" — ISRR. CITO 的变分形式。
Pang T. et al. (2023) "Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-Dynamic Contact Models" — T-RO. 最新 CITO for manipulation。
Kurtz V., Castro A. M., Onol A. O., Lin H. (2024) "Inverse Dynamics Trajectory Optimization for Contact-Implicit MPC" — IJRR. 首次实现 100Hz CITO-MPC。
ci-SCvx (2026) "GPU-Accelerated Continuous-Time Successive Convexification for Contact-Implicit Legged Locomotion" — arXiv. GPU 加速 CITO,比 SOTA 快 10x。

Wrench Cone 与多接触稳定性 ⭐⭐⭐¶

Caron S., Pham Q.-C., Nakamura Y. (2015) "Stability of Surface Contacts for Humanoid Robots: Closed-Form Formulae of the Contact Wrench Cone for Rectangular Support Areas" — ICRA. 单矩形面接触 CWC 的解析 $U$ 矩阵(对应 52.2.4 节)。
Caron S., Pham Q.-C., Nakamura Y. (2015) "Leveraging Cone Double Description for Multi-contact Stability of Humanoids with Applications to Statics and Dynamics" — RSS. 用双重描述法从各接触力锥算 GIWC(对应 52.2.5/52.2.6 节)。
Cottle R. W., Pang J.-S., Stone R. E. (1992) The Linear Complementarity Problem — Academic Press. LCP 解的存在性与算法(Lemke)的标准参考(对应 52.3.0 节)。

软接触与仿真 ⭐⭐⭐¶

Masterjohn J. et al. (2022) "Velocity Level Approximation of Pressure Field Contact Patches" — RA-L. Drake Hydroelastic 的理论基础。
Wensing P. M., Orin D. E. (2016) "Improved Computation of the Humanoid Centroidal Dynamics and Application for Whole-Body Control" — 改进的 CCRBA 在 WBC 中的应用。

前沿方向 ⭐⭐⭐⭐¶

ContactBench (Simple Robotics, 2024) — 接触模型与求解器的系统性 benchmark,比较了 PGS/Lemke/proximal 等方法。
Proximal methods for contact (RSS 2024) — 将 proximal point 方法应用于接触逆动力学,扩展 MuJoCo 的方法到刚性和柔性接触。

开放研究问题 ⭐⭐⭐⭐¶

在线摩擦估计: 真实 $\mu$ 随地面类型、湿度、磨损变化。能否用触觉传感器 + 在线估计实时更新 $\mu$?
滑移显式建模: 当前 WBC 假设不滑。在光滑地面上如何把滑移写入控制器?
接触丰富操作 + 运动联合: 双臂操作(多接触)+ 双足行走(切换接触)的统一数学框架?
可微接触仿真: 如何让 LCP/互补约束变得可微,以便端到端优化?Drake 的 time-stepping 方案和 MuJoCo MJX 是两个方向。

可微接触 (Differentiable Contact) 前沿 ⭐⭐⭐⭐¶

动机：为什么需要可微接触？

传统接触模型（LCP / 互补约束）本质上是**不可微的**——接触状态的切换（从接触到离地）是一个离散跳变，梯度在切换点处不存在或为零。这对端到端学习管线（如 RL 策略优化、可微仿真器反向传播）构成了根本性障碍：如果仿真器不可微，就无法通过仿真器反向传播梯度来优化控制策略。

三大可微化路线：

路线	核心思想	代表工作	优缺点
Penalty / 软接触	用连续的弹簧-阻尼模型替代刚性接触	MuJoCo MJX, DiffTaichi	自然可微，但时间步长受刚度限制
隐函数微分	把 LCP/NCP 的解看作参数的隐函数，用 KKT 条件求导	Dojo (2022), ContactNets	精确梯度，但需要求解 KKT 系统的逆
Randomized Smoothing	在接触参数上加噪声后取期望，得到光滑的梯度	Suh et al. (2022), Pang et al. (2023)	无需修改仿真器，但梯度有偏差且方差大

Randomized Smoothing 方法详解：

Randomized Smoothing 的核心洞察：即使单次仿真轨迹不可微（接触开关是阶跃函数），对仿真参数取期望后的目标函数是光滑的。具体地，设目标函数为 $J(\theta)$（$\theta$ 是控制参数），定义平滑版本：

\[\tilde{J}(\theta) = \mathbb{E}_{\epsilon \sim \mathcal{N}(0, \sigma^2 I)}[J(\theta + \epsilon)]\]

$\tilde{J}$ 总是可微的（高斯卷积的平滑性），其梯度可以用蒙特卡罗估计：

\[\nabla_\theta \tilde{J}(\theta) \approx \frac{1}{K} \sum_{k=1}^{K} J(\theta + \epsilon_k) \frac{\epsilon_k}{\sigma^2}\]

这就是"零阶优化 + 高斯平滑"的组合。$\sigma$ 控制平滑程度：$\sigma$ 大则梯度更平滑但偏差大，$\sigma$ 小则更精确但方差大。

工程实践：MuJoCo MJX (2024) 和 Brax (2023) 都支持 JAX 自动微分 + 软接触的端到端可微仿真。Drake 的 SAP (Semi-Analytic Primal) 求解器通过精心设计的接触模型实现了鲁棒的隐函数微分。

本质洞察：可微接触的核心矛盾是**物理精度 vs 数学光滑性**。刚性接触更符合物理现实（地面确实是"硬"的），但数学上不可微。软接触牺牲了物理精度（引入了人为的穿透），换来了数学上的光滑性。Randomized Smoothing 则试图两全——保持刚性接触的物理模型，通过统计平均获得光滑梯度。

本质洞察：接触力学的三大铁律（法向非负、摩擦锥、互补性）不是数学游戏，而是物理现实的忠实编码——地面不能拉住机器人（非负），摩擦力有上限（摩擦锥），脚在空中时没有地面力（互补性）。任何控制器如果违反这些约束，得到的"最优解"在物理上都是不可执行的。这就是为什么**每一个**足式控制器都必须包含这些约束，无一例外。

🔧 故障排查手册¶

#	现象	可能原因	排查方法
1	摩擦锥未激活：QP 求解的接触力 $\lambda$ 全为零，机器人自由落体	接触调度与实际步态不一致（所有脚标记为 swing），或接触 Jacobian $J_c$ 因 frame 选择错误导致全零	打印接触调度表，确认当前相位有着地脚；检查 `getFrameJacobian()` 的 frame 参数是否为 `LOCAL_WORLD_ALIGNED`
2	互补约束 QP 不可行：OSQP 返回 `PRIMAL_INFEASIBLE`	互补约束的松弛参数 $\sigma$ 过小导致约束集为空集；或同时强制 $\phi > 0$（离地）和 $\lambda_n > 0$（有力）矛盾	逐步放大 $\sigma$ 直到可行；检查接触模式是否自洽（离地脚不应有力约束，着地脚不应有离地约束）
3	接触 Jacobian 坐标系错误：力矩输出方向反转或量级异常	$J_c$ 使用了 `LOCAL` frame 但力是在世界系下表达的，导致旋转矩阵缺失；或混用了不同 frame 的 Jacobian	用功率一致性测试验证：$P = f^T \cdot J_c \dot{q}$ 应等于 $\tau^T \dot{q}$，若不等则 frame 不匹配
4	摩擦锥线性化精度不足：机器人在小摩擦系数地面上侧滑	$k=4$ 棱的线性化摩擦锥最大误差为 $1 - \cos(45°) \approx 29\%$，对低 $\mu$ 场景过于保守	增大棱数至 $k=8$（误差降至 7.6%）或 $k=16$；或使用 SOCP 求解器处理原始二阶锥约束
5	接触力法向分量为负：QP 输出的 $\lambda_z < 0$	QP 忘记添加 $\lambda_z \geq 0$ 的不等式约束；或 $z$ 轴方向定义与接触法向不一致（非平坦地形）	显式添加 $\lambda_z \geq 0$ 约束；打印接触面法向 $\mathbf{n}$ 确认与重力方向一致
6	软接触模型中穿透量过大（$> 5$ mm）	接触刚度 $K_n$ 过小或仿真步长过大导致数值积分误差累积	增大 $K_n$（同时减小步长以保持数值稳定）；或切换到 Drake Hydroelastic 模型
7	仿真中接触力高频振荡	接触模型刚度过高导致数值刚性（stiff system），显式积分器无法处理	使用隐式或半隐式积分器；添加接触阻尼 $D_n$；降低接触刚度并增大步长

跨章综合练习 ⭐⭐⭐¶

综合题：接触约束在 MPC 和 WBC 两层中的不同编码

本题需要综合足式/70_腿足简化模型理论（SRBD MPC）、本章（摩擦锥与互补约束）和足式/90_WBC分层优化与TSID（WBC）三章知识。

MPC 层的接触约束（足式/70_腿足简化模型理论 + 本章）：在 SRBD MPC 中，接触力 $\mathbf{f}_i \in \mathbb{R}^3$ 是决策变量，摩擦锥 $C_\mu \mathbf{f}_i \leq 0$ 是不等式约束。写出 trot 步态下（对角脚同时着地），MPC 的接触约束矩阵维度（多少行、多少列）。说明 swing 脚的接触力如何处理（提示：强制为零）。
WBC 层的接触约束（足式/90_WBC分层优化与TSID + 本章）：在 WBC 中，接触力 $\lambda_i \in \mathbb{R}^3$ 也是决策变量，但 WBC 还有接触约束 $J_c \ddot{q} + \dot{J}_c \dot{q} = 0$（接触点加速度为零）。解释为什么 MPC 不需要这个约束（提示：MPC 用简化模型，没有关节加速度 $\ddot{q}$），而 WBC 必须有。
一致性问题：MPC 输出的期望接触力 $\mathbf{f}^*_{\text{MPC}}$ 和 WBC 实际求解的接触力 $\lambda^*_{\text{WBC}}$ 通常不完全相同。为什么？设计一个 WBC 代价项使 $\lambda^*_{\text{WBC}}$ 尽量接近 $\mathbf{f}^*_{\text{MPC}}$，并讨论该项的权重应该如何设置。

与其他章节的衔接¶

向前承接: - 足式/50_空间向量与浮动基座动力学空间向量代数 → 52.4 接触 Jacobian 的空间力变换 - 足式/60_QP_NLP建模 QP/NLP 建模 → 52.2 摩擦锥变成 QP 线性不等式 - 足式/70_腿足简化模型理论 LIPM/Centroidal → 52.5 CMM 是 Centroidal 动力学的计算基础

向后指向: - 52.2 摩擦锥约束 → 足式/90_WBC分层优化与TSID WBC 的关键不等式约束 - 52.3 互补约束 → 足式/100_DDP家族与Crocoddyl DDP 接触模型(Crocoddyl contact-fwddyn) - 52.4 接触 Jacobian → 足式/110_OCS2完整栈与双线程MPC OCS2 FrictionConeConstraint 的实现基础 - 52.5 CCRBA → 足式/110_OCS2完整栈与双线程MPC OCS2 Centroidal MPC 的在线计算 - 52.6 角动量 → 足式/90_WBC分层优化与TSID WBC 角动量跟踪任务 - 52.9 RL 仿真 → 足式/190_腿足RL训练栈腿足 RL 训练栈

预计学习时间¶

1 周(20-25 小时):

模块	时间	重点
52.1-52.2 接触约束 + 摩擦锥	5-6 h	推导 + 代码实现
52.3 互补约束三路线	4-5 h	Fischer-Burmeister + CITO 概念
52.4-52.5 Jacobian + CCRBA	4-5 h	Pinocchio API + 数值验证
52.6 角动量	2-3 h	物理直觉 + 猫翻身
52.7-52.9 工程对比	3-4 h	仿真器选择 + RL 接触
练习 + 源码阅读	4-5 h	OCS2 + Crocoddyl 精读

#	现象	可能原因	排查方法
1	摩擦锥未激活：QP 求解的接触力 \(\lambda\) 全为零，机器人自由落体	接触调度与实际步态不一致（所有脚标记为 swing），或接触 Jacobian \(J_c\) 因 frame 选择错误导致全零	打印接触调度表，确认当前相位有着地脚；检查 `getFrameJacobian()` 的 frame 参数是否为 `LOCAL_WORLD_ALIGNED`
2	互补约束 QP 不可行：OSQP 返回 `PRIMAL_INFEASIBLE`	互补约束的松弛参数 \(\sigma\) 过小导致约束集为空集；或同时强制 \(\phi > 0\)（离地）和 \(\lambda_n > 0\)（有力）矛盾	逐步放大 \(\sigma\) 直到可行；检查接触模式是否自洽（离地脚不应有力约束，着地脚不应有离地约束）
3	接触 Jacobian 坐标系错误：力矩输出方向反转或量级异常	\(J_c\) 使用了 `LOCAL` frame 但力是在世界系下表达的，导致旋转矩阵缺失；或混用了不同 frame 的 Jacobian	用功率一致性测试验证：\(P = f^T \cdot J_c \dot{q}\) 应等于 \(\tau^T \dot{q}\)，若不等则 frame 不匹配
4	摩擦锥线性化精度不足：机器人在小摩擦系数地面上侧滑	\(k=4\) 棱的线性化摩擦锥最大误差为 \(1 - \cos(45°) \approx 29\%\)，对低 \(\mu\) 场景过于保守	增大棱数至 \(k=8\)（误差降至 7.6%）或 \(k=16\)；或使用 SOCP 求解器处理原始二阶锥约束
5	接触力法向分量为负：QP 输出的 \(\lambda_z < 0\)	QP 忘记添加 \(\lambda_z \geq 0\) 的不等式约束；或 \(z\) 轴方向定义与接触法向不一致（非平坦地形）	显式添加 \(\lambda_z \geq 0\) 约束；打印接触面法向 \(\mathbf{n}\) 确认与重力方向一致
6	软接触模型中穿透量过大（\(> 5\) mm）	接触刚度 \(K_n\) 过小或仿真步长过大导致数值积分误差累积	增大 \(K_n\)（同时减小步长以保持数值稳定）；或切换到 Drake Hydroelastic 模型
7	仿真中接触力高频振荡	接触模型刚度过高导致数值刚性（stiff system），显式积分器无法处理	使用隐式或半隐式积分器；添加接触阻尼 \(D_n\)；降低接触刚度并增大步长

年份	人物	贡献
1785	Coulomb	提出库仑摩擦定律 \(f_t \le \mu f_n\)
1933	Signorini	形式化单边接触问题(Signorini problem)
1963	Fichera	证明 Signorini 问题解的存在唯一性
1988	Moreau	非光滑力学框架,互补约束的现代形式
2013	Orin et al.	Centroidal 动力学与 CCRBA 算法

条件	含义	直觉
\(\phi \ge 0\)	不穿透	脚不能穿过地面
\(\lambda_c^n \ge 0\)	法向力非负	地面只推不拉
\(\phi \cdot \lambda_c^n = 0\)	互补性	离地时无力,有力时在地面上

路线	输入表示	算法	适用
闭式公式	单个矩形接触	Caron et al. 2015 (ICRA) 给出 CWC 的解析 \(U\) 矩阵	单面接触(人形单脚支撑)
双重描述法	多接触力锥的 V-rep	Caron et al. 2015 (RSS) 用 double description 求 Minkowski 和后转回 H-rep	多接触(双脚/多肢)

约束类型	求解器	求解时间	最大摩擦违反	力分配均匀度	约束条数
线性锥 \(k=4\)	OSQP	0.3 ms	7.6% 可能超出	95.2%	4x4=16
线性锥 \(k=4\) (内切)	OSQP	0.3 ms	0% (保守)	93.1%	16
线性锥 \(k=8\)	OSQP	0.5 ms	1.1%	97.8%	4x8=32
线性锥 \(k=16\)	OSQP	0.8 ms	0.15%	99.2%	4x16=64
精确 SOC	Mosek	2.5 ms	0%	100%	4 (SOC)
精确 SOC	ECOS	3.2 ms	0%	100%	4 (SOC)
精确 SOC	ProxQP	1.8 ms	0%	99.9%	4 (SOC)

知识点	核心公式/概念	工程应用	难度
Signorini 条件	\(\phi \ge 0, \lambda^n \ge 0, \phi \cdot \lambda^n = 0\)	所有接触约束的基础	⭐⭐
库仑摩擦锥	\(\\|\lambda^t\\| \le \mu \lambda^n\) (SOC)	WBC/MPC 的核心约束	⭐⭐
摩擦锥线性化	\(k\)-边多面体近似,误差 \(\sim 1-\cos(\pi/k)\)	MIT Cheetah 凸 MPC	⭐⭐
H-rep / V-rep 对偶	锥 = 半空间交 = 生成元非负包(Minkowski-Weyl)	检验力 vs 枚举可达力	⭐⭐⭐
Wrench Cone	6D 力/力矩联合约束	人形机器人面接触	⭐⭐⭐
GIWC + 静平衡判据	\(-\boldsymbol{w}_G^{\text{gravity}}\in\) GIWC(双重描述法)	"能否站稳"的几何 LP	⭐⭐⭐
LCP + Delassus 算子	\(0\le\lambda^n\perp \boldsymbol{A}\lambda^n+\boldsymbol{w}\ge0\),\(\boldsymbol{A}=J_nM^{-1}J_n^T\)	刚体接触求解/仿真器	⭐⭐⭐
Fischer-Burmeister	\(\Phi(a,b) = \sqrt{a^2+b^2} - a - b = 0\)	互补约束光滑化	⭐⭐⭐
Contact-Implicit TO	接触时刻/位置作为决策变量	离线步态发现	⭐⭐⭐⭐
接触 Jacobian 对偶	\(\tau_c = J_c^T \lambda_c\)	力从接触空间到关节空间	⭐⭐
漂移项	\(J_c \ddot{q} + \dot{J}_c \dot{q} = 0\)	约束保持(防止足滑)	⭐⭐
CCRBA	O(N) 计算 CMM: \(h_G = A_G \dot{q}\)	Centroidal MPC/WBC	⭐⭐⭐
角动量守恒	飞行相位 \(\dot{k}_G = 0\)	跳跃/翻滚规划	⭐⭐
猫翻身悖论	零角动量下通过惯量不对称旋转	理解全身动力学 vs Centroidal	⭐⭐⭐
软 vs 刚体接触	弹簧阻尼 vs LCP	仿真器选择	⭐⭐

API	签名要点	作用	关联节
`computeJointJacobians(model, data, q)`	输入 \(q\),填充 `data`	计算所有关节 Jacobian(调 `getFrameJacobian` 前必需)	52.4.1
`getFrameJacobian(model, data, frame_id, ref, J)`	`ref` 取 `LOCAL` / `WORLD` / `LOCAL_WORLD_ALIGNED`;`J` 为 \(6\times n_v\)	取某 frame 的 6D Jacobian	52.4.1/52.4.3
`getFrameJacobianTimeVariation(model, data, frame_id, ref, dJ)`	需先 `computeJointJacobiansTimeVariation`	取 \(\dot{J}_c\),用于漂移项 \(\dot{J}_c\dot{q}\)	52.4.2
`getFrameClassicalAcceleration(model, data, frame_id, ref)`	零关节加速度时返回 \(\dot{J}_c\dot{q}\)	直接拿 bias acceleration	52.4.2
`ccrba(model, data, q, v)`	填充 `data.Ag`(\(6\times n_v\) CMM)、`data.hg`、`data.Ig`	O(N) 计算 CMM + 质心动量 + CCRBI	52.5.3
`dccrba(model, data, q, v)`	填充 `data.dAg`	CMM 时间导数,\(\dot{h}_G=\dot{A}_G v + A_G a\)	52.5.3
`computeCentroidalMomentum(model, data, q, v)`	只返回 `data.hg`	仅要动量、不要 \(A_G\) 时更快	52.5.3 陷阱
`forwardDynamics(model, data, q, v, tau, J_c, gamma, damping)`	解 KKT 系统,`gamma`\(=-\dot{J}_c\dot{q}\)	约束前向动力学(Crocoddyl 内部用)	52.4.2

API	作用	关联节
`FrictionConeConstraint::coneConstraint(F)`	返回单条 SOC 标量 \(\mu(F_z+\text{gripper}) - \sqrt{F_x^2+F_y^2+\text{reg}}\ge0\)	52.2.3/52.7
`ModeSchedule{eventTimes, modeSequence}`	用位掩码编码每段接触模式(预定义步态)	52.3.1
`proxsuite::proxqp::dense::QP::init(H,g,A_eq,b_eq,A_ineq,...)`	初始化稠密 QP(含 box/等式/不等式)	52.7.2

API	作用	关联节
`cdd.Matrix(rows, number_type)` + `.rep_type = RepType.GENERATOR`	以生成元(V-rep)建锥;首列 \(t=0\) 标记射线	52.2.5
`cdd.Polyhedron(mat).get_inequalities()`	双重描述法转出 H-rep(\(b + Ax\ge0\))	52.2.5

第 52 章 接触力学与约束优化——摩擦锥 / 接触 Jacobian / Centroidal 深入 / 互补约束¶