人形机器人状态估计怎么搭：从 IMU、关节编码器到足底接触融合的实作指南

这篇文章解决的是：当你的人形机器人明明装了 IMU、腿部编码器，脚底也能判断接触，却依然不知道骨盆到底在哪里、速度是不是在飘、控制器为什么总像“踩空”时，该怎样把状态估计真正搭起来。适合已经有下肢样机、双足试验台或移动双臂底盘的人。最关键的工程判断是：状态估计不是一个“IMU 滤波器”，而是一个带接触置信度管理的本体里程计子系统。先把接触、坐标系、时间戳和腿部运动学约束做对，再谈步态、WBC 和视觉融合。

很多团队第一次做人形机器人，会把“站不稳、脚步飘、落脚后姿态突然跳一下”归咎于控制器参数、步态规划器或者执行器不够强。但在真实工程里，更常见的根因其实是底层状态估计不可信。只要骨盆位姿、线速度、支撑脚判断里有一项是错的，越往上的模块越像在拿错地图开车。

这也是为什么，真正可用的人形机器人栈，往往不是先把“全局定位”做得很漂亮，而是先把高频、低延迟、可回放、可在接触切换时保持稳定的本体状态估计做好。对大多数早期样机来说，你首先需要的不是绝对世界真值，而是一个在站立、下蹲、重心转移、原地抬脚这些动作里都足够诚实的局部 odom。

这篇适合谁

• 已经有 IMU 和腿部编码器，准备上站立、踏步或第一版步态的人形机器人团队
• 在调 WBC、MPC、步态切换时，经常分不清问题来自控制器还是状态估计的人
• 想先把本体高频估计做稳，再接视觉、外部定位或遥操作监督的人
• 正在做双足、轮足、或“固定底盘 + 双腿试验架”这类下肢原型的人

先纠正几个很常见的误区

• 误区 1：有 IMU 就够了。 IMU 只能给你高频惯性传播，不能自己解决位置漂移，也无法判断哪只脚当前真的值得信。
• 误区 2：脚底接触就是 0/1 开关。 真正落地时会有脚跟先碰、脚尖滚动、微滑、线缆牵扯、软鞋底形变，接触更像置信度，不是绝对真值。
• 误区 3：先把视觉接上，状态估计自然就好了。 视觉适合做低频纠偏，不适合替代控制环所需的高频本体估计。控制器不会等相机。
• 误区 4：滤波器发散就是算法不够高级。 很多时候根因是坐标系错、编码器零位偏、IMU 安装方向错、时间戳漂移，而不是 EKF 还是 InEKF 选错了。

关键实现判断

对早期人形机器人，最稳妥的第一版架构通常是三层：

1. IMU 姿态层，先把 roll、pitch、角速度、加速度方向做对，保证高频传播可用。
2. 腿部运动学约束层，在“当前哪只脚可作为支撑脚”的假设下，用编码器和模型反推骨盆相对支撑足的位置与速度约束。
3. 接触感知融合层，根据足底载荷、足端速度、一致性检查去决定什么时候该强信脚约束，什么时候该弱化甚至关闭修正。

这里可以借鉴几个很实用的外部工程参照：

• robot_localization 的传感器准备文档，最值得学的不是它的默认滤波器，而是它对 base_link、odom、imu_link、时间戳、协方差和消息语义的严谨要求。
• imu_tools 适合做 IMU 通路的第一阶段 bring-up，先验证姿态方向、重力补偿和噪声级别，不要一上来就把全部问题塞进总滤波器。
• Pronto 给出的经验很值得学，高频本体估计线程和低频视觉 / 激光纠偏线程要分层，不要混成一个延迟不可控的大杂烩。
• legged_state_estimator / invariant-ekf 一类实现 则提醒你，接触与运动学约束最好被建模成一等公民，而不是事后打补丁。

分步实践指南

第 1 步，先把坐标系和时间戳做干净

