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

外感知融合¶

难度：⭐⭐~⭐⭐⭐⭐ | 前置：Ch23（李群与 manif）、Ch47（Pinocchio）、Ch49（空间向量代数与浮动基座动力学）、Ch52（接触力学）

核心参考文献： - Bloesch M. et al. (2012) "State Estimation for Legged Robots - Consistent Fusion of Leg Kinematics and IMU", RSS 2012 - Hartley R. et al. (2020) "Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation", IJRR - Camurri M. et al. (2020) "Pronto: A Multi-Sensor State Estimator for Legged Robots in Real-World Scenarios", Frontiers in Robotics and AI - Wisth D. et al. (2023) "VILENS: Visual, Inertial, Lidar, and Leg Odometry for All-Terrain Legged Robots", IEEE T-RO

建议学时：2 周（25--30 小时）

前置自测¶

📋 前置自测（答不出 $\geq 2$ 题 → 先回对应章节复习）

[Ch23] 什么是 $SE(3)$ 的李代数 $\mathfrak{se}(3)$？Exp 映射和 Log 映射分别做什么？
[Ch47] 在 Pinocchio 中，如何计算某个 frame 的正运动学位置和 Jacobian？getFrameJacobian() 返回矩阵的维度是什么？
[Ch49] 浮动基座动力学方程 $M(q)\ddot{q} + h(q,\dot{q}) = S^T\tau + J_c^T\lambda$ 中，$S$ 矩阵的作用是什么？为什么基座的 6 行没有 $\tau$？
[Ch52] 接触力的三大铁律是什么？互补性条件 $\phi \cdot \lambda^n = 0$ 的物理含义？
[Kalman 滤波基础] 标准 Kalman 滤波的预测步和更新步分别做什么？Kalman 增益 $K$ 的物理含义？

如果第 5 题答不出——本章核心内容依赖 Kalman 滤波。请先学习 Kalman 滤波基础（可参考 v8 Ch39 的 ESKF 内容）。

本章目标¶

学完本章后，你应该能够：

**解释**腿足机器人状态估计的核心挑战——浮动基座没有直接位置测量，必须融合 IMU 和腿部运动学
手写 IMU 运动学方程，理解陀螺仪偏差和加速度计重力耦合问题
推导 MIT Cheetah 风格的线性 KF 状态估计器，理解状态向量 $[\boldsymbol{p}, \boldsymbol{v}, \boldsymbol{p}_{f_1}, \ldots, \boldsymbol{p}_{f_4}]$ 的设计思路
理解 Invariant EKF（InEKF）的理论基础——为什么把状态嵌入 $SE_k(3)$ 群可以获得自治误差动力学
**实现**接触状态检测——从简单 GRF 阈值到 Schmitt 触发器到概率接触估计
**掌握**滑移检测的基本方法——基于速度残差的滑移判别
**理解**外感知（LiDAR/Camera）融合如何修正纯本体感受的长期漂移

57.1 状态估计在腿足控制中的地位 ⭐¶

动机：控制器需要知道"我在哪"¶

本节解决的问题：为什么腿足机器人需要状态估计器？它需要估计哪些状态量？为什么这个问题比轮式机器人和固定基座机械臂更难？

考虑一个最基本的控制场景：你想让 Go2 四足机器人向前走 1 米。控制器（无论是 MPC 还是 WBC）需要知道：

机器人**现在在哪**——基座位置 $\boldsymbol{p} \in \mathbb{R}^3$
机器人**朝哪个方向**——基座姿态 $\boldsymbol{R} \in SO(3)$
机器人**正在以多快的速度移动**——基座线速度 $\boldsymbol{v} \in \mathbb{R}^3$ 和角速度 $\boldsymbol{\omega} \in \mathbb{R}^3$

没有这些信息，控制器就是"闭着眼睛走路"。MPC 无法预测未来轨迹（Ch55），WBC 无法计算正确的关节扭矩（Ch53）。

如果不做状态估计会怎样¶

场景 1：直接用 IMU 积分

最简单的想法：IMU 给出加速度 $\boldsymbol{a}$ 和角速度 $\boldsymbol{\omega}$，直接积分得到速度和位置。

\[\boldsymbol{v}(t) = \boldsymbol{v}(0) + \int_0^t \boldsymbol{a}(\tau)\,d\tau, \quad \boldsymbol{p}(t) = \boldsymbol{p}(0) + \int_0^t \boldsymbol{v}(\tau)\,d\tau\]

问题：加速度计有偏差（bias），假设偏差为 $0.01\,\mathrm{m/s^2}$（这是典型的 MEMS IMU 偏差量级）：

积分时间	速度误差	位置误差
1 秒	0.01 m/s	0.005 m
10 秒	0.1 m/s	0.5 m
60 秒	0.6 m/s	18 m

位置误差以 $t^2$ 增长——仅 1 分钟就漂移 18 米！这对于需要精确到厘米级的足式控制完全不可接受。

场景 2：只用关节编码器

关节编码器告诉你"每个关节转了多少度"，但**不告诉你基座在世界系中的位置和姿态**。对于固定基座机械臂，基座位姿是已知常量。但对于浮动基座的腿足机器人，基座是"自由漂浮"的——12 个关节角配合 6 个基座自由度共 18 个广义速度（Ch49），编码器只提供其中 12 个。

场景 3：用 GPS

GPS 可以给出全局位置——但典型的民用 GPS 精度只有 1--3 米（RTK-GPS 可达厘米级但需要基站），而且在室内、地下、森林等环境完全不可用。腿足机器人的核心应用场景恰恰是这些 GPS 受限环境。

腿足状态估计的完整状态向量¶

一个典型的腿足状态估计器需要估计以下状态量：

状态量	符号	维度	来源	难度
基座姿态	$\boldsymbol{R} \in SO(3)$	3（李代数）	IMU 陀螺仪积分 + 加速度计校正	中
基座位置	$\boldsymbol{p} \in \mathbb{R}^3$	3	腿部运动学 + IMU 双积分	高
基座线速度	$\boldsymbol{v} \in \mathbb{R}^3$	3	腿部运动学 + IMU 单积分	中
基座角速度	$\boldsymbol{\omega} \in \mathbb{R}^3$	3	IMU 陀螺仪直读	低
足端位置	$\boldsymbol{p}_{f_i} \in \mathbb{R}^3$	$3 \times N_{\text{feet}}$	正运动学 + 接触约束	中
IMU 偏差	$\boldsymbol{b}_g, \boldsymbol{b}_a$	6	在线估计	高

**角速度**通常不需要估计——IMU 陀螺仪的直读值（去偏差后）已经足够精确，因为角速度不需要积分就可以使用。这与位置和速度的情况不同：位置需要对加速度双重积分，误差累积极快。

腿足状态估计 vs SLAM 的对比¶

如果你有 SLAM（同步定位与地图构建）背景，会发现腿足状态估计和 SLAM 使用**完全相同的数学框架**（Kalman 滤波 / 因子图），但**状态定义和观测模型完全不同**：

维度	SLAM（LIO/VIO）	腿足状态估计
估计什么	基座位姿 + 地图特征点	基座位姿 + 速度 + 足端位置
主要观测	LiDAR 点云 / 相机图像	关节编码器 + IMU + 接触力
特有观测	点云配准残差、重投影残差	腿式里程计（Leg Odometry）
不确定性来源	数据关联错误（误匹配）	齿轮背隙、脚底滑动
观测频率	LiDAR 10 Hz / 相机 30 Hz	IMU 400--1000 Hz / 编码器 1 kHz
漂移特性	长距离累积，依赖回环	接触瞬间修正，短期低漂移
计算平台	通常有 GPU	通常只有实时 CPU

关键启示：腿足状态估计和 SLAM 是**互补的**——腿足估计提供高频精确的 base pose（毫秒级延迟），SLAM 提供长距离无漂移的全局位姿（百毫秒级延迟）。这好比人走路时的感知系统:本体感觉(肌肉、关节感受器)告诉你"脚刚踩到了地面、膝盖弯了多少度",反应极快但无法判断你在城市中的绝对位置;而视觉系统告诉你"我在第三街区的路口",但处理速度慢且需要识别地标。腿足估计对应本体感觉,SLAM 对应视觉定位,两者融合才能既快又准。工业实践正是两者融合：如 ANYmal 的 Pronto（EKF 融合 leg odometry + LiDAR）、VILENS（因子图融合 Visual + Inertial + LiDAR + Leg）。

传感器概览¶

一个典型的四足机器人（如 Unitree Go2）的传感器配置：

传感器	测量量	频率	精度	用于
IMU（加速度计）	比力 $\boldsymbol{a}_m$（加速度 + 重力）	400--1000 Hz	偏差 ~0.01 m/s$^2$	速度/位置预测
IMU（陀螺仪）	角速度 $\boldsymbol{\omega}_m$	400--1000 Hz	偏差 ~0.01 rad/s	姿态积分
关节编码器	关节角度 $q_i$	1 kHz	~0.01$^\circ$	正运动学
关节电流传感器	关节扭矩 $\tau_i$（通过电流推算）	1 kHz	~5%	接触力估计
足端力传感器（可选）	法向接触力 $F_n$	1 kHz	~1 N	接触检测
LiDAR / 相机（可选）	点云 / 图像	10--30 Hz	厘米级	漂移修正

⚠️ 概念误区：IMU 加速度计测量的是加速度

新手想法："加速度计测量加速度，所以静止时读数为零。"

实际上：加速度计测量的是**比力**（specific force），即加速度减去重力：$\boldsymbol{a}_m = \boldsymbol{a} - \boldsymbol{g}$。当 IMU 静止时，$\boldsymbol{a} = 0$，读数为 $\boldsymbol{a}_m = -\boldsymbol{g}$，即指向"上方"的 $9.81\,\mathrm{m/s^2}$。这不是 bug——爱因斯坦的等效原理告诉我们，加速度计无法区分重力和加速度。

为什么重要：在状态估计器中，必须从加速度计读数中**减去重力**才能得到真实加速度。而减去重力需要**知道当前姿态**（因为重力在 body frame 中的投影取决于机器人朝向）——这形成了姿态估计和加速度计校正的**耦合**。

🧠 思维陷阱：认为"只用 IMU 就够了"

陷阱："IMU 有加速度和角速度，积分就能得到位置和姿态，不需要其他传感器。"

实际上：如上表所示，加速度计偏差仅 $0.01\,\mathrm{m/s^2}$ 就会在 1 分钟内导致 18 米的位置漂移。陀螺仪偏差 $0.01\,\mathrm{rad/s}$ 会在 1 分钟内导致约 $34°$ 的姿态漂移。纯 IMU 积分只能在**极短时间**内（<1 秒）提供可靠估计。

正确思维：IMU 是**高频预测源**，但必须与其他传感器（腿部运动学、LiDAR、Camera）融合来校正其累积漂移。

历史演进¶

年代	方法	代表工作	关键突破
2008	互补滤波	早期四足	简单有效的姿态估计
2012	EKF + Leg Odometry	Bloesch et al. (RSS 2012)	首次严格融合 IMU + 腿运动学
2018	Linear KF	Di Carlo / MIT Cheetah	极简高效的线性 KF，工业广泛采用
2020	InEKF	Hartley et al. (IJRR 2020)	利用李群结构获得自治误差动力学
2020	Pronto	Camurri et al.	多传感器模块化 EKF，4 平台验证
2023	VILENS	Wisth et al. (T-RO 2023)	因子图紧耦合 Visual + Inertial + LiDAR + Leg
2024--2025	神经增强滤波	多个团队	CNN/Transformer 学习接触检测和滑移补偿

练习¶

练习 57.1.1：计算以下场景下纯 IMU 积分的位置漂移。假设 IMU 加速度计偏差为 $b_a = 0.02\,\mathrm{m/s^2}$，陀螺仪偏差为 $b_g = 0.005\,\mathrm{rad/s}$： (a) 30 秒后的位置误差（假设偏差恒定） (b) 30 秒后的姿态误差（度） (c) 讨论：为什么位置漂移比姿态漂移严重得多？

练习 57.1.2：比较腿足状态估计与车载 VIO（如手机上的 ARKit）的异同。列出至少 3 个共同点和 3 个差异。

57.2 IMU 运动学与偏差模型 ⭐⭐¶

动机：IMU 是状态估计的"引擎"¶

本节解决的问题：IMU 陀螺仪和加速度计的测量模型是什么？偏差和噪声如何影响积分？预积分（preintegration）概念如何在 KF 框架中避免重复计算？

在腿足状态估计中，IMU 扮演的角色是**高频预测源**——它以 400--1000 Hz 的频率提供加速度和角速度测量，驱动 Kalman 滤波的预测步。而腿部运动学（频率通常也是 1 kHz 但信息含量不同）提供**观测更新**。理解 IMU 的测量模型是正确构建预测方程的前提。

IMU 测量模型¶

陀螺仪测量模型：

\[\boldsymbol{\omega}_m = \boldsymbol{\omega} + \boldsymbol{b}_g + \boldsymbol{n}_g\]

其中： - $\boldsymbol{\omega}_m \in \mathbb{R}^3$：陀螺仪测量值（body frame 中的角速度） - $\boldsymbol{\omega}$：真实角速度 - $\boldsymbol{b}_g \in \mathbb{R}^3$：陀螺仪偏差（缓慢变化，时间常数约数十秒到数分钟） - $\boldsymbol{n}_g \sim \mathcal{N}(0, \sigma_g^2 I)$：高斯白噪声

加速度计测量模型：

\[\boldsymbol{a}_m = \boldsymbol{R}^T(\boldsymbol{a} - \boldsymbol{g}) + \boldsymbol{b}_a + \boldsymbol{n}_a\]

