机器人怎么知道自己站着？足式状态估计入门

问一个看起来很傻的问题：一台四足机器人怎么知道自己是站着的？

它身上没有任何一个传感器会输出「我现在站得好好的」。它有的只是一堆原始数字——加速度计三个轴的读数、陀螺仪三个轴的角速度、十几个关节的角度和电流。从这堆数字到「我的身体在空间中的位置、朝向、速度是多少」，中间隔着一整个模块，叫状态估计。

这个模块在足式机器人里的地位怎么强调都不为过。控制器的所有决策——该往哪迈腿、每条腿出多大力——都建立在状态估计的输出上。估计错了，控制得再精妙也是错的。

这篇讲清它的原理。我们会拿 unitree_guide 这个配套教学的开源实现做参照，看真实工程里这个模块是怎么摆放的。按惯例说明：本文基于公开仓库的代码结构与该领域的通用方法，我没有真机验证，不会有实测描述。

先说清楚要估什么

状态估计的输出，通常是这么几组量：

身体姿态     —— 横滚角 roll、俯仰角 pitch、偏航角 yaw
身体角速度   —— 三个轴
身体位置     —— 在世界坐标系里的 x、y、z
身体线速度   —— 三个方向
足端位置     —— 四只脚相对身体在哪
接触状态     —— 每只脚是不是踩在地上

其中最难的是身体位置和线速度。姿态相对容易，接触状态可以间接判断，但"我相对起点走了多远、现在移动得多快"这件事，没有任何传感器能直接告诉你。

三路信息，各有各的毛病

机器人手上有三类可用信息，没有一路是够用的。

IMU：快，但会漂

惯性测量单元给你两样东西：加速度计测三轴比力，陀螺仪测三轴角速度。它的优点是频率高、不依赖外部环境、不怕黑不怕滑。

问题在于，你要的量都得靠积分得到：

角速度 --积分--> 姿态角
加速度 --积分--> 速度 --再积分--> 位置

积分会把误差累积起来。陀螺仪有零偏——静止时它也报一个小小的非零值，积分下去姿态角就慢慢转。加速度计噪声更大，而且两次积分意味着误差按时间平方增长。纯靠 IMU 推位置，几秒钟就跑飞了。

还有一个坑：加速度计测的不是"运动加速度"，而是"比力"——它把重力也算进去了。静止放在桌上的加速度计，输出的是向上一个 g。所以要先知道姿态，才能把重力分量扣掉；而姿态又来自陀螺仪积分。两件事互相纠缠。

关节编码器 + 运动学：准，但只在脚不动时有效

每个关节的角度都能精确读到。配合机器人的几何模型（连杆长度、关节轴线），用正运动学就能算出每只脚相对身体在哪。

这里有一个足式机器人独有的关键思路：

只要有一只脚踩在地上没打滑，这只脚就是一个临时的地面参考点。

想清楚这句话，你就懂了一半。脚不动，身体动了，通过腿的关节角度变化，就能反推身体相对这只脚移动了多少。这相当于给了你一个"腿式里程计"——功能上类似轮式机器人数轮子转了几圈。

但它有两个前提：

• 你得知道哪只脚在地上
• 那只脚不能打滑

前一个靠接触检测解决，后一个……只能祈祷，外加在打滑时靠其他信息修正。这也是为什么足式机器人在冰面、沙地上的定位会明显变差。

足端接触检测：间接，但必需

怎么知道脚踩没踩地？大多数足式机器人身上没有专门的足底压力传感器。常见做法是靠关节电机的力矩反推。

这正是我们在关节电机那篇里讲的力控方案带来的额外好处：低减速比的准直驱电机，电流和输出力矩近似线性对应，读电流就能估算关节力矩，再通过腿的雅可比矩阵反推足端受力。足端受力超过某个阈值，判定为触地。

这个判断不是绝对可靠的——落地瞬间的冲击、摆动腿的惯性力都可能造成误判。所以实践中通常还会结合步态相位（控制器本来就知道现在这条腿"应该"处于支撑相还是摆动相）来交叉验证。

融合：卡尔曼滤波在做什么

三路信息各有各的毛病，但毛病的性质不同：

来源	频率	短期	长期
IMU	高	准	漂
腿式里程计	中	有噪声、依赖不打滑	不漂（相对）

高频但会漂 + 低频但不漂，这是传感器融合的经典组合。融合的思路是：用 IMU 做高频的状态推进，用腿式里程计做低频的修正。

