1. 研究背景与问题定义
在足式机器人领域，精确的状态估计是实现稳定运动控制的基础前提。传统基于IMU、关节编码器等本体传感器的状态估计算法，往往建立在两个理想化假设之上：脚部与地面接触时不会发生打滑现象，以及机器人腿部结构在运动过程中保持刚性不变。然而在实际应用场景中，这两个假设经常被打破。
当机器人在湿滑地面、斜坡或非结构化地形移动时，脚部打滑几乎不可避免。我们的实测数据显示，即使在干燥的硬质地面上，四足机器人在快速小跑时也会产生3-7mm的瞬时滑动。更棘手的是，腿部结构在动态负载下会产生微米级的弹性变形，这种看似微小的变化会通过运动学链不断累积，最终导致厘米级的位姿估计误差。
关键发现：在持续10分钟的自主行走实验中，传统EKF算法产生的累积位置误差可达实际移动距离的15-20%，严重制约了机器人的自主导航能力。
2. 双β-卡尔曼滤波器设计原理
2.1 整体架构设计
研究团队提出的Dual β-KF采用分而治之的策略，将状态估计问题分解为两个相对独立的子问题：
- 参数滤波器：专注于估计随时间变化的腿部几何参数
- 状态滤波器：负责机器人本体位姿的鲁棒估计
这种解耦设计的优势在于：
- 参数估计不受状态估计误差的反向污染
- 每个滤波器可以针对特定问题优化目标函数
- 计算复杂度被分摊到两个并行的处理流程中
2.2 腿部参数滤波器实现
该模块采用改进的静力学测量模型，仅依赖关节扭矩τ和脚部接触力f来估计等效腿长l：
code复制l̂ = argminₗ ‖τ - Jᵀ(l)f‖² + λ‖l - lₚ‖²
其中J(l)是依赖于腿长的雅可比矩阵，lₚ是上一时刻的估计值，λ是正则化系数。这种设计巧妙规避了传统方法中状态信息与参数估计的交叉干扰问题。
2.3 β-卡尔曼状态滤波器
研究团队从概率角度重新诠释卡尔曼滤波，将其视为一个最小化KL散度的优化问题。针对脚部打滑导致的测量离群值，引入β-散度构建新的代价函数：
L(θ) = (1/β)[p(x)^β - (1+β)q(x|θ)^β p(x)^(1-β)] + q(x|θ)^(1+β)
当β→0时退化为标准KL散度，而适当增大β值可显著提升滤波器对异常测量的鲁棒性。实验表明β=0.5时能在灵敏度和鲁棒性间取得最佳平衡。
3. 实验验证与方法对比
3.1 仿真环境搭建
在Gazebo中构建了包含以下要素的测试场景：
- 不同摩擦系数的地面材质（μ=0.3-1.2）
- 可编程的腿部柔性参数
- 多种运动模式（行走、小跑、跳跃）
测试对比了5种主流算法：
- 标准QEKF
- UKF with outlier rejection (UKF-OR)
- Iterated EKF (IEKF)
- Weighted KF
- 提出的Dual β-KF
3.2 真实实验配置
采用宇树GO2四足机器人平台，传感器配置包括：
- 200Hz IMU（BMI088）
- 500Hz关节编码器（AS5048A）
- 1kHz足底力传感器（FlexiForce A401）
- NOKOV度量动作捕捉系统（0.1mm精度@100Hz）
操作要点：动作捕捉系统标定需在实验前完成坐标系对齐，建议采用三点标定法将机器人基坐标系与动作捕捉系统坐标系统一。
3.3 结果分析
在直线行走测试中，各算法表现对比如下：
| 算法类型 | 位置误差(m) | 姿态误差(°) | 腿长估计误差(mm) |
|---|---|---|---|
| QEKF | 0.48 | 3.2 | 12.5 |
| UKF-OR | 0.35 | 2.8 | N/A |
| IEKF | 0.29 | 2.1 | 9.3 |
| Dual β-KF | 0.17 | 1.4 | 4.7 |
特别值得注意的是，在故意制造的滑移场景中（地面喷洒硅油），Dual β-KF展现出显著优势：
code复制[滑移发生时刻]
QEKF位置误差突增：+0.32m
Dual β-KF位置误差变化：+0.07m
4. 工程实现关键点
4.1 实时性优化
算法在NVIDIA Jetson AGX Orin上的实测性能：
- 参数滤波器：0.8ms/cycle
- 状态滤波器：1.2ms/cycle
- 总延迟：<5ms（满足100Hz控制需求）
内存占用方面：
- 状态向量：28维（位置3+姿态4+速度3+角速度3+腿长参数15）
- 峰值内存：12.6MB
4.2 参数调试经验
通过大量实验总结出关键参数调节规律：
-
β值选择：
- 平坦地面：β=0.3-0.5
- 复杂地形：β=0.5-0.7
- 需避免β>0.8导致滤波器响应迟钝
-
过程噪声矩阵Q调参：
- 对角元素初始值建议：
- 位置：0.01m²/s
- 姿态：0.001rad²/s
- 腿长：0.0001m²/s
- 对角元素初始值建议：
-
测量噪声矩阵R设置：
- IMU加速度：0.05m²/s⁴
- IMU角速度：0.01rad²/s²
- 关节编码器：0.001rad²
5. 典型问题排查指南
5.1 腿长估计发散
可能原因：
- 雅可比矩阵计算错误
- 接触力传感器标定失效
- 正则化系数λ设置不当
解决方案：
- 检查雅可比矩阵符号一致性
- 重新进行力传感器零点标定
- 逐步增大λ直到估计稳定
5.2 状态估计滞后
常见表现：
- 机器人转向时位姿估计响应延迟
- 快速运动时轨迹跟踪误差增大
调试步骤：
- 检查传感器时间同步（建议使用PTP协议）
- 降低β值提高灵敏度
- 适当增大过程噪声Q中速度相关项
5.3 动作捕捉数据异常
处理流程：
- 检查反光标记点遮挡情况
- 验证坐标系转换矩阵
- 对比原始光学数据与解算结果
- 必要时重新进行手眼标定
6. 应用前景与扩展方向
该方法已成功应用于多个机器人项目：
- 野外勘探四足机器人（最大负载15kg）
- 电力巡检轮腿式机器人
- 仿生服务机器人
未来改进方向包括：
- 融合视觉信息的多模态估计
- 在线学习调整β参数
- 考虑地面刚度影响的接触模型
在实际部署中发现，将Dual β-KF与SLAM系统结合时，建议采用松耦合架构，以20Hz频率交换位姿信息，既能保证估计精度，又可避免计算负载过大。