其中： - $\boldsymbol{a}_m \in \mathbb{R}^3$：加速度计测量值（body frame 中的比力） - $\boldsymbol{R} \in SO(3)$：body frame 到 world frame 的旋转 - $\boldsymbol{a}$：world frame 中的真实加速度 - $\boldsymbol{g} = [0, 0, -9.81]^T$：world frame 中的重力向量 - $\boldsymbol{b}_a \in \mathbb{R}^3$：加速度计偏差 - $\boldsymbol{n}_a \sim \mathcal{N}(0, \sigma_a^2 I)$：高斯白噪声

这里有一个关键的物理细节：加速度计测量的是**比力**（specific force），不是加速度。比力是"加速度减去重力"在 body frame 中的投影。因此，要从加速度计读数中恢复 world frame 的加速度：

\[\boldsymbol{a} = \boldsymbol{R}(\boldsymbol{a}_m - \boldsymbol{b}_a - \boldsymbol{n}_a) + \boldsymbol{g}\]

这个公式揭示了一个深刻的耦合：要从加速度计得到正确的加速度，必须先知道姿态 $\boldsymbol{R}$。而姿态是通过陀螺仪积分得到的，陀螺仪积分又有漂移——加速度计的静态分量（重力）恰好可以用来**校正姿态漂移**。这就是互补滤波和 Kalman 滤波的物理基础。

为什么加速度计可以校正姿态？ 当机器人静止或匀速运动时（加速度为零），加速度计的测量方向就是重力在 body frame 中的投影方向。通过比较测量方向与 world frame 中已知的重力方向 $[0, 0, -9.81]^T$，可以反算出 roll 和 pitch 角。Yaw 角无法通过加速度计校正——因为绕重力轴旋转不改变重力的投影方向。这也是为什么大多数腿足估计器中 yaw 的漂移最难控制。

偏差模型¶

IMU 偏差不是常数——它会随温度、振动、时间缓慢变化。在 Kalman 滤波中，偏差通常建模为**随机游走**（Brownian motion / random walk）：

\[\dot{\boldsymbol{b}}_g = \boldsymbol{n}_{bg}, \quad \dot{\boldsymbol{b}}_a = \boldsymbol{n}_{ba}\]

其中 $\boldsymbol{n}_{bg} \sim \mathcal{N}(0, \sigma_{bg}^2 I)$，$\boldsymbol{n}_{ba} \sim \mathcal{N}(0, \sigma_{ba}^2 I)$ 是驱动偏差缓慢变化的噪声。

为什么是随机游走？ 这不是物理精确的模型——实际的 IMU 偏差受温度影响有更复杂的动态。但随机游走是一个**简单且有效**的近似：

它允许 Kalman 滤波器**在线估计偏差**——偏差作为状态的一部分被估计和修正
随机游走参数 $\sigma_{bg}$, $\sigma_{ba}$ 控制"偏差能变化多快"——如果设太大，滤波器会过度追踪噪声；如果设太小，偏差估计跟不上实际变化
离散化后：$\boldsymbol{b}_{g,k+1} = \boldsymbol{b}_{g,k} + \boldsymbol{n}_{bg,k} \sqrt{\Delta t}$

如果不估计偏差会怎样？ 假设使用出厂标定值作为固定偏差补偿。问题是：偏差在不同温度下可能变化 10--100 倍（例如从室内 25$^\circ$C 到户外 -10$^\circ$C），出厂标定值在这些条件下完全无效。在线估计偏差是现代 IMU 状态估计器的**标配**。

历史背景：偏差建模为随机游走最早来自 1960 年代的惯性导航系统（INS）研究。Kalman 本人在 1960 年的经典论文中就讨论了如何估计缓慢变化的系统参数。1990 年代，这种方法被引入到 GPS/INS 组合导航中，2000 年代后被 SLAM 和机器人领域广泛采用。

典型 IMU 参数（以 Unitree Go2 内置 IMU 为例）：

参数	符号	典型值	单位
陀螺仪噪声密度	$\sigma_g$	$0.004$	rad/s/$\sqrt{\text{Hz}}$
加速度计噪声密度	$\sigma_a$	$0.04$	m/s$^2/\sqrt{\text{Hz}}$
陀螺仪偏差不稳定性	$\sigma_{bg}$	$5 \times 10^{-5}$	rad/s$^2/\sqrt{\text{Hz}}$
加速度计偏差不稳定性	$\sigma_{ba}$	$5 \times 10^{-4}$	m/s$^3/\sqrt{\text{Hz}}$

姿态的陀螺仪积分¶

已知陀螺仪测量 $\boldsymbol{\omega}_m$，姿态的连续时间微分方程为：

\[\dot{\boldsymbol{R}} = \boldsymbol{R} [\boldsymbol{\omega}_m - \boldsymbol{b}_g - \boldsymbol{n}_g]_\times\]

其中 $[\cdot]_\times$ 是反对称矩阵（Ch49）。

离散化：在时间步长 $\Delta t$ 内，假设角速度近似恒定：

\[\boldsymbol{R}_{k+1} = \boldsymbol{R}_k \cdot \text{Exp}\big((\boldsymbol{\omega}_{m,k} - \boldsymbol{b}_{g,k}) \cdot \Delta t\big)\]

这里 $\text{Exp}: \mathbb{R}^3 \to SO(3)$ 是 Ch23 中学过的指数映射。对于小角度（$\|\boldsymbol{\omega}\| \cdot \Delta t \ll 1$），$\text{Exp}(\boldsymbol{\theta}) \approx I + [\boldsymbol{\theta}]_\times$，但在实际实现中应使用精确的 Rodrigues 公式以保证数值稳定性。

为什么不用四元数积分？ 四元数积分也很常见：$\boldsymbol{q}_{k+1} = \boldsymbol{q}_k \otimes \boldsymbol{q}(\boldsymbol{\omega}_k \Delta t)$。两种方法数学上等价，但旋转矩阵形式在理论推导中更直观（特别是 InEKF），而四元数在实际代码中内存更紧凑（4 个 float vs 9 个 float）。Pinocchio 和大多数现代代码库同时支持两者。

预积分概念¶

预积分是什么？ 在因子图优化或 Kalman 滤波中，当 IMU 偏差估计发生变化时，理论上需要重新积分所有 IMU 测量。预积分（Forster et al., RSS 2015 / TRO 2017）通过将 IMU 积分**与偏差解耦**，避免重复计算。

核心思想：将两个关键帧之间的 IMU 测量"预先积分"成一个**相对运动增量**（$\Delta R_{ij}$, $\Delta v_{ij}$, $\Delta p_{ij}$），这些增量以**零偏差**计算。当偏差估计更新时，通过一阶 Taylor 修正项快速更新预积分结果：

\[\Delta \tilde{R}_{ij}(b_g) \approx \Delta \tilde{R}_{ij}(b_g^0) \cdot \text{Exp}\left(\frac{\partial \Delta R_{ij}}{\partial b_g} \delta b_g\right)\]

在腿足状态估计中的地位：对于 Kalman 滤波框架（线性 KF、InEKF），IMU 预积分通常不需要——因为 KF 每步都直接用 IMU 测量做预测，不存在"重新积分"的问题。预积分主要用于**因子图框架**（如 VILENS）和**固定滞后平滑器**。但理解预积分对于理解 SLAM 与腿足估计的融合架构至关重要。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：忘记去除重力分量

错误做法：直接把加速度计读数当作 world frame 加速度积分。

现象：即使机器人静止，位置也在快速漂移（约 $9.81\,\mathrm{m/s^2}$ 的加速度累积）。1 秒后速度误差 9.81 m/s，位置误差 4.9 m。

根本原因：加速度计测量比力，包含重力分量。必须先用当前姿态估计将重力转换到 body frame 并减去。

正确做法：$\boldsymbol{a}_{\text{world}} = \boldsymbol{R} (\boldsymbol{a}_m - \boldsymbol{b}_a) + \boldsymbol{g}$

自检方法：让 IMU 静止放置，检查去重力后的加速度是否接近零向量。

💡 概念误区：陀螺仪偏差很小可以忽略

新手想法："偏差只有 0.01 rad/s，这么小可以忽略吧？"

实际上：$0.01\,\mathrm{rad/s} \times 60\,\mathrm{s} = 0.6\,\mathrm{rad} \approx 34°$。一分钟后姿态误差 34 度！而且姿态误差会通过重力补偿耦合到加速度估计，进一步放大位置漂移。

正确做法：陀螺仪偏差必须在线估计和补偿。这是 Kalman 滤波器的核心功能之一。

🧠 思维陷阱：认为"IMU 频率越高越好"

陷阱："IMU 频率从 400 Hz 提高到 4000 Hz，精度应该提高 10 倍。"

实际上：白噪声误差随积分时间的平方根增长——频率翻倍只减少 $\sqrt{2}$ 倍的噪声。而偏差引起的误差与频率无关（偏差是恒定的，积分时间相同则误差相同）。真正限制精度的瓶颈是**偏差稳定性**，而不是采样频率。

正确思维：选 IMU 先看偏差不稳定性（bias instability），再看噪声密度（noise density），最后才看采样频率。对于腿足控制，400 Hz 的高质量 IMU 远胜 4000 Hz 的低质量 IMU。

练习¶

练习 57.2.1：给定 IMU 陀螺仪偏差 $b_g = [0.01, 0.005, -0.008]^T\,\mathrm{rad/s}$，初始姿态为单位矩阵。数值积分 100 步（$\Delta t = 0.001\,\mathrm{s}$，无真实角速度即 $\boldsymbol{\omega} = 0$），计算最终姿态误差（用轴角表示）。

练习 57.2.2：解释为什么加速度计可以用来校正陀螺仪的姿态漂移的 roll/pitch 分量，但**不能**校正 yaw 分量。在什么条件下磁力计可以校正 yaw？磁力计在室内金属结构附近有什么问题？

练习 57.2.3（推导题）：从 $\dot{R} = R[\omega]_\times$ 出发，推导离散化公式 $R_{k+1} = R_k \cdot \text{Exp}(\omega_k \Delta t)$。这个离散化隐含了什么假设？当角速度在 $\Delta t$ 内变化较大时（如落地冲击），误差有多大？

57.3 腿部运动学接触约束 ⭐⭐¶

动机：腿就是"伪 GPS"¶

本节解决的问题：如何利用腿部运动学作为位置和速度观测？当脚接触地面时，脚是"锚点"，基座通过腿的运动学相对脚移动——这提供了类似 GPS 的位置约束。

IMU 提供加速度和角速度，但缺乏绝对位置信息——仅靠 IMU 积分，位置会无限漂移。腿足机器人有一个 SLAM 和轮式机器人都没有的**独特观测源**：腿部运动学 + 接触约束。

核心思想：当第 $i$ 只脚接触地面且不滑动时，它的世界坐标 $\boldsymbol{p}_{f_i}$ 保持不变。通过正运动学（Ch47），我们可以从关节编码器读数计算出脚相对于基座的位置 ${}^B\boldsymbol{p}_{f_i}$。结合脚在世界系中的固定位置，就得到了基座在世界系中的位置约束：

\[\boldsymbol{p}_{\text{base}} = \boldsymbol{p}_{f_i} - \boldsymbol{R} \cdot {}^B\boldsymbol{p}_{f_i}(q_{\text{joints}})\]

这就是为什么人们称腿部运动学为"pseudo-GPS"——每只接触脚相当于一个已知位置的"卫星"。

如果不用腿部运动学会怎样¶

在没有腿部运动学观测的情况下，状态估计器退化为纯 IMU 积分——如 57.1 节所示，位置误差以 $t^2$ 增长。而有了腿部运动学，每个接触相（约 100--300 ms）提供一次"锚定"，将漂移限制在接触相之间的短暂飞行相累积量内。

数值对比（以 trot 步态为例）：

场景	1 秒后位置误差	10 秒后位置误差
纯 IMU 积分	~5 cm	~5 m
IMU + 腿部运动学	~2 mm	~2 cm

差距超过两个数量级——这就是腿式里程计的价值。

正运动学回顾¶

使用 Pinocchio（Ch47），给定关节角度 $q_{\text{joints}}$，可以计算第 $i$ 只脚的足端在基座坐标系中的位置：

\[{}^B\boldsymbol{p}_{f_i} = \text{FK}_i(q_{\text{joints}})\]

以及足端 Jacobian：

\[{}^B\boldsymbol{J}_i = \frac{\partial \text{FK}_i}{\partial q_{\text{joints}}} \in \mathbb{R}^{3 \times n_j}\]

其中 $n_j$ 是与第 $i$ 条腿相关的关节数（Go2 每条腿 3 个关节，$n_j = 3$）。

// Pinocchio: compute foot position and Jacobian
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/jacobian.hpp>

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

// Get foot position in world frame (oMf = world-to-frame placement)
auto foot_frame_id = model.getFrameId("FR_foot");
Eigen::Vector3d p_foot_world = data.oMf[foot_frame_id].translation();

// Get foot Jacobian (LOCAL_WORLD_ALIGNED frame)
Eigen::MatrixXd J(6, model.nv);
J.setZero();
pinocchio::getFrameJacobian(model, data, foot_frame_id,
                            pinocchio::LOCAL_WORLD_ALIGNED, J);
// J.topRows(3) gives the linear velocity part
// J.bottomRows(3) gives angular velocity part

腿式里程计（Leg Odometry）¶

基座速度观测：当第 $i$ 只脚接触地面且不滑动时，脚的世界系速度为零：

\[\boldsymbol{v}_{f_i}^{world} = \boldsymbol{v}_{\text{base}} + \boldsymbol{\omega}_{\text{base}} \times \boldsymbol{R} \cdot {}^B\boldsymbol{r}_{f_i} + \boldsymbol{R} \cdot {}^B\boldsymbol{J}_i \dot{q}_{\text{joints}} = \boldsymbol{0}\]

由此解出基座线速度的观测值：

