当前位置：首页 > news >正文

告别‘零速假设’：用多IMU+EKF解决足式机器人打滑检测难题（附开源代码解读）

news 2026/6/2 21:53:59

多IMU融合与触地动力学建模：足式机器人定位精度的革新之路

在四足机器人快速发展的今天，定位精度始终是制约其自主能力的关键瓶颈。传统本体感知里程计（Proprioceptive Odometry, PO）依赖"零速假设"——即认为足部触地瞬间速度为零，这一简化模型在复杂地形中往往导致显著的定位漂移。最新研究表明，通过足部多IMU节点与改进的触地动力学建模，能够在不增加显著计算负担的前提下，将定位漂移从传统方法的10%以上降低至2%左右。

1. 传统PO的局限性与多IMU方案的崛起

传统本体感知里程计主要依赖三个数据源：躯干IMU、关节编码器和足部接触传感器。这种配置在理想平坦地面上表现尚可，但遇到以下场景就会出现明显问题：

光滑表面：足部打滑导致零速假设完全失效
不平地形：足部滚动或部分接触造成速度估计偏差
软质地面：足部沉陷带来的额外位移未被建模

关键痛点在于，传统方法将足地接触简化为二元状态（接触/非接触），而忽略了触地瞬间丰富的动力学信息。MIT Cheetah 3等知名平台采用的线性估计器虽然计算高效，但旋转估计误差会随时间不断累积。

多IMU方案的核心创新在于：

分布式感知：在足端部署低成本IMU（如MPU9250），直接测量足部运动
动力学建模：建立足部触地时的转动模型，替代简单的零速假设
自适应滤波：通过马氏距离动态评估接触质量，实现打滑检测

# 典型的多IMU系统硬件配置示例 robot_config = { 'body_imu': 'BMI088', # 躯干6轴IMU 'foot_imus': ['MPU9250']*4, # 每足1个9轴IMU 'joint_encoders': ['AS5048A']*12, # 每个关节绝对编码器 'processor': 'NVIDIA Jetson Xavier NX' # 边缘计算单元 }

2. 触地转动模型：重新定义足地交互

当足部接触地面时，机器人实际上在以接触点为瞬时旋转中心进行运动。这一物理现象构成了触地转动模型的理论基础：

$$ \mathbf{v}_{foot} = \boldsymbol{\omega} \times \mathbf{d} $$

其中：

$\boldsymbol{\omega}$ 是足部角速度（由足端IMU测量）
$\mathbf{d}$ 是从接触点到IMU安装位置的向量
$\mathbf{v}_{foot}$ 是足端线速度

模型优势体现在：

兼容滑动和滚动接触
自动适应不同地面刚度
不需要额外的接触力传感器

参数	物理意义	获取方式
$d_0$	足端到IMU的固定距离	机械设计已知
$\mathbf{n}$	接触面法向量	通过躯干IMU和关节角度估计
$\boldsymbol{\omega}_f$	足部角速度	足端IMU直接测量

注意：实际实现时需要校准IMU与足端的相对安装姿态$R_f^b(\phi)$，这部分可通过运动学标定完成

3. 改进的EKF框架设计与实现

扩展卡尔曼滤波(EKF)作为状态估计的经典方法，在多IMU系统中需要重新设计状态量和观测模型。

3.1 状态量扩展

与传统PO相比，新增足部速度状态：

$$ \mathbf{x} = [\mathbf{p}; \mathbf{v}; \boldsymbol{\theta}; \mathbf{s}_1...\mathbf{s}_4; \dot{\mathbf{s}}_1...\dot{\mathbf{s}}_4] $$

其中：

$\mathbf{p},\mathbf{v},\boldsymbol{\theta}$：躯干位置、速度、姿态
$\mathbf{s}_j$：第j足的世界坐标
$\dot{\mathbf{s}}_j$：第j足的世界系速度

3.2 预测模型更新

足部状态预测需融合足端IMU数据：

$$ \dot{\mathbf{s}}_{k+1} = \dot{\mathbf{s}}_k + \Delta t (R(\boldsymbol{\theta}_k)R_f^b(\phi)\mathbf{a}_f - \mathbf{g}_w) $$

3.3 自适应量测更新

通过马氏距离实现鲁棒更新：

def mahalanobis_update(z, S, sigma=3.0): """打滑检测逻辑""" M = np.sqrt(z.T @ np.linalg.inv(S) @ z) if M < sigma: return K @ z # 正常更新 else: return 0 # 忽略该次量测

实现技巧：