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

外感知融合¶

难度：⭐⭐~⭐⭐⭐⭐ | 前置：数学/李群与 manif（李群基础）、足式/30_Pinocchio深度精读（Pinocchio）、足式/50_空间向量与浮动基座动力学（空间向量代数与浮动基座动力学）、足式/80_接触力学与约束优化（接触力学）

核心参考文献： - 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$ 题 → 先回对应章节复习）

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

如果第 5 题答不出——本章核心内容依赖 Kalman 滤波。请先学习 Kalman 滤波基础（可参考经典教材 Bar-Shalom "Estimation with Applications to Tracking and Navigation" 或 Sola 2017 "Quaternion Kinematics for the Error-State Kalman Filter" 中的 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 无法预测未来轨迹（足式/110_OCS2完整栈与双线程MPC），WBC 无法计算正确的关节扭矩（足式/90_WBC分层优化与TSID）。

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

场景 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 个广义速度（足式/50_空间向量与浮动基座动力学），编码器只提供其中 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$ 是反对称矩阵（足式/50_空间向量与浮动基座动力学）。

离散化：在时间步长 $\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)$ 是李群基础中学过的指数映射(回顾:Exp 映射将李代数中的旋转向量 $\boldsymbol{\theta} = \boldsymbol{\omega}\Delta t$ 映射到 $SO(3)$ 上的旋转矩阵,几何含义是绕轴 $\boldsymbol{\omega}/\|\boldsymbol{\omega}\|$ 旋转 $\|\boldsymbol{\omega}\|\Delta t$ 弧度)。对于小角度（$\|\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 与腿足估计的融合架构至关重要。

预积分增量的递推定义¶

为了让"预积分"从概念变成可实现的公式，我们需要写出三个增量 $(\Delta \boldsymbol{R}_{ij}, \Delta \boldsymbol{v}_{ij}, \Delta \boldsymbol{p}_{ij})$ 的精确定义。设关键帧 $i$ 和 $j$ 之间有 $n$ 个 IMU 采样（$k = i, i+1, \ldots, j-1$），每步时长 $\Delta t$。Forster et al. (2017) 给出的核心观察是：把世界系的运动方程"除掉"第 $i$ 帧的姿态 $\boldsymbol{R}_i$ 和重力项之后，剩下的增量**只依赖 IMU 测量和偏差**，与第 $i$ 帧的绝对状态无关。

为什么要"除掉" $\boldsymbol{R}_i$？ 回顾连续方程 $\dot{\boldsymbol{v}} = \boldsymbol{R}(\boldsymbol{a}_m - \boldsymbol{b}_a) + \boldsymbol{g}$——速度增量里既有 IMU 项又有重力项，而重力项 $\boldsymbol{g}\,\Delta t$ 不含任何测量、姿态项 $\boldsymbol{R}$ 又随时间变化。如果我们把所有量都左乘 $\boldsymbol{R}_i^T$（投影到第 $i$ 帧的 body frame），并把重力的解析积分单独拿出来，那么剩下的"纯 IMU 积分"部分就与 $\boldsymbol{R}_i, \boldsymbol{v}_i, \boldsymbol{p}_i$ 解耦了。这正是预积分能"预先"算好、不必等到 $i$ 帧状态确定的根本原因。

三个增量的离散递推（忽略噪声、偏差取当前估计 $\boldsymbol{b}^0$）为：

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

\[\Delta \boldsymbol{v}_{ik+1} = \Delta \boldsymbol{v}_{ik} + \Delta \boldsymbol{R}_{ik}\,(\boldsymbol{a}_{m,k} - \boldsymbol{b}_a^0)\,\Delta t\]

\[\Delta \boldsymbol{p}_{ik+1} = \Delta \boldsymbol{p}_{ik} + \Delta \boldsymbol{v}_{ik}\,\Delta t + \tfrac{1}{2}\Delta \boldsymbol{R}_{ik}\,(\boldsymbol{a}_{m,k} - \boldsymbol{b}_a^0)\,\Delta t^2\]

初值 $\Delta \boldsymbol{R}_{ii} = \boldsymbol{I}$，$\Delta \boldsymbol{v}_{ii} = \boldsymbol{0}$，$\Delta \boldsymbol{p}_{ii} = \boldsymbol{0}$。注意这三个式子**完全不出现 $\boldsymbol{R}_i, \boldsymbol{v}_i, \boldsymbol{p}_i, \boldsymbol{g}$**——它们就是"两帧之间的相对运动"，可以在测量到达时立刻累加。

得到增量后，绝对状态之间的约束（即因子图中的"IMU 因子"残差）写为：

\[\boldsymbol{R}_j = \boldsymbol{R}_i\,\Delta \boldsymbol{R}_{ij}, \quad \boldsymbol{v}_j = \boldsymbol{v}_i + \boldsymbol{g}\,\Delta t_{ij} + \boldsymbol{R}_i\,\Delta \boldsymbol{v}_{ij}, \quad \boldsymbol{p}_j = \boldsymbol{p}_i + \boldsymbol{v}_i\,\Delta t_{ij} + \tfrac{1}{2}\boldsymbol{g}\,\Delta t_{ij}^2 + \boldsymbol{R}_i\,\Delta \boldsymbol{p}_{ij}\]

重力和初始状态在这里才重新出现——它们是"已知的解析部分"，与优化变量 $(\boldsymbol{R}_i, \boldsymbol{v}_i, \boldsymbol{p}_i)$ 线性（或群上）耦合，因此每次优化迭代改变 $i$ 帧状态时，不需要重新积分 IMU，只需把新的 $\boldsymbol{R}_i, \boldsymbol{v}_i, \boldsymbol{p}_i$ 代入上式。这就是预积分省下的计算量。

偏差更新的一阶修正¶

预积分增量是在某个"线性化偏差" $\boldsymbol{b}^0$ 处计算的。当优化器更新偏差估计到 $\boldsymbol{b}^0 + \delta\boldsymbol{b}$ 时，严格做法是用新偏差重新积分所有 IMU 测量——这恰恰是预积分想避免的。Forster 的解法是对增量关于偏差做**一阶 Taylor 展开**，把雅可比 $\partial \Delta(\cdot)/\partial \boldsymbol{b}$ 在递推时一并累积：

\[\Delta \tilde{\boldsymbol{R}}_{ij}(\boldsymbol{b}) \approx \Delta \boldsymbol{R}_{ij}(\boldsymbol{b}^0) \cdot \text{Exp}\left(\frac{\partial \Delta \boldsymbol{R}_{ij}}{\partial \boldsymbol{b}_g}\,\delta\boldsymbol{b}_g\right)\]

