做腿足机器人控制的这几年我最深的体会是状态估计这个模块平时没人看得见但它一崩整台机器直接在地上抽搐。MIT Cheetah 能在野外稳定小跑、跳跃甚至后空翻除了 MPC 和 WBC 那套控制律背后还有一个不起眼却极其关键的模块——State Estimator。它用 EKF 把 IMU 的加速度、角速度数据和关节编码器算出来的足端运动学约束融合在一起实时输出机身位置、姿态和速度。这篇文章把这条链路从理论到实践完整过一遍适合正在做四足、双足机器人状态估计的工程师以及想深入了解 MIT Cheetah 技术栈的爱好者。1. 场景剖析四足机器人的状态估计到底难在哪1.1 浮动基座系统到底意味着什么做轮式机器人的人可能很难理解为什么四足机器人的状态估计会是一个专门的研究方向。小车有轮式里程计轮子转多少圈、车走多少米一算就知道AGV 有磁条或反光板GPS 不够还能上激光雷达匹配。四足机器人有什么它其实是一个典型的浮动基座系统机身在空间中是自由悬浮的没有轮子那种天然的里程约束四条腿是唯一的执行器也是唯一能跟环境发生交互的机构。问题在于腿和地面的接触是时变的。站立时四条腿都是支撑腿约束还算充足小跑时对角腿交替支撑支撑相和摆动相切换很快跳跃腾空时四条腿全部离地整台机器完全处于自由飞行状态只能依靠 IMU 积分。这种“约束条件随运动模态剧烈变化”的特性让状态估计问题从“用轮速算里程”变成了“在间歇性运动学约束下做多传感器融合”。另一个麻烦在于四足机器人的运动动态性很强。小跑时机身垂直方向的加速度可以轻松超过 1 个 g跳跃落地瞬间足端冲击力能到几百牛顿普通滤波算法在这种工况下很容易饱和或者发散。所以状态估计器不仅要准还要足够鲁棒能够在传感器信号被剧烈扰动时依然保持稳定。1.2 传感器到底有哪些能用的信号和不能用的信号先盘一下四足机器人机身上常见的传感器家底。IMU 是标配通常包含三轴陀螺仪和三轴加速度计高端的还会带磁力计但磁力计在电机强磁场环境下基本不可用所以一般忽略。关节编码器也是标配每个关节一个用来测量电机输出轴或关节输出轴的角度通过正运动学可以算出每条腿末端足端相对于机身的位置。足端力传感器不是标配很多方案用关节力矩估计器代替通过电机电流估算关节力矩再换算成足端接触力。还有一个经常被忽略的信号源是关节力矩和电机电流。MIT Cheetah 在接触检测时就会用到这个信息因为足端有没有牢牢踩住地面直接看足端力最直观。这些信号组合起来能够提供的有效信息包括机身的角速度和加速度IMU、关节角度编码器、足端位置正运动学、接触状态力或力矩。不能直接提供的则是机身在世界坐标系中的绝对位置和速度这正是状态估计器需要解决的。需要注意的是传感器信号存在明显的频率和质量差异。IMU 的积分在短时间尺度内非常准但长时间积分后位置漂移呈时间立方增长编码器和正运动学在低频下提供绝对约束但容易受关节间隙、柔性形变和编码器噪声影响。这两种特性恰好互补一个管短时间内准一个管长时间不飘这也是后续 EKF 融合方案能够成立的根本前提。2. 方案选型MIT Cheetah 为什么用 EKF 融合 IMU 和编码器2.1 滤波器选型对比为什么不是互补滤波也不是因子图状态估计算法从简单到复杂排序大概有互补滤波、扩展卡尔曼滤波EKF、无迹卡尔曼滤波UKF、因子图优化这几类。互补滤波在飞控领域用得非常多原理简单计算量极小把陀螺仪的短期积分和加速度计/磁力计的长期参考做高通低通组合。但它的处理方式比较“平铺直叙”很难把足端接触约束这种复杂的非线性运动学约束塞进去。因子图优化是近年视觉SLAM领域的主流精度高、灵活性强可以通过滑动窗口做局部优化把IMU预积分、视觉重投影误差、足端接触约束全部建模成因子。但因子图的实时性在嵌入式平台上有压力尤其当控制周期是 1kHz 时每次控制循环都做非线性优化不现实。UKF 不需要推雅可比矩阵对强非线性系统更鲁棒但计算量比 EKF 大得多在腿足机器人这种高频率控制场景下显得有点奢侈。EKF 的优势在于它是“能用解析雅可比、计算量可控、可以跑在 1kHz 控制循环里”的最优解。它能够自然地把非线性运动学模型在当前状态附近线性化融合IMU预测和编码器约束更新而且工程实践经验丰富遇到问题也容易定位。MIT Cheetah 选择 EKF本质上是算力约束、实时性要求和融合精度三者之间的平衡决策。2.2 IMU 与编码器在多时间尺度上的互补性理解这套方案的关键在于理解 IMU 和编码器在信息论意义上的互补关系。IMU 测量的是机体系的角速度和比力频率可以很高1kHz 甚至更高短期精度好。但加速度计无法区分重力加速度和运动加速度陀螺仪存在零漂单纯积分几分钟就能偏到天上去。编码器测量的关节角度通过正运动学得到的足端位置在静态支撑工况下非常准确。当脚踩死地面时足端在世界系中的速度理论上为零这个约束相当于给状态估计器提供了一个“绝对参考点”无论 IMU 积分怎么漂只要脚没动你的机身速度就能被拉回到符合运动学的范围内。用大白话说IMU 是一个短期记忆极好但方向感极差的人编码器加接触约束像一个每隔几步就能看到路标的人EKF 就是让这两个人互相纠偏的合作机制。这套方案和汽车上的组合导航逻辑也有些相似。汽车用 IMU 做短周期递推用 GPS 做长周期修正四足机器人没有 GPS就用“足端触地不动”这个天然的里程约束替代 GPS。MIT Cheetah 的聪明之处在于它把物理世界中本来就存在的接触约束变成了虚拟传感器不需要额外硬件却又达到了类似外部位姿传感器的修正效果。3. 核心原理EKF 的状态模型与观测模型推导3.1 状态向量设计19维状态和18维误差状态状态向量怎么设计直接决定了滤波器能不能收敛、好不好调参。MIT Cheetah 的状态估计器采用了一个比较经典的结构世界系中的机身位置 p、姿态四元数 q、世界系中的线速度 v、机体系中的角速度 ω_b、加速度计偏置 b_a、陀螺仪偏置 b_g。完整写出来就是x [p, q, v, ω_b, b_a, b_g]拆开看维度位置 3 维四元数 4 维线速度 3 维角速度 3 维加速度计偏置 3 维陀螺仪偏置 3 维总共 19 维。这里就出现了一个经典的细节问题四元数是 4 维但只有 3 个自由度如果直接在 19 维空间里做标准 EKF 协方差更新协方差矩阵会因为四元数约束而奇异。实际工程里通常分两层名义状态nominal state用于积分状态量误差状态error-state用于传播协方差误差状态把四元数扰动表示成 3 维旋转向量所以对应的协方差矩阵是 18×18。这就是为什么你翻开源代码时状态维度和协方差维度对不上的原因。这种“名义状态 误差状态”的做法在惯性导航领域叫 error-state EKF也叫间接卡尔曼滤波是处理旋转变量时的标准手段。好处是协方差始终保持正定更新后把误差状态叠加回名义状态再清零误差状态避免数值病态。3.2 预测步IMU 积分与四元数运动学预测步做的事情非常简单用 IMU 的角速度和加速度数据把状态从上一时刻推到当前时刻。假设标定后的陀螺仪输出为 ω_m加速度计输出为 a_m过程模型可以写成p_{k1} p_k v_k * dt 0.5 * R(q_k) * (a_m,k - b_a,k) * dt²v_{k1} v_k (R(q_k) * (a_m,k - b_a,k) g) * dtq_{k1} q_k ⊗ q_delta( (ω_m,k - b_g,k) * dt )b_a,k1 b_a,k n_bab_g,k1 b_g,k n_bg这里 R(q) 是四元数对应的旋转矩阵g 是世界系重力向量通常取 (0,0,-9.81) 或根据坐标系定义取正。四元数更新的 q_delta 是把角速度乘以时间步长后的旋转向量转换成四元数。每一步做完之后必须对四元数做归一化这是四元数递推最容易忽略却又最容易出问题的地方。预测步的协方差传播公式是标准 EKF 形式P_{k1} F P_k F^T Q其中 F 是状态转移函数对状态向量的雅可比矩阵Q 是过程噪声协方差矩阵。这里需要特别强调四元数对应的那一块雅可比需要做“误差状态”处理不能直接对四元数四个分量求偏导。IMU 偏置在预测步中被建模为随机游走它本身不会随运动更新但过程噪声会逐渐增大其不确定性。这个设计的妙处在于如果某个偏置量真的漂了EKF 可以通过观测残差把它估计出来缓慢修正。但偏置的可观测性依赖于运动激励四足机器人原地站立时加速度计偏置和重力方向是耦合的不可完全观测这就需要后续调参时小心处理。3.3 观测步足端速度约束如何修正漂移预测步做完之后状态会随着时间漂移观测步的作用就是拿外部约束来“纠偏”。MIT Cheetah 最核心的观测不是来自某个额外传感器而是来自一个运动学事实当某条腿处于支撑状态时其足端在世界系中的速度应当为零。用公式表达是这样的。先通过关节编码器角度和正运动学算出第 i 条腿的足端在机体系中的位置 r_i^B。然后利用当前估计的机身位置、姿态和速度预测足端在世界系中的速度v_i^W v R(q) * (ω_b × r_i^B)如果这条腿确实稳稳踩在地面上那这个预测速度应该等于零。实际计算中因为正运动学误差、机身柔性、地面形变等因素会存在残差这个残差就是 EKF 更新步的创新量y_i 0 - v_i^W对应的观测方程可以近似写成 H_i * x其中 H_i 是对当前状态求偏导得到的雅可比矩阵。有了 H_i 和残差 y_i标准 EKF 更新公式就能算增益矩阵 K然后修正状态和协方差。这里有一个重要的工程细节足端速度约束的三个方向置信度并不相同。水平方向切向上只要脚没有打滑约束非常硬噪声方差可以设得很小垂直方向法向上由于腿足结构存在柔性、地面有弹性和压缩量约束相对软一些噪声方差适当调大。有些实现甚至只约束水平方向垂直方向完全不管具体取舍取决于你的机械结构刚度和应用场景。3.4 接触检测支撑脚判断是状态估计的命门接触检测是整个观测模型运行的前提。用一个错误的接触状态去做更新比不更新更危险。举个极端例子本来脚已经离地了你却把它当成支撑腿EKF 会认为机身不应该运动于是把真实运动全部判为残差疯狂修正速度估计最后直接把机身位置拉飞。常见的接触检测方法有三种第一种是基于足端力传感器最直接有传感器直接读力超过阈值就是接触第二种是基于关节力矩估计通过电机电流估算关节力矩再换算成地面反力MIT Cheetah 没有在足底装昂贵的三维力传感器主要靠的就是力矩模型的估计第三种是基于运动学启发式判断比如用腿长变化和足端位置判断是否触地但鲁棒性差一些。阈值设置也不简单。单纯设一个恒定力阈值在高速奔跑时会出问题腾空阶段腿部摆动带来的离心力和惯性力可能超过阈值造成误判落地瞬间的冲击峰值又可能让滤波器在几个毫秒内剧烈切换。工程上推荐的做法是加迟滞——进入接触的阈值高一些退出接触的阈值低一些避免高频抖动。更高级的方案是把接触概率做成连续的用接触概率对观测噪声协方差做插值让约束的强弱随概率平滑变化。4. 实操落地从公式到代码的实现细节4.1 数据流、控制频率和传感器同步做真机状态估计第一件要理清楚的事情不是公式而是数据从哪个传感器来、在哪个线程里跑、需要多新鲜。MIT Cheetah 的控制循环在 1kHz 运行IMU 数据通常也是 1kHz 输出关节编码器在底层电流环里更新频率更高但给到状态估计器的是一份在控制周期开始时刻锁存的最新角度。推荐的数据流结构是这样的IMU 数据到达后立即触发一次 EKF 预测步保证状态估计以 IMU 频率更新编码器角度按控制周期读取正运动学计算足端位置接触检测模块根据力矩估计和运动状态计算每条腿的支撑状态当检测到支撑腿时在控制循环里执行一次或多次观测更新。这样做的好处是预测步永远能吃到最新鲜的 IMU 数据而更新步只需要在接触状态有效时执行计算量可控。传感器时间戳对齐是真机上的一大坑。IMU 和编码器如果来自不同通信总线它们各自的数据到达时间存在偏移直接拿不同时刻的数据做融合会在动态运动时引入难以察觉的相位误差。简单可靠的解法是在每条数据里打上本地时间戳并在状态估计器入口做时间对齐如果底层驱动不支持时间戳就静待在固定控制周期内用最近一次采样保证系统性偏差恒定再通过标定补偿。4.2 预测步与更新步的代码级实现用伪代码把整个 EKF 流程过一遍。先定义状态向量、协方差矩阵和观测噪声矩阵然后按控制循环执行。预测步核心逻辑如下def predict(state, cov, imu_acc, imu_gyro, dt): q state.q p state.p v state.v b_a state.b_a b_g state.b_g # 修正偏置 a_corrected imu_acc - b_a w_corrected imu_gyro - b_g # 四元数积分旋转向量转四元数 delta_q quat_from_axis_angle(w_corrected * dt) q_new quat_multiply(q, delta_q) q_new quat_normalize(q_new) R_mat quat_to_rotation_matrix(q) p_new p v * dt 0.5 * R_mat a_corrected * dt * dt v_new v (R_mat a_corrected g) * dt # 协方差传播 P F P F^T Q F compute_state_transition_jacobian(q, a_corrected, dt) cov F cov F.T Q state.set(p_new, q_new, v_new)更新步的伪代码要处理每一条支撑腿。这里的关键是正运动学计算足端位置向量以及构造速度约束观测矩阵def update(state, cov, leg_index, joint_positions, contact_prob): if contact_prob 0: return r_foot_B forward_kinematics(leg_index, joint_positions) R_mat quat_to_rotation_matrix(state.q) # 预测足端世界系速度 v_foot_pred state.v R_mat np.cross(state.w_b, r_foot_B) # 观测残差期望速度为0 innovation -v_foot_pred # 观测矩阵 H对状态量求导得到 H compute_foot_velocity_jacobian(R_mat, r_foot_B, state.w_b) # 根据接触概率调整观测噪声 R_obs R_base / max(contact_prob, 0.1) S H cov H.T R_obs K cov H.T np.linalg.inv(S) delta_x K innovation state.inject_error_state(delta_x) cov (np.eye(dim) - K H) cov这段代码里inject_error_state 就是把误差状态叠加回名义状态并把误差状态清零。实际工程中通常还会给更新步加上门控gating逻辑如果创新量某个分量超过阈值比如足端速度预测值达到每秒几米说明接触检测或正运动学存在问题直接跳过这次更新防止异常观测把滤波器拉崩。4.3 四元数和协方差传播最容易被忽略的坑四元数更新看起来简单踩坑的人却最多。直接对四元数做加法更新是错的四元数必须用乘法更新。协方差传播时更不能直接对四元数四个分量求偏导否则计算出来的雅可比矩阵是秩亏的协方差会慢慢变形最终滤波器表现异常。推荐的做法是前面提到的 error-state 方式。名义状态用四元数乘法积分误差状态里把姿态误差表示为三维旋转向量这个旋转向量对应着小角度下的李代数扰动协方差传播和观测雅可比都基于这个三维表示计算。更新完成后将姿态误差重新映射回四元数并乘到名义四元数上然后把误差状态清零。代码里看起来多绕了一圈但数值稳定性和可观测性处理都会干净很多。另一个容易被忽略的小地方是四元数归一化。即使每次积分后都做了归一化多次迭代后依然可能因为浮点精度慢慢偏离模长1。建议在每次预测和更新结束后都显式归一化一次成本极低却能在长时间运行中避免很多“莫名其妙”的漂移。4.4 编码器精度和关节零位对估计结果的实际影响状态估计精度不仅取决于滤波算法还取决于底层传感器质量。关节编码器就是一个典型的“下限决定者”。网上一些伺服驱动器默认编码器线数很高比如汇川 MS1H4 系列在驱动器里默认编码器线数为 262144换算一下是 18 位分辨率。放在腿足机器人上这个精度到底够不够要看使用场景。关节编码器分辨率越高正运动学算出来的足端位置越准观测约束的噪声方差就可以设得越小状态估计器修正力度越强。262144 线/圈意味着每个脉冲对应的角度约为 0.00137 度假设腿长 0.3 米足端位置分辨率大约在微米级这个精度对状态估计来说完全够用。反而需要注意的是减速器回程间隙和连杆柔性它们带来的足端误差可能远大于编码器量化误差处理不好会让观测模型出现系统性偏差。编码器类型选择也有讲究。绝对值编码器上电就知道关节绝对角度省去找零位的麻烦增量式编码器必须上电后执行回零动作在四足机器人这种多关节系统里比较麻烦。磁编码器像 AS5047P 这类芯片抗污染能力强、体积小是中空关节的常见选择电感式编码器精度更高、温漂更小但成本也更高。工程上还会遇到“编码器虚轴”的问题也就是电机端编码器测到的角度和关节输出实际角度之间存在弹性形变状态估计器把虚拟角度当成真实角度导致足端位置计算偏离真实值这在半直驱腿足机器人上尤其常见需要建立额外的弹性模型或者用关节端编码器做校正。5. 调参与排障真机调试中的经验清单5.1 六个常见发散场景与排查手段状态估计器发散是常态收敛才需要调参。我整理了一下真机调试中最常遇到的六种场景每一条都是赔过机器换来的经验。第一种开机瞬间姿态就翻转。绝大多数情况是 IMU 重力对齐做错了。IMU 加速度计上电读数先要静置求平均值确定重力方向然后把初始姿态设到与重力对齐的状态。很多人直接用单位四元数初始化忽略了上电时机器人可能不是水平放置的EKF 会在第一个预测步里被重力方向带偏。第二种机器人站在原地估计速度却越来越大。这个问题通常不是滤波器算错了而是接触检测或观测更新失效了。检查一下支撑腿判定是否总是为假或者观测噪声是不是设得太大把有效约束直接“稀释”掉了。第三种接触状态抖动导致估计位置跳变。对应前面说的阈值无迟滞问题。解决方法是给接触检测加迟滞带或者用接触概率做平滑过渡。第四种编码器零位没标好足端位置整体偏移。这种情况在慢走时会出现规律性的位置漂移每走一步跑偏一点。排查方法是把足端位置通过正运动学画出来和机器人实际姿态做对看很容易发现偏移方向。第五种剧烈冲击后滤波器长时间无法恢复。落地冲击会让加速度计数据瞬间饱和EKF 的线性化假设失效。常见对策是做传感器数据的异常值剔除或者在大加速度条件下临时增大过程噪声协方差让更新步更多依赖观测约束。第六种IMU 坐标系方向和机身坐标系没对齐。这是一个低级但高发的问题如果 IMU 的 x 轴方向装反了或者绕 z 轴转了90度姿态估计会在运动时出现系统性偏差。上真机前先用旋转台或者手动旋转机身对比 IMU 输出和控制板姿态确认三个轴的正方向。5.2 一套从仿真到真机的验证流程状态估计模块最适合按“仿真验证逻辑真机验证数据”的思路来推进。第一步把 EKF 放到仿真环境里跑仿真环境的好处是有状态真值可以定量对比估计误差快速暴露代码逻辑错误。MIT Cheetah 自己的代码仓库里就带了 state_estimator_tb 这类仿真测试平台先在这种环境里跑通再练真机能省下大量时间。第二步做整机静置测试。机器人上电站立不动观察估计速度和位置是否有稳态偏置。一个合格的估计器在静置几分钟后位置漂移应当在厘米级以内速度噪声应当在每秒几厘米以内。第三步做慢速行走测试。没有动捕系统的话可以给机器人绑一个标记点用手机视频逐帧对比粗略评估位置估计是否和实际走向一致。这一步主要验证接触约束是否生效、编码器零位是否正确。第四步做快速运动测试比如原地小跑或跳跃。这个阶段重点观察腾空阶段的位置漂移和落地瞬间的修正跳变。如果落地时位置出现明显突变说明腾空阶段 IMU 积分漂移过大或者接触检测的触发时机偏晚。全部验证通过后再考虑动捕系统或激光跟踪仪做高精度定量验证。动捕能测出 1 厘米以内的真值配合离线数据回放可以精确调节 Q 矩阵和 R 矩阵的数量级关系。5.3 传感器标定和状态估计的边界很多状态估计问题表面上是滤波器问题实际上是标定问题。IMU 的内参标定是第一步包括陀螺仪零偏、加速度计零偏、比例因子和安装误差不标定就做融合相当于戴着歪眼镜开车。Kalibr 是相机与 IMU 联合标定的常用工具虽然最初是为视觉惯性系统服务的但它输出的 IMU 内参结果可以直接用于腿足机器人。如果系统里有视觉或激光雷达参与融合还需要做外参标定包括 IMU 到相机、IMU 到激光雷达的位姿关系。相机和激光雷达的联合标定流程在自动驾驶领域已经比较成熟Kalibr 可以处理相机与 IMU 的外参和时延标定IMU 与激光雷达的外参也有对应的开源工具链。不要小看时延标定几毫秒的时延在快速运动时会表现为几厘米的位置误差而且非常难排查。状态估计器也不是万能的。它只能估计出模型和传感器能观测到的量如果机械结构存在明显的柔性形变或者连杆在高速运动下产生振动运动学模型本身就不准了再厉害的 EKF 也会被系统性误差折磨得头疼。边界意识很重要状态估计能把能做的做到极限但机械刚度和传感器精度决定的上限是无法通过算法突破的。6. 从 MIT Cheetah 出发的可扩展方向6.1 从四足到双足、轮足机器人MIT Cheetah 的这套 EKF 融合框架本质上并不限于四足机器人。双足机器人同样有支撑腿和摆动腿的切换同样可以利用足端速度约束只是双足支撑相比例更小、单脚支撑时约束更少滤波器对 IMU 积分的依赖会更重需要把过程噪声调得更保守一些。轮足机器人则是另一种有趣的变体。轮足机器人的轮子本身就是一个连续旋转的编码器相当于天然的里程计但轮子在打滑时里程计会失效。把 IMU 和轮速里程计通过 EKF 融合并用轮足腿部的关节力估计轮地接触状态可以比单纯轮式里程计鲁棒很多。这种方案在物流机器人和巡检机器人上已经有落地案例。从底层原理看这套方案的精神内核是识别系统中可靠的物理约束并把约束建模成虚拟观测。四足用“支撑脚不动”双足可以用“支撑脚不动 髋关节零速”轮足可以用“轮地接触点线速度为零”。约束越多、越准状态估计就越稳。6.2 引入视觉惯性融合与外参标定如果任务环境有丰富的视觉特征可以在 EKF 基础上引入相机观测组成视觉惯性里程计。MIT Cheetah 的后续版本也探索过视觉与运动学融合的路线。核心思路不变IMU 做高频预测视觉提供低频绝对约束足端接触约束作为额外的运动学观测继续保留。视觉融合对外参标定的要求极高IMU 和相机之间的旋转和平移误差哪怕只有一两厘米在远距离观测时就会放大成很大的位置误差。Kalibr 做相机与 IMU 联合标定时需要在标定板前充分激励六自由度运动采集时间不宜太短否则外参收敛不到全局最优。激光雷达融合也是类似逻辑只是外参标定换成 IMU 与雷达之间的标定视觉特征换成点云几何约束。这一类融合方案让机器人可以在更复杂的环境中实现准确的全局定位代价是计算量上升实时性需要重新评估。我个人在实际调试中还有一个习惯不管融合多少传感器先把 EKF 这套纯粹基于 IMU 和编码器的版本调稳再叠加视觉或雷达。因为纯运动学融合版本是下限保障就算视觉突然失效机器人也能靠它维持基本稳定。把基础打牢再逐步引入更多传感器是四足机器人状态估计调试最稳妥的路线。