\[\boldsymbol{v}_{\text{base}} = -\boldsymbol{\omega}_{\text{base}} \times \boldsymbol{R} \cdot {}^B\boldsymbol{r}_{f_i} - \boldsymbol{R} \cdot {}^B\boldsymbol{J}_i \dot{q}_{\text{joints},i}\]

这个公式给出了一个"不需要位置积分的速度估计"——只需要当前的关节位置/速度、IMU 角速度和姿态估计。这是腿式里程计最核心的公式。

公式推导过程：

Step 1：足端在世界系中的位置可以写为： $$\boldsymbol{p}_{f_i}^{world} = \boldsymbol{p}_{\text{base}} + \boldsymbol{R} \cdot {}^B\boldsymbol{p}_{f_i}(q_{\text{joints}})$$

Step 2：对时间求导： $$\dot{\boldsymbol{p}}_{f_i}^{world} = \dot{\boldsymbol{p}}_{\text{base}} + \dot{\boldsymbol{R}} \cdot {}^B\boldsymbol{p}_{f_i} + \boldsymbol{R} \cdot \frac{d}{dt}{}^B\boldsymbol{p}_{f_i}$$

Step 3：利用 $\dot{\boldsymbol{R}} = \boldsymbol{R}[\boldsymbol{\omega}^B]_\times = [\boldsymbol{\omega}^W]_\times \boldsymbol{R}$，以及 $\frac{d}{dt}{}^B\boldsymbol{p}_{f_i} = {}^B\boldsymbol{J}_i \dot{q}$，得到： $$\dot{\boldsymbol{p}}_{f_i}^{world} = \boldsymbol{v}_{\text{base}} + \boldsymbol{\omega}^W \times (\boldsymbol{R} \cdot {}^B\boldsymbol{p}_{f_i}) + \boldsymbol{R} \cdot {}^B\boldsymbol{J}_i \dot{q}$$

Step 4：设 $\dot{\boldsymbol{p}}_{f_i}^{world} = 0$（不滑动），解出 $\boldsymbol{v}_{\text{base}}$。

多脚接触时的处理：如果有 $k$ 只脚同时接触地面（例如 trot 步态时 2 只，stand 时 4 只），每只脚给出一组独立的速度估计。常见处理方式：

方法	公式	适用场景
简单平均	$\boldsymbol{v} = \frac{1}{k}\sum_i \boldsymbol{v}_i$	平坦地形
加权平均	$\boldsymbol{v} = \sum_i w_i \boldsymbol{v}_i$，$w_i \propto$ 接触力	软地面
Kalman 融合	每只脚作为独立观测	最优但计算多

基座位置观测：对于每只接触脚 $i$，假设脚在世界系中的位置 $\boldsymbol{p}_{f_i}^{world}$ 已知（从上一次着地时记录），则：

\[\boldsymbol{p}_{\text{base}} = \boldsymbol{p}_{f_i}^{world} - \boldsymbol{R} \cdot {}^B\boldsymbol{p}_{f_i}(q_{\text{joints}})\]

误差来源分析¶

腿式里程计不是完美的——它有以下误差来源：

误差源	量级	影响	缓解方法
关节编码器噪声	~0.01$^\circ$	足端位置误差 ~1 mm	Kalman 滤波平滑
齿轮背隙（backlash）	~0.1--0.5$^\circ$	足端位置误差 ~5 mm	运动方向感知补偿
关节弹性	取决于负载	高负载时误差增大	刚度模型补偿
脚底滑动	不确定	直接违反"不滑动"假设	滑移检测（57.7 节）
运动学模型误差	连杆长度标定	系统性偏差	离线标定
接触点不确定性	脚底非点接触	等效接触点位置不确定	接触面模型

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Jacobian 参考系不匹配

错误做法：使用 pinocchio::LOCAL frame 的 Jacobian 直接与 world frame 的速度做运算。

现象：计算出的基座速度在机器人转弯时出现大误差，直行时看起来正常。

根本原因：LOCAL frame 的 Jacobian 将关节速度映射到 body frame 的足端速度。如果公式中使用的是 world frame 速度，必须使用 LOCAL_WORLD_ALIGNED 或手动用 $R$ 旋转。

正确做法：统一参考系——要么全部在 body frame 计算，要么全部在 world frame 计算。推荐使用 LOCAL_WORLD_ALIGNED（world frame 方向、原点在 frame 位置的坐标系）。

自检方法：让机器人原地旋转（不平移），检查腿式里程计输出的线速度是否为零。如果 Jacobian 参考系错误，原地旋转时会出现虚假的线速度。

💡 概念误区：接触脚越多，估计越准

新手想法："4 只脚都接触时，有 4 组观测，比 2 只脚接触时准 2 倍。"

实际上：4 只脚的观测**不是完全独立的**——它们共享相同的基座状态和 IMU 测量。4 只脚确实比 2 只好，但改善不是线性的。更重要的是，过约束可能暴露运动学模型误差：如果 4 只脚的观测互相矛盾（因为模型不完美），Kalman 滤波可能产生不一致的结果。

延伸：VILENS 的做法是引入一个"速度偏差"项来吸收运动学模型的系统性误差——这是一个优雅的工程解决方案。

🧠 思维陷阱：认为"腿式里程计可以替代 IMU"

陷阱："既然腿运动学可以估计位置和速度，为什么还需要 IMU？"

反例：在飞行相（如 bounding 步态或跳跃）时，没有任何脚接触地面——腿式里程计完全没有观测。此时只有 IMU 可以维持估计。而且腿式里程计的频率受限于接触/非接触切换，IMU 提供连续不间断的高频预测。两者是互补的：IMU 负责高频预测，腿运动学负责低频校正。

练习¶

练习 57.3.1：推导当 Go2 的 FR（前右）脚接触地面时，基座速度观测方程的显式形式。假设 FR 腿有 3 个关节（hip abduction, hip flexion, knee flexion），写出 $\boldsymbol{v}_{\text{base}}$ 关于 $\boldsymbol{\omega}$, $q_1, q_2, q_3$, $\dot{q}_1, \dot{q}_2, \dot{q}_3$ 的表达式。

练习 57.3.2：讨论在 trot 步态中（对角两脚交替接触），腿式里程计的可观测性。哪些自由度能被观测到？哪些不能？（提示：考虑绕接触脚连线的旋转。）

练习 57.3.3（编程题）：用 Pinocchio 在 Python 中实现一个简单的腿式里程计，读取仿真中的关节数据和 IMU 数据，计算基座速度估计，与真值对比。

57.4 线性 KF 基座估计器 ⭐⭐¶

动机：最简单有效的腿足状态估计器¶

本节解决的问题：如何用标准线性 Kalman 滤波器融合 IMU 加速度和腿部运动学，估计基座的位置、速度和足端位置？这就是 MIT Cheetah 和 legged_control 中广泛使用的 LinearKalmanFilter。

MIT Cheetah 团队（Di Carlo et al., 2018）提出了一个极其简洁的状态估计方案：假设姿态由 IMU 单独提供（不在 KF 中估计），只用线性 KF 估计位置、速度和足端位置。这个设计选择看似简化，实则深思熟虑：

姿态估计的非线性（SO(3) 流形）被隔离到 IMU 处理模块
剩余的状态（位置、速度、足端位置）都是欧氏向量，动力学方程**精确线性**
线性 KF 是**最优的**（在高斯假设下），不需要近似线性化

代价：姿态估计不受腿部运动学约束的修正——IMU 的姿态漂移无法通过腿部观测来补偿。这是后续 InEKF 要解决的局限。

这个设计背后的权衡：MIT Cheetah 的控制循环运行在 1 kHz，状态估计器必须在 < 500 $\mu$s 内完成。线性 KF 的 18 维矩阵运算只需要 ~10 $\mu$s，而完整的 InEKF（30+ 维 + 李群运算）需要 ~50--100 $\mu$s。在早期的嵌入式平台上，这个差异是决定性的。

状态向量设计¶

状态向量 $\boldsymbol{x} \in \mathbb{R}^{18}$：

\[\boldsymbol{x} = \begin{bmatrix} \boldsymbol{p} \\ \boldsymbol{v} \\ \boldsymbol{p}_{f_1} \\ \boldsymbol{p}_{f_2} \\ \boldsymbol{p}_{f_3} \\ \boldsymbol{p}_{f_4} \end{bmatrix} = \begin{bmatrix} \text{基座位置 (3)} \\ \text{基座速度 (3)} \\ \text{FL 脚世界坐标 (3)} \\ \text{FR 脚世界坐标 (3)} \\ \text{RL 脚世界坐标 (3)} \\ \text{RR 脚世界坐标 (3)} \end{bmatrix}\]

为什么包含足端位置？ 因为足端位置在接触期间是观测方程的"参考点"——基座位置的观测通过正运动学和足端位置的差来计算。如果足端位置也有不确定性（例如着地时的位置不完全已知），把它放入状态向量可以让 KF 同时优化基座和足端的估计。

为什么不包含姿态？ 因为旋转矩阵/四元数不是欧氏向量，放入线性 KF 会破坏线性假设。MIT Cheetah 的做法是用互补滤波或 Madgwick/Mahony 滤波器单独处理姿态，然后在线性 KF 中把姿态当作**已知输入**（不是状态）。

为什么不包含 IMU 偏差？ 在线性 KF 的框架下，IMU 偏差由外部的姿态滤波器估计（通过加速度计提供的重力方向修正）。把偏差放入 KF 需要非线性观测方程（偏差与姿态耦合），会破坏线性性。InEKF 通过把偏差作为 $SE_k(3)$ 的扩展状态来解决这个问题。

预测方程（Process Model）¶

连续时间动力学：

\[\dot{\boldsymbol{p}} = \boldsymbol{v}, \quad \dot{\boldsymbol{v}} = \boldsymbol{a}, \quad \dot{\boldsymbol{p}}_{f_i} = \boldsymbol{0} \quad (\text{接触脚不动})\]

其中 $\boldsymbol{a}$ 是 world frame 的加速度，由 IMU 测量计算：

\[\boldsymbol{a} = \hat{\boldsymbol{R}}(\boldsymbol{a}_m - \hat{\boldsymbol{b}}_a) + \boldsymbol{g}\]

$\hat{\boldsymbol{R}}$ 是当前姿态估计（来自 IMU 处理模块），$\hat{\boldsymbol{b}}_a$ 是加速度计偏差估计。

离散化（零阶保持，时间步 $\Delta t$）：

\[\boldsymbol{x}_{k+1} = F \boldsymbol{x}_k + B \boldsymbol{u}_k\]

其中：

\[F = \begin{bmatrix} I_3 & I_3 \Delta t & 0 & 0 & 0 & 0 \\ 0 & I_3 & 0 & 0 & 0 & 0 \\ 0 & 0 & I_3 & 0 & 0 & 0 \\ 0 & 0 & 0 & I_3 & 0 & 0 \\ 0 & 0 & 0 & 0 & I_3 & 0 \\ 0 & 0 & 0 & 0 & 0 & I_3 \end{bmatrix}, \quad B = \begin{bmatrix} \frac{1}{2}I_3 \Delta t^2 \\ I_3 \Delta t \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix}, \quad \boldsymbol{u}_k = \boldsymbol{a}_k\]

注意 $F$ 是常数矩阵！ 这就是"线性"KF 的关键——状态转移矩阵不依赖当前状态。

过程噪声协方差 $Q$：

\[Q = \text{diag}(\sigma_p^2 I_3, \sigma_v^2 I_3, \sigma_{f_1}^2 I_3, \sigma_{f_2}^2 I_3, \sigma_{f_3}^2 I_3, \sigma_{f_4}^2 I_3) \cdot \Delta t\]

足端位置的过程噪声 $\sigma_{f_i}^2$ 根据接触状态动态调整：

接触脚：$\sigma_{f_i}^2$ 很小（~$10^{-4}$），表示"脚几乎不动"
摆动脚：$\sigma_{f_i}^2$ 很大（~$10^{2}$），表示"对脚的位置完全不确定"

这是一个精巧的设计：通过调整过程噪声，同一个 KF 框架自动适应不同的接触模式——无需切换不同的滤波器或改变状态维度。

观测方程（Measurement Model）¶

对于每只接触脚 $i$，观测量是"基座到脚的向量"：

\[\boldsymbol{z}_i = {}^W\boldsymbol{p}_{f_i} - \boldsymbol{p}_{\text{base}} = \hat{\boldsymbol{R}} \cdot \text{FK}_i(q_{\text{joints}})\]

写成线性形式：

\[\boldsymbol{z}_i = H_i \boldsymbol{x}, \quad H_i = \begin{bmatrix} -I_3 & 0 & \ldots & I_3 & \ldots & 0 \end{bmatrix}\]

其中 $H_i$ 的 $I_3$ 在对应第 $i$ 只脚位置的列位置。

额外的速度观测：除了位置观测，还可以加入基于腿式里程计的速度观测（57.3 节）。对应的 $H$ 矩阵选取速度行：$H_{v,i} = [0, I_3, 0, \ldots]$。

观测噪声 $R$：同样根据接触状态动态调整——摆动脚的观测噪声设为极大值（等效于"忽略该观测"）。

完整 KF 循环¶

