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 accelerometerMPU_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 gyroscopeMPU_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 magnetometerMPU_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 ROSraw_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 statusif (measurements_received_ < 12)return false;for (int i = 0; i < 6; i++){if (orientation_count_[i] == 0)return false;}// solve least squaresEigen::VectorXd xhat = meas_.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(ref_);// extract solutionfor (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 measurementsgyro_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 calibrationdouble raw_accel[3];//output array for calibrated accelerationdouble corrected_accel[3];//pass received acceleration to input arrayraw_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 objectsensor_msgs::Imu corrected;corrected.header.stamp = ros::Time::now();corrected.header.frame_id = "imu_link";//pass calibrated acceleration to corrected IMU data objectcorrected.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 objectcorrected.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 datacorrected_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 timelast_time_ = time;initialized_ = true;}// determine dt: either constant, or from IMU timestampfloat 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作为测量数据对编码器的航迹推算数据进行校正

第四部分：优化

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

优化在IMU自动校准论文和开源框架代码部分介绍

https://blog.csdn.net/qq_29313679/article/details/105752021

开源框架校准结果与rikirobot方法比较

https://blog.csdn.net/qq_29313679/article/details/106001812

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

OpenSense -运动学与IMU数据
[以下教程示例适用于opensim4.1] OpenSense - Kinematics with IMU Data OpenSense是用于使用惯性测量单位inertial measurement ...
多帧点云数据拼接合并_自动驾驶：Lidar 3D传感器点云数据和2D图像数据的融合标注...
自动驾驶汽车的发展已经见证了硬件传感器记录感官数据的容量和准确度的发展.传感器的数量增加了,新一代传感器正在记录更高的分辨率和更准确的测量结果. 在本文中,我们将探讨传感器融合如何在涉及环环相扣的数据 ...
ros rviz显示rosbag中的图像和imu数据
一.rosbag相关的指令 1. rostopic list //列举出系统中正在发布的ros 话题 2. rosbag record -a //录制系统中所有正在发布的ros 话题 3. rosba ...
基于光流传感器定位和导航的自主飞行无人机
基于光流传感器定位和导航的自主飞行无人机 An Autonomous UAV with an Optical Flow Sensor for Positioning and Navigation 注: ...
异构传感器定位论文概述
一.简介最近在研究异构传感器定位方面的东西,读了一些论文.写个文章做个小结,主要是为了自我总结,也给有缘的网友们一起看看探讨探讨.以后每读一篇论文就再加一节. 异构传感器(Heterogeneous ...
多传感器融合定位第八章基于滤波的融合方法进阶
多传感器融合定位第八章基于滤波的融合方法进阶参考博客:深蓝学院-多传感器融合定位-第8章作业代码下载:https://github.com/kahowang/sensor-fusion-for ...
惯性传感器（IMU）简介【转】
近两年来,车联网.自动驾驶.无人驾驶.汽车智能化.网联化等成为了汽车行业的热点话题,未来汽车一定是朝着安全.可靠及舒适的方向发展.而这一切背后的发展都离不开传感器的作用,今天我们就来聊聊用途越来越广的 ...
自动驾驶传感器评估 ——IMU惯性测量单元
自动驾驶传感器评估 --IMU惯性测量单元前言对于自动驾驶来说,高精度定位必不可少.为了让自动驾驶系统更高频率的获取定位信息,就必须引入更高频率的传感器,这时就体现出了惯性测量单元(Inertia ...
ROS2手写接收IMU数据(Imu)代码并发布
目录前言接收IMU数据 IMU的串口连接问题 python接收串口数据 python解析数据 ROS2发布IMU数据可视化IMU数据效果前言在前面测试完了单独用激光雷达建图之后,一直想把 ...

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

第一部分:数据类型

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

第三部分：rikirobot源码解析

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

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

3.3 apply_calib节点的角速度校准

3.4 四元数的计算

第四部分：优化

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

最新文章

热门文章