本体感知与状态估计

一句话定义

本体感知（proprioception）通过编码器、IMU、电流等内部信号回答"我自己现在处于什么状态"，配合滤波算法（状态估计）输出控制器与策略所需的高频状态量。

为什么重要

• 控制闭环的输入就是状态：没有可靠的状态估计，位置控制、力控、RL 策略全都无从谈起。
• 对足式机器人，机身姿态与接触状态估计是行走的第一前提——RL 策略的观测向量里一大半是本体量。
• 它是高频（200~1000 Hz）、永不遮挡的感知通道，是系统可靠性的兜底。

前置知识

kp-101；概率论基础。

核心概念

原始信号源：

• 编码器：关节角度。绝对式编码器开机即知位姿；增量式需回零。双编码器（输出端+电机端）可测传动柔性。
• 电流/力矩传感：电机电流 ∝ 力矩（准直驱方案靠它做力控）；部分关节有应变片直接测力矩。
• IMU：加速度计（比力）+ 陀螺仪（角速度），高频但漂移；磁力计提供航向参考（室内易受干扰）。

状态估计目标：从带噪观测推断状态向量，如足式机器人的 $\mathbf{x} = [\text{机身位姿}, \text{速度}, \text{各关节角}, \text{接触概率}]$。

卡尔曼滤波族：线性高斯的最优估计器（KF）；非线性系统用扩展卡尔曼（EKF，一阶泰勒展开）或无迹卡尔曼（UKF，采样传播）；因子图/图优化（GTSAM）处理更大规模的历史约束。

原理与机制

滤波 = 预测 + 更新 的循环：用运动模型把状态往前推一步（预测，不确定性增大），用传感器观测把不确定性拉回来（更新）。互补滤波是它的极简版：陀螺仪积分提供短期准的角度，加速度计长期平均提供重力方向参考，加权融合。

足式机器人典型估计流水线（Bloesch et al. 2012 起）：

编码器+IMU ─► 腿式里程计（假设支撑足不滑，反推机身位移）
              +
加速度计积分 ─► 速度估计（EKF）
              +
接触检测（力传感/力矩阈值/观测器）──► 决定哪些腿可作为支撑
              ▼
输出：机身位置/速度/姿态 + 接触状态 ──► 送给 MPC/RL 策略（[kp-204][kp-206]）

为什么策略喜欢本体观测：编码器/IMU 是毫秒级、无遮挡、不依赖光照的稳定输入；外部视觉只作为低频修正（位置漂移校正），这样的观测分层让 RL 策略训练更稳。

公式与图示

离散卡尔曼滤波两步：

预测：$\hat{\mathbf{x}}_{k|k-1} = \mathbf{F}_k \hat{\mathbf{x}}_{k-1} + \mathbf{B}_k \mathbf{u}_k, \quad \mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1} \mathbf{F}_k^\top + \mathbf{Q}_k$

更新：$\mathbf{K}_k = \mathbf{P}_{k|k-1}\mathbf{H}_k^\top(\mathbf{H}_k\mathbf{P}_{k|k-1}\mathbf{H}_k^\top + \mathbf{R}_k)^{-1}$，$\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k(\mathbf{z}_k - \mathbf{H}_k\hat{\mathbf{x}}_{k|k-1})$

直觉：$\mathbf{K}$（卡尔曼增益）是"这次更相信模型还是相信观测"的自动配比——模型准且观测噪则少信观测，反之亦然。

直观类比

• 闭眼摸鼻尖：不看镜子也能知道手和鼻子的相对位置——这就是本体感知，来自关节与肌肉的本体感受器。
• 走夜路数步子：步数积分（预测）会偏，路灯下瞄一眼环境（观测更新）拉回误差——卡尔曼滤波就是"一边数步子一边瞄一眼"的数学化。

实例与案例

• Unitree 四足/人形：官方 SDK 输出的机身姿态与速度即来自此类估计器；RL 策略（如 legged_gym）观测向量包含重力方向投影、角速度、关节角/角速度、上一步动作。
• 无人机：EKF2/EKF3（PX4/ArduPilot）融合 IMU + 气压计 + 光流 + 视觉里程计，是消费无人机的标配。
• 关节柔性估计：高负载臂的谐波减速器有弹性，双编码器 + 柔性关节模型（De Luca 的 flexible joint 建模）才能做精确力控。

常见误区

• 误区一：IMU 姿态积分=姿态。陀螺仪零偏会让角度几分钟漂移数度；必须用重力/磁力参考做长期校正。
• 误区二：状态估计只是导航的事。机械臂的关节状态、人形的接触状态都是状态估计；任何闭环控制器都隐含一个估计器。
• 误区三：策略训练可以不管估计误差。真机上策略收到的是带噪延迟的估计值；仿真中要显式注入观测噪声（kp-402），否则 Sim2Real 必崩。

自测题

1. 写出卡尔曼滤波的预测与更新两步，并解释卡尔曼增益的直觉意义。
2. 足式机器人如何从"支撑足不滑"假设得到机身速度？这个假设何时失效？
3. 为什么 RL 运动策略的观测以本体量为主、视觉为辅？

与其他知识点的关系

• kp-103 SLAM：同为状态估计，SLAM 额外估计地图。
• kp-204 步态/全身控制与 kp-206 RL 控制：估计输出是控制/策略输入。
• kp-402 Sim2Real：观测噪声建模是迁移成功的关键项。

延伸阅读

• Kalman, R. E., A New Approach to Linear Filtering and Prediction Problems, 1960。
• Bloesch et al., State Estimation for Legged Robots - Consistent Fusion of Leg Kinematics and IMU, RSS 2012。
• Solà, J., Quaternion kinematics for the error-state Kalman filter（免费长文，IMU 预积分前必修）。