robot_localization多传感器定位-IMU数据的校准与融合

 

robot_localization多传感器融合中的IMU数据获取分四个部分来说明，分别是数据类型、IMU校准及融合理论、rikirobot源码解析以及优化

第一部分:数据类型

robot_localization多传感器融合中的IMU数据，需要四元数和角速度，因此除了角速度的校准，还需要磁力计来获取融合后的四元数的信息。这个在robot_localization的论文数据表格中可以看出来

第二部分：IMU校准及融合理论

首先引用https://www.cnblogs.com/hiram-zhang/p/10398959.html的理论说明IMU数据融合的算法实现过程

完成数据的校准以后，利用高低通互补滤波、扩展卡尔曼滤波EKF、Mahony滤波中的一种实现姿态数据融合，获取四元数

高低通互补滤波如下所示

第三部分：rikirobot源码解析

rikirobot的IMU数据首先是获取原始里程计数据，然后分别进行标定，使用do_calib节点进行加速度校准，apply_calib节点进行角速度校准，以及原始IMU数据标定后到sensor_msgs/Imu转换，最后imu_filter_madgwick节点融合获取四元数。

3.1 STM32控制板获取IMU原始数据以及发布

void publish_imu()
{
	
    //geometry_msgs::Vector3 acceler, gyro, mag;
    //this function publishes raw IMU reading
    //measure accelerometer
		
    MPU_Get_Accelerometer(&ax,&ay,&az);
	acc.x=ax*Accelerometer_SCALE;
	acc.y=ay*Accelerometer_SCALE;
	acc.z=az*Accelerometer_SCALE;
	raw_imu_msg.linear_acceleration = acc;
		
    //measure gyroscope
    MPU_Get_Gyroscope(&gx,&gy,&gz);
	gyr.x=gx*Gyroscope_SCALE;
	gyr.y=gy*Gyroscope_SCALE;
	gyr.z=gz*Gyroscope_SCALE;	
	raw_imu_msg.angular_velocity = gyr;
		
    //measure magnetometer
    MPU_Get_Magnetometer(&mx,&my,&mz);
	meg.x=mx*Magnetometer_SCALE;
	meg.y=my*Magnetometer_SCALE;
	meg.z=mz*Magnetometer_SCALE;	
	raw_imu_msg.magnetic_field = meg;

    //publish raw_imu_msg object to ROS
		
    raw_imu_pub.publish(&raw_imu_msg);
		

}

 

3.2 do_calib节点获取加速度校准参数

回调函数中通过在xyz的正负方向采样的数据，对加速度进行校准

void DoCalib::imuCallback(riki_msgs::Imu::ConstPtr imu)
{
  bool accepted;

  switch (state_)
  {
  case START:
    calib_.beginCalib(6*measurements_per_orientation_, reference_acceleration_);
    state_ = SWITCHING;
    break;


  case SWITCHING:
    if (orientations_.empty())
    {
      state_ = COMPUTING;
    }
    else
    {
      current_orientation_ = orientations_.front();

      orientations_.pop();
      measurements_received_ = 0;

      std::cout << "Orient IMU with " << orientation_labels_[current_orientation_] << " facing up. Press [ENTER] once done.";
      std::cin.ignore();
      std::cout << "Calibrating! This may take a while...." << std::endl;

      state_ = RECEIVING;
    }
    break;


  case RECEIVING:
    accepted = calib_.addMeasurement(current_orientation_,
                                     imu->linear_acceleration.x,
                                     imu->linear_acceleration.y,
                                     imu->linear_acceleration.z);

    measurements_received_ += accepted ? 1 : 0;
	//每次采集500次数据
    if (measurements_received_ >= measurements_per_orientation_)
    {
      std::cout << " Done." << std::endl;
      state_ = SWITCHING;
    }
    break;


  case COMPUTING:
    std::cout << "Computing calibration parameters...";
    if (calib_.computeCalib())
    {
      std::cout << " Success!"  << std::endl;

      std::cout << "Saving calibration file...";
      if (calib_.saveCalib(output_file_))
      {
        std::cout << " Success!" << std::endl;
      }
      else
      {
        std::cout << " Failed." << std::endl;
      }
    }
    else
    {
      std::cout << " Failed.";
      ROS_ERROR("Calibration failed");
    }
    state_ = DONE;
    break;


  case DONE:
    break;
  }
}

其中computeCalib()函数使用UV分解得到尺度偏差矩阵和零偏参数

