ROS学习笔记（八）

咋学都不累zgc

已于 2023-11-24 22:56:53 修改

阅读量199

点赞数 1

文章标签：学习笔记

于 2023-11-24 22:53:38 首次发布

本文链接：https://blog.csdn.net/m0_46392159/article/details/134608431

版权

ROS中的IMU惯性测量单元消息包

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

IMU消息包格式：

# 这是一条信息，用于保存来自 IMU（惯性测量单元）的数据
#
# 加速度单位应为 m/s^2（而不是 g），旋转速度单位应为 rad/sec。
#
# 如果协方差数值已知，就将其填充到协方差矩阵中。
# 若协方差数值未知，则将协方差矩阵全部置为零。
#
# 若协方差矩阵对应的数值不存在（比如IMU没有输出orientation姿态数据），那么该协方差矩阵的第一个数值置为-1。
# 如果要使用的这个消息包里的某个数据，需要先对其协方差矩阵的第一个数值进行判断：
# 如果数值为-1，表明要使用的数据是不存在的，不要再去读取它。
#

Header header

geometry_msgs/Quaternion orientation # 空间位姿
float64[9] orientation_covariance # Row major about x, y, z axes 协方差矩阵

geometry_msgs/Vector3 angular_velocity # 描述机器人在XYZ三个坐标轴上的旋转速度，由IMU测量到的输入值。
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 z 协方差矩阵，用于后期的优化和滤波。

包含angular_velocity旋转加速度和linear_acceleration矢量加速度一共六个轴的数值分量的IMU模块，就是常说的六轴IMU。

另外有些IMU模块额外提供了XYZ三个轴向的磁强计输出，就变成了九轴IMU。

磁强计数据在ROS中有专门的消息格式，没有包含在这个IMU消息包中。

机器人在XYZ三个坐标轴的旋转速度和矢量加速度都是直接测量到的数据，也叫裸数据。

而根据这些数值融合得到的空间姿态描述，就是消息包格式中的第一个Quaternion orientation。

他描述的是机器人的朝向相对于空间中XYZ三个坐标轴的偏移量。

orientation是一个Quaternion（四元数）类型的数值。四元数是为了解决欧垃角中存在的万向锁的问题。

使用x,y,z,w四个值来描述机器人朝向。

欧拉角和万向锁

欧拉角是由三个角度表示，表示物体从原始姿态到目标姿态的一个变换。无论（α，β，γ）三个角度值是多少，都是得从物体的原始姿态开始进行绕轴进行旋转。所以物体的姿态都是相对于其最原始姿态的，上述图例中说到的，先绕X轴旋转30度，再绕Y轴旋转90度，再绕Z轴旋转10度得到的最终姿态和先绕X轴旋转20度，再绕Y轴旋转90度的结果一样。因此相对于最初姿态而言，当一个欧拉角包含绕Y轴旋转90度时，绕X轴和绕Z轴旋转已经是在绕同一个轴在进行旋转，这个时候只有两个轴在起作用。这个时候就是万向锁的状态。

自己拿一个手机来做一下试验，有助于理解万向锁：

可以拿出手机放在桌面上，屏幕朝上，手机的最长边垂直与桌子的边缘设置为X轴，这个时候屏幕的短边平行于桌子的边缘设置为Y轴，因此垂直与屏幕的向量为Z轴。我们先绕手机的最长边X轴顺时针旋转30度，这个时候手机离开桌面，留下一个长边与桌子接触；然后再绕Y轴，也就是手机的短边旋转90度，让屏幕面与桌子的边缘平行；再绕Z轴旋转10度，也就是绕垂直于屏幕的轴旋转10度，这个时候你会发现，绕Z轴旋转时，屏幕面还是平行桌子的边缘，而此时绕Z轴旋转的角度给手机姿态带来的影响和最开始旋转X轴给手机姿态带来的影响是一样的——都是使手机最终的姿态（已经绕Y轴旋转了90度使得手机屏幕与桌子边缘平行）为绕着垂直于屏幕的轴旋转一定的角度。你完全可以不用绕Z轴旋转，通过调节绕X轴旋转的角度数，使得最终手机的姿态和上述旋转过程达到的姿态一样。此时就是造成了万向锁。

使用C++实现IMU数据获取

首先需要直到IMU的话题：

imu/data_raw (sensor_msgs/Imu)

加速度计输出的矢量加速度和陀螺仪输出的旋转角速度

imu/data (sensor_msgs/Imu)

imu/data_rawd的数据再加上融合后的四元数姿态描述

imu/mag (sensor_msgs/MagneticField)

磁强计输出磁强数据

接下来构建一个数据获取节点：

实现步骤：

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

代码如下：

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"//

void IMUCallback(sensor_msgs::Imu msg)
{
    if(msg.orientation_covariance[0] < 0) // 协方差矩阵的第一个元素小于0，说明IMU没有准备好
    {
        ROS_INFO("IMU is not ready!");
        return;
    }
    else
    {
        tf::Quaternion quaternion(
            msg.orientation.x,
            msg.orientation.y,
            msg.orientation.z,
            msg.orientation.w
        ); // 将消息包里的四元数转换成tf里的四元数，以便使用TF工具将四元数转换成欧拉角。

        double roll, pitch, yaw;// roll - 滚转角 pitch - 俯仰角 yaw - 偏航角 定义欧拉角变量
        tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);//使用TF工具先将四元数转换成一个3x3的矩阵，再将矩阵转换成欧拉角。
        roll = roll * 180.0 / M_PI;//将弧度转换成角度
        pitch = pitch * 180.0 / M_PI;
        yaw = yaw * 180.0 / M_PI;
        ROS_INFO("滚转角 = %.0f, 俯仰角 = %.0f, 偏航角 = %.0f", roll, pitch, yaw);//打印欧拉角
    }
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "zh_CN.UTF-8");

    ros::init(argc, argv, "imu_node");

    ros::NodeHandle n;
    ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, IMUCallback); // 订阅IMU数据
    
    ros::spin(); //让节点持续运行


    return 0;
}