\[\Delta \tilde{\boldsymbol{v}}_{ij}(\boldsymbol{b}) \approx \Delta \boldsymbol{v}_{ij}(\boldsymbol{b}^0) + \frac{\partial \Delta \boldsymbol{v}_{ij}}{\partial \boldsymbol{b}_g}\,\delta\boldsymbol{b}_g + \frac{\partial \Delta \boldsymbol{v}_{ij}}{\partial \boldsymbol{b}_a}\,\delta\boldsymbol{b}_a\]

\[\Delta \tilde{\boldsymbol{p}}_{ij}(\boldsymbol{b}) \approx \Delta \boldsymbol{p}_{ij}(\boldsymbol{b}^0) + \frac{\partial \Delta \boldsymbol{p}_{ij}}{\partial \boldsymbol{b}_g}\,\delta\boldsymbol{b}_g + \frac{\partial \Delta \boldsymbol{p}_{ij}}{\partial \boldsymbol{b}_a}\,\delta\boldsymbol{b}_a\]

注意旋转增量的修正是**右乘一个小旋转**（因为 $\Delta \boldsymbol{R}$ 活在 $SO(3)$ 流形上，加法无意义），而速度/位置增量的修正是**普通向量加法**（它们活在 $\mathbb{R}^3$）。这五个雅可比块都可以在前向递推时用与 $\Delta \boldsymbol{R}, \Delta \boldsymbol{v}, \Delta \boldsymbol{p}$ 相同的循环顺带更新，无需额外遍历。

本质洞察：预积分的"魔法"不是某个聪明的积分技巧，而是**把状态空间做了一次正确的因式分解**——它把"两帧间的相对运动"（只依赖测量与偏差）从"绝对位姿"（优化变量）中剥离出来。剥离之后，相对运动可以提前一次性算好，绝对位姿的每次更新只是廉价的群乘法。这与 InEKF 的思路异曲同工：两者都通过寻找"与状态无关的不变量"来换取计算上的解耦。区别在于预积分剥离的是时间维度上的相对量，InEKF 剥离的是误差动力学对状态的依赖。

与本章 KF 框架的精确对照：线性 KF（§57.4）和 InEKF（§57.5）是**滤波器**——它们在每个 IMU 步上"用掉"测量后即丢弃，状态是当前时刻的边缘分布，过去不再重新线性化，因此天然不需要预积分。因子图（§57.8 的 VILENS）是**平滑器**——它在一个滑动窗口内保留多个关键帧并反复重新线性化，每次迭代都会改动旧帧的偏差估计，若不做预积分就要反复重积分，代价不可接受。一句话：滤波器边走边忘，所以不预积分；平滑器回头重算，所以必须预积分。 这条界线解释了为什么腿足实时估计器（1 kHz 控制环内）几乎都用滤波器，而高精度离线/低频后端才用因子图。

⚠️ 常见陷阱¶

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

错误做法：直接把加速度计读数当作 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}$ 保持不变。通过正运动学（足式/30_Pinocchio深度精读），我们可以从关节编码器读数计算出脚相对于基座的位置 ${}^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（足式/30_Pinocchio深度精读），给定关节角度 $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 节）
运动学模型误差	连杆长度标定	系统性偏差	离线标定
接触点不确定性	脚底非点接触	等效接触点位置不确定	接触面模型

浮动基座状态与腿式里程计的关系¶

本节解决的问题：腿式里程计观测的究竟是浮动基座 18 维广义坐标中的哪几个？为什么它能观测速度却几乎不能观测全局位置和 yaw？

回顾足式/50_空间向量与浮动基座动力学：浮动基座机器人的广义坐标分为两部分——基座的 6 个自由度 $\boldsymbol{q}_b = (\boldsymbol{p}, \boldsymbol{R}) \in SE(3)$ 和关节的 $n_j$ 个自由度 $\boldsymbol{q}_j$。对 Go2 而言 $n_j = 12$，总广义坐标 $\boldsymbol{q} = (\boldsymbol{q}_b, \boldsymbol{q}_j)$ 维度为 $6 + 12 = 18$（速度层面）。关键事实是：编码器只测量 $\boldsymbol{q}_j$（12 维），而控制器需要的恰恰是无法直接测量的 $\boldsymbol{q}_b$（6 维）。状态估计的全部任务，就是从"看得见的 $\boldsymbol{q}_j$ + IMU"反推"看不见的 $\boldsymbol{q}_b$"。

腿式里程计提供的约束是 §57.3 推导的足端零速度方程。把它写成对基座广义速度的线性约束更能看清结构——对第 $i$ 只接触脚：

\[\boldsymbol{J}_{b,i}\,\boldsymbol{\nu}_b + \boldsymbol{J}_{j,i}\,\dot{\boldsymbol{q}}_j = \boldsymbol{0}, \quad \boldsymbol{\nu}_b = (\boldsymbol{v}_{\text{base}}, \boldsymbol{\omega}_{\text{base}})\]

其中 $\boldsymbol{J}_{b,i} \in \mathbb{R}^{3\times 6}$ 是足端接触雅可比对基座 6 维速度的块，$\boldsymbol{J}_{j,i} \in \mathbb{R}^{3\times n_j}$ 是对关节速度的块。这是一个 3 行方程——单只接触脚只提供 3 个标量约束。

可观测性逐项分析（这是腿式里程计最容易被误解的地方）：

量	单脚接触	双脚接触（trot）	四脚接触（stand）	原因
基座线速度 $\boldsymbol{v}_{\text{base}}$	可观测	可观测（过约束）	可观测（强过约束）	零速度约束 + 已知 $\boldsymbol{\omega}, \dot{\boldsymbol{q}}_j$ 直接解出
基座姿态 roll/pitch	间接（靠 IMU 重力）	间接	间接	腿运动学不含绝对姿态信息
基座姿态 yaw	不可观测	不可观测	不可观测	绕重力轴整体旋转不改变任何足端相对几何
基座全局位置 $\boldsymbol{p}_{\text{base}}$	不可观测（绝对）	不可观测（绝对）	不可观测（绝对）	只有相对首次着地点的增量可积分

为什么速度可观测但绝对位置不可观测？ 这是一个深刻的对称性问题。把整个"机器人 + 已记录的所有足端落点"在世界系里**平移**一个常向量 $\boldsymbol{c}$，或绕重力轴**旋转**一个常角 $\psi$，所有的相对几何（脚相对基座、脚相对脚）完全不变，因此所有腿运动学观测的残差也完全不变——这意味着 $(\boldsymbol{p}_{\text{base}}, \psi)$ 方向上存在一个**未被任何观测约束的零空间**（4 维：3 个平移 + 1 个 yaw）。状态估计器在这些方向上只能做"航位推算"（dead-reckoning，对速度积分），漂移不可避免。这正是 §57.8 必须引入外感知的根本原因：LiDAR/相机的环境特征锚定了世界系，填补了这 4 维零空间。

