Gazebo UUV仿真中如何利用IMU姿态降低积分误差求解线速度

核心逻辑：IMU原始输出的线加速度是载体系（imu_link坐标系）下的测量值，直接积分会引入姿态带来的投影误差，需要先通过orientation将线加速度转换到统一的导航坐标系（如Gazebo世界坐标系），抵消重力分量后再做积分，结合3D位姿传感器的校正就能有效控制误差。

1 预处理IMU原始数据 #

• 时间戳对齐：将3D位姿传感器的输出做线性插值，匹配到IMU的采样时间点，避免时间不同步导致的姿态转换误差，时间戳计算方式为 t = secs + nsecs * 1e-9
• 零偏校正：采集UUV静止状态下10s以上的IMU数据，计算线加速度的平均零偏，后续所有IMU线加速度原始输出都先减去该零偏值，仿真环境下可直接从Gazebo获取真值零偏简化操作

2 姿态转换与重力补偿 #

你拿到的orientation是四元数格式，代表载体系到导航系的旋转关系，操作流程如下：

• 四元数旋转：把载体系下的线加速度转换到导航系下，以C++ Eigen库实现为例：

// 四元数定义顺序为w、x、y、z
Eigen::Quaterniond q_wb(orientation.w, orientation.x, orientation.y, orientation.z);
Eigen::Vector3d a_b(linear_acceleration.x, linear_acceleration.y, linear_acceleration.z);
// 得到导航系下的线加速度测量值
Eigen::Vector3d a_w = q_wb * a_b;

• 重力分量抵消：导航系如果是Gazebo默认z轴向上的世界坐标系，重力矢量为(0, 0, -9.81)，减去重力后得到纯运动加速度：

Eigen::Vector3d g_w(0, 0, -9.81);
Eigen::Vector3d a_motion_w = a_w - g_w;

注意：如果你的导航系定义不同，重力矢量的分量需要对应调整

3 线速度积分计算 #

推荐用中值积分替代普通欧拉积分，可降低离散采样带来的固有误差：

• 记录上一时刻的导航系运动加速度a_motion_w_prev、上一时刻线速度v_w_prev、上一时刻时间戳t_prev
• 计算两帧IMU的时间差dt = t_current - t_prev
• 中值积分得到当前速度：

Eigen::Vector3d v_w_current = v_w_prev + (a_motion_w_prev + a_motion_w) / 2 * dt;

4 3D位姿传感器漂移校正 #

纯IMU积分的速度会随时间累积漂移，你配备的3D位姿传感器可做闭环校正，两种实现可选：

• 轻量互补滤波：通过相邻位姿的位置差除以时间差得到观测速度，和IMU积分的高频速度做加权融合：

double gain = 0.98; // 可根据传感器精度调整，IMU可信度越高权重越大
v_w_current = gain * v_w_current + (1 - gain) * v_w_obs;

• 高精度EKF融合：直接用IMU输出的协方差矩阵作为噪声权重，输入扩展卡尔曼滤波做多传感器融合，ROS环境下可以直接用imu_filter_madgwick、robot_localization功能包实现，无需手写滤波逻辑。

额外优化点 #

• 水下场景如果UUV存在静止状态，可以加入零速修正（ZUPT）策略，检测到静止时直接将速度置零，大幅抑制积分漂移
• 仿真环境下可以用Gazebo输出的真值速度做校准，调整滤波参数的适配性

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