// MIT Cheetah style Linear KF — simplified but compilable pseudocode
void LinearKalmanFilter::update(double dt,
                                 const Eigen::Vector3d& accel_world,
                                 const Eigen::Vector4d& contacts,
                                 const std::array<Eigen::Vector3d, 4>& fk_body) {
    // === Predict ===
    // State transition: p += v*dt + 0.5*a*dt^2, v += a*dt, feet unchanged
    F_.block<3,3>(0,3) = Eigen::Matrix3d::Identity() * dt;
    x_ = F_ * x_;
    x_.segment<3>(0) += 0.5 * dt * dt * accel_world;
    x_.segment<3>(3) += dt * accel_world;

    // Covariance prediction
    P_ = F_ * P_ * F_.transpose() + Q_;

    // === Update (per foot) ===
    for (int i = 0; i < 4; ++i) {
        // Adjust process noise based on contact state
        double trust = contacts(i);  // 1.0 = contact, 0.0 = swing
        double foot_noise = trust < 0.5 ? 1e2 : 1e-4;
        Q_.block<3,3>(6+3*i, 6+3*i) =
            foot_noise * Eigen::Matrix3d::Identity() * dt;

        // Observation: foot_world - base = R * FK(q)
        Eigen::Vector3d z_meas = R_imu_ * fk_body[i];
        Eigen::Vector3d z_pred = x_.segment<3>(6+3*i) - x_.segment<3>(0);
        Eigen::Vector3d innov = z_meas - z_pred;

        // Observation matrix H_i: [-I, 0, ..., I_at_foot_i, ..., 0]
        Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(3, 18);
        H_i.block<3,3>(0, 0) = -Eigen::Matrix3d::Identity();
        H_i.block<3,3>(0, 6+3*i) = Eigen::Matrix3d::Identity();

        // Observation noise (high for swing legs)
        double obs_noise = trust < 0.5 ? 1e4 : 1e-2;
        Eigen::Matrix3d R_obs = obs_noise * Eigen::Matrix3d::Identity();

        // Standard Kalman update
        Eigen::Matrix3d S = H_i * P_ * H_i.transpose() + R_obs;
        Eigen::MatrixXd K = P_ * H_i.transpose() * S.inverse();
        x_ += K * innov;
        // Joseph form for numerical stability
        auto I_KH = Eigen::MatrixXd::Identity(18,18) - K * H_i;
        P_ = I_KH * P_ * I_KH.transpose() + K * R_obs * K.transpose();
    }
}

Kalman 增益的物理解读¶

Kalman 增益 $K$ 在腿足估计器中有直观的物理含义：

\[K = P H^T (H P H^T + R)^{-1}\]

当 $R \to \infty$（不信任观测，例如摆动脚）：$K \to 0$，观测被忽略
当 $R \to 0$（完全信任观测）：$K \to H^{-1}$（如果 $H$ 可逆），状态完全由观测决定
当 $P$ 很大（预测不确定）：$K$ 更大，更依赖观测
当 $P$ 很小（预测确信）：$K$ 更小，更保留预测

在腿足估计中的具体含义：刚着地的脚（从摆动切换到接触），其足端位置的 $P$ 分量很大（摆动期间不确定性累积），此时 $K$ 较大——正运动学观测会**快速拉回**足端位置估计。这对应物理直觉："刚落地时，用正运动学重新校准脚的位置。"

反过来，一只已经接触地面很久的脚（$P$ 分量因为持续接触而变小），其 $K$ 较小——即使正运动学给出稍有不同的观测，滤波器也不会大幅调整。这对应："脚已经稳定接触，应该相信当前位置估计。"

⚠️ 常见陷阱¶

⚠️ 编程陷阱：协方差矩阵不对称或不正定

错误做法：使用 $P = (I - KH)P$ 更新协方差（标准形式）。

现象：运行一段时间后，$P$ 出现负对角元素或不对称，滤波器数值发散。

根本原因：浮点运算的舍入误差在标准更新公式中会逐步累积，破坏 $P$ 的对称正定性。这个问题在 18 维状态空间中可能在数千步后显现。

正确做法：使用 Joseph 形式：$P = (I - KH)P(I - KH)^T + KRK^T$。这个形式在数学上与标准形式等价，但保证结果对称正定，代价是多一次矩阵乘法（~$18^3$ 次浮点运算，在 1 kHz 下可忽略）。

自检方法：每隔 1000 步检查 $P$ 的最小特征值是否为正。

💡 概念误区：线性 KF 不如 EKF 好

新手想法："EKF 考虑了非线性，肯定比线性 KF 更准确。"

实际上：MIT Cheetah 的线性 KF 在实际四足机器人上表现非常好。原因是：(1) 姿态解耦后，剩余状态的动力学确实是线性的——不存在线性化近似；(2) EKF 的线性化引入额外近似误差，不一定比精确的线性 KF 好；(3) 线性 KF 计算更快，适合 1 kHz 实时控制。选择方法应该基于具体需求，而不是"更复杂 = 更好"。

什么时候需要升级？ 当你发现 IMU 姿态漂移是主要误差来源（例如长时间运行或快速旋转场景），需要升级到 ESKF 或 InEKF，把姿态也放入滤波器中估计。

练习¶

练习 57.4.1：手写 Go2 四足机器人线性 KF 的 $F$, $B$, $H$, $Q$, $R$ 矩阵的数值形式。假设 $\Delta t = 0.001\,\mathrm{s}$，当前只有 FR 和 RL 脚接触（trot 步态）。

练习 57.4.2：在上述 KF 中，讨论以下参数对估计性能的影响：(a) 增大接触脚的过程噪声 $\sigma_f^2$；(b) 减小 IMU 加速度的过程噪声；(c) 增大观测噪声 $R$。画出定性的 bias-variance tradeoff 曲线。

练习 57.4.3（代码阅读）：精读 legged_control 的 LinearKalmanFilter.cpp（约 300 行），列出代码中的 $F$, $Q$, $H$, $R$ 矩阵与本节推导的对应关系。指出代码中任何你认为不直观的设计选择，并解释其原因。

57.5 EKF 与 InEKF ⭐⭐⭐¶

动机：为什么线性 KF 不够¶

本节解决的问题：如果想同时估计姿态和位置（不解耦），需要处理 SO(3) 的非线性。经典 EKF/ESKF 可以做到，但存在一致性问题。Invariant EKF（InEKF）利用李群结构获得更好的理论保证。

线性 KF（57.4 节）的核心限制是**姿态与位置解耦**——IMU 的姿态漂移无法被腿部运动学修正。考虑以下场景：

场景：Go2 在平坦地面上原地旋转
- IMU 陀螺仪有偏差 → 姿态估计逐渐偏离
- 腿部运动学告诉你"脚在 body frame 中的位置"→ 但线性 KF 中姿态不是状态，无法修正
- 结果：姿态持续漂移，导致重力补偿错误，速度/位置估计也跟着偏

解决方案：把姿态也放入估计器。但姿态 $\boldsymbol{R} \in SO(3)$ 是流形而非向量空间——不能直接用线性 KF。这好比在地球表面导航:你不能在一张平面地图(向量空间)上无限制地做加减法,因为地球是球面(流形)。在小范围内(切空间),平面近似可行;但跨越大范围时,必须回到流形上操作。EKF/ESKF 的策略正是"在切空间做加法,然后映射回流形"。

本质洞察:InEKF 相对于传统 EKF 的核心优势不在于"更准"(在短期内两者精度相近),而在于**误差动力学与状态无关(自治性)**。传统 EKF 的线性化点依赖当前状态估计——估计越差,线性化越不准,形成恶性循环;InEKF 利用李群的对称性,使得误差方程的系数矩阵不依赖状态估计值,从根本上打破了这个恶性循环。这就是为什么 InEKF 在初始化不良或长时间无观测时表现更鲁棒。

Error State Kalman Filter（ESKF）回顾¶

ESKF 的核心思想（如 FAST-LIO2 中使用的）：

名义状态：在 SO(3) 流形上积分名义轨迹
误差状态：在切空间（李代数 $\mathfrak{so}(3)$）上定义 3 维误差向量 $\delta\boldsymbol{\theta}$
Kalman 滤波：对误差状态做线性 KF
重投影：每次更新后，用误差状态修正名义状态，然后将误差状态重置为零

\[\boldsymbol{R}_{\text{true}} = \boldsymbol{R}_{\text{nominal}} \cdot \text{Exp}(\delta\boldsymbol{\theta})\]

ESKF 在 SLAM 社区非常成功。但它有一个微妙的问题——

ESKF 的一致性问题¶

ESKF 的误差动力学方程为：

\[\delta\dot{\boldsymbol{\xi}} = F(\hat{\boldsymbol{x}}) \delta\boldsymbol{\xi} + G \boldsymbol{w}\]

关键：Jacobian 矩阵 $F$ 依赖于当前状态估计 $\hat{\boldsymbol{x}}$。这意味着：

每一步都需要重新计算 $F$——增加计算开销
当初始估计误差很大时，$F(\hat{\boldsymbol{x}})$ 的线性化在真实状态附近不准确——可能导致**一致性问题**（滤波器过度自信或发散）

"一致性"的含义：一个一致的估计器，其实际误差应该与估计的不确定性（协方差）匹配。不一致的估计器可能报告"我很确信位置误差 < 1cm"，但实际误差是 10cm——这对控制器是灾难性的，因为控制器会基于错误的置信度做决策。

NEES（Normalized Estimation Error Squared）检验**是评估一致性的标准工具：$\text{NEES} = \boldsymbol{e}^T P^{-1} \boldsymbol{e}$，应服从 $\chi^2_n$ 分布。如果 NEES 持续偏高，说明估计器**过度自信（$P$ 低估了实际不确定性）。

Invariant EKF 的核心洞察¶

Barrau 和 Bonnabel（2017, IEEE TAC）提出了一个深刻的洞察：

如果把整个状态（姿态 + 速度 + 位置 + 足端位置）嵌入一个更大的李群 $SE_k(3)$，并选择正确的误差定义，那么误差动力学方程的 Jacobian $F$ 变成常数矩阵——不依赖当前状态估计。

这就是"Invariant"（不变）的含义：误差动力学在群的左/右作用下保持不变。

为什么这很重要？

性质	ESKF	InEKF
误差 Jacobian $F$	依赖 $\hat{\boldsymbol{x}}$	常数（不依赖状态）
每步计算	重新计算 $F$	$F$ 只算一次
大初始误差	可能发散	局部收敛域通常更大
一致性	可能不一致	理论一致性更好（在群仿射和观测假设下）
适用场景	通用非线性系统	系统具有群仿射结构

这个突破的历史背景：传统 EKF 的线性化一直是状态估计领域的"痛点"。2008 年，Bonnabel 首次提出在对称群上的不变观测器。2014--2017 年，Barrau 和 Bonnabel 发展了完整的 InEKF 理论。2018 年，Hartley 在 RSS 上首次将 InEKF 应用于腿足机器人（Cassie 双足机器人），2020 年在 IJRR 上发表了完整版本。此后 InEKF 成为腿足状态估计的"黄金标准"方法之一。

$SE_k(3)$ 群结构¶

Hartley et al. (2020, IJRR) 将腿足状态估计的状态嵌入 $SE_k(3)$（扩展特殊欧氏群）。对于四足机器人（$k=6$，包含速度 + 位置 + 4 只脚的位置），状态矩阵为：

\[\boldsymbol{X} = \begin{bmatrix} \boldsymbol{R} & \boldsymbol{v} & \boldsymbol{p} & \boldsymbol{d}_1 & \boldsymbol{d}_2 & \boldsymbol{d}_3 & \boldsymbol{d}_4 \\ \boldsymbol{0}^T & 1 & 0 & 0 & 0 & 0 & 0 \\ \boldsymbol{0}^T & 0 & 1 & 0 & 0 & 0 & 0 \\ \boldsymbol{0}^T & 0 & 0 & 1 & 0 & 0 & 0 \\ \boldsymbol{0}^T & 0 & 0 & 0 & 1 & 0 & 0 \\ \boldsymbol{0}^T & 0 & 0 & 0 & 0 & 1 & 0 \\ \boldsymbol{0}^T & 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \in SE_6(3) \subset \mathbb{R}^{9 \times 9}\]

其中： - $\boldsymbol{R} \in SO(3)$：body-to-world 旋转矩阵 - $\boldsymbol{v} \in \mathbb{R}^3$：世界系中的基座线速度 - $\boldsymbol{p} \in \mathbb{R}^3$：世界系中的基座位置 - $\boldsymbol{d}_i \in \mathbb{R}^3$：第 $i$ 只脚在世界系中的位置

$SE_k(3)$ 的群运算：群乘法就是矩阵乘法。逆元素：

\[\boldsymbol{X}^{-1} = \begin{bmatrix} \boldsymbol{R}^T & -\boldsymbol{R}^T\boldsymbol{v} & -\boldsymbol{R}^T\boldsymbol{p} & -\boldsymbol{R}^T\boldsymbol{d}_1 & \ldots \\ \boldsymbol{0}^T & 1 & 0 & 0 & \ldots \\ \vdots & & \ddots & & \\ \end{bmatrix}\]

为什么要把速度和足端位置放进群里？ 普通的 $SE(3) = SO(3) \ltimes \mathbb{R}^3$ 只包含旋转和平移。$SE_k(3)$ 是 $SE(3)$ 的扩展——它在矩阵的右侧增加了额外的列，每列对应一个"附属"的 3D 向量（速度、足端位置）。这些向量在群变换下有**与位置相同的变换规则**——这正是群仿射性质的关键要求。

右不变误差定义¶

InEKF 的关键选择是**误差的定义方式**。右不变误差（right-invariant error）：

\[\boldsymbol{\eta} = \hat{\boldsymbol{X}} \boldsymbol{X}^{-1}\]

对应的李代数误差向量 $\boldsymbol{\xi} = \log(\boldsymbol{\eta}) \in \mathfrak{se}_k(3)$。

为什么选右不变？ 这取决于系统的对称性。对于 IMU 驱动的系统（输入是 body frame 的角速度和比力），右不变误差给出**自治的**（autonomous）误差动力学。

"自治"的含义：误差的时间演化不依赖于真实状态 $\boldsymbol{X}$，只依赖于输入 $\boldsymbol{u}$。数学上：

\[\dot{\boldsymbol{\xi}} = A_t \boldsymbol{\xi} + \text{noise}\]

