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

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

---

前言

---

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度（保留一位小数）

（2）横滚角ROLL（沿着X轴的旋转方向）的观测

将机器人向左侧倾斜：
读取到IMU的欧拉角数值为：Roll: -150.856759, Pitch: -7.867335, Yaw: 7.86
此时横滚角度为-150.9度（保留一位小数）

（3）俯仰角Pitch（沿着X轴的旋转方向）的观测

将机器人向正后方倾倒：
读取到IMU的欧拉角数值为：Roll: -89.735427, Pitch: 89.999940, Yaw: -84.293126
此时横滚角度为 -84.3度（保留一位小数）

四、IMU航向锁定实例

imu_node节点订阅IMU数据话题（/imu/data）的同时，发布速度控制话题（/cmd_vel）使机器人能对姿态的变化作反应。

设计思路：
1.让NodeHandle发布速度控制话题/cmd_vel
2.设定一个目标朝向角，当姿态信息和目标朝向不一致时，控制机器人转向目标角度；

代码部分

#include <ros/ros.h>//引入ROS头文件
#include <sensor_msgs/Imu.h>//引入IMU消息的头文件
#include <tf/tf.h>//引入tf转换库
#include <geometry_msgs/Twist.h>//引入速度消息的头文件

ros::Publisher vel_pub;//发布者对象

/*imu_node节点订阅/imu/data话题的同时会发布/cmd_vel话题*/
void IMUCallback(const sensor_msgs::Imu imu_msg)
{
    
    //如果协方差矩阵对应的数值不存在（比如IMU没有输出orientation姿态数据），那么协方差矩阵的第一个数值为-1；
    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);//将四元数转换成tf::Quaternion对象
    double roll, pitch, yaw;//用于装载转换后的旋转角度（欧拉角）
    tf::Matrix3x3(q).getRPY(roll, pitch, yaw);//将tf::Quaternion转换成3x3的矩阵对象
    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);

    geometry_msgs::Twist cmd_vel_msgs;//创建一个速度控制消息包cmd_vel_msgs
    // double target_speed = 0.1;//机器人希望的速度
    double target_angular = 90;//机器人希望的转向角度
    double diff_angular = target_angular-yaw;//计算yaw角度的误差
    cmd_vel_msgs.angular.z = diff_angular * 0.01;//根据yaw角度的误差，计算出需要的转向角度

    vel_pub.publish(cmd_vel_msgs);//发布速度话题
}

int main(int argc,char *argv[])
{
    
    
    setlocale(LC_ALL, "");//设置C++ locale，避免中文乱码
    ros::init(argc,argv,"imu_node");//初始化节点
    printf("This is imu_node\n");

    ros::NodeHandle nh;//在节点初始化完成后定义一个NodeHandle对象
    ros::Subscriber imu_sub = nh.subscribe("/imu/data",10,IMUCallback);//设置订阅IMU的话题/imu/data、消息缓存长度、回调函数名
    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);//发布速度话题/cmd_vel，控制电机的运转

    ros::spin();//让节点一直运行，直到被关闭

    return 0;
}

启动ros
roscore

启动wpr_simulation
rosluanch wpr_simulation wpb_simple.launch

查看节点间的通信方式
rqt_graph

拖动机器人，给定机器人一个偏航角；
现象：机器人回到设定的目标角度；
通信节点间的通信连接如下：

---

总结

简单分析了IMU部分关键数据的组成与使用方法，通过仿真观测了机器人的欧拉角，实现了IMU锁定航向的实例；