多IMU视觉惯性腿里程计在足式机器人中的应用

1. 多IMU视觉惯性腿里程计技术背景

在足式机器人领域，精确的状态估计是实现稳定运动控制的基础前提。传统基于单IMU的视觉惯性里程计(VIO)系统在四足机器人这类动态平台上表现欠佳，主要原因在于：

1. 足式机器人的运动特性导致剧烈冲击和振动，单IMU容易达到测量饱和
2. 腿部运动带来的高频振动会污染IMU信号
3. 动态接触过程中传感器噪声呈现非高斯特性

我们团队开发的Cerberus2系统通过多IMU配置解决了这些痛点。具体来说，在机器人的躯干和每条腿部都安装了独立的IMU模块，构建分布式惯性测量网络。这种设计带来三个核心优势：

• 冗余测量：多个IMU提供空间分布的角速度和线性加速度测量，通过统计融合降低单个传感器失效的风险
• 噪声抑制：腿部IMU可以捕捉接触冲击的高频成分，与躯干IMU信号互补
• 运动解耦：通过腿部IMU数据可分离躯干运动与腿部相对运动

实际部署中发现，IMU安装位置的选择直接影响系统性能。我们最终将躯干IMU固定在机器人重心附近，腿部IMU则安装在关节驱动器外侧，这样既能获得干净的躯干运动测量，又能准确捕捉足端接触动力学。

2. 系统架构与传感器融合方案

2.1 硬件配置方案

Cerberus2的传感器套件包含：

1. 视觉传感器：采用全局快门单目相机，帧率15Hz，安装在机器人前部倾斜30°以获得理想视野
2. 惯性测量单元：1个躯干IMU(200Hz) + 4个腿部IMU(各200Hz)，全部采用BMI088六轴IMU
3. 关节编码器：各关节配备17位绝对值编码器，采样频率1kHz
4. 接触传感器：足端安装FSR压力传感器用于接触检测

传感器时空同步是实现精准融合的前提。我们设计了三级同步机制：

1. 硬件触发：相机曝光信号触发IMU采样
2. 软件时间对齐：使用PTP协议同步各处理器时钟
3. 动态时间标定：在线估计并补偿各传感器通道的固定延迟

2.2 因子图优化框架

系统采用基于GTSAM的因子图优化架构，包含以下关键因子类型：

特别地，多IMU数据处理采用分层融合策略：

1. 先对各IMU数据独立预积分
2. 通过腿部运动学模型建立IMU间的空间约束
3. 在优化层面对所有IMU测量进行联合优化

这种方案相比集中式融合计算量增加约15%，但定位精度提升超过40%。

3. 核心算法实现细节

3.1 基于四元数的误差状态卡尔曼滤波

对于姿态估计，我们采用误差状态卡尔曼滤波(ESKF)框架，其核心状态向量包含：

x = [δθ, δv, δp, δbg, δba]^T

其中δθ采用三维误差角参数化，避免四元数过参数化问题。关键推导步骤：

1. 根据四元数微分方程建立连续时间动力学模型
2. 使用一阶泰勒展开进行离散化
3. 推导过程雅可比矩阵实现ESKF预测

在实测中发现，传统ESKF在足式机器人动态运动中会出现两个问题：

1. 剧烈冲击导致IMU饱和时误差状态发散
2. 腿部摆动期间陀螺仪噪声特性变化

解决方案是引入自适应噪声协方差估计：

// 伪代码：噪声协方差在线调整 if (detected_impact) { gyro_noise *= 5.0; accel_noise *= 10.0; } else if (swing_phase) { gyro_noise *= 2.0; }

3.2 接触相位检测算法

准确的接触状态判断对腿部里程计至关重要。我们融合三种检测方式：

1. 阈值法：FSR压力值超过设定阈值
2. 运动学法：足端速度低于阈值(0.1m/s)
3. 学习法：LSTM网络分析IMU时序特征

最终接触概率通过贝叶斯滤波融合：

p(contact) = η * p(FSR) * p(kinematic) * p(LSTM)

实测表明，在草地、沙地等复杂地形下，多方法融合的检测准确率达到98.7%，比单一方法提升至少15%。

4. 系统实现与性能优化

4.1 实时性保障措施

为保证算法在机载计算机(Intel NUC i7)上实时运行，采取了以下优化：

1. 关键线程分配：

   • 视觉前端：专用CPU核心
   • IMU处理：固定优先级实时线程
   • 因子图优化：异步执行，最大延迟控制

2. 计算加速技巧：

   • 使用Eigen的SIMD指令优化矩阵运算
   • 对重投影因子采用解析雅可比
   • 稀疏矩阵求解器预分配内存

3. 关键参数：

   • 滑动窗口大小：15帧
   • 优化迭代次数：每次更新3次迭代
   • 边缘化频率：每5帧执行一次

4.2 典型问题排查指南

在实际部署中遇到的典型问题及解决方案：

特别值得注意的是IMU饱和问题，我们的解决策略是：

1. 实时监测加速度计/陀螺仪输出值
2. 当超过量程90%时触发饱和标志
3. 在状态估计中降低对应测量权重
4. 利用腿部运动学提供替代测量

5. 实测性能与分析

5.1 定量评估结果

在四种典型地形下的定位误差对比(%)：

关键发现：

1. 多IMU配置在平坦地形表现最优
2. 传统视觉方法在特征缺失环境易失效
3. 完整系统在各种地形都保持稳定

5.2 计算资源占用

系统各模块的计算负载分布：

在实际部署中，我们通过以下方式进一步优化：

1. 对非关键模块实施动态降频
2. 采用ROS2的实时通信机制
3. 关键路径代码手写汇编优化

经过这些优化后，系统可以在15W低功耗平台上稳定运行，满足四足机器人对能耗的严格要求。