修改编译规则：

add_executable(imu_node src/imu_node.cpp)
add_dependencies(imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(imu_node
  ${catkin_LIBRARIES}
)

编译cpp文件。

接下来终端输入

roslaunch wpr_simlation wpb_simple.launch
rosrun imu_pkg imu_node

得到结果如下：

获取IMU数据的Python节点

实现思路和C++大致相同

不同的是：首先需要shebang。

#!usr/bin/env python3 是一个称为 shebang 的特殊行，它在 Unix 和 Unix-like 系统（如 Linux）中用于指示脚本的解释器。

这行代码的意义是：当你尝试运行这个脚本时，系统会使用 /usr/bin/env 命令来找到 python3 解释器，并使用它来执行脚本。

详细解释如下：

#!：这是 shebang 的开始，它告诉系统这个文件是一个脚本，而不是一个二进制文件。
/usr/bin/env：这是一个 Unix 命令，它可以在系统的 PATH 环境变量中查找指定的程序。使用 env 而不是直接指定解释器的路径（例如 /usr/bin/python3）的好处是，它允许脚本在不同的系统上运行，即使 python3 的路径在不同的系统上可能不同。
python3：这是你想要用来执行脚本的解释器。在这个例子中，它是 Python 3 解释器。

注意，为了让这个 shebang 行生效，你需要给脚本添加可执行权限。你可以使用 chmod 命令来做这件事，例如 chmod +x imu_node.py。然后，你就可以直接运行这个脚本，例如 ./imu_node.py，而不需要先调用 Python 解释器，例如 python3 imu_node.py。

代码如下：

#!/usr/bin/env python3
#coding = utf-8

import rospy
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import math

def imu_callback(msg):
    if msg.orientation_covariance[0] < 0:
        return
    else:
        quaternion = [
            msg.orientation.x,
            msg.orientation.y,
            msg.orientation.z,
            msg.orientation.w
        ]
        (roll, pitch, yaw) = euler_from_quaternion(quaternion)# 从四元数转换为欧拉角
        roll = roll*180/math.pi
        pitch = pitch*180/math.pi
        yaw = yaw*180/math.pi
        rospy.loginfo("滚转角 =  %.0f, 俯仰角 = %.0f, 朝向角 = %.0f", roll, pitch, yaw)

if __name__ == "__main__":
    rospy.init_node("imu_node")
    imu_sub = rospy.Subscriber("/imu/data", Imu, imu_callback, queue_size=10)
    rospy.spin()

IMU航向锁定C++

在上一节建立的数据获取节点imu_node中添加发布运动控制指令的功能，使机器人能对姿态的变化做出反应，实现航向锁定的效果。

实现步骤：

让大管家NodeHandle发布速度控制话题/cmd_vel。
设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不一致时，控制机器人转向目标朝向角。

代码如下：

首先在函数外定一个全局变量 ros::Publisher vel_pub,

然后在main函数中添加代码让rospy大管家发布节点速度控制话题/cmd_vel。

vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10)

最后在回调函数中添加如下代码：

        double target_yaw = 90;// 目标角度为90度
        double diff_angle = target_yaw - yaw; // 计算目标角度与当前角度的差值
        geometry_msgs::Twist vel_cmd;// 定义速度控制指令变量
        vel_cmd.angular.z = diff_angle * 0.01;// 速度控制，比例系数为0.01
        vel_cmd.linear.x = 0.1;// 机器人直线前进速度为0.1m/s
        vel_pub.publish(vel_cmd);// 发布速度控制指令，控制机器人转向接近目标角度

IMU航向锁定Python

实现步骤和C++类似

代码如下：

global vel_pub # 定义速度发布者为全局变量
vel_pub= rospy.Publisher("/cmd_vel", Twist, queue_size=10) # 定义速度发布者，发布话题为/cmd_vel
## 回调函数中加入以下代码：##
        target_yaw = 90
        diff_angle = target_yaw -yaw # 计算目标角度与当前角度的差值
        vel_cmd = Twist() # 定义速度消息
        vel_cmd.angular.z = diff_angle*0.01 # 转向速度控制，比例系数为0.01
        vel_cmd.linear.x = 0.1 # 0.1m/s
        vel_pub.publish(vel_cmd)

咋学都不累zgc

关注

1
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
ROS学习笔记（八）

IMU (Inertial Measurement Unit) 惯性测量单元 - 一种安装在机器人内部的传感器模块，用于测量机器人的空间姿态。包含旋转加速度和矢量加速度一共六个轴的数值分量的IMU模块，就是常说的六轴IMU。另外有些IMU模块额外提供了XYZ三个轴向的磁强计输出，就变成了九轴IMU。磁强计数据在ROS中有专门的消息格式，没有包含在这个IMU消息包中。机器人在XYZ三个坐标轴的旋转速度和矢量加速度都是直接测量到的数据，也叫裸数据。
复制链接

扫一扫