标准工具是卡尔曼滤波（足式机器人上常用扩展卡尔曼滤波，因为姿态相关的方程是非线性的）。它的工作循环只有两步：

预测：根据上一时刻的状态和 IMU 读数，推算这一时刻的状态应该是多少，同时把不确定度放大——因为推算是有误差的，时间越长越不确定。

更新：拿到腿式里程计的观测，和预测值比对。两者不一致时，按各自的不确定度加权融合。预测很确定而观测噪声大，就少信观测；反过来就多信观测。融合之后不确定度缩小。

这个"按可信度加权"的机制，就是卡尔曼滤波的全部精髓。它需要你给出两个协方差矩阵——过程噪声和观测噪声，实践中这两个矩阵的调参是个体力活，往往比推导公式更花时间。

这也解释了为什么我们在DDS 通信那篇里提到的那些 *WithCovariance 消息类型如此重要：协方差不是可有可无的装饰，它是融合算法的必需输入。

从代码结构看这个模块的位置

unitree_guide 的 include/control/ 下有四个文件，把控制侧的分工摆得很清楚：

control/Estimator.h        —— 状态估计
control/BalanceCtrl.h      —— 平衡控制
control/ControlFrame.h     —— 控制框架
control/CtrlComponents.h   —— 控制组件集合

Estimator 和 BalanceCtrl 并列，一个负责"我现在什么状态"，一个负责"该出什么力"。这个划分是足式控制的标准结构。

common/ 下还有一个相关的文件：

common/LowPassFilter.h

低通滤波器。IMU 原始数据噪声大，尤其是足端落地冲击会在加速度计上产生剧烈尖峰。直接喂给估计器会让输出抖动，所以要先滤一道。

代价是延迟。滤波越狠，信号越平滑，但也越滞后。控制回路里的延迟会直接吃掉相位裕度，让系统更容易振荡。这个取舍在任何控制系统里都存在，做过PID 调参的话应该很熟悉这种感觉。

interface/ 目录也值得一提：

interface/IOInterface.h   —— 抽象接口
interface/IOSDK.h         —— 接真机
interface/IOROS.h         —— 接仿真

状态估计器读到的数据，来自这层抽象。同一套估计代码，在仿真和真机上跑的是同一份逻辑，只是数据源不同。这种设计让你可以先在仿真里把估计器调通——而且仿真里你有真值可以对照，这是真机上没有的奢侈条件。

哪些量注定会漂，哪些不会

这是本篇最值得记住的一点，很多人没意识到。

横滚角和俯仰角不会漂。 因为有重力这个绝对参考——加速度计长期平均下来指向重力反方向，这给了 roll 和 pitch 一个不会累积误差的绝对约束。

偏航角会漂。 重力是竖直的，它对"你朝哪个水平方向"没有任何约束。除非你装磁力计（室内金属环境下不可靠）或者用激光雷达/视觉做回环，否则偏航角只能靠陀螺仪积分，必然缓慢漂移。

水平位置会漂，而且漂得更快。 腿式里程计是相对测量，每一步的误差都会累加进去，没有任何绝对参考能拉回来。走得越久偏得越多。

高度也会漂，而且有个特殊困难：机器人上坡下坡时，"地面高度"本身在变，估计器很难分清是身体升高了还是地面抬高了。

这个结论的实际意义是：状态估计只对"局部、短期"可靠。 它足以支撑控制器做平衡和迈步——这些只需要几百毫秒内的准确状态。但你不能指望它告诉你"我从出发点走了 200 米"，那个数字一定是错的。

需要长期、全局的定位怎么办？必须引入外部感知——激光雷达或视觉，通过和环境地图的匹配来消除累积误差。这就是激光雷达里程计和 SLAM 要解决的问题，它们和本篇讲的本体状态估计是互补而非替代的关系：本体估计给你高频的、局部准确的状态，外部感知给你低频的、全局无漂移的修正。两者往往再融合一次。

三句话带走

状态估计的核心矛盾是：高频信息会漂，不漂的信息频率低。 融合就是让两者互补。

"支撑足是临时地面参考点"是足式机器人独有的思路，它让关节编码器变成了一种里程计。前提是接触判断准、脚不打滑。

roll 和 pitch 有重力兜底不会漂，yaw 和位置没有绝对参考必然漂。 想要全局定位，必须上外部感知。

想看这套估计结果如何被控制器消费，可以读unitree_guide 的传统控制实现；想动手感受传感器融合，机器人与具身智能卷里的自平衡车是最短的路径——那上面用的互补滤波是卡尔曼滤波的简化版，思路完全一致。