欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-03-04 18:42:46
...

 

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

第一部分:数据类型

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

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

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

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

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

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

高低通互补滤波如下所示

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

第三部分: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作为测量数据对编码器的航迹推算数据进行校正

第四部分:优化

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