这是最不显眼，但最能决定后面是否省时间的一步。最低限度要显式定义：

• base_link 或 pelvis，给控制器和上层规划使用
• imu_link，并确认它到骨盆框架的刚性外参真的与实物一致
• left_foot / right_foot 接触框架，位置必须对应真实鞋底受力区域，不要偷懒直接用 CAD 名义点
• odom，作为短时间局部一致的世界系

工程上最常见的翻车方式，不是滤波器公式错，而是 IMU 实际安装绕了 90 度、URDF 外参旧了、编码器和 IMU 不是同一时钟源，最后控制器看到的是“逻辑上正确、物理上错误”的状态。

如果你的编码器是 1 kHz、IMU 是 400 Hz、接触检测是 200 Hz，却都在用户态回调里临时打时间戳，那你已经人为制造了一层相位误差。先把时钟统一，再调滤波器。

第 2 步，只验证 IMU 通路，不要一上来就融合全部传感器

这一阶段的目标很简单：确认 IMU 的姿态、角速度、线加速度在方向和量级上都可信。可参考 imu_tools 先做一个独立姿态链路，验证这些事：

• 机器人静止时，roll / pitch 是否能稳定对准重力
• 手动前倾、后仰、左右侧倾时，姿态方向和符号是否正确
• 缓慢转动机器人时，yaw rate 是否平滑，没有明显反号或饱和
• 多次断电重启后，初始姿态是否一致，偏置是否能收敛

如果这一步都不稳，后面接再多编码器和接触逻辑都只是在掩盖问题。

第 3 步，把腿部运动学路径单独跑通

接下来先不谈滤波，只问一个更简单的问题：如果某只脚真的固定在地面上，根据当前关节角，骨盆应该在哪里？

做几个非常朴素但很有效的动作：

• 双脚站立，记录左右腿各自反解出来的骨盆高度和姿态，检查二者是否一致
• 慢速下蹲，观察骨盆高度曲线是否光滑，是否出现不合物理的锯齿
• 做缓慢左右重心转移，检查哪条腿的运动学估计更早开始失真

如果左右腿在双支撑时推出来的骨盆位置差很多，优先怀疑以下几项：

• 髋、膝、踝编码器零位偏了
• 某个关节轴方向在模型里写反了
• 鞋底真实接触面和足端 frame 定义不一致
• 结构间隙、柔性连接、鞋底压缩被你完全忽略了

第 4 步，把足底接触当成独立子系统来做

接触检测最好不要埋在滤波器内部做黑箱判断，而要变成可观测、可记录、可回放的单独输出。第一版就够用的输入通常包括：

• 垂直载荷阈值，来自 FSR、薄膜压力、六维力传感器或简化 load cell
• 足端速度接近零的运动学一致性检查
• 关节电流 / 力矩作为辅助信号，而不是唯一真相
• 步态状态机的命令上下文，比如当前是否进入摆动相

更实用的做法不是输出一个生硬的 on/off，而是至少分成三档：

• 可信支撑：载荷足够、足端速度接近零、姿态变化也合理
• 可疑接触：有载荷，但足端切向速度偏大，可能在滑
• 不可信：已经离地、正在换脚、或信号互相矛盾

这样你在融合层里就能动态调测量噪声，而不是一旦接触标志点亮就把足端当作完全静止。

第 5 步，再做融合层，先追求稳定而不是炫技

第一版状态变量一般够用到这些：

• 骨盆姿态
• 骨盆位置
• 骨盆线速度
• 陀螺仪偏置
• 加速度计偏置
• 必要时加上当前支撑足位姿或接触点状态

融合逻辑可以很朴素：

• 预测阶段 用 IMU 高频传播，保证低延迟连续性
• 修正阶段 在支撑足可信时，用腿部运动学约束把漂移拉回来
• 降级阶段 一旦进入可疑接触或明显打滑，就提高足端测量噪声，必要时暂时只保留惯性传播