本质洞察：腿式里程计不是"伪 GPS"那么简单——它更像一个**只测速度、不测绝对位置**的高精度速度计。真正的 GPS 提供绝对位置（消除漂移），而腿式里程计提供的是高精度的**速度观测**，对它积分得到的位置仍会缓慢漂移（漂移率约行走距离的 1--5%，见 §57.8）。把"脚是锚点"这个直觉推得太远，会误以为腿式里程计能消除位置漂移——它不能。它能做的是把漂移速率从纯 IMU 的 $O(t^2)$ 压到 $O(t)$，并且常数小得多。

这对滤波器设计的直接影响：因为 $(\boldsymbol{p}_{\text{base}}, \psi)$ 不可观测，它们对应的协方差分量会**单调增长**（在没有外感知更新时永不收敛）。一个常见的工程错误是看到位置协方差一直涨就以为滤波器出 bug 了——实际上这是可观测性的正确反映。正确做法是接受这种增长，或引入外感知/回环来约束这 4 维。InEKF 的一个优雅之处（§57.5）正在于：它把不可观测子空间与群结构对齐，使得这些方向的误差传播保持一致（consistent），不会被错误地"假装收敛"。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：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 给出了完整的验证过程。

右不变误差的对数线性动力学——常数 $A_t$ 矩阵¶

本节解决的问题：前面反复强调"InEKF 的误差 Jacobian 是常数、不依赖状态估计"。但常数到什么程度？它的具体结构长什么样？这一节把那个抽象的 $A_t$ 矩阵显式写出来，让"自治"从口号变成可验证的矩阵。

考虑最小的腿足 InEKF 状态——$SE_2(3)$（含 $\boldsymbol{R}, \boldsymbol{v}, \boldsymbol{p}$，暂不含足端，先把核心讲清）。右不变误差 $\boldsymbol{\eta} = \hat{\boldsymbol{X}}\boldsymbol{X}^{-1}$，对应的李代数误差向量按 $(\delta\boldsymbol{\theta}, \delta\boldsymbol{v}, \delta\boldsymbol{p})$ 排列（旋转、速度、位置各 3 维，共 9 维）。Barrau & Bonnabel (2017) 证明、Hartley (2020) 在腿足语境下复述的核心结果是：右不变误差满足**对数线性**（log-linear）微分方程

\[\dot{\boldsymbol{\xi}} = A_t\,\boldsymbol{\xi} + \boldsymbol{w}, \qquad A_t = \begin{bmatrix} \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} \\ [\boldsymbol{g}]_\times & \boldsymbol{0} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{I}_3 & \boldsymbol{0} \end{bmatrix}\]

逐块解读这个矩阵——每一块都有清晰的物理含义：

左下块 $[\boldsymbol{g}]_\times$（速度误差受姿态误差驱动）：姿态估计偏一点，重力补偿就投影错，导致速度误差以 $[\boldsymbol{g}]_\times\delta\boldsymbol{\theta}$ 的速率累积。这正是 §57.2 反复强调的"姿态-加速度耦合"，在 InEKF 里以一个常数块的形式出现。注意它是 $[\boldsymbol{g}]_\times$ 而**不是** $[\hat{\boldsymbol{R}}\,\text{something}]_\times$——重力 $\boldsymbol{g}$ 是世界系常向量，所以这一块是真正的常数，与 $\hat{\boldsymbol{X}}$ 无关。
中下块 $\boldsymbol{I}_3$（位置误差受速度误差驱动）：位置是速度的积分，$\dot{\delta\boldsymbol{p}} = \delta\boldsymbol{v}$，天经地义。
第一行全零（姿态误差自治）：右不变姿态误差的演化不受其他误差影响——它只被陀螺仪噪声驱动。

与 ESKF 的对照一目了然：ESKF 的对应矩阵里，这些块会变成 $-\hat{\boldsymbol{R}}[\boldsymbol{a}_m - \hat{\boldsymbol{b}}_a]_\times$ 之类**含 $\hat{\boldsymbol{R}}$、含当前测量、含偏差估计**的项——每一步都要重算，且估计越差这个线性化点越偏。InEKF 把同样的物理耦合"重写"到一个不含 $\hat{\boldsymbol{X}}$ 的坐标系里，这就是"自治"的全部秘密。每只接触脚 $\boldsymbol{d}_i$ 加入后，只是给 $A_t$ 再添一个 $3\times3$ 的**全零行块**（因为 $\dot{\boldsymbol{d}}_i = \boldsymbol{0}$，足端在世界系静止，其误差不被任何状态驱动），结构的"常数性"完全不被破坏。

离散化时，状态转移矩阵 $\boldsymbol{\Phi} = \exp(A_t\,\Delta t)$。因为 $A_t$ 幂零（$A_t^3 = \boldsymbol{0}$，可直接验证三个块相乘两次即归零），这个矩阵指数有**闭式精确解**，无需级数截断：

\[\boldsymbol{\Phi} = \boldsymbol{I} + A_t\,\Delta t + \tfrac{1}{2}A_t^2\,\Delta t^2 = \begin{bmatrix} \boldsymbol{I}_3 & \boldsymbol{0} & \boldsymbol{0} \\ [\boldsymbol{g}]_\times\Delta t & \boldsymbol{I}_3 & \boldsymbol{0} \\ \tfrac{1}{2}[\boldsymbol{g}]_\times\Delta t^2 & \boldsymbol{I}_3\Delta t & \boldsymbol{I}_3 \end{bmatrix}\]

这个 $\boldsymbol{\Phi}$ 在每一步都**完全相同**（只要 $\Delta t$ 不变），可以预计算一次后反复用——这是 InEKF 协方差预测比 ESKF 省算力的具体来源。

不完美 InEKF——把 IMU 偏差并入状态¶

上面的"干净" InEKF 假设 IMU 没有偏差。但 §57.2 已经讲过，偏差必须在线估计。问题来了：偏差 $\boldsymbol{b}_g, \boldsymbol{b}_a$ 活在普通向量空间 $\mathbb{R}^6$，不属于 $SE_k(3)$ 群——把它们硬塞进群会破坏群仿射性质。

Hartley (2020) 的解法叫 "不完美" InEKF（Imperfect InEKF）：把偏差作为**附加的欧氏状态**拼在群状态旁边，整体状态是"李群 × 向量空间"的半直积。代价是误差动力学不再完全自治——偏差误差会通过几个新的块耦合进来：

