ROS常用消息之Imu

sensor_msgs/Imu —— 惯性测量单元消息官方文档sensor_msgs/Imu Message源码位置File: sensor_msgs/Imu.msg1. 概述sensor_msgs/Imu用于承载IMUInertial Measurement Unit惯性测量单元输出的数据包括姿态四元数、角速度、线加速度及其各自的协方差是状态估计、融合滤波EKF / UKF中最重要的传感器消息之一。典型使用场景机器人 / 无人机的姿态估计与航向保持与里程计、视觉融合的位姿估计robot_localization 等振动检测、急停判定、IMU 预热与零偏估计何时使用当需要发布或接收惯性传感器数据时使用本消息。2. 消息定义Raw Message Definition#This is a message to hold data from anIMU(Inertial Measurement Unit)##Accelerations should be in m/s^2(notin gs),androtational velocity should be in rad/sec##If the covariance of the measurement is known,it should be filledin(ifall you know is the#varianceof each measurement,e.g.from the datasheet,just put those along the diagonal)#A covariance matrix of all zeros will be interpreted ascovariance unknown,andto use the#dataa covariance will have to be assumedorgotten from some other source##If you have no estimateforone of the dataelements(e.g.your IMU doesnt produce an orientation#estimate),please set element0of the associated covariance matrix to-1#If you are interpretingthismessage,please checkfora value of-1in the first element of each#covariancematrix,anddisregard the associated estimate.Header header geometry_msgs/Quaternion orientation float64[9]orientation_covariance # Row major about x,y,z axes geometry_msgs/Vector3 angular_velocity float64[9]angular_velocity_covariance # Row major about x,y,z axes geometry_msgs/Vector3 linear_acceleration float64[9]linear_acceleration_covariance # Row major x,y zCompact Message Definitionstd_msgs/Header header geometry_msgs/Quaternion orientation float64[9]orientation_covariance geometry_msgs/Vector3 angular_velocity float64[9]angular_velocity_covariance geometry_msgs/Vector3 linear_acceleration float64[9]linear_acceleration_covariance3. 字段速查表字段类型单位说明headerstd_msgs/Header—时间戳、坐标系通常为 IMU 本体坐标系如imu_linkorientationgeometry_msgs/Quaternion—姿态估计四元数orientation_covariancefloat64[9]—姿态协方差3x3行优先angular_velocitygeometry_msgs/Vector3rad/s角速度陀螺仪输出angular_velocity_covariancefloat64[9]—角速度协方差3x3行优先linear_accelerationgeometry_msgs/Vector3m/s²线加速度加速度计输出含重力分量linear_acceleration_covariancefloat64[9]—线加速度协方差3x3行优先单位提醒线加速度单位是m/s² 而不是 g角速度单位是rad/s。协方差的特殊约定协方差状态含义接收方处理全部为 0协方差未知需自行假设或从其他来源获取第一个元素为 -1该项数据无估计如 IMU 不输出姿态忽略对应的测量值4. 核心概念三个数据分量orientation姿态四元数表示 IMU 在全局参考系下的朝向。不是所有 IMU 都输出姿态若没有则orientation_covariance[0] -1。angular_velocity角速度绕 x、y、z 轴的旋转速度来自陀螺仪。linear_acceleration线加速度沿 x、y、z 轴的加速度来自加速度计。静止时线加速度不为零加速度计测量的是比力specific force静止平放时读数为(0, 0, 9.8) m/s²即重力。这是初学者最常见的疑惑若要获得不含重力的真实运动加速度需要减去重力分量。利用这一特性可以从加速度计估算俯仰 / 横滚角但无法直接给出偏航角。角速度积分与姿态对角速度积分可以得到角度变化量但陀螺仪存在零偏漂移长时间积分会导致姿态发散。工程上常将加速度计 陀螺仪 磁力计通过滤波融合互补滤波 / EKF得到稳定姿态。四元数与欧拉角的换算参考 Odometry.md 第 4 节。发布频率IMU 数据频率通常较高100 ~ 1000 Hz应保证header.stamp使用硬件时间戳否则高频数据的时间戳抖动会严重影响融合效果。5. 实践案例案例一订阅 IMU获取姿态与加速度C#includeros/ros.h#includesensor_msgs/Imu.h#includetf2/LinearMath/Quaternion.h#includetf2_geometry_msgs/tf2_geometry_msgs.hvoidimuCallback(constsensor_msgs::Imu::ConstPtrmsg){// 1. 判断姿态是否可用if(msg-orientation_covariance[0]0.0){ROS_WARN(IMU does not provide orientation estimate.);}else{// 2. 四元数 - 欧拉角tf2::Quaternion q;tf2::fromMsg(msg-orientation,q);doubleroll,pitch,yaw;tf2::Matrix3x3(q).getRPY(roll,pitch,yaw);ROS_INFO(orientation RPY: (%.3f, %.3f, %.3f) rad,roll,pitch,yaw);}// 3. 读取角速度rad/s与线加速度m/s^2ROS_INFO(angular_velocity: (%.3f, %.3f, %.3f) rad/s,msg-angular_velocity.x,msg-angular_velocity.y,msg-angular_velocity.z);ROS_INFO(linear_acceleration: (%.3f, %.3f, %.3f) m/s^2,msg-linear_acceleration.x,msg-linear_acceleration.y,msg-linear_acceleration.z);}intmain(intargc,char**argv){ros::init(argc,argv,imu_subscriber);ros::NodeHandle nh;ros::Subscriber subnh.subscribe(/imu/data_raw,100,imuCallback);ros::spin();return0;}案例二由加速度计估算俯仰 / 横滚角C#includeros/ros.h#includesensor_msgs/Imu.h#includecmath// 利用静止时重力矢量方向估算俯仰 / 横滚// 注意此方法无法估算偏航角且动态时误差较大voidestimateTilt(constsensor_msgs::Imu::ConstPtrmsg,doubleroll,doublepitch){doubleaxmsg-linear_acceleration.x;doubleaymsg-linear_acceleration.y;doubleazmsg-linear_acceleration.z;rollstd::atan2(ay,az);pitchstd::atan2(-ax,std::sqrt(ay*ayaz*az));}intmain(intargc,char**argv){ros::init(argc,argv,imu_tilt_estimator);ros::NodeHandle nh;ros::Subscriber subnh.subscribe(/imu/data_raw,100,[](constsensor_msgs::Imu::ConstPtrmsg){doubleroll,pitch;estimateTilt(msg,roll,pitch);ROS_INFO(accel-based tilt: roll%.3f rad, pitch%.3f rad,roll,pitch);});ros::spin();return0;}案例三发布模拟 IMU 数据C测试用#includeros/ros.h#includesensor_msgs/Imu.h#includetf2/LinearMath/Quaternion.h#includetf2_geometry_msgs/tf2_geometry_msgs.hintmain(intargc,char**argv){ros::init(argc,argv,fake_imu_publisher);ros::NodeHandle nh;ros::Publisher imu_pubnh.advertisesensor_msgs::Imu(/imu/data_raw,100);ros::Rateloop_rate(100);// 100 Hzwhile(ros::ok()){sensor_msgs::Imu imu;imu.header.stampros::Time::now();imu.header.frame_idimu_link;// 静止平放姿态为单位四元数tf2::Quaternion q;q.setRPY(0,0,0);imu.orientationtf2::toMsg(q);// 角速度零imu.angular_velocity.x0.0;imu.angular_velocity.y0.0;imu.angular_velocity.z0.0;// 线加速度静止时读数为重力 (0, 0, 9.8)imu.linear_acceleration.x0.0;imu.linear_acceleration.y0.0;imu.linear_acceleration.z9.8;// 协方差对角线填入典型方差单位见文档未知项首个元素为 -1for(inti0;i9;i){imu.orientation_covariance[i]0.0;imu.angular_velocity_covariance[i]0.0;imu.linear_acceleration_covariance[i]0.0;}imu.orientation_covariance[0]1e-4;// 姿态可用imu.angular_velocity_covariance[0]1e-6;imu.linear_acceleration_covariance[0]1e-4;imu_pub.publish(imu);loop_rate.sleep();}return0;}6. 常见问题与避坑问题说明 / 正确做法单位搞错线加速度是m/s²不是 g角速度是rad/s不是 deg/s静止时加速度不为 0加速度计读数为比力静止时含重力(0, 0, 9.8)属正常现象用加速度计直接估偏航角加速度计无法分辨水平旋转偏航角必须靠磁力计或陀螺仪积分并融合未检查协方差-1标记发布端用-1表示该项无估计接收端务必先判断再使用高频数据时间戳抖动高频 IMU 必须使用硬件时间戳避免用ros::Time::now()顶替长期对角速度积分陀螺仪零偏会导致积分漂移需与加速度计 / 磁力计融合frame_id约定通常为 IMU 本体坐标系如imu_link与 tf 树中的名称一致