这时候你就能理解 Pronto 那类架构为什么实用，它强调的是“高频本体估计负责控制闭环，低频视觉 / 激光只负责缓慢纠偏”。对人形机器人尤其重要，因为视觉延迟、遮挡和场景脏乱在真实实验室里太常见了。

第 6 步，把控制器和估计器一起看，不要分开各自自证清白

很多问题只有在控制器启用后才出现，这并不奇怪。因为状态估计延迟、接触状态延迟、控制器观测延迟三者叠起来后，系统相位裕度可能突然掉光。这里可以参考一些开源腿足控制栈，比如 legged_control 的做法，它把状态估计和控制主循环放在一起看待，而不是当成独立无关模块。

你至少应该同时记录：

• 原始 IMU 与编码器数据
• 接触置信度
• 估计出的骨盆姿态、位置、速度
• 控制器输出的期望接触状态、足端轨迹、质心目标
• 所有换脚、重置、异常降级事件

没有这条证据链，你会不断在“是控制器不稳，还是估计器撒谎”之间来回猜。

最容易翻车的地方

• 落脚瞬间姿态跳变：通常是接触启用太早，脚底还在滚动就被当作刚性约束。
• 双支撑稳定，单支撑发散：往往是单支撑足在微滑，或编码器零位在髋膝踝链路里积累误差。
• 吊装测试很稳，一落地就不行：说明问题多半在接触逻辑、鞋底顺应性或线缆牵扯，不在纯 IMU 传播。
• roll / pitch 看着正常，但速度和位置漂得离谱：通常是只做了惯性传播，没把足端约束真正纳入修正。
• 一开控制器就炸：先查总延迟链路和时间戳，不要急着重写滤波器。

怎么验证你真的搭对了

不要从“静止”直接跳到“自主行走”，更有效的是按下面这条验证梯子往上走：

1. 静置测试：机器人固定不动，速度估计应接近零，偏置应收敛。
2. 人工倾斜测试：挂在支架上前后左右轻微摆动，姿态方向必须全对。
3. 双支撑慢下蹲：左右腿推出来的骨盆高度要一致，轨迹不能抖。
4. 慢速重心转移：接触置信度要平滑过渡，不能在换重心时速度尖峰乱跳。
5. 原地抬脚：估计器应明显切换为信任支撑脚，摆动脚落地时不能把骨盆“弹飞”。
6. 带保护绳首步测试：拿视频、AprilTag、顶置相机或其他外部参考做低频对照，检查每一次换脚前后的估计偏差。

如果你能把这 6 级都稳定通过，再去接视觉里程计、地图定位或上层规划，效率会高得多。

下一步怎么做

• 给足底增加更可靠的接触与微滑观测，而不是长期靠电流阈值猜
• 加入低频视觉 / AprilTag / 顶置相机纠偏，但保持与高频控制估计分层
• 把状态估计日志接入统一回放链路，和 WBC / 步态状态机一起复盘
• 把“估计可信度”暴露给上层，让规划器知道什么时候该保守、该减速、该请求人工接管

延伸阅读

• 如何搭建 humanoid 机器人的“大脑”：感知、规划、控制与调试
• 人形机器人软件平台怎么搭：从 ROS 2 生命周期、控制接口到仿真回归的实作指南
• 人形机器人测试工装怎么搭：从任务定义、日志回放到回归评测的实作指南

Sources / Further Reading

• ROS 文档，Preparing Your Data for Use with robot_localization
• ROS Index，imu_tools
• Camurri et al., Pronto: A Multi-Sensor State Estimator for Legged Robots in Real-World Scenarios
• GitHub，legged_state_estimator
• Hartley et al., Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation
• GitHub，legged_control

如果你只记住一条经验，那就是：每次觉得步态控制“突然不行了”的时候，先回看支撑脚假设和骨盆状态估计在出事前 200 毫秒里有没有开始失真。 这一步经常比继续调控制增益更值钱。