当前位置：首页 > web >正文

ROS学习——IMU惯性测量单元节点的原理与编写(含C++和Python代码)

web 2025/5/8 21:22:15

ROS 学习

IMU（惯性测量单元）

IMU简介

IMU(Inertial Measurement Unit)即惯性测量单元,是一种安装在机器人内部的一种传感器模块，用于测量机器人的空间姿态

ROS Index中的介绍

在ROS Index中搜索sensor_msgs
找到对应自己ROS版本的软件包
进入Website详情页面
进入右侧Msg/Srv API消息包详情页
进入IMU详情介绍页面，观察官网对IMU的介绍

分析IMU消息类型的格式

在这里插入图片描述

头部Header: 用于记录消息发送的时间戳和消息ID
orientation: 融合消息包其余数据得到的对机器人的空间姿态描述，用于描述机器人相对于空间中XYZ三个轴的偏移量
angular_velocity: 是描述机器人在XYZ三个坐标轴上的旋转速度，即角速度
linear_acceleration: 是描述机器人在在XYZ三个坐标轴上的直行速度的变化速度，即线速度加速度
带有covariance的数据: 协方差矩阵，主要用于后期优化和滤波
学习注释，当要使用消息包中的某个数据时，需要先对协方差矩阵第一个数据的数值进行判断，当数值为-1时，表示数据不存在；当协方差矩阵中所有数据的数值均为0时，表示协方差内容未知

接收IMU消息的话题订阅者节点的编写

IMU消息发布的话题

imu/data_raw(sensor_msgs/Imu): 用于输出矢量加速度和陀螺仪输出的旋转角速度
imu/data(sensor_msgs/Imu): data_raw话题中的数据再加上融合后的四元数姿态描述即orientation中的数据
imu/mag(sensor_msgs/MagneticField): 磁强计输出的磁强数据

代码实现思路

构建一个新的软件包，包名叫做imu_pkg
在软件包中新建一个节点，节点名叫做imu_node。
在节点中，向ROS大管家NodeHandle/rospy申请订阅话题/imu/data,并设置回调函数为IMUCallback()
构建回调函数IMUCallback(), 用来接收和处理IMU数据
使用TF工具将四元数转换成欧拉角
调用ROS_INFO()显示转换后的欧拉角数值

C ++ 节点的实现

imu_node.cpp

#include "ros/ros.h"
#include "tf/tf.h"
#include "sensor_msgs/Imu.h"inline double toDegrees(double radian) {return radian * 180.0 / M_PI; // 弧度转化角度
}void IMUCallback(const sensor_msgs::Imu& msg)
{if (msg.orientation_covariance[0] < 0) {ROS_INFO("数据无效");return;}// Imu原始四元数数据 转换为 tf 中的四元数对象tf::Quaternion quaternion(msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w);double roll, pitch, yaw;//通过3x3矩阵的getRPY方法将四元数对象转化成欧拉角，即（滚转角，俯仰角，朝向角）tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);ROS_INFO("\n滚转(Roll): %.2f°\n俯仰(Pitch): %.2f°\n朝向(Yaw): %.2f°",toDegrees(roll), toDegrees(pitch), toDegrees(yaw));
}int main(int argc, char *argv[])
{setlocale(LC_ALL, ""); // 避免中文输出乱码ros::init(argc, argv, "imu_node"); // 初始化节点ros::NodeHandle nh;ros::Subscriber imu_sub = nh.subscribe("/imu/data", 10, IMUCallback);// 订阅消息， 话题， 队伍长度， 回调函数ros::spin(); // 定时执行回调函数return 0;
}

在CMakeLists.txt文件末尾添加节点声明

add_executable(imu_node src/imu_node.cpp)
target_link_libraries(imu_node${catkin_LIBRARIES}
)

Python 节点的实现

imu_node.py

#!/usr/bin/env python3
#coding = utf-8
import math
import rospy
from tf.transformations import euler_from_quaternion
from sensor_msgs.msg import Imu# 弧度转化成角度
def toDegree(x):return x * 180.0 / math.pi# 回调函数
def IMUCallback(msg):if msg.orientation_covariance[0] < 0:rospy.loginfo("数据无效")returnquaternion = [msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w]# 将imu四元数对象转化成欧拉角，即（滚转角，俯仰角，朝向角）roll, pitch, yaw = euler_from_quaternion(quaternion)rospy.loginfo(f"\n滚转(Roll): {roll:.2f}°\n俯仰(Pitch): {pitch:.2f}°\n朝向(Yaw): {yaw:.2f}°\n")def main():rospy.init_node("imu_node")  # 初始化 ROS 节点# 话题名称 消息类型 回调函数名 队伍长度lidar_sub = rospy.Subscriber("/imu/data", Imu, IMUCallback, queue_size=10)rospy.spin()if __name__ == "__main__":main()