\[\dot{\boldsymbol{\xi}}_{\text{aug}} = \begin{bmatrix} A_t & A_b \\ \boldsymbol{0} & \boldsymbol{0} \end{bmatrix}\boldsymbol{\xi}_{\text{aug}} + \boldsymbol{w}, \qquad A_b \approx \begin{bmatrix} -\hat{\boldsymbol{R}} & \boldsymbol{0} \\ -[\hat{\boldsymbol{v}}]_\times\hat{\boldsymbol{R}} & -\hat{\boldsymbol{R}} \\ -[\hat{\boldsymbol{p}}]_\times\hat{\boldsymbol{R}} & \boldsymbol{0} \end{bmatrix}\]

关键观察：左上的 $A_t$ 块仍然是常数（保留了 InEKF 的好处），但耦合偏差的 $A_b$ 块**重新出现了 $\hat{\boldsymbol{R}}, \hat{\boldsymbol{v}}, \hat{\boldsymbol{p}}$——也就是说，偏差通道把"对状态估计的依赖"又带回来了一部分。这就是名字里"不完美"的由来：它不是理论上纯净的不变滤波器，而是一个**工程折中——核心的姿态/速度/位置误差仍享受自治性，只在偏差耦合上退回到类 EKF 的线性化。

本质洞察：不完美 InEKF 揭示了 InEKF 理论的真实边界——群仿射性质只对"能被群作用搬运"的量成立。姿态、速度、位置、足端位置都能被 $SE_k(3)$ 的作用一致地变换，所以它们享受自治误差；而 IMU 偏差是传感器内禀属性，不随机器人位姿变换而变换，因此天生在群结构之外。理解这一点，就不会盲目相信"InEKF 处处比 EKF 好"——它只在系统的群仿射部分占优。实践中（Hartley 报告，以及 Cassie/Digit 的部署经验）这种折中已足够：偏差变化慢、$A_b$ 引入的线性化误差小，整体仍显著优于全 EKF。

⚠️ 概念误区：以为加了偏差就丧失了 InEKF 的全部优势

新手想法："既然 $A_b$ 又依赖状态了，那不完美 InEKF 不就退化成 ESKF 了吗？"

实际上：退化是**局部的、只在偏差通道上的**。$A_t$ 主块（9×9，姿态/速度/位置）仍是常数，初始姿态误差大时的收敛优势主要来自这一块。偏差误差量级小（$\sim 10^{-2}$）、变化慢，$A_b$ 引入的线性化误差是二阶小量。所以不完美 InEKF 在"大初始姿态误差"这个 InEKF 最看重的场景下，依然明显胜过 ESKF。

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}}$ 是伴随矩阵（Adjoint representation）。

为什么协方差传播中出现了 $\text{Adj}_{\hat{X}}$？ 这是 InEKF 相对于标准 EKF 的关键区别之一,值得深入理解。在标准 EKF 中,过程噪声直接叠加在状态上：$P^- = \Phi P \Phi^T + Q$。但在 InEKF 中,过程噪声(IMU 噪声)定义在 body frame 中,而误差状态定义在群上(世界系)。从 body frame 噪声到群误差的变换恰好是群的伴随表示 $\text{Adj}_X$——它把"在 body frame 中的小扰动"变换为"在群切空间中的小扰动"。对于 $SE_k(3)$ 群,$\text{Adj}_X$ 是一个 $(3+3+3k) \times (3+3+3k)$ 的矩阵,其结构取决于当前状态估计 $\hat{X}$ 中的旋转和平移分量。直觉上,$\text{Adj}_{\hat{X}} Q \text{Adj}_{\hat{X}}^T$ 的作用是"把 IMU 噪声从机器人自身坐标系转换到世界系,然后加到协方差上"——和经典 EKF 中用 $F Q F^T$ 传播噪声的逻辑相同,只是坐标变换用了群论工具。

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

计算观测残差：$\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 偏差校正	无（需外部）	有	有（扩展状态）

另一条路线：Bloesch 的两状态隐式滤波器（TSIF）¶

本节解决的问题：前面所有滤波器（KF/ESKF/InEKF）都建立在"显式过程模型"上——先写出 $\dot{\boldsymbol{x}} = f(\boldsymbol{x}, \boldsymbol{u})$ 再线性化。但有些信息天然是"约束"而非"状态转移"，硬塞进过程模型很别扭。Bloesch et al. (2017) 的 TSIF 提供了一条完全不同的建模路线。

Bloesch 是腿足状态估计领域绕不开的名字。他 2012 年的 RSS 论文（本章开篇引用）提出了基于四元数的观测式 EKF（QEKF），首次严格融合 IMU 与腿运动学，是整个领域的奠基工作。但他 2017 年在 IEEE RA-L 上提出的**两状态隐式滤波器**（Two-State Implicit Filter, TSIF）走了一条更激进的路，并且成为 ANYbotics ANYmal 机器人的默认估计器——值得专门讲清。

TSIF 与传统滤波器的根本区别——隐式 vs 显式：

维度	传统滤波器（KF/EKF/InEKF）	TSIF
过程模型	显式：$\boldsymbol{x}_{k+1} = f(\boldsymbol{x}_k, \boldsymbol{u}_k)$	隐式：用残差 $\boldsymbol{r}(\boldsymbol{x}_k, \boldsymbol{x}_{k+1}, \boldsymbol{u}) = \boldsymbol{0}$ 描述
信息形式	区分"预测"与"观测"两类	统一为残差——所有信息（IMU、运动学、接触）都是残差项
状态范围	当前单帧 $\boldsymbol{x}_k$	两帧 $(\boldsymbol{x}_k, \boldsymbol{x}_{k+1})$ 同时出现在残差里
数学引擎	Kalman 增益更新	扩展信息滤波器（Extended Information Filter）上的高斯-牛顿

为什么叫"两状态"？ 因为它的残差同时约束相邻两个时刻的状态。一个典型残差形如 $\boldsymbol{r} = \boldsymbol{h}(\boldsymbol{x}_{k+1}) - \boldsymbol{g}(\boldsymbol{x}_k, \boldsymbol{u}_k)$——它把"$k$ 时刻的状态加上 IMU 输入应该导出 $k+1$ 时刻的状态"这件事写成一个**应当为零的残差**，而不是一个**显式的赋值**。求解时，滤波器在每步对所有残差做一次高斯-牛顿迭代，同时更新 $(\boldsymbol{x}_k, \boldsymbol{x}_{k+1})$ 的信息矩阵。

