【ROS中的IMU惯性测量单元】

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

前言
一、IMU的部分关键数据解析
二、常规IMU模块与九轴
三、使用C++实现获取IMU数据的节点
- 1.IMU中的话题
- 2.获取IMU数据程序设计思路
- 3.获取IMU数据程操作步骤与实验现象
- - （1）偏航角Yaw（沿着Z轴的旋转方向）的观测
  - （2）横滚角ROLL（沿着X轴的旋转方向）的观测
  - （3）俯仰角Pitch（沿着X轴的旋转方向）的观测
总结

前言

IMU（Inertial Measurement Unit）

IMU用于测量机器人的空间姿态；

一、IMU的部分关键数据解析

index.ros.org搜索sensor_msgs
找到Neotic版本，进入website，找到sensor_msgs/Imu Message,这里可以查看ROS对IMU消息格式的定义；

消息头部是个header,记录了消息发送的时间戳和坐标系ID；
变量orientation，是IMU根据消息包中的原始矢量加速度和角速度解算出来的数据，描述了机器人的朝向相对于空间中XYZ三个坐标轴的偏移量；orientation是由x,y,z,w四个64位的浮点数组成，其中w是基于三个坐标轴旋转偏移量的描述，即欧拉角的描述；在某些姿态下会存在一种叫万向锁的问题，使用Quaternion数学方法，即使用xyzw四个值来描述机器人朝向的方法，也称四元数描述法，可以有效避免万向锁问题。在实际的应用中，四元数描述法通常只在进行旋转变换的过程中使用，旋转后得到结果还是会转换成欧拉角进行处理

geometry_msgs/Vector3 angular_velocity IMU测得的角速度，作为传感器数据输入量，对比geometry_msgs/Vector3 angular是机器人的角速度输出量；

geometry_msgs/Vector3 linear_acceleration 描述的是三个轴上的加速度；

geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Quaternion angular_velocity
float64[9] angular_velocity_covariance #角速度
geometry_msgs/Quaternion linear_acceleration
float64[9] linear_acceleration_covariance #矢量加速度
上述三个数据成员都各自带了一个协方差矩阵，主要用于后期的优化和滤波；

如果协方差数值已知就将其填充到协方差矩阵中；
如果协方差数值未知，则将协方差矩阵全部置为零；

如果协方差矩阵对应的数值不存在（比如IMU没有输出orientation姿态数据），那么协方差矩阵的第一个数值为-1；

如果要使用这个消息包里的某个数据，需要先对协方差矩阵的第一个数值进行判断；
如果数值为-1，表明要使用的数据是不存在的，不要去读取它；

二、常规IMU模块与九轴

部分IMU模块额外提供了XYZ三个轴向的磁强计输出，也就是九轴模块；
磁强计数据在ROS中有专门的消息格式（不再IMU的消息包中）；

为了方便使用，IMU会根据消息包中的原始数值进行融合得到空间姿态描述，即orientation中的数据；

三、使用C++实现获取IMU数据的节点

1.IMU中的话题

imu/data_raw(sensor_msgs/Imu)
加速度计输出的矢量加速度和陀螺仪输出的旋转角速度；

imu/data(sensor_msgs/Imu)
即imu/data_raw（原始的数据）加上融合后的四元数姿态描述；

imu/mag(sensor_msgs/MagneticField)
磁强计输出磁强数据，只有九轴才会发布这个话题；

2.获取IMU数据程序设计思路

1.创建一个新的软件包，包名为imu_pkg;
2.在软件包中新建一个节点，节点名称为imu_node;
3.在节点中，向ROS中的NodeHandle申请订阅话题/imu/data并设置回调函数为IMUCallback();
4.构建回调函数IMUCallback(),用来接收和处理IMU数据；
5.使用TF工具将四元数转化为欧拉角；
6.调用ROS_INFO()显示转换后的欧拉角数值；

3.获取IMU数据程操作步骤与实验现象

打开终端
CTRL+ALT+T

打开catkin_ws/src/工作目录
cd catkin_ws/src/

创建一个新的软件包，包名为imu_pkg
catkin_create_pkg imu_pkg roscpp rospy sensor_msgs

在vscode中的imu_pkg的src文件中创建imu_node.cpp文件

在imu_node.cpp编写程序代码

#include <ros/ros.h>//引入ROS头文件
#include <sensor_msgs/Imu.h>//引入IMU消息的头文件
#include <tf/tf.h>//引入tf转换库
void IMUCallback(const sensor_msgs::Imu imu_msg)
{
    if(imu_msg.orientation_covariance[0] < 0)
    {
        return;
    }
    tf::Quaternion q(imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w);
    double roll, pitch, yaw;
    tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
    roll = roll * 180.0 / M_PI;
    pitch = pitch * 180.0 / M_PI;
    yaw = yaw * 180.0 / M_PI;
    ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f", roll, pitch, yaw);
}

int main(int argc,char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc,argv,"imu_node");
    printf("This is imu_node\n");

    ros::NodeHandle nh;
    ros::Subscriber imu_sub = nh.subscribe("/imu/data",10,IMUCallback);
    ros::spin();

    return 0;
}

运行ros
roscore

运行仿真环境
roslaunch wpr_simulation wpb_simple.launch

运行imu_node节点
rosrun imu_pkg imu_node

查看节点间的通信方式
rqt_graph
在这里插入图片描述

（1）偏航角Yaw（沿着Z轴的旋转方向）的观测

调整仿真机器人的位置和角度，
传感器读取的角度值为：Roll: -0.000097, Pitch: 0.000098, Yaw: 0.000002，单位是度
在这里插入图片描述当机器人向右旋转一定的角度的时候：
读取到IMU的欧拉角数值为：Roll: 0.000022, Pitch: 0.000144, Yaw: 53.769232
（旋转正方向由右手定则确定）
此时偏航角Yaw是53.8度（保留一位小数）