bool AccelCalib::computeCalib()
{
  // check status
  if (measurements_received_ < 12)
    return false;

  for (int i = 0; i < 6; i++)
  {
    if (orientation_count_[i] == 0)
      return false;
  }

  // solve least squares
  Eigen::VectorXd xhat = meas_.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(ref_);

  // extract solution
  for (int i = 0; i < 9; i++)
  {
    SM_(i/3, i%3) = xhat(i);
  }

  for (int i = 0; i < 3; i++)
  {
    bias_(i) = xhat(9+i);
  }

  calib_ready_ = true;
  return true;
}

3.3 apply_calib节点的角速度校准

角速度校准在源码的回调函数中只是进行了零偏校准，然后发布了校准后转换的sensor_msgs::Imu数据

void ApplyCalib::rawImuCallback(riki_msgs::Imu::ConstPtr raw)
{
  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_;

    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;
  }

  //input array for acceleration calibration
  double raw_accel[3];
  //output array for calibrated acceleration
  double corrected_accel[3];

  //pass received acceleration to input array
  raw_accel[0] = raw->linear_acceleration.x;
  raw_accel[1] = raw->linear_acceleration.y;
  raw_accel[2] = raw->linear_acceleration.z;

  //apply calibrated 
  calib_.applyCalib(raw_accel, corrected_accel);

  //create calibrated data object
  sensor_msgs::Imu corrected;

  corrected.header.stamp = ros::Time::now();
  corrected.header.frame_id = "imu_link";
  
  //pass calibrated acceleration to corrected IMU data object
  corrected.linear_acceleration.x = corrected_accel[0];
  corrected.linear_acceleration.y = corrected_accel[1];
  corrected.linear_acceleration.z = corrected_accel[2];
  
  //add calibration bias to  received angular velocity and pass to to corrected IMU data object
  corrected.angular_velocity.x = raw->angular_velocity.x - gyro_bias_x_;
  corrected.angular_velocity.y = raw->angular_velocity.y - gyro_bias_y_;
  corrected.angular_velocity.z = raw->angular_velocity.z - gyro_bias_z_;

  //publish calibrated IMU data
  corrected_pub_.publish(corrected);


  sensor_msgs::MagneticField mag_msg;

  mag_msg.header.stamp = ros::Time::now();

  //scale received magnetic (miligauss to tesla)
  mag_msg.magnetic_field.x = raw->magnetic_field.x * 0.000001;
  mag_msg.magnetic_field.y = raw->magnetic_field.y * 0.000001;
  mag_msg.magnetic_field.z = raw->magnetic_field.z * 0.000001;
  
  mag_pub_.publish(mag_msg);
}

3.4 四元数的计算

到校准后的IMU数据，最后进行九轴融合的四元数获取，使用imu_filter_madgwick节点进行计算

void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
{
  boost::mutex::scoped_lock lock(mutex_);

  const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
  const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;

  ros::Time time = imu_msg_raw->header.stamp;
  imu_frame_ = imu_msg_raw->header.frame_id;

  if (!initialized_ || stateless_)
  {
    geometry_msgs::Quaternion init_q;
    if (!StatelessOrientation::computeOrientation(world_frame_, lin_acc, init_q))
    {
      ROS_WARN_THROTTLE(5.0, "The IMU seems to be in free fall, cannot determine gravity direction!");
      return;
    }
    filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
  }

  if (!initialized_)
  {
    ROS_INFO("First IMU message received.");
    check_topics_timer_.stop();

    // initialize time
    last_time_ = time;
    initialized_ = true;
  }

  // determine dt: either constant, or from IMU timestamp
  float dt;
  if (constant_dt_ > 0.0)
    dt = constant_dt_;
  else
  {
    dt = (time - last_time_).toSec();
    if (time.isZero())
      ROS_WARN_STREAM_THROTTLE(5.0, "The IMU message time stamp is zero, and the parameter constant_dt is not set!" <<
                                    " The filter will not update the orientation.");
  }

  last_time_ = time;

  if (!stateless_)
    filter_.madgwickAHRSupdateIMU(
      ang_vel.x, ang_vel.y, ang_vel.z,
      lin_acc.x, lin_acc.y, lin_acc.z,
      dt);

  publishFilteredMsg(imu_msg_raw);//发布imu数据
  if (publish_tf_)
    publishTransform(imu_msg_raw);
}

但是分析代码后发现，将use_mag和use_magnetic_field_msg设置false后不再考虑磁力计信息，使用加速度和角速度获取相应的四元数。

上面的代码运行后将获取相应的IMU数据，将话题数据载入robot_localization作为测量数据对编码器的航迹推算数据进行校正

第四部分：优化

上面的角速度没有进行尺度偏差等校准，缺少磁力计的融合，后面进行分析