其中 $A_t$ 只依赖 IMU 测量（输入），不依赖当前状态估计 $\hat{\boldsymbol{X}}$。这是 InEKF 的核心理论贡献——Barrau & Bonnabel (2017) 证明了这个性质的 Lie 对数具有**对数线性**（log-linear）动力学，即 $\log(\eta)$ 满足线性微分方程，从而保证了局部稳定性。

对比 ESKF：ESKF 的误差动力学 $\dot{\delta\boldsymbol{x}} = F(\hat{\boldsymbol{x}}) \delta\boldsymbol{x} + \ldots$ 中，$F$ 依赖 $\hat{\boldsymbol{x}}$——这就是 InEKF 优于 ESKF 的核心理论优势。

群仿射性质（Group Affine Property）¶

InEKF 的"魔法"从何而来？ 答案是**群仿射性质**。一个系统 $\dot{\boldsymbol{X}} = f_u(\boldsymbol{X})$ 被称为群仿射的，如果对所有 $\boldsymbol{X}_1, \boldsymbol{X}_2$：

\[f_u(\boldsymbol{X}_1 \boldsymbol{X}_2) = f_u(\boldsymbol{X}_1) \boldsymbol{X}_2 + \boldsymbol{X}_1 f_u(\boldsymbol{X}_2) - \boldsymbol{X}_1 f_u(I) \boldsymbol{X}_2\]

IMU 驱动的惯性导航方程恰好满足这个性质！这不是巧合——它反映了物理定律在坐标变换下的对称性：牛顿第二定律在不同惯性系中形式相同。

具体验证：IMU 驱动的连续时间方程为：

\[\dot{\boldsymbol{R}} = \boldsymbol{R}[\boldsymbol{\omega}]_\times, \quad \dot{\boldsymbol{v}} = \boldsymbol{R}\boldsymbol{a}_b + \boldsymbol{g}, \quad \dot{\boldsymbol{p}} = \boldsymbol{v}, \quad \dot{\boldsymbol{d}}_i = \boldsymbol{0}\]

把这些写成 $\dot{\boldsymbol{X}} = f_u(\boldsymbol{X})$（$u = (\boldsymbol{\omega}, \boldsymbol{a}_b)$），可以验证群仿射性质成立。Hartley 2020 的论文 Section III 给出了完整的验证过程。

InEKF 的完整算法流程¶

预测步（IMU 驱动）：

用 IMU 测量积分名义状态：$\hat{\boldsymbol{X}}_{k+1}^{-} = f_{\Delta t}(\hat{\boldsymbol{X}}_k, \boldsymbol{u}_k)$
协方差传播：$P_{k+1}^{-} = \Phi_k P_k \Phi_k^T + \text{Adj}_{\hat{X}_k} Q_k \text{Adj}_{\hat{X}_k}^T$

其中 $\Phi_k$ 是离散化的误差状态转移矩阵（常数！），$\text{Adj}_{\hat{X}}$ 是伴随矩阵。

更新步（腿部运动学观测）：

计算观测残差：$\boldsymbol{z}_i = \hat{\boldsymbol{X}} \boldsymbol{y}_i - \boldsymbol{b}_i$
计算 Kalman 增益：$K_i = P^{-} H_i^T (H_i P^{-} H_i^T + \hat{\boldsymbol{X}} N_i \hat{\boldsymbol{X}}^T)^{-1}$
计算误差修正：$\boldsymbol{\xi} = K_i \boldsymbol{z}_i$
状态更新（左乘）：$\hat{\boldsymbol{X}} \leftarrow \text{Exp}(\boldsymbol{\xi}) \hat{\boldsymbol{X}}$
协方差更新：$P \leftarrow (I - K_i H_i) P$

代码实现参考¶

Ross Hartley 提供了开源的 InEKF C++ 实现（github.com/RossHartley/invariant-ekf，GPL 许可）。核心约 1000 行 C++，已经在 Cassie、Digit、Spot 等机器人上验证。

InEKF vs 线性 KF vs ESKF 对比¶

特性	线性 KF (57.4)	ESKF	InEKF
姿态估计	外部（不在 KF 内）	KF 内估计	KF 内估计
状态空间	$\mathbb{R}^{18}$	$\mathbb{R}^{15+}$（误差状态）	$SE_k(3)$ 李群
误差 Jacobian	常数（精确线性）	依赖 $\hat{x}$	常数（群仿射）
一致性	最优（精确线性）	近似，可能不一致	理论一致性更好
大初始误差收敛	N/A（不估计姿态）	收敛域较小	收敛域通常更大，但仍依赖初始化、噪声和接触观测质量
实现复杂度	低（~300 行）	中（~500 行）	中（~1000 行）
计算量	最低（~10 $\mu$s）	中（~30 $\mu$s）	中（~50 $\mu$s）
适用平台	1 kHz 实时	1 kHz 实时	1 kHz 实时
IMU 偏差校正	无（需外部）	有	有（扩展状态）

⚠️ 常见陷阱¶

⚠️ 编程陷阱：InEKF 中的左右不变误差搞混

错误做法：系统选了右不变误差 $\eta = \hat{X}X^{-1}$，但观测更新用了左不变的公式 $\hat{X} \leftarrow \hat{X} \text{Exp}(\xi)$（右乘）。

现象：滤波器在初始化阶段看似正常，运行一段时间后协方差矩阵异常膨胀或收缩。

根本原因：左不变和右不变对应不同的误差定义和不同的伴随变换。右不变误差要求左乘更新 $\hat{X} \leftarrow \text{Exp}(\xi) \hat{X}$，左不变误差要求右乘更新。混用会破坏不变性结构。

正确做法：全程使用同一种不变性。对于 IMU + 腿运动学系统，全部用右不变（Hartley 2020 的推荐）。

💡 概念误区：InEKF 总是比 ESKF 好

新手想法："InEKF 有理论保证，应该总是选 InEKF。"

实际上：InEKF 的优势在**大初始误差**和**一致性**方面。在初始误差小、运行环境好的场景下，ESKF 和 InEKF 的性能差异很小。而且 InEKF 要求系统具有群仿射结构——不是所有系统都满足。例如，带有速度依赖的空气阻力模型的无人机就不满足群仿射性质。

正确思维：选择估计器应该考虑：(1) 初始误差有多大？(2) 系统是否有群仿射结构？(3) 团队对哪种方法更熟悉？实际工程中，MIT 的线性 KF 已经在大量四足机器人上证明了有效性。

练习¶

练习 57.5.1：验证 $SE_2(3)$（包含 $\boldsymbol{R}, \boldsymbol{v}, \boldsymbol{p}$，不含足端位置）的群乘法和逆运算。写出两个 $SE_2(3)$ 矩阵相乘的结果，并验证乘法结果仍然是 $SE_2(3)$ 的元素。

练习 57.5.2（论文精读）：精读 Hartley et al. (2020) 的 Section III（Right-Invariant EKF），回答：(a) 右不变误差 $\eta = \hat{X} X^{-1}$ 的物理含义——它度量了什么？(b) 为什么 $\dot{\eta}$ 不依赖 $X$？(c) 这对协方差传播有什么好处？

练习 57.5.3（对比题）：对比 FAST-LIO2 中的 ESKF 与腿足 InEKF 的状态定义、误差定义、Jacobian 计算复杂度。列一个详细的对照表。

57.6 接触状态检测 ⭐⭐¶

动机：估计器的"开关"¶

本节解决的问题：57.3--57.5 的所有方法都假设"已知哪只脚接触了地面"。但接触检测本身是一个问题——错误的接触检测会导致状态估计完全错误。

⚠️ 工程陷阱：触地误判的两类严重后果

误判一：脚已触地但未检测到 控制器认为脚仍在空中，继续执行摆动腿位置控制。由于足端实际被地面约束，位置控制器试图驱动关节到目标位置会产生巨大力矩，可能导致电机过流保护触发甚至齿轮损坏。

误判二：脚在空中但误判为触地 控制器将该腿切换为支撑模式，试图通过它施加地面反力。但腿在空中无法产生反力，导致实际支撑腿数量减少，机器人失去平衡甚至摔倒。

触地检测的可靠性直接影响控制安全，是工程部署中最需要反复验证的模块之一。

状态估计器依赖接触信息来决定**哪些观测是有效的**： - 接触脚：启用正运动学观测，足端位置噪声设小 - 摆动脚：禁用观测，足端位置噪声设大

如果把一只摆动脚误判为接触脚，估计器会用一个错误的"锚点"来修正基座位置——导致瞬间的位置跳变。反之，如果把接触脚误判为摆动脚，估计器丢失有价值的观测，退化为纯 IMU 积分，加速漂移。

方法 1：基于步态时钟（Gait Schedule）¶

最简单的方法：根据步态规划器（Ch56）的时钟判断"此时这只脚应该在接触相还是摆动相"。

// Simplest contact detection: gait schedule
bool is_contact = gait_scheduler.getPhase(leg_id) < swing_ratio;

优点：零延迟，不需要额外传感器。

问题：这是"**应该**接触"而非"**实际**接触"。地形不平时，脚可能提前或延迟着地——偏差可达 20--50 ms，在 1 kHz 控制频率下对应 20--50 步的错误接触状态。

方法 2：GRF 阈值法¶

如果机器人有足端力传感器（如 Go2 的足底压力传感器），直接用法向力阈值判断：

\[c_i = \begin{cases} 1 & \text{if } F_{n,i} > F_{\text{threshold}} \\ 0 & \text{otherwise} \end{cases}\]

典型阈值 $F_{\text{threshold}} = 20\text{--}50\,\mathrm{N}$。

问题：简单阈值在接触建立/断开的过渡阶段会产生**高频抖动**（chattering）——力在阈值附近反复穿越，导致接触状态快速切换（可能每毫秒翻转一次），引起估计器的不稳定。

方法 3：Schmitt 触发器¶

Schmitt 触发器是解决阈值抖动问题的经典方法——使用**两个阈值**和**滞回**（hysteresis）：

\[c_i(t) = \begin{cases} 1 & \text{if } F_{n,i} > F_{\text{high}} \text{ 持续 } > T_{\text{on}} \\ 0 & \text{if } F_{n,i} < F_{\text{low}} \text{ 持续 } > T_{\text{off}} \\ c_i(t-1) & \text{otherwise（保持上一状态）} \end{cases}\]

参数设计经验： - $F_{\text{low}} = 20\,\mathrm{N}$，$F_{\text{high}} = 50\,\mathrm{N}$（滞回区间 30 N） - $T_{\text{on}} = 5\,\mathrm{ms}$，$T_{\text{off}} = 5\,\mathrm{ms}$（延迟确认）

Schmitt 触发器的名字来历：Otto Schmitt 在 1934 年发明了这种电路，灵感来自鱿鱼神经元的阈值行为——神经元的激活阈值和抑制阈值不同（滞回）。在机器人学中，Fallon et al. (2015) 首次将 Schmitt 触发器应用于 Atlas 人形机器人的接触检测，将测量的垂直 GRF 分类为多种接触状态。

// Schmitt trigger contact detector
class SchmittTrigger {
    double threshold_high_, threshold_low_;
    double time_on_, time_off_;
    double timer_ = 0.0;
    bool state_ = false;

public:
    SchmittTrigger(double high, double low, double t_on, double t_off)
        : threshold_high_(high), threshold_low_(low),
          time_on_(t_on), time_off_(t_off) {}

    bool update(double force, double dt) {
        if (!state_) {  // Currently NOT in contact
            if (force > threshold_high_) {
                timer_ += dt;
                if (timer_ > time_on_) { state_ = true; timer_ = 0.0; }
            } else { timer_ = 0.0; }
        } else {  // Currently IN contact
            if (force < threshold_low_) {
                timer_ += dt;
                if (timer_ > time_off_) { state_ = false; timer_ = 0.0; }
            } else { timer_ = 0.0; }
        }
        return state_;
    }
};

方法 4：基于动量观测器的力估计¶

如果机器人**没有足端力传感器**，可以从动力学方程反推接触力。广义动量观测器（Generalized Momentum Observer）：