为什么"隐式/残差"建模更灵活？ 考虑足端接触约束"接触脚速度为零"。在显式 KF 里，这个约束必须被改写成一个观测方程 $\boldsymbol{z} = \boldsymbol{H}\boldsymbol{x}$（§57.4 费了不少力气），且接触/摆动切换时要手动调噪声。而在 TSIF 里，它直接就是一条残差 $\boldsymbol{r}_{\text{contact}} = \boldsymbol{v}_{\text{foot}}(\boldsymbol{x}) - \boldsymbol{0}$，接触时启用、摆动时关掉，无需改写成"过程"或"观测"的二选一形式。对于"难以写出干净过程模型"的信息（如带噪声的运动学约束、软接触），残差形式天然更顺手——这正是 Bloesch 设计 TSIF 的初衷。

本质洞察：TSIF 模糊了"预测步"与"更新步"的经典二分。在标准 Kalman 框架里，预测（用过程模型外推）和更新（用观测修正）是两个泾渭分明的阶段。TSIF 把两者统一成"一堆残差的联合最小二乘"——IMU 测量是残差、运动学是残差、先验是残差，地位平等。这个视角与因子图（§57.8）惊人地相似：因子图也是"一堆因子（残差）的联合优化"。区别在于 TSIF 只保留**两帧**（固定的极小窗口），所以仍是 $O(1)$ 复杂度的递归滤波器；而因子图保留多帧滑窗，是更重的平滑器。可以把 TSIF 理解为"窗口长度为 2 的因子图"，或"用残差语言重写的滤波器"——它站在滤波与平滑的交界线上。

TSIF 在腿足语境下的实用优势（来自 ANYmal 部署经验）：

建模一致性：IMU、关节运动学、接触约束全部以残差形式加入，新增传感器只需添一条残差，无需判断它属于"过程"还是"观测"
接触切换平滑：接触约束作为可开关的残差，切换时只是残差权重变化，没有显式过程模型那种状态维度跳变（对比 §57.5 要点 1 里 InEKF 动态增删 $SE_k(3)$ 列的麻烦）
计算复杂度可控：信息滤波形式下复杂度与扩展信息滤波器相当，仍能 1 kHz 实时运行

何时选 TSIF？ 当你的信息源以"约束/残差"为主（大量运动学约束、软接触、难以显式建模的耦合），或你希望一个统一的残差接口来增删传感器时，TSIF 比经典 KF 更顺手。当系统有干净的群仿射结构、且最看重大初始误差下的收敛保证时，InEKF 仍是首选。三者并非互斥——它们代表了"显式滤波（KF/InEKF）— 隐式滤波（TSIF）— 滑窗平滑（因子图）"这条连续谱上的三个点。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：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）¶

最简单的方法：根据步态规划器（足式/120_步态管理与接触序列）的时钟判断"此时这只脚应该在接触相还是摆动相"。

// 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 可以将广义外力映射到足端接触力（足式/80_接触力学与约束优化）：

