如何最小化IMU采集的3D角速度数据积分误差以获取线性速度

核心误差来源 #

朴素积分的漂移主要来自陀螺仪零偏、加速度计零偏、重力分量剥离误差、时间同步误差，转弯场景下角速度的积分误差会被放大，进一步传导到线加速度的坐标系转换环节，最终导致速度积分结果快速发散。

ROS生态可用方案 #

• imu_tools/imu_filter_madgwick：首先通过互补滤波融合IMU的加速度计、陀螺仪、磁力计（如有）数据，输出无漂移的姿态解算结果，你可以基于输出的高精度姿态，将IMU的体坐标系线加速度转换到世界坐标系，剥离重力分量后再做积分，能大幅降低姿态误差带来的速度漂移。
• robot_localization：ROS生态最常用的多传感器状态估计功能包，内置EKF/UKF两种滤波算法，直接输入IMU原始数据，同时可接入UUV常用的DVL、压力计、GPS（水面场景）等观测数据做约束，不需要自己实现积分逻辑，配置完成后会直接输出包含线性速度、姿态、位置的里程计话题，是UUV仿真场景的首选方案。
• msf（多传感器融合框架）：适合多传感器高精度融合场景，支持松耦合融合IMU与其他观测数据，可在线估计IMU的陀螺仪、加速度计零偏，能长期抑制积分漂移。

非ROS通用方案 #

• 带零速更新（ZUPT）的互补滤波：实现逻辑简单、算力开销小，如果你的UUV存在可检测的静止时段，静止时直接将速度观测设为0修正积分漂移，适合对精度要求不高的场景。
• 误差状态卡尔曼滤波（ESKF）：专门针对IMU状态估计设计，数值稳定性优于普通EKF，可在线估计IMU零偏，每一步迭代都修正姿态、速度的积分误差，比朴素积分精度提升明显。
• IMU预积分+图优化：适合离线数据处理场景，将IMU的姿态、速度、位置增量作为因子加入图优化框架，配合DVL、视觉等其他观测的约束做全局优化，可完全消除长时序的累计误差，可基于Ceres Solver、GTSAM库实现。

代码参考 #

1. ESKF速度解算简化核心代码 #

#include <Eigen/Core>
#include <Eigen/Geometry>

// 状态变量
Eigen::Quaterniond q_wb; // 世界系到体坐标系的旋转
Eigen::Vector3d vel_w;   // 世界系下的线速度
Eigen::Vector3d acc_bias; // 加速度计零偏
Eigen::Vector3d gyro_bias; // 陀螺仪零偏
const Eigen::Vector3d GRAVITY = {0, 0, 9.81};

void imu_callback(const Eigen::Vector3d& imu_acc, const Eigen::Vector3d& imu_gyro, double timestamp, double last_timestamp) {
    double dt = timestamp - last_timestamp;
    // 1. 零偏补偿
    Eigen::Vector3d acc_calib = imu_acc - acc_bias;
    Eigen::Vector3d gyro_calib = imu_gyro - gyro_bias;

    // 2. 姿态传播（中值积分）
    Eigen::Quaterniond delta_q(1, 0.5*gyro_calib.x()*dt, 0.5*gyro_calib.y()*dt, 0.5*gyro_calib.z()*dt);
    q_wb = q_wb * delta_q;
    q_wb.normalize();

    // 3. 加速度转世界系，剥离重力
    Eigen::Vector3d acc_world = q_wb * acc_calib - GRAVITY;

    // 4. 速度积分
    vel_w += acc_world * dt;

    // 可选：零速更新修正漂移
    const double ACC_NOISE_THRESH = 0.1;
    const double GYRO_NOISE_THRESH = 0.01;
    if (acc_calib.norm() < ACC_NOISE_THRESH && gyro_calib.norm() < GYRO_NOISE_THRESH) {
        vel_w.setZero();
    }
}

2. robot_localization EKF配置核心参数节选 #

ekf_se_node:
  frequency: 50
  sensor_timeout: 0.2
  two_d_mode: false # UUV为3D运动，关闭2D模式
  world_frame: odom
  base_link_frame: base_link
  publish_tf: true

  # IMU传感器配置
  imu0: /imu/data_raw
  imu0_config: [false, false, false,  # 不使用IMU输入的姿态
                true, true, true,     # 使用角速度x/y/z
                true, true, true,     # 使用线加速度x/y/z
                false, false, false,  # 不使用IMU输入的速度
                false, false, false]  # 不使用IMU输入的位置
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true # 自动剥离重力

  # 输出话题：/odometry/filtered 包含解算后的线速度、姿态、位置

内容的提问来源于stack exchange，提问作者Bob9710