rikirobot的IMU数据融合优化——解决导航中静止状态下的姿态漂移

在差速和全向轮小车movebase导航中，静止一段时间后出现角度上的漂移，导致地图与激光数据的不匹配。经分析此部分误差是由于里程计的定位信息误差导致的，里程计提供了整个定位与导航的位姿变换信息。

PART1 误差分析

里程计的定位信息来自两个传感器的融合

1. 编码器
2. IMU的加速度和陀螺仪以及磁力计传感器

经过测试编码器的误差可以忽略，查看IMU校准后的数据，加速度的校准误差很大，在六轴融合求解四元数的过程中，必然带来较大的角度误差，校准后的加速度如下图所示

 

通过观察校准后的静止状态下相邻两帧数据，可以发现角速度校准与实际相符，但是加速度较大且四元数数值变化较大，这与实际的静止状态不符。而传感器融合的定位需要IMU的角速度和四元数位姿信息，这将导致多传感器融合的定位信息出现较大的误差。

PART2 加速度优化

上面的IMU校准结果数据表明，rikirobot的IMU加速度校准误差较大，优化分两步进行

1. 简单的加速度零偏校准
2. imu_tk的IMU校准参数获取

由于时间问题，此处先给出简单的零偏校准，参考角速度的零偏校准，对apply_calib.cpp文件中的角速度零偏校准处添加代码

if (calibrate_gyros_)
  {
    ROS_INFO_ONCE("Calibrating gyros; do not move the IMU");
    // recursively compute mean gyro measurements
    gyro_sample_count_++;
    gyro_bias_x_ = ((gyro_sample_count_ - 1) * gyro_bias_x_ + raw->angular_velocity.x) / gyro_sample_count_;
    gyro_bias_y_ = ((gyro_sample_count_ - 1) * gyro_bias_y_ + raw->angular_velocity.y) / gyro_sample_count_;
    gyro_bias_z_ = ((gyro_sample_count_ - 1) * gyro_bias_z_ + raw->angular_velocity.z) / gyro_sample_count_;
    acc_bias_x_ = ((gyro_sample_count_ - 1) * acc_bias_x_ + raw->linear_acceleration.x) / gyro_sample_count_;
    acc_bias_y_ = ((gyro_sample_count_ - 1) * acc_bias_y_ + raw->linear_acceleration.y) / gyro_sample_count_;
    acc_bias_z_ = ((gyro_sample_count_ - 1) * acc_bias_z_ + raw->linear_acceleration.z) / gyro_sample_count_;

    if (gyro_sample_count_ >= gyro_calib_samples_)
    {
      ROS_INFO("Gyro calibration complete! (bias = [%.3f, %.3f, %.3f])", gyro_bias_x_, gyro_bias_y_, gyro_bias_z_);
      calibrate_gyros_ = false;
    }

    return;
  }

  corrected.linear_acceleration.x = raw->linear_acceleration.x-acc_bias_x_;
  corrected.linear_acceleration.y = raw->linear_acceleration.y-acc_bias_y_;
  corrected.linear_acceleration.z = raw->linear_acceleration.z-acc_bias_z_;

 

编译后运行结果如下图

优化后相邻两帧的IMU校准结果数据如上图所示，静止状态下角速度的基本为零，加速度有较小的数值，融合后的四元数变化很小，这样发布了更为精确的里程计信息，在静止状态下的漂移得到了改善，但是要完全避免漂移，应当在AMCL的全局定位中限制里程计变化的阈值。