\[\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	无	高	中	研究前沿
纯运动学（方法 6）	否	~1 ms	中	中	低	无力无电流传感器

方法 6：纯运动学接触检测（无任何力/电流传感器）¶

前面的方法 2、3 需要力传感器，方法 4 需要可靠的关节扭矩（电流）。但有些低成本平台**两者都没有**——只有关节编码器和 IMU。此时仍可做接触检测，依据是一个纯几何/运动学的事实：接触脚的足端在世界系中既不动、又贴着地面。

由此得到两个互补的判据，对第 $i$ 只脚：

\[\text{判据一（足端速度）}：\quad \|\boldsymbol{v}_{f_i}^{world}\| = \|\boldsymbol{v}_{\text{base}} + \boldsymbol{\omega}\times(\boldsymbol{R}\,{}^B\boldsymbol{r}_{f_i}) + \boldsymbol{R}\,{}^B\boldsymbol{J}_i\dot{\boldsymbol{q}}\| < v_{\text{thresh}}\]

\[\text{判据二（足端高度）}：\quad |z_{f_i}^{world} - \hat{z}_{\text{ground}}| < h_{\text{thresh}}\]

判据一说"接触脚不应该在动"——但它有个循环依赖：算 $\boldsymbol{v}_{f_i}^{world}$ 需要 $\boldsymbol{v}_{\text{base}}$，而 $\boldsymbol{v}_{\text{base}}$ 又来自状态估计（用上一步的值，见下面的 chicken-and-egg 讨论）。判据二说"接触脚应该在地面高度附近"——它需要一个地面高度估计 $\hat{z}_{\text{ground}}$（可由 §57.7 的平面拟合给出，或在平地上设为常数）。

为什么要两个判据求"与"？ 单用足端速度会误判：摆动腿在轨迹最高点速度也很小（即将换向），可能被误判为接触。单用高度也会误判：脚刚好擦过地面但未承重时高度满足、却不是真接触。两者**同时满足**才判接触，能滤掉这两类假阳性。这是典型的"多证据融合降低误检"思路。

局限要诚实说明：纯运动学检测在**软地面**（脚陷进沙/雪，速度判据失效因为脚一直在缓慢下沉）和**斜坡/台阶**（高度判据失效因为 $\hat{z}_{\text{ground}}$ 不是常数）上明显变差。它是"没有更好传感器时的兜底方案"，精度介于步态时钟与 Schmitt 触发器之间。现代高端平台（ANYmal、Spot）都配了足端力或可靠电流，不依赖纯运动学；但 Go2 这类消费级平台在某些固件下确实退回到运动学 + 步态时钟的组合。

前沿：形态感知的接触感知网络。2024 年的 MI-HGNN（Morphology-Informed Heterogeneous Graph Neural Network，本章延伸阅读已列）把机器人的运动学树编码成异构图，节点是关节/连杆、边是运动学连接，用图神经网络从本体感受信号同时推断四只脚的接触概率。它的卖点是**跨机器人泛化**——在一种四足上训练的网络，因为图结构显式编码了形态，能迁移到关节布局不同的另一种四足，而不必从零重训。这代表了接触检测从"手工特征 + 逻辑回归"（方法 5）走向"结构化学习"的趋势。

概率接触如何严格融入滤波器协方差¶

§57.6 前面用 $p_c$ 调制观测噪声 $R_i$ 给了直觉，这里补上更严格、也更常用于 InEKF 的做法——把接触概率直接作用在**过程噪声**上。回顾 §57.4：接触脚的足端过程噪声 $\sigma_{f_i}^2$ 小（脚不动）、摆动脚的大（位置不确定）。用连续的 $p_c$ 在这两个极端之间插值：

\[\boldsymbol{Q}_{f_i} = \big[p_{c,i}\,\sigma_{\text{contact}}^2 + (1 - p_{c,i})\,\sigma_{\text{swing}}^2\big]\,\boldsymbol{I}_3\,\Delta t\]

当 $p_{c,i}\to 1$，足端噪声趋于 $\sigma_{\text{contact}}^2$（脚被"钉住"）；$p_{c,i}\to 0$ 时趋于 $\sigma_{\text{swing}}^2$（脚自由漂移，等价于"释放"该脚的约束）。这种软切换比硬阈值有两个好处：(1) 接触建立/断开的过渡帧不会出现协方差的阶跃跳变，估计更平滑；(2) 当 $p_c$ 处于中间值（检测不确定）时，滤波器自动只**部分**信任该脚——这正是 §57.6 思维陷阱里"模糊接触优于错误二元判断"的数学落地。Bloesch 的 TSIF（§57.5）更进一步，把接触约束作为**可连续加权的残差**，权重直接由 $p_c$ 给出，机制上更统一。

接触检测的 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 等高速步态中，着地瞬间的冲击力可能瞬间超过摩擦锥极限（足式/80_接触力学与约束优化），导致微滑移。

练习¶

练习 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）精度更高但需要从头设计整个估计器。

与 SLAM/LIO 的具体衔接（松耦合工程细节）¶

本节解决的问题：上面说"SLAM 输出作为修正观测"一句话带过，但真正接线时坑很多——坐标系怎么对齐？延迟怎么补？SLAM 自己也会跳变（回环），怎么不把跳变灌进控制器？本节把松耦合落到可实现的细节。

设本体感受估计器（§57.4 的线性 KF 或 §57.5 的 InEKF）以 1 kHz 输出高频位姿 $({}^{O}\hat{\boldsymbol{R}}_b, {}^{O}\hat{\boldsymbol{p}}_b)$，其中 $O$ 是估计器的里程计系（odometry frame，启动时刻定义、随时间缓慢漂移）；SLAM/LIO（如 FAST-LIO2）以 10 Hz 输出 $({}^{M}\boldsymbol{R}_b, {}^{M}\boldsymbol{p}_b)$，其中 $M$ 是地图系（map frame，全局一致但低频、可能因回环而跳变）。两套位姿描述同一个基座，但活在不同参考系——衔接的本质是估计并维护两系之间的变换 ${}^{M}\boldsymbol{T}_{O} \in SE(3)$。

第一步：把 SLAM 当作对 ${}^{M}\boldsymbol{T}_{O}$ 的观测，而不是直接覆盖位姿。 这是松耦合最关键、也最常被做错的一点。新手做法是每来一帧 SLAM 就把估计器位姿直接设成 SLAM 值——结果是控制器看到的位姿每 100 ms 抖一下（10 Hz 阶跃），且 SLAM 回环时位姿瞬间跳几十厘米，控制器直接发疯。正确做法是：SLAM 与同时刻的里程计位姿之比给出一次对 ${}^{M}\boldsymbol{T}_{O}$ 的测量

\[{}^{M}\boldsymbol{T}_{O}^{\text{meas}} = {}^{M}\boldsymbol{T}_b \cdot ({}^{O}\boldsymbol{T}_b)^{-1}\]

然后对 ${}^{M}\boldsymbol{T}_{O}$ 做**低通滤波/小增益 KF 更新**（因为两系相对漂移很慢，秒级时间常数）。控制器始终用平滑的高频里程计位姿 ${}^{O}\boldsymbol{T}_b$ 做实时控制，只有需要全局目标（导航航点）时才用 ${}^{M}\boldsymbol{T}_{O}\cdot{}^{O}\boldsymbol{T}_b$ 把它投到地图系。回环跳变被 ${}^{M}\boldsymbol{T}_{O}$ 这一层"吸收"，不传染给控制器——这套"双系 + 缓慢对齐变换"正是 ROS 里 map → odom → base_link 三段 TF 树的设计哲学。

第二步：延迟补偿。 SLAM 的 10 Hz 位姿对应的是约 50--100 ms **之前**的时刻（点云累积 + ICP 配准耗时）。直接拿它和当前里程计比，会在 ${}^{M}\boldsymbol{T}_{O}^{\text{meas}}$ 里混入这段时间的运动误差（1 m/s 下 8 cm，§57.8 陷阱已算过）。正确做法是用 SLAM 帧自带的**时间戳**回查里程计的历史缓冲区，取**同一时刻**的 ${}^{O}\boldsymbol{T}_b$ 来求比值。所以估计器必须维护一个按时间戳索引的位姿环形缓冲（保存最近 ~0.5 s 即可），这是延迟松耦合的标配数据结构。

衔接形式对照：

衔接方式	谁是主估计器	SLAM 的角色	跳变隔离	实现成本
直接覆盖（❌ 错误）	无	直接写位姿	无，跳变灌入控制	极低
双系对齐（✅ 松耦合）	本体感受估计器	观测 ${}^{M}\boldsymbol{T}_{O}$	${}^{M}\boldsymbol{T}_{O}$ 层吸收	低-中
位姿先验注入	本体感受估计器	给滤波器加位姿因子/伪观测	靠 innovation 门限	中
紧耦合（VILENS）	统一因子图	同级因子	联合优化天然一致	高

本质洞察：松耦合衔接的核心不是"用 SLAM 修正里程计"，而是**承认两套估计各有不可替代的频域分工，并用一个缓变变换把它们缝起来**。里程计系负责"快而局部光滑"（控制需要的连续性），地图系负责"慢而全局一致"（导航需要的无漂移）。把它们强行合成一套位姿，必然在"光滑"和"全局一致"之间二选一而两头不讨好。这与 §57.1 开篇的洞察一脉相承——本体感受与外感知是互补而非替代，衔接层的任务是让各自在擅长的频段说话。

与紧耦合的边界：松耦合的代价是丢失交叉约束——SLAM 不能反过来帮本体感受估计 IMU 偏差，本体感受也不能给 SLAM 的点云配准提供运动先验去畸变。VILENS 的紧耦合（§57.8 前文）正是为了拿回这些交叉约束：在同一个因子图里，腿式里程计因子能约束视觉尺度、IMU 预积分能给 LiDAR 去畸变提供高频运动。工程权衡：已有成熟 LIO 栈（如 FAST-LIO2）、想最快出原型 → 松耦合双系对齐；追求极限精度、有算力和人力从头搭后端 → 紧耦合因子图。

⚠️ 常见陷阱¶

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

错误做法：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 检验, 鲁棒性优先

本质洞察:腿足状态估计的核心矛盾是**信息的互补性与耦合性**。IMU 提供高频动态信息但会漂移,腿部运动学提供绝对位置约束但依赖接触假设,外部感知提供全局校正但有延迟。三者的信息在频域上互补(IMU 高频准、外感知低频准、腿部运动学中频准),在时域上耦合(接触检测错误会污染所有信息源)。一个好的状态估计器不是简单地"融合"三者,而是在**不同时间尺度上动态调节三者的权重**——接触瞬间重信运动学,飞行相重信 IMU,长距离重信外感知。这种自适应权重调节正是 Kalman 滤波器的数学本质——$K = PH^T(HPH^T + R)^{-1}$ 中,当某个传感器的不确定性 $R$ 增大时,其权重自动降低。

故障排查手册¶

腿足状态估计在真机部署中最常见的故障场景及排查方法:

症状	可能原因	排查步骤	相关小节
位置估计持续漂移(>0.1 m/min)	IMU 加速度计偏差未被正确估计;接触检测阈值过高导致接触观测缺失	1. 打印在线估计的 $b_a$ 值,检查是否收敛 2. 静态放置 60s 观察位置漂移量级 3. 打印接触检测状态,确认触地时确实检测到接触 4. 对比不同 $Q/R$ 下的漂移速率	57.2, 57.6
姿态估计突然跳变(>5度)	加速度计受到冲击(如落地)导致错误的重力方向估计;磁力计干扰(如果使用)	1. 检查跳变时刻是否对应落地冲击 2. 在 innovation 检验中设置更严格的阈值($2\sigma$) 3. 增大加速度计测量噪声 $R_a$ 降低冲击时刻的信任度	57.2, 57.4
速度估计出现尖刺(>1 m/s 跳变)	脚底滑移导致运动学观测错误;接触切换瞬间的不连续性	1. 检查滑移检测模块是否正确标记滑移 2. 在接触切换前后 50ms 内增大运动学观测噪声 $R_{\text{kin}}$ 3. 检查足端 Jacobian 在关节奇异位置附近是否数值不稳定	57.3, 57.7
滤波器发散(协方差矩阵无限增大)	初始协方差 $P_0$ 设置过大;过程噪声 $Q$ 过大;系统不可观(所有脚都在摆动)	1. 检查是否存在所有脚同时摆动的时段(如 gallop 的飞行相)——此时运动学观测全部失效 2. 减小 $P_0$ 3. 在飞行相切换到纯 IMU 预测模式并限制协方差增长	57.4, 57.5
真机精度远低于仿真	关节编码器存在背隙/柔性;IMU 安装位置与 URDF 定义不一致;时间戳同步问题	1. 用 Allan 方差工具分析真实 IMU 数据,获取噪声参数 2. 检查 IMU 到基座的外参标定 3. 检查编码器和 IMU 数据的时间戳对齐(允许偏差 <1ms)	57.9

Allan 方差调参实战流程 ⭐⭐¶

Allan 方差是确定 IMU 噪声参数(白噪声密度 $\sigma_w$ 和随机游走 $\sigma_{rw}$)的标准方法。以下是实战流程:

Step 1: 采集静态数据。将 IMU 水平静止放置,采集 2-4 小时的原始加速度和陀螺仪数据。环境温度尽量恒定。

Step 2: 计算 Allan 方差。对每个轴独立计算:

\[\sigma^2_A(\tau) = \frac{1}{2(N-1)} \sum_{k=1}^{N-1} [\bar{y}_{k+1}(\tau) - \bar{y}_k(\tau)]^2\]

其中 $\bar{y}_k(\tau)$ 是第 $k$ 个长度为 $\tau$ 的窗口内的均值。

Step 3: 读取噪声参数。在 log-log 图上: - 白噪声密度 $\sigma_w$ 对应斜率 $-1/2$ 段在 $\tau = 1$ s 处的截距 - 随机游走 $\sigma_{rw}$ 对应斜率 $+1/2$ 段在 $\tau = 3$ s 处的截距(需乘以 $\sqrt{3}$)

Step 4: 设置 Kalman 滤波参数。 - 连续时间过程噪声: $Q_c^{\text{gyro}} = \sigma_w^2 I_{3\times3}$, $Q_c^{\text{bias}} = \sigma_{rw}^2 I_{3\times3}$ - 离散化: $Q_d = Q_c \cdot \Delta t$(白噪声），$Q_d^{\text{bias}} = Q_c^{\text{bias}} \cdot \Delta t$（随机游走）

工程经验: Allan 方差给出的参数通常需要在真机上微调——放大 2-5 倍作为初始值(宁可过信观测),再根据 innovation 检验结果逐步收紧。

接触辅助 InEKF 实战要点(Hartley 2020) ⭐⭐⭐¶

Ross Hartley 的 Contact-Aided InEKF 是当前腿足 InEKF 的标准实现。以下是从部署经验中提炼的关键要点:

要点 1: 状态维度随接触脚数动态变化。每只接触脚对应 $SE_k(3)$ 中的一列。当一只脚从摆动切换到支撑时,需要在群矩阵中"添加"一列;从支撑切换到摆动时"删除"一列。这在代码实现中需要动态调整矩阵大小,是 InEKF 实现中最容易出 bug 的地方。

要点 2: 初始化接触脚位置。新接触的脚的位置 $d_i$ 需要用当前状态估计 + 正运动学来初始化。初始化精度直接影响后续估计质量——如果正运动学计算有误(比如 URDF 参数不准),初始化误差会传播到基座位置估计中。

要点 3: 右不变 vs 左不变的选择。Hartley 2020 推荐**右不变**(Right-Invariant EKF),因为 IMU 驱动的惯性导航方程在右不变表示下具有群仿射结构。选错不变性类型(如用左不变)会导致误差动力学不再自治,丧失 InEKF 的核心优势。

要点 4: 与 MPC 的时间同步。InEKF 的估计频率通常为 400-1000 Hz(与 IMU 同频),而 MPC 的查询频率为 50-100 Hz。两者之间需要时间戳对齐:MPC 查询时,估计器应返回最接近查询时刻的状态估计。如果延迟超过 2-3 ms,MPC 的预测起点就会偏离真实状态,导致控制性能下降。

方法选择指南¶

需要状态估计器？
│
├─ 只需短距离控制（< 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/  (足式/90_WBC分层优化与TSID-足式/120_步态管理与接触序列)
├── model/    (足式/30_Pinocchio深度精读-足式/50_空间向量与浮动基座动力学)
└── gait/     (足式/120_步态管理与接触序列)

与前后章的连接： - 向前承接：足式/30_Pinocchio深度精读（Pinocchio 正运动学）提供足端位置/Jacobian 计算，足式/50_空间向量与浮动基座动力学（浮动基座动力学）定义状态空间结构，足式/80_接触力学与约束优化（接触力学）提供接触力约束的数学基础 - 向后指向：状态估计器的输出（基座位姿、速度）直接送入足式/90_WBC分层优化与TSID（WBC）的 QP 和足式/110_OCS2完整栈与双线程MPC（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	报警 + 降速 + 仅用本体估计

跨章综合练习¶

[跨章综合题] 练习 57.X：状态估计 + 接触力学 + 控制的闭环分析

本题需要综合足式/80_接触力学与约束优化(接触力学)、足式/90_WBC分层优化与TSID(WBC)和本章(状态估计)的知识。

场景: Go2 四足机器人在平地上 trot 行走,右前脚踩到一块冰面(摩擦系数从 0.6 骤降到 0.1)。

(足式/80_接触力学与约束优化) 踩到冰面后,摩擦锥约束 $\|\boldsymbol{\lambda}_t\| \le \mu \lambda_n$ 的可行域如何变化?原来的接触力方案是否仍然可行?
(本章 57.7) 滑移检测模块如何发现这只脚在打滑?写出基于速度残差的检测逻辑。从接触到检测到滑移,大约需要多长时间(考虑滤波器延迟)?
(本章 57.4/57.6) 滑移检测触发后,状态估计器应该做什么?如果不做任何处理(继续把滑移的脚当作静止接触),位置估计会如何受影响?
(足式/90_WBC分层优化与TSID) 假设状态估计正确地标记了滑移,WBC 应该如何调整力分配?写出修改后的 QP 约束(对滑移腿降低摩擦锥约束或直接移除该腿的接触假设)。

开放研究问题¶

学习增强估计：用神经网络学习接触瞬间的冲击修正、地形自适应噪声调度、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 融合	每只脚作为独立观测	最优但计算多

量	单脚接触	双脚接触（trot）	四脚接触（stand）	原因
基座线速度 \(\boldsymbol{v}_{\text{base}}\)	可观测	可观测（过约束）	可观测（强过约束）	零速度约束 + 已知 \(\boldsymbol{\omega}, \dot{\boldsymbol{q}}_j\) 直接解出
基座姿态 roll/pitch	间接（靠 IMU 重力）	间接	间接	腿运动学不含绝对姿态信息
基座姿态 yaw	不可观测	不可观测	不可观测	绕重力轴整体旋转不改变任何足端相对几何
基座全局位置 \(\boldsymbol{p}_{\text{base}}\)	不可观测（绝对）	不可观测（绝对）	不可观测（绝对）	只有相对首次着地点的增量可积分

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

维度	传统滤波器（KF/EKF/InEKF）	TSIF
过程模型	显式：\(\boldsymbol{x}_{k+1} = f(\boldsymbol{x}_k, \boldsymbol{u}_k)\)	隐式：用残差 \(\boldsymbol{r}(\boldsymbol{x}_k, \boldsymbol{x}_{k+1}, \boldsymbol{u}) = \boldsymbol{0}\) 描述
信息形式	区分"预测"与"观测"两类	统一为残差——所有信息（IMU、运动学、接触）都是残差项
状态范围	当前单帧 \(\boldsymbol{x}_k\)	两帧 \((\boldsymbol{x}_k, \boldsymbol{x}_{k+1})\) 同时出现在残差里
数学引擎	Kalman 增益更新	扩展信息滤波器（Extended Information Filter）上的高斯-牛顿

策略	方法	适用场景
增大观测噪声	\(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（独立线程）

症状	可能原因	排查步骤	相关小节
位置估计持续漂移(>0.1 m/min)	IMU 加速度计偏差未被正确估计;接触检测阈值过高导致接触观测缺失	1. 打印在线估计的 \(b_a\) 值,检查是否收敛 2. 静态放置 60s 观察位置漂移量级 3. 打印接触检测状态,确认触地时确实检测到接触 4. 对比不同 \(Q/R\) 下的漂移速率	57.2, 57.6
姿态估计突然跳变(>5度)	加速度计受到冲击(如落地)导致错误的重力方向估计;磁力计干扰(如果使用)	1. 检查跳变时刻是否对应落地冲击 2. 在 innovation 检验中设置更严格的阈值(\(2\sigma\)) 3. 增大加速度计测量噪声 \(R_a\) 降低冲击时刻的信任度	57.2, 57.4
速度估计出现尖刺(>1 m/s 跳变)	脚底滑移导致运动学观测错误;接触切换瞬间的不连续性	1. 检查滑移检测模块是否正确标记滑移 2. 在接触切换前后 50ms 内增大运动学观测噪声 \(R_{\text{kin}}\) 3. 检查足端 Jacobian 在关节奇异位置附近是否数值不稳定	57.3, 57.7
滤波器发散(协方差矩阵无限增大)	初始协方差 \(P_0\) 设置过大;过程噪声 \(Q\) 过大;系统不可观(所有脚都在摆动)	1. 检查是否存在所有脚同时摆动的时段(如 gallop 的飞行相)——此时运动学观测全部失效 2. 减小 \(P_0\) 3. 在飞行相切换到纯 IMU 预测模式并限制协方差增长	57.4, 57.5
真机精度远低于仿真	关节编码器存在背隙/柔性;IMU 安装位置与 URDF 定义不一致;时间戳同步问题	1. 用 Allan 方差工具分析真实 IMU 数据,获取噪声参数 2. 检查 IMU 到基座的外参标定 3. 检查编码器和 IMU 数据的时间戳对齐(允许偏差 <1ms)	57.9

参数	调大的效果	调小的效果	调参方向
过程噪声 \(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）¶

右不变误差的对数线性动力学——常数 \(A_t\) 矩阵¶

不完美 InEKF——把 IMU 偏差并入状态¶

InEKF 的完整算法流程¶

代码实现参考¶

InEKF vs 线性 KF vs ESKF 对比¶

另一条路线：Bloesch 的两状态隐式滤波器（TSIF）¶

⚠️ 常见陷阱¶

练习¶

57.6 接触状态检测 ⭐⭐¶

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

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

方法 2：GRF 阈值法¶

方法 3：Schmitt 触发器¶

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

方法 5：概率接触估计¶

方法对比¶

方法 6：纯运动学接触检测（无任何力/电流传感器）¶

概率接触如何严格融入滤波器协方差¶

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

⚠️ 常见陷阱¶

练习¶

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

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

滑移对状态估计的影响¶

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

多脚一致性检测¶

检测到滑移后的处理¶

地形分类¶

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

⚠️ 常见陷阱¶

练习¶

57.8 外感知融合 ⭐⭐⭐¶

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