\[\hat{\boldsymbol{\tau}}_{\text{ext}} = K_O \left( \boldsymbol{p}_{\text{mom}} - \int_0^t (\boldsymbol{\tau}_{\text{motor}} + \hat{\boldsymbol{\tau}}_{\text{ext}} - C^T \dot{q} + g(q))\, d\tau' \right)\]

其中 $\boldsymbol{p}_{\text{mom}} = M(q)\dot{q}$ 是广义动量，$K_O$ 是观测器增益。通过接触 Jacobian 可以将广义外力映射到足端接触力（Ch52）：

\[\hat{\boldsymbol{\lambda}}_c = J_c^{+T} \hat{\boldsymbol{\tau}}_{\text{ext}}\]

然后对 $\hat{\boldsymbol{\lambda}}_c$ 的法向分量施加阈值判断。

方法 5：概率接触估计¶

最先进的方法不做二元判断，而是估计**接触概率** $p_c \in [0, 1]$。

Camurri et al. (2017, RAL) 的方法：使用逻辑回归（logistic regression）分类器，输入特征包括：

足端力估计值 $F_n$
足端速度的模 $\|v_{\text{foot}}\|$
步态相位 $\phi_{\text{gait}}$

输出：$p_c = \sigma(w_1 F_n + w_2 \|v_{\text{foot}}\| + w_3 \phi_{\text{gait}} + b)$，其中 $\sigma$ 是 sigmoid 函数。

概率接触如何融入 KF？ 不是"接触/不接触"的二元选择，而是用 $p_c$ 作为观测噪声的调制因子：

\[R_i = \frac{R_{\text{contact}}}{p_c + \epsilon} + R_{\text{swing}} \cdot (1 - p_c)\]

当 $p_c \to 1$ 时，$R_i \approx R_{\text{contact}}$（小噪声，信任观测）；当 $p_c \to 0$ 时，$R_i \approx R_{\text{swing}}$（大噪声，忽略观测）。中间值（如 $p_c = 0.6$）给出一个折中的噪声——滤波器**部分信任**该观测。

方法对比¶

方法	需要力传感器	延迟	抖动	精度	计算量	适用
步态时钟	否	0	无	低	极低	已知平坦地形
GRF 阈值	是	0	高	中	极低	简单场景
Schmitt 触发器	是	5--10 ms	无	中高	低	工业标配
动量观测器	否	~5 ms	中	中	中	无力传感器
概率估计	可选	~1 ms	无	高	中	研究前沿

接触检测的 Chicken-and-Egg 问题¶

存在一个循环依赖：

状态估计**需要**接触检测结果（决定哪些观测有效）
接触检测（方法 4, 5）**需要**状态估计结果（计算运动学/力估计）

工程解法：使用**上一步**的状态估计做接触检测，结果用于**当前步**的状态更新。这引入一步延迟（~1 ms at 1 kHz），在实践中影响可忽略——因为接触状态在 1 ms 内极少发生变化。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Schmitt 触发器参数不匹配步态频率

错误做法：$T_{\text{on}} = 50\,\mathrm{ms}$，但步态周期只有 200 ms，接触相只有 100 ms。

现象：接触检测延迟过大，接触脚的前 50 ms 被当作摆动脚——丢失 50% 的观测。

正确做法：$T_{\text{on}}$ 和 $T_{\text{off}}$ 应远小于接触相持续时间。经验法则：$T < 0.1 \times T_{\text{stance}}$。

🧠 思维陷阱：追求"完美"的接触检测

陷阱："必须 100% 准确地检测接触，否则状态估计会崩溃。"

实际上：概率接触估计的思想告诉我们——不需要二元判断。用 $p_c$ 调制观测噪声，让 Kalman 滤波器自己权衡。一个"模糊"的接触估计（$p_c = 0.6$）远比一个错误的二元判断（接触 vs 非接触搞反）要好。

延伸：最新的研究（2024, MERL）甚至提出了"同时估计状态和接触"的多模型 Kalman 滤波方法——用多个 KF 并行运行不同的接触假设，根据 innovation likelihood 自动选择最佳假设。

练习¶

练习 57.6.1：为 Go2 机器人设计 Schmitt 触发器参数。假设 trot 步态频率 2 Hz（周期 500 ms，接触相 300 ms，摆动相 200 ms），选择合适的 $F_{\text{high}}$, $F_{\text{low}}$, $T_{\text{on}}$, $T_{\text{off}}$，并解释选择理由。

练习 57.6.2：实现概率接触检测器的 Python 原型。输入：足端力 $F_n$、足端速度 $v_{\text{foot}}$、步态相位 $\phi$。输出：接触概率 $p_c$。用仿真数据训练逻辑回归参数。

练习 57.6.3（思考题）：讨论接触检测延迟对状态估计的影响。如果接触检测延迟 10 ms，在 1 kHz 的 KF 中，这 10 步使用了错误的噪声参数——定量估算这会导致多少位置误差。

57.7 滑移检测与地形分类 ⭐⭐⭐¶

上一节解决了"脚是否接触地面"的判断问题。但即使脚确实在地面上,还有一个更隐蔽的威胁:脚在地面上滑动。接触检测只判断"有没有力",而滑移检测要判断"脚底是否在移动"——两者的失效模式和信号特征完全不同。

动机：当"不滑动"假设失效¶

本节解决的问题：57.3 节的腿式里程计假设接触脚不滑动。但在湿滑地面、碎石、冰面等场景下，脚底会滑动——违反假设后状态估计会出现大误差。如何检测滑移？检测到后怎么办？

滑移对状态估计的影响¶

当脚滑动时：

实际足端速度 $\boldsymbol{v}_{f_i} \neq 0$，但估计器假设 $\boldsymbol{v}_{f_i} = 0$
基于"零速度"假设计算的基座速度观测**包含系统性误差**
如果多只脚同时滑动，误差会快速累积

量化影响：假设一只脚滑动速度为 $v_{\text{slip}} = 0.1\,\mathrm{m/s}$，如果估计器对此完全不知情（观测噪声 $R$ 设得很小，Kalman 增益 $K$ 较大），这个错误观测会以 $K \cdot v_{\text{slip}}$ 的量级被"注入"状态估计。持续 100 ms 的滑移可以导致 ~1 cm 的位置误差。

基于速度残差的滑移检测¶

核心思想：比较腿式里程计计算的基座速度与 IMU 预测的基座速度——如果差距过大，说明某只脚可能在滑动。

残差计算：对于第 $i$ 只接触脚，腿式里程计给出的速度观测为 $\boldsymbol{v}_{\text{leg},i}$，IMU 预测的速度为 $\boldsymbol{v}_{\text{imu}}$。残差：

\[\boldsymbol{r}_i = \boldsymbol{v}_{\text{leg},i} - \boldsymbol{v}_{\text{imu}}\]

判别准则（简单阈值）：

\[\text{slip}_i = \begin{cases} \text{true} & \text{if } \|\boldsymbol{r}_i\| > r_{\text{threshold}} \\ \text{false} & \text{otherwise} \end{cases}\]

典型阈值 $r_{\text{threshold}} = 0.1\text{--}0.3\,\mathrm{m/s}$，具体取决于地形和步态。

更精细的方法——Mahalanobis 距离（考虑不确定性）：

\[d_M = \boldsymbol{r}_i^T (H_i P H_i^T + R_i)^{-1} \boldsymbol{r}_i\]

如果 $d_M > \chi^2_3(\alpha)$（3 自由度卡方分布的 $\alpha$ 分位数，如 $\chi^2_3(0.99) = 11.34$），则判定为异常——可能是滑移。Mahalanobis 距离的优势是它自动适应不同状态维度的不确定性——在不确定性大的方向上容忍更大的残差。

多脚一致性检测¶

当多只脚同时接触时，可以利用**脚间一致性**检测滑移：

\[\epsilon_{ij} = \boldsymbol{v}_{\text{leg},i} - \boldsymbol{v}_{\text{leg},j}\]

如果两只接触脚都不滑动，它们应该给出相同的基座速度估计（误差在噪声范围内）。如果 $\|\epsilon_{ij}\|$ 显著大于预期噪声，说明至少一只脚在滑动。

通过比较所有脚对的一致性，可以定位具体哪只脚滑动——不一致的脚是"少数派"（如果只有一只脚滑动，它与其他脚的 $\epsilon$ 都很大）。

检测到滑移后的处理¶

策略	方法	适用场景
增大观测噪声	$R_i \leftarrow R_i \cdot \alpha$（$\alpha \gg 1$）	轻微滑移
禁用该脚观测	$p_{c,i} \leftarrow 0$	明显滑移
降低信任权重	$w_i \propto 1/\\|\boldsymbol{r}_i\\|$	连续变化
切换到纯 IMU	暂停所有腿部观测	所有脚滑移（如被推）

地形分类¶

💡 工程提示：坡度估计的直觉理解

当坡度估计错误时：坡下方的腿表现得"如同踩进坑里"（支撑相位延长），坡上方的腿表现得"如同踩到石头上"（提前触地）。简单的平面方程估计 $z = a + bx + cy$ 可通过最小二乘法从多脚触地点拟合，在温和坡度下足以使用。

坡度估计的最小二乘推导¶

给定 $n$ 个触地脚的世界坐标系位置 $(x_i, y_i, z_i)$，假设局部地面为平面 $z = a + bx + cy$，则地面法向量为 $\mathbf{n} = (-b, -c, 1)^T / \|(-b, -c, 1)\|$。

参数 $(a, b, c)$ 通过最小二乘求解：

\[\begin{bmatrix} 1 & x_1 & y_1 \\ 1 & x_2 & y_2 \\ \vdots & \vdots & \vdots \\ 1 & x_n & y_n \end{bmatrix} \begin{bmatrix} a \\ b \\ c \end{bmatrix} = \begin{bmatrix} z_1 \\ z_2 \\ \vdots \\ z_n \end{bmatrix}\]

即 $\mathbf{A}\mathbf{p} = \mathbf{z}$，最小二乘解 $\mathbf{p}^* = (\mathbf{A}^T\mathbf{A})^{-1}\mathbf{A}^T\mathbf{z}$。

⚠️ 失效条件

触地脚不足 3 个：方程组欠定，无法确定平面（如 trot 飞行相只有 0 或 2 脚触地）

触地脚共线：3 个或更多脚的投影 $(x_i, y_i)$ 共线时，$\mathbf{A}^T\mathbf{A}$ 退化为奇异矩阵（如 pace 步态的同侧两脚）

非平面地形：阶梯、凹坑等地形违反平面假设，拟合结果会产生系统性偏差

工程上通常在 trot/walk 步态的 4 脚或 3 脚支撑阶段更新坡度估计，飞行相和 2 脚支撑阶段保持上一次估计值。

通过持续监测接触力和滑移模式，可以推断地形类型：

地形	摩擦系数 $\mu$	接触力特征	滑移特征
干燥水泥	~0.7	稳定	几乎无
湿水泥	~0.4	稳定	偶发
碎石	~0.3--0.5	波动大	频繁小滑
冰面	~0.1	稳定	频繁大滑
草地	~0.4	波动中	软变形

近期研究进展（2024--2025）：

CNN 滑移检测：卷积神经网络从关节力矩时间序列中学习滑移检测，在多腿同时滑移场景下速度估计 RMSE 降低 48%，最大误差降低 47%（Frontiers in Mechanical Engineering, 2025）
在线摩擦系数估计：通过最小化刚体接触动力学残差（参数化摩擦系数），实时估计地面摩擦系数，在湿滑地形上显著提升控制稳定性（arXiv 2502.16843, 2025）
本体感受地形建图：四足机器人从行走过程中的内部传感器数据估计地形高程、滑移风险、能量成本和稳定性裕度（arXiv 2510.18986）

⚠️ 常见陷阱¶

⚠️ 编程陷阱：滑移检测阈值设太低

错误做法：$r_{\text{threshold}} = 0.01\,\mathrm{m/s}$，正常行走的运动学噪声（编码器噪声 + Jacobian 近似）就会频繁触发"滑移"判断。

现象：大量正常观测被错误丢弃，估计器退化为纯 IMU 积分，快速漂移。

正确做法：阈值应设为正常观测噪声的 3--5 倍标准差。可以在平坦地面上标定正常残差分布（收集 $\|\boldsymbol{r}_i\|$ 的统计数据），然后设阈值为 $\mu + 3\sigma$。

💡 概念误区：滑移只发生在冰面上

实际上：快速转弯时的离心力、急加速/急减速时的惯性力、地形突变的边缘——都可能导致滑移，即使在高摩擦地面上。特别是 bounding 和 gallop 等高速步态中，着地瞬间的冲击力可能瞬间超过摩擦锥极限（Ch52），导致微滑移。

练习¶

练习 57.7.1：设计一个基于 Mahalanobis 距离的滑移检测器。给定 KF 的预测协方差 $P$ 和观测矩阵 $H$，计算每只接触脚的异常分数，并用卡方检验（$\alpha = 0.99$）判断是否滑移。

练习 57.7.2（思考题）：讨论"所有脚同时滑移"的情况（如机器人站在冰面上被推）。此时速度残差方法还能工作吗？多脚一致性检测还有效吗？需要什么额外信息来检测全局滑移？

57.8 外感知融合 ⭐⭐⭐¶

动机：本体感受的漂移极限¶

本节解决的问题：纯本体感受（IMU + 腿运动学）的状态估计不可避免地存在长期漂移。如何利用外感知传感器（LiDAR、Camera）提供漂移修正？

纯本体感受估计的根本问题：

**IMU 偏差**会随时间缓慢变化，腿部运动学的修正只能部分补偿
**脚底微小滑动**的累积效应——即使每步滑 1 mm，走 1000 步就是 1 m 的漂移
运动学模型误差——连杆长度标定不完美带来的系统性偏差

实测数据表明，纯本体感受的位置漂移率约为行走距离的 1--5%（即走 100 m 漂移 1--5 m）。对于长距离导航（如矿井巡检、灾区搜救），这是不可接受的。

外感知传感器的角色¶

外感知传感器（LiDAR、相机）提供**环境参考**——它们观测到的特征点/平面/纹理锚定在世界坐标系中，可以修正本体感受的累积漂移。

传感器	观测类型	频率	漂移修正能力	失效场景
LiDAR	点云配准（ICP/NDT）	10--20 Hz	强（3D 结构约束）	退化场景（长走廊、开阔空地）
立体相机	视觉特征匹配	30 Hz	中（2D 投影约束）	纹理缺失、光照剧变
深度相机	深度图 + RGB	30 Hz	中强	户外强光、远距离

VILENS：紧耦合因子图融合¶

Wisth et al. (2023, IEEE T-RO) 提出的 VILENS（Visual, Inertial, LiDAR, and Leg Odometry Navigation System）是目前最全面的多传感器腿足估计系统。

核心架构：因子图（Factor Graph）框架，同时优化以下因子：

因子类型	约束	频率	关键创新
IMU 预积分因子	连续两帧间的惯性约束	400 Hz	标准（Forster 2017）
腿式里程计因子	接触脚的零速度约束	250 Hz	预积分速度因子 + 速度偏差估计
LiDAR 因子	点云配准约束	10 Hz	ICP/GICP
视觉因子	特征匹配约束	15 Hz	立体 VO

VILENS 的三个关键创新：

腿式里程计预积分：类似 IMU 预积分，将腿式里程计的速度测量预积分为位移增量，减少因子图的计算开销
速度偏差估计：在状态中加入一个线速度偏差项 $\boldsymbol{b}_v$，用于在线校正腿式里程计的系统性误差（如运动学标定不完美）。这个偏差是**可观测的**——因为它被视觉和 LiDAR 因子约束
退化检测：自动检测 LiDAR 或视觉是否退化（如长走廊、暗环境），退化时降低对应因子的权重

性能：在 ANYmal 四足机器人上验证，总行走距离 1.8 km，2 小时运行时间。实验环境包括碎石坡道、泥地、暗洞穴等挑战场景。相比松耦合方法，平移误差降低 62%，旋转误差降低 51%。

Pronto：模块化 EKF 融合¶

Camurri et al. (2020, Frontiers in Robotics and AI) 的 Pronto 采用不同的架构——模块化 EKF：

核心设计：

高频本体感受线程（250--1000 Hz）：IMU 预测 + 腿式里程计更新，运行在实时控制环中
低频外感知线程（1--15 Hz）：LiDAR 或相机的观测到来时，作为 EKF 的延迟更新

Pronto 的特点：

跨平台验证：在 4 个完全不同的机器人上测试——NASA Valkyrie（人形）、Boston Dynamics Atlas（人形）、IIT HyQ（液压四足）、ANYbotics ANYmal（电驱四足），总运行 2 小时，行走 1.37 km
模块化接口：新传感器只需实现一个观测更新模块，插入即用
开源：github.com/ori-drs/pronto，C++ 实现，ROS 封装

Pronto vs VILENS 对比：

特性	Pronto	VILENS
框架	EKF（滤波）	因子图（优化）
优势	计算轻量、模块化好	精度高、支持延迟观测
劣势	不支持重新线性化	计算量较大
适用	计算资源受限、多平台	精度优先、有足够算力

松耦合 vs 紧耦合¶

类型	方法	优点	缺点
松耦合	本体感受估计 + 外部 SLAM 独立运行，SLAM 输出作为全局位姿修正	简单，模块独立	无法利用交叉约束
紧耦合	所有传感器在同一个估计器中联合优化	精度最高，充分利用信息	实现复杂，计算量大

工程建议：如果团队已有成熟的 SLAM 系统（如 FAST-LIO2），松耦合是最快的集成方式——SLAM 以 10 Hz 输出全局位姿，作为 KF 的低频修正观测。紧耦合（如 VILENS）精度更高但需要从头设计整个估计器。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：外感知更新的时间同步

错误做法：LiDAR 10 Hz 的位姿估计直接与 KF 当前时刻的状态做比较。

现象：机器人快速运动时，位置估计出现"锯齿"——每次 LiDAR 更新都拉回一个旧时刻的位姿。

根本原因：LiDAR 处理需要时间（50--100 ms），观测对应的时刻已经过去了。把 80 ms 前的位姿观测当作"当前"位姿，在机器人以 1 m/s 运动时引入 8 cm 的系统性误差。

正确做法：使用**延迟状态更新**——保留历史状态缓冲区，LiDAR 观测到来时更新对应历史时刻的状态，然后重新预测到当前时刻。Pronto 和 VILENS 都实现了这一机制。

💡 概念误区：外感知一定比本体感受准

实际上：在 LiDAR 退化场景（如长走廊——只有平行平面，没有横向约束）或相机退化场景（暗处、烟雾），外感知可能比本体感受更差。好的融合系统必须能**检测退化**并自动降权。VILENS 论文中专门设计了退化检测模块。

练习¶

练习 57.8.1：设计一个松耦合融合方案。本体感受 KF 以 1 kHz 运行，SLAM 以 10 Hz 输出位姿 $(R_{\text{slam}}, p_{\text{slam}})$。写出 SLAM 观测的观测方程和噪声模型。考虑 SLAM 输出的延迟。

练习 57.8.2（思考题）：讨论 VILENS 中"腿式里程计预积分"与"IMU 预积分"的类比。两者的状态是什么？偏差项分别对应什么物理量？为什么需要偏差项？

57.9 Pronto 与工程实战 ⭐⭐¶

动机：从理论到工程部署¶

本节解决的问题：理论上的最优估计器在工程部署中面临大量实际问题——传感器故障、计算延迟、参数调试。本节以 Pronto 为例，讨论工程实践中的关键考量。

Pronto 的系统架构¶

┌─────────────────────────────────────────────┐
│              Pronto State Estimator          │
│                                              │
│  ┌──────────┐  ┌──────────┐  ┌──────────┐  │
│  │   IMU    │  │  Leg     │  │ LiDAR/   │  │
│  │ Module   │  │ Odometry │  │ Vision   │  │
│  │ (1kHz)   │  │ (250Hz)  │  │ (10Hz)   │  │
│  └────┬─────┘  └────┬─────┘  └────┬─────┘  │
│       │              │              │        │
│       ▼              ▼              ▼        │
│  ┌────────────────────────────────────────┐  │
│  │         EKF Core (predict + update)    │  │
│  │   State: [R, p, v, b_g, b_a]          │  │
│  └────────────┬───────────────────────────┘  │
│               │                              │
│               ▼                              │
│  ┌──────────────────┐                        │
│  │ Output: pose,    │                        │
│  │ velocity, bias   │                        │
│  └──────────────────┘                        │
└─────────────────────────────────────────────┘

工程实践关键问题¶

问题 1：初始化

估计器启动时，所有状态都是未知的。初始化策略：

姿态：静止 2--3 秒，用加速度计方向确定 roll/pitch（通过重力方向），yaw 设为 0 或磁力计值
位置/速度：设为零，给予大初始协方差（如 $P_0 = \text{diag}(10^2, 10^2, \ldots)$）
IMU 偏差：静止期间平均测量值——陀螺仪均值就是初始偏差估计，加速度计均值减去预期重力就是初始偏差估计
足端位置：用初始姿态 + 正运动学计算

// Initialization procedure
void StateEstimator::initialize() {
    // Collect stationary IMU data for 2 seconds
    while (!isStationary(imu_buffer_)) { collectIMU(); }

    // Initial orientation from gravity direction
    Eigen::Vector3d accel_avg = averageAccel(imu_buffer_);
    Eigen::Matrix3d R0 = alignToGravity(accel_avg);

    // Initial gyro bias = average gyro reading (should be near zero)
    Eigen::Vector3d bg0 = averageGyro(imu_buffer_);

    // Initial accel bias = average accel - expected gravity in body frame
    Eigen::Vector3d ba0 = accel_avg - R0.transpose() * gravity_;

    state_.R = R0; state_.p.setZero(); state_.v.setZero();
    state_.bg = bg0; state_.ba = ba0;
}

问题 2：参数调试（Tuning）

KF/EKF 的性能高度依赖噪声参数 $Q$ 和 $R$ 的选择。这是工程中最耗时的部分——没有通用公式，需要经验和反复实验。

参数	设太小	设太大	调试策略
$Q_{\text{imu}}$	不信任 IMU，过度依赖观测	过度信任 IMU，观测修正弱	从 IMU 数据手册值开始
$Q_{\text{foot}}$（接触）	脚位置"锁死"不更新	脚位置不稳定	~$10^{-4}$ 开始
$Q_{\text{foot}}$（摆动）	摆动脚信息不释放	立即重新初始化	~$10^2$
$R_{\text{fk}}$	过度信任运动学，模型误差放大	运动学修正太弱	正运动学重复性实验标定

实用调试流程：

先调 IMU 参数：让机器人静止，观察位置漂移速率——漂移太快说明 $Q_{\text{imu}}$ 的加速度项太大
再调观测参数：让机器人慢走直线，比较估计速度与 motion capture 真值
最后调接触切换参数：在不同步态（stand, trot, bound）下测试接触/摆动切换的平滑性

问题 3：异常检测与重置

传感器可能故障：IMU 数据突变、编码器丢步、力传感器饱和。好的工程系统需要：

Innovation 检验：如果 $\boldsymbol{z} - H\hat{\boldsymbol{x}}$ 的 Mahalanobis 距离超过 $5\sigma$，丢弃该观测（不做更新）
协方差监控：如果 $P$ 的对角元素超过预设上限，触发警告或重新初始化
传感器健康监控：检测 IMU 数据率异常（丢帧）、编码器值突变（>10$^\circ$/步）、力传感器饱和

问题 4：实时性

在 1 kHz 控制循环中，状态估计器的计算时间必须严格受限：

方法	状态维度	典型耗时	适用频率
线性 KF	18	~10 $\mu$s	1 kHz
InEKF	21--30	~50--100 $\mu$s	1 kHz
因子图（VILENS）	滑窗优化	~5--20 ms	100--200 Hz（独立线程）

因子图方法通常不运行在控制循环中，而是在独立线程以较低频率运行，输出通过共享内存或 ROS 话题传递给控制器。

MIT Cheetah 估计器 vs Pronto 对比¶

特性	MIT Cheetah KF	Pronto
框架	线性 KF	EKF
姿态估计	外部（Madgwick/Mahony）	KF 内
IMU 偏差	不估计	在线估计
外感知	不支持	模块化支持（LiDAR, Camera）
验证平台	Mini Cheetah	Atlas, Valkyrie, HyQ, ANYmal
代码规模	~300 行	~5000 行
适用场景	短距离高频控制	长距离多传感器导航

⚠️ 常见陷阱¶

⚠️ 编程陷阱：EKF 更新频率与 IMU 频率不匹配

错误做法：IMU 以 1000 Hz 发布数据，但 EKF 只以 200 Hz 运行——丢弃 80% 的 IMU 数据。

现象：高频运动（如落地冲击）被低通滤波掉，估计器对快速变化的响应迟钝。

正确做法：EKF 的预测步应该以 IMU 频率运行（每收到一个 IMU 测量就做一次预测）。观测更新可以低频（有观测时才做）。如果计算量不允许 1 kHz 全更新，至少保证 IMU 预积分覆盖所有测量——不要丢弃任何 IMU 数据。

🧠 思维陷阱：过度关注估计器精度而忽略鲁棒性

陷阱："把所有参数调到在测试场地上的 RMSE 最优。"

实际上：最优参数是环境依赖的——在平地上最优的参数在粗糙地形上可能发散。工程上应优先保证**鲁棒性**（在所有预期环境中不发散），然后再追求精度。一个"差一点但不会崩"的估计器远比"在平地上很准但碎石上崩溃"的估计器有价值。

实践建议：先在最苛刻的场景（快速步态 + 粗糙地形 + 传感器噪声加倍）下调参使其稳定，然后微调以提升平均场景下的精度。

练习¶

练习 57.9.1：为 Pronto 设计一个传感器健康监控模块。定义 IMU、编码器、力传感器的异常检测指标和阈值。当检测到异常时，估计器应该如何响应？

练习 57.9.2：讨论在以下场景中选择哪种估计器最合适，并解释理由： (a) 四足机器人在平坦厂房中巡检，只需本体感受 (b) 四足机器人在矿井中进行长距离探索（1 km+），携带 LiDAR (c) 人形机器人在建筑工地上作业，需要精确的手部定位

57.10 本章小结¶

知识点总结¶

知识点	难度	核心内容	关键公式/概念
状态估计地位	⭐	浮动基座缺乏直接位置测量	传感器融合架构
IMU 运动学	⭐⭐	偏差模型、重力耦合	$\boldsymbol{a}_m = R^T(a-g) + b_a + n_a$
腿部运动学约束	⭐⭐	正运动学 → 基座位置/速度观测	$v_{\text{base}} = -\omega \times r - J\dot{q}$
线性 KF	⭐⭐	MIT Cheetah 风格估计器	$x = [p, v, p_{f_1}, \ldots, p_{f_4}]$, $F$ 常数
InEKF	⭐⭐⭐	李群上的不变 EKF	$SE_k(3)$, 自治误差动力学, 群仿射
接触检测	⭐⭐	从阈值到概率估计	Schmitt 触发器, 逻辑回归
滑移检测	⭐⭐⭐	速度残差, 多脚一致性	Mahalanobis 距离, $\chi^2$ 检验
外感知融合	⭐⭐⭐	VILENS, Pronto 架构	因子图 vs EKF, 松/紧耦合
工程实战	⭐⭐	初始化, 调参, 异常处理	Innovation 检验, 鲁棒性优先

方法选择指南¶

需要状态估计器？
│
├─ 只需短距离控制（< 100m）？
│   ├─ 是 → 线性 KF（MIT Cheetah 风格）
│   │       最简单、最快、在大多数场景够用
│   │
│   └─ 否 → 需要长距离？
│            │
│            ├─ 有 LiDAR/Camera？
│            │   ├─ 计算资源充足 → VILENS（因子图紧耦合）
│            │   └─ 计算资源受限 → Pronto（模块化 EKF）
│            │
│            └─ 纯本体感受？
│                └─ InEKF（最佳纯本体感受精度和一致性）
│
└─ 需要修正 IMU 姿态漂移？
    ├─ 是 → InEKF 或 ESKF（姿态在滤波器内）
    └─ 否 → 线性 KF + 外部姿态滤波

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

累积项目：从零构建四足站立/行走控制器

本章新增：状态估计器模块

本章新增的模块：

quadruped_controller/
├── estimation/
│   ├── linear_kf.hpp/cpp          ← 本章: 线性KF基座估计
│   ├── contact_detector.hpp/cpp   ← 本章: Schmitt触发器接触检测
│   ├── leg_odometry.hpp/cpp       ← 本章: 腿式里程计
│   └── state_estimator.hpp/cpp    ← 本章: 估计器管理器
├── control/  (Ch53-Ch56)
├── model/    (Ch47-Ch49)
└── gait/     (Ch56)

与前后章的连接： - 向前承接：Ch47（Pinocchio 正运动学）提供足端位置/Jacobian 计算，Ch49（浮动基座动力学）定义状态空间结构，Ch52（接触力学）提供接触力约束的数学基础 - 向后指向：状态估计器的输出（基座位姿、速度）直接送入 Ch53（WBC）的 QP 和 Ch55（MPC）的预测模型。没有准确的状态估计，控制器无法工作

本章项目任务： 1. 实现 LinearKF 类：18 维状态，预测+更新循环，Joseph 形式协方差更新 2. 实现 SchmittTriggerContactDetector 类：四路 Schmitt 触发器，可配置参数 3. 实现 LegOdometry 类：计算每只接触脚的基座速度观测 4. 在 Gazebo/MuJoCo 仿真中运行，与 ground truth 对比位置/速度估计精度 5. 可选进阶：用 Ross Hartley 的 invariant-ekf 库替换线性 KF，对比大初始姿态误差下的收敛行为

57.12 延伸阅读¶

必读经典¶

文献	类型	难度	内容
Bloesch M. et al. (2012) "State Estimation for Legged Robots - Consistent Fusion of Leg Kinematics and IMU", RSS 2012	论文	⭐⭐	腿足 EKF 的奠基之作，首次严格融合 IMU 和腿运动学
Hartley R. et al. (2020) "Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation", IJRR	论文	⭐⭐⭐	InEKF 应用于腿足的完整理论和实验
Barrau A., Bonnabel S. (2017) "The Invariant Extended Kalman Filter as a Stable Observer", IEEE TAC	论文	⭐⭐⭐⭐	InEKF 的数学理论基础

系统级参考¶

文献	类型	难度	内容
Camurri M. et al. (2020) "Pronto: A Multi-Sensor State Estimator for Legged Robots in Real-World Scenarios", Frontiers in Robotics and AI	论文	⭐⭐	多平台验证的模块化 EKF
Wisth D. et al. (2023) "VILENS: Visual, Inertial, Lidar, and Leg Odometry for All-Terrain Legged Robots", IEEE T-RO	论文	⭐⭐⭐	因子图紧耦合多传感器融合
Forster C. et al. (2017) "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", IEEE TRO	论文	⭐⭐⭐	IMU 预积分理论（SLAM 背景必读）

近期进展（2024--2025）¶

文献	类型	难度	内容
"Proprioceptive Slip Detection and State Estimation of Multi-Legged Robots in Slippery Scenarios", Frontiers in Mechanical Engineering, 2025	论文	⭐⭐⭐	CNN 滑移检测，多腿同时滑移处理
"Simultaneous State Estimation and Contact Detection for Legged Robots by Multiple-Model Kalman Filtering", MERL, 2024	论文	⭐⭐⭐	多模型 KF 同时估计状态和接触
"Online Friction Coefficient Identification for Legged Robots on Slippery Terrain", arXiv 2502.16843, 2025	论文	⭐⭐⭐	在线摩擦系数估计
"MI-HGNN: Morphology-Informed Heterogeneous Graph Neural Network for Legged Robot Contact Perception", 2024	论文	⭐⭐⭐⭐	图神经网络接触感知

开源代码¶

项目	语言	功能	链接
legged_control LinearKalmanFilter	C++	MIT Cheetah 风格线性 KF (~300 行)	legged_estimation/
invariant-ekf (Hartley)	C++	InEKF 参考实现 (~1000 行)	github.com/RossHartley/invariant-ekf
Pronto	C++ / ROS	模块化多传感器 EKF (~5000 行)	github.com/ori-drs/pronto

InEKF 收敛性分析 ⭐⭐⭐¶

InEKF 相较于标准 EKF 的核心理论优势在于**误差动力学的自治性**——误差传播方程不依赖于状态估计值本身，使得收敛性分析成为可能。

直觉解释：标准 EKF 在 $\hat{x}$ 处线性化，当 $\hat{x}$ 离真值 $x^*$ 很远时，线性化误差很大，导致 EKF 发散。InEKF 利用群结构，将误差定义在群上（$\eta = X^{-1}\hat{X}$ 或 $\eta = \hat{X}X^{-1}$），使得误差动力学方程的 $A$ 矩阵与状态估计值无关——即使初始估计误差很大，只要系统可观测，InEKF 都能收敛。

定量对比（典型四足场景，初始姿态误差变化）：

估计器	初始误差 5 度	初始误差 15 度	初始误差 30 度	初始误差 45 度
线性 KF	收敛 (~0.5s)	收敛 (~1.2s)	通常不收敛	不收敛
标准 EKF	收敛 (~0.3s)	收敛 (~0.8s)	有时收敛	不收敛
InEKF	收敛 (~0.3s)	收敛 (~0.5s)	收敛 (~1.0s)	收敛 (~2.0s)

这个收敛域的差异在实际部署中很重要——真机启动时 IMU 可能未完成校准，初始姿态估计可能偏离 20-30 度。

传感器融合的工程经验总结 ⭐⭐¶

以下经验从多个四足项目中提炼——不在教科书中，但对部署成败影响很大：

经验 1：Innovation 检验是最好的异常检测手段。每次 Kalman 更新前，检查 innovation 是否在 $3\sigma$ 范围内。超出范围的观测应跳过而非强行融合。

经验 2：噪声参数的调整策略

参数	调大的效果	调小的效果	调参方向
过程噪声 $Q$	更信任观测，估计跟观测走	更信任模型，估计更平滑	从大往小调（宁可信观测）
观测噪声 $R$	更信任模型，观测被"打折"	更信任观测，估计跟观测走	根据传感器规格书设初始值

经验 3：传感器故障的优雅降级

传感器故障	检测方法	降级策略
IMU 数据丢失	时间戳间隔 > 2x 正常周期	用上一帧数据外推，标记"不确定"
编码器卡死	连续 N 帧速度为零但 IMU 显示运动	切换到纯 IMU 推算模式
力传感器漂移	Innovation 检验持续异常	放大该传感器的 $R$，降低权重
全部外感知丢失	无 LiDAR/Camera 更新超过 5s	报警 + 降速 + 仅用本体估计

开放研究问题¶

学习增强估计：用神经网络学习接触瞬间的冲击修正、地形自适应噪声调度、IMU 偏差预测
视觉-腿式紧耦合：将腿式里程计直接嵌入 VIO/LIO 的前端（不只是后端因子），实现毫秒级响应
软地面估计：沙地、雪地、草地上的接触模型不是刚性的——需要新的观测模型考虑地面变形
Event Camera 辅助：超高频视觉传感器（>1000 Hz）与 IMU/编码器对齐，有望替代低频传统相机
基于 Transformer 的多模态融合：用注意力机制自动学习不同传感器的时变权重，替代手工设计的噪声参数

参数	符号	典型值	单位
陀螺仪噪声密度	\(\sigma_g\)	\(0.004\)	rad/s/\(\sqrt{\text{Hz}}\)
加速度计噪声密度	\(\sigma_a\)	\(0.04\)	m/s\(^2/\sqrt{\text{Hz}}\)
陀螺仪偏差不稳定性	\(\sigma_{bg}\)	\(5 \times 10^{-5}\)	rad/s\(^2/\sqrt{\text{Hz}}\)
加速度计偏差不稳定性	\(\sigma_{ba}\)	\(5 \times 10^{-4}\)	m/s\(^3/\sqrt{\text{Hz}}\)

状态量	符号	维度	来源	难度
基座姿态	\(\boldsymbol{R} \in SO(3)\)	3（李代数）	IMU 陀螺仪积分 + 加速度计校正	中
基座位置	\(\boldsymbol{p} \in \mathbb{R}^3\)	3	腿部运动学 + IMU 双积分	高
基座线速度	\(\boldsymbol{v} \in \mathbb{R}^3\)	3	腿部运动学 + IMU 单积分	中
基座角速度	\(\boldsymbol{\omega} \in \mathbb{R}^3\)	3	IMU 陀螺仪直读	低
足端位置	\(\boldsymbol{p}_{f_i} \in \mathbb{R}^3\)	\(3 \times N_{\text{feet}}\)	正运动学 + 接触约束	中
IMU 偏差	\(\boldsymbol{b}_g, \boldsymbol{b}_a\)	6	在线估计	高

传感器	测量量	频率	精度	用于
IMU（加速度计）	比力 \(\boldsymbol{a}_m\)（加速度 + 重力）	400--1000 Hz	偏差 ~0.01 m/s\(^2\)	速度/位置预测
IMU（陀螺仪）	角速度 \(\boldsymbol{\omega}_m\)	400--1000 Hz	偏差 ~0.01 rad/s	姿态积分
关节编码器	关节角度 \(q_i\)	1 kHz	~0.01\(^\circ\)	正运动学
关节电流传感器	关节扭矩 \(\tau_i\)（通过电流推算）	1 kHz	~5%	接触力估计
足端力传感器（可选）	法向接触力 \(F_n\)	1 kHz	~1 N	接触检测
LiDAR / 相机（可选）	点云 / 图像	10--30 Hz	厘米级	漂移修正

方法	公式	适用场景
简单平均	\(\boldsymbol{v} = \frac{1}{k}\sum_i \boldsymbol{v}_i\)	平坦地形
加权平均	\(\boldsymbol{v} = \sum_i w_i \boldsymbol{v}_i\)，\(w_i \propto\) 接触力	软地面
Kalman 融合	每只脚作为独立观测	最优但计算多

性质	ESKF	InEKF
误差 Jacobian \(F\)	依赖 \(\hat{\boldsymbol{x}}\)	常数（不依赖状态）
每步计算	重新计算 \(F\)	\(F\) 只算一次
大初始误差	可能发散	局部收敛域通常更大
一致性	可能不一致	理论一致性更好（在群仿射和观测假设下）
适用场景	通用非线性系统	系统具有群仿射结构

策略	方法	适用场景
增大观测噪声	\(R_i \leftarrow R_i \cdot \alpha\)（\(\alpha \gg 1\)）	轻微滑移
禁用该脚观测	\(p_{c,i} \leftarrow 0\)	明显滑移
降低信任权重	\(w_i \propto 1/\\|\boldsymbol{r}_i\\|\)	连续变化
切换到纯 IMU	暂停所有腿部观测	所有脚滑移（如被推）

参数	设太小	设太大	调试策略
\(Q_{\text{imu}}\)	不信任 IMU，过度依赖观测	过度信任 IMU，观测修正弱	从 IMU 数据手册值开始
\(Q_{\text{foot}}\)（接触）	脚位置"锁死"不更新	脚位置不稳定	~\(10^{-4}\) 开始
\(Q_{\text{foot}}\)（摆动）	摆动脚信息不释放	立即重新初始化	~\(10^2\)
\(R_{\text{fk}}\)	过度信任运动学，模型误差放大	运动学修正太弱	正运动学重复性实验标定

方法	状态维度	典型耗时	适用频率
线性 KF	18	~10 \(\mu\)s	1 kHz
InEKF	21--30	~50--100 \(\mu\)s	1 kHz
因子图（VILENS）	滑窗优化	~5--20 ms	100--200 Hz（独立线程）

参数	调大的效果	调小的效果	调参方向
过程噪声 \(Q\)	更信任观测，估计跟观测走	更信任模型，估计更平滑	从大往小调（宁可信观测）
观测噪声 \(R\)	更信任模型，观测被"打折"	更信任观测，估计跟观测走	根据传感器规格书设初始值

第 57 章：腿足状态估计——IMU / 腿部运动学 / KF / InEKF / 接触检测 / 外感知融合¶

前置自测¶

本章目标¶

57.1 状态估计在腿足控制中的地位 ⭐¶

动机：控制器需要知道"我在哪"¶

如果不做状态估计会怎样¶

腿足状态估计的完整状态向量¶

腿足状态估计 vs SLAM 的对比¶

传感器概览¶

历史演进¶

练习¶

57.2 IMU 运动学与偏差模型 ⭐⭐¶

动机：IMU 是状态估计的"引擎"¶

IMU 测量模型¶

偏差模型¶

姿态的陀螺仪积分¶

预积分概念¶

⚠️ 常见陷阱¶

练习¶

57.3 腿部运动学接触约束 ⭐⭐¶

动机：腿就是"伪 GPS"¶

如果不用腿部运动学会怎样¶

正运动学回顾¶

腿式里程计（Leg Odometry）¶

误差来源分析¶

⚠️ 常见陷阱¶

练习¶

57.4 线性 KF 基座估计器 ⭐⭐¶

动机：最简单有效的腿足状态估计器¶

状态向量设计¶

预测方程（Process Model）¶

观测方程（Measurement Model）¶

完整 KF 循环¶

Kalman 增益的物理解读¶

⚠️ 常见陷阱¶

练习¶

57.5 EKF 与 InEKF ⭐⭐⭐¶

动机：为什么线性 KF 不够¶

Error State Kalman Filter（ESKF）回顾¶

ESKF 的一致性问题¶

Invariant EKF 的核心洞察¶

\(SE_k(3)\) 群结构¶

右不变误差定义¶

群仿射性质（Group Affine Property）¶

InEKF 的完整算法流程¶

代码实现参考¶

InEKF vs 线性 KF vs ESKF 对比¶

⚠️ 常见陷阱¶

练习¶

57.6 接触状态检测 ⭐⭐¶

动机：估计器的"开关"¶

方法 1：基于步态时钟（Gait Schedule）¶

方法 2：GRF 阈值法¶

方法 3：Schmitt 触发器¶

方法 4：基于动量观测器的力估计¶

方法 5：概率接触估计¶

方法对比¶

接触检测的 Chicken-and-Egg 问题¶

⚠️ 常见陷阱¶

练习¶

57.7 滑移检测与地形分类 ⭐⭐⭐¶

动机：当"不滑动"假设失效¶

滑移对状态估计的影响¶

基于速度残差的滑移检测¶

多脚一致性检测¶

检测到滑移后的处理¶

地形分类¶

坡度估计的最小二乘推导¶

⚠️ 常见陷阱¶

练习¶

57.8 外感知融合 ⭐⭐⭐¶

动机：本体感受的漂移极限¶

外感知传感器的角色¶

VILENS：紧耦合因子图融合¶

Pronto：模块化 EKF 融合¶

松耦合 vs 紧耦合¶

⚠️ 常见陷阱¶

练习¶

57.9 Pronto 与工程实战 ⭐⭐¶

动机：从理论到工程部署¶