IMU在ROS2中的使用

实验介绍

​ IMU(Inertial Measurement Unit，惯性测量单元)是一种用于测量和感知物体运动状态的传感器装置。它通常由多个惯性传感器组成，包括加速度计、陀螺仪和磁强计，用于测量宿主载体在空间中的加速度、角速度和磁场强度等参数。
IMU 被广泛应用于机器人领域，包括无人机、自动驾驶汽车、机器人手臂、人体运动追踪系统等。它能为机器人提供实时而精确的运动参数，使机器人能够感知自身的运动姿态，并做出相应的反应和决策。

实验过程

IMU数据获取

​ IMU数据的获取是通过订阅IMU节点发布的话题，从话题中获取IMU节点发出的消息包来实现的，如下图所示。

​ 下面将实现一个订阅者节点订阅“/imu/data”话题，从此话题中接收 sensor_msgs::lmu类型的消息包，并解析出机器人的滚转、俯仰和航向角等姿态数据。

编写 IMU 数据获取程序

​ 首先在工作空间中创建一个软件包。打开一个新的终端窗口，输入如下指令，进入工作空间。

cd ~/ros2_ws/src

​ 然后用如下指令创建一个名为“imu_pkg”的软件包

ros2 pkg create imu_pkg

​ 创建好软件包后，接下来在这个软件包中创建一个节点，具体操作步骤如下

1.编写节点代码

​ 先创建这个节点的源码文件。在VSCode中找到[imu_pkg]软件包，用鼠标石健单击它的[src]子目录，在弹出的快捷菜单中选择「新建文件]。此时会提示输人文件名，输入“imu_data.cpp”，然后按[Enter]键创建文件。

​ 下面编写这个源码文件，其内容如下。

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

std::shared_ptr<rclcpp::Node> node;

void IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
    tf2::Quaternion tf2_quaternion;
    tf2_quaternion.setX(msg->orientation.x);
    tf2_quaternion.setY(msg->orientation.y);
    tf2_quaternion.setZ(msg->orientation.z);
    tf2_quaternion.setW(msg->orientation.w);

    tf2::Matrix3x3 matrix(tf2_quaternion);

    double roll, pitch, yaw;
    matrix.getRPY(roll, pitch, yaw);
    roll = roll * 180 / M_PI;
    pitch = pitch * 180 / M_PI;
    yaw = yaw * 180 / M_PI;
    RCLCPP_INFO(node->get_logger(), "roll= %.0f pitch= %.0f yaw= %.0f", roll, pitch, yaw);
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);

    node = std::make_shared<rclcpp::Node>("imu_data_node");

    auto sub = node->create_subscription<sensor_msgs::msg::Imu>("/imu", 10, IMUCallback);

    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

​ 上述代码可以从 wpr_simulation2的例程文件中找到。若编译报错，可以与 wpr_simulation2\demo_cpp\6_imu_data.cpp 文件中的代码进行对比。

2.设置编译规则

​ 节点源码的编译规则写在imu_pkg的CMakeLists.txt文件里。在 VSCode 中打开这个文件，在这个文件里添加节点的编译规则。首先使用如下代码寻找节点代码中用到的依赖项。

find_package(rclcpp REQUIRED) 
find_package(sensor_msgs REQUIRED) 
find_package(tf2 REQUIRED)

​ 然后使用如下代码添加节点源码的编译规则

add_executable(imu_data src/imu_data.cpp)
ament_target_dependencies(imu_data "rclcpp" "sensor_msgs" "tf2")

​ 最后使用如下代码添加节点编译完成后的安装规则

install(TARGETS  imu_data
DESTINATION  lib/${PROJECT_NAME})

​ 这些内容可以从 wpr_simulation2的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\6_imu_data.txt 文件中的代码进行比对。

​ 上述规则添加完毕后，一定要保存文件，否则规则无法生效。

3.修改软件包信息

​ 在 VSCode 中打开[imu_pkg]下的[package.xml]文件，使用如下代码添加依赖项

<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>

​ 这些内容可以从 wpr_ simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_package\6_imu_data.xml 文件中的代码进行比对。

​ 文件修改后，一定要保存文件，否则新的包信息无法生效

4.编译软件包

​ 修改完上述文件后，打开终端窗口，执行如下指令，进入工作空间。

cd ~/ros2_ws

​ 然后执行如下指令，对工作空间里的所有软件包进行编译

colcon build

仿真运行 IMU 数据获取程序

​ 下面运行刚才编写的节点。在运行前，确认已经按照之前下载了wpr_simulation2 仿真项目。然后，加载当前工作空间中的环境设置，这样ros2指令才能找到刚才编译后的软件包和节点文件。保持终端的当前路径依然在工作空间目录ros2_ws中，然后执行如下指令。

source install/setup.bash

​ 然后，先启动带有机器人的仿真环境。

ros2 launch wpr_simulation2 wpb_simple.launch.py

​ 这时会启动一个仿真窗口，里面有一台仿真机器人，面对一个书柜。接下来运行刚才编写的节点imu_data。在 Terminator 终端中按组合键[Ctrl+shift+O]，将终端分为上、下两个子窗口。

​ 在新的终端窗口中执行如下指令，加载新的工作空间环境设置

source install/setup.bash

​ 然后使用如下指令运行刚才编写的节点，

ros2 run imu_pkg imu_data

​ 下面通过改变机器人的姿态来观察节点获取的数值是否会跟着变化。切换到仿真环境单击工具栏中的[旋转]按钮。

​ 然后选中仿真环境中的机器人模型，这时机器人周围会出现正交的3个颜色的圆环，分别可以改变机器人的X轴、Y轴、Z轴3个轴向上的姿态角度。

​ 将指针移动到水平旋转的圆环上，将机器人原地旋转一个角度

​ 这时切换到终端窗口，以看到，机器人的航向角 yaw的值发生了相应的变化，IMU 数据获取实验成功。

基于IMU的航向锁定实现

​ 下面将在前一个实验的基础上，实现一个航向锁定的功能。编写一个节点，先订阅“/imw/data”话题，从此话题中接收IMU 节点发来的 sensor_msgs::Imu 类型消息包，解析出机器人的姿态角度。设置一个目标航向角度，让机器人朝着这个航向运动。若运行过程中机器人的姿态被改变了，导致航向角与目标角度不一致，则根据航向角偏差值，计算出纠速度值，打包成geometry_msgs::Twist 类型的消息包，发布到“/cmd_vel”话题中。机器人底盘节点会从“/cmd_vel”话题中获取速度值并执行，以此来实现航向锁定的行为

编写航向锁定程序

​ 按照上一个实验，在工作空间中创建一个imu_pkg软件包。如果已经创建好了，可以直接使用，不用重复创建。具体操作步骤如下。

1.编写节点代码

​ 先创建这个节点的源码文件。在VSCode 中找到[imu_pkg]软件包用鼠标右键单击它的[src]子目录，在弹出的快捷菜单中选择[新建文件]。此时会提示输入文件名，输入“imu_behavior.cpp”，然后按[Enter]键创建文件。

​ 下面编写这个源码文件，其内容如下。

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

std::shared_ptr<rclcpp::Node> node;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub;

void IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
    tf2::Quaternion tf2_quaternion;
    tf2_quaternion.setX(msg->orientation.x);
    tf2_quaternion.setY(msg->orientation.y);
    tf2_quaternion.setZ(msg->orientation.z);
    tf2_quaternion.setW(msg->orientation.w);

    tf2::Matrix3x3 matrix(tf2_quaternion);

    double roll, pitch, yaw;
    matrix.getRPY(roll, pitch, yaw);
    roll = roll * 180 / M_PI;
    pitch = pitch * 180 / M_PI;
    yaw = yaw * 180 / M_PI;
    RCLCPP_INFO(node->get_logger(), "roll= %.0f pitch= %.0f yaw= %.0f", roll, pitch, yaw);

    double target_yaw = 90;

    geometry_msgs::msg::Twist vel_msg;
    double diff_angle = target_yaw - yaw;
    vel_msg.angular.z = diff_angle * 0.01;
    vel_msg.linear.x = 0.1;
vel_pub->publish(vel_msg);
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);

    node = rclcpp::Node::make_shared("imu_behavior_node");

    auto sub = node->create_subscription<sensor_msgs::msg::Imu>("imu", 10, IMUCallback);

    vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

​ 代码编写完毕后，需要保存代码。保存成功后，编辑界面文件名后面的圆点符号会变成个一叉符号。

2.设置编译规则

​ 节点源码的编译规则写在imu_pkg的CMakeLists.txt 文件里。在 VSCode 中打开这个文件，在这个文件里添加节点的编译规则。首先使用如下代码寻找节点代码中用到的依赖项。

find_package(rclcpp REQUIRED) 
find_package(sensor_msgs REQUIRED) 
find_package(tf2 REQUIRED)
find_package(geometry_msgs REQUIRED)

​ 然后使用如下代码添加节点源码的编译规则

add_executable(imu_behavior src/imu_behavior.cpp)
ament_target_dependencies(imu_behavior "rclcpp" "sensor_msgs" "tf2" "geometry_msgs")

​ 最后使用如下代码在安装规则里添加新的节点

install(TARGETS  imu_data  imu_behavior
DESTINATION  lib/${PROJECT_NAME})

​ 这些内容可以从 wpr- simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\6_imu_behavior. txt 文件中的代码进行比对。

​ 上述规则添加完毕后，一定要保存文件，否则规则无法生效。

3.修改软件包信息

​ 在VSCode 中打开[imu_pkg]下的[package.xml]文件，使用如下代码添加依赖项信息

<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>geometry_msgs</depend>

​ 这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_package\6_imu_behavior.xml 文件中的代码进行比对。
如果之前已经添加过了，可以不必重复添加。文件修改后，一定要保存文件，否则新的包信息无法生效。

4.编译软件包

​ 修改完上述文件后，打开终端窗口。执行如下指令，进入工作空间

cd ~/ros2_ws

​ 然后执行如下指令，对工作空间中的所有软件包进行编译，

colcon build

仿真运行航向锁定程序

​ 面运行刚才编写的节点。在运行前，确认已经按照之前下载了wpr_simulation2 仿真项目。然后，加载当前工作空间中的环境设置，这样ros2指令才能找到刚才编译后的软件包和节点文件。保持终端的当前路径依然在工作空间目录ros2_ws中，然后执行如下指令。

source install/setup.bash

​ 然后，先启动带有机器人的仿真环境。执行如下指令。

ros2 launch wpr_simulation2 wpb_simple.launch.py

​ 这时会启动一个仿真窗口，里面有一台仿真机器人，面对一个书柜

​ 接下来运行刚才编写的节点。在Terminator 终端中按组合键[Ctrl+Shift+O]，将终端分为上、下两个子窗口。

​ 在新的终端窗口中加载新的工作空间环境设置

source install/setup.bash

​ 然后使用如下指令运行刚才编写的节点

ros2 run imu_pkg imu_behavior

​ 节点运行起来之后，切换到刚才的仿真窗口。如下图所示，可以看到机器人一边旋转，一边向着左侧 90°的方向移动。当转到面向左侧方向时，机器人不再转动，变成直行。

​ 这时尝试改变机器人的航向，单击仿真窗口工具栏中的[旋转]按钮。然后选中仿真环境中的机器人模型，这时机器人周围会出现正交
的3个颜色的圆环，分别可以改变机器人的X轴、Y轴、Z轴3个轴向上的姿态角度

​ 将指针移动到水平旋转的圆环上，将机器人旋转一个角度

​ 机器人姿态发生改变后，它会立刻调整自己的朝向，再次转向90°航向的方向。这相当于机器人已经锁定 90°方向为自己的目标航向，航向锁定功能成功实现。

实验总结

​ 在本次实验中，我们成功实现了IMU（惯性测量单元）在ROS2中的应用，具体包括IMU数据的获取和基于IMU的航向锁定功能。

​ 我们创建了一个名为imu_data_node的ROS2节点，该节点订阅了/imu/data话题，接收sensor_msgs::msg::Imu类型的消息包。通过解析IMU数据，提取了机器人的滚转角（roll）、俯仰角（pitch）和航向角（yaw），并将这些角度转换为度数后打印出来。实验验证了IMU数据的正确性和实时性，确保了后续控制逻辑的基础数据准确性。

​ 在原有的imu_pkg软件包基础上，我们新增了一个节点imu_behavior_node。该节点不仅订阅了/imu话题获取IMU数据，还发布了geometry_msgs::msg::Twist类型的消息到/cmd_vel话题，用于控制机器人的线速度和角速度。我们设置了目标航向角为90度，并根据当前航向角与目标航向角之间的差异计算出需要调整的角度，进而控制机器人的转向。实验结果显示，机器人能够在遇到外部干扰导致航向偏移时，自动调整方向回到设定的目标航向上，实现了航向锁定功能。

​ 通过运行仿真环境wpr_simulation2，我们在仿真环境中观察到机器人能够实时显示其姿态数据，并且在手动改变机器人的姿态后，节点能够准确地检测到姿态变化并进行相应的调整。航向锁定功能的测试表明，机器人能够稳定地维持在目标航向90度，即使受到外部干扰也能迅速恢复到目标航向上，证明了算法的有效性和鲁棒性。

不仅订阅了/imu话题获取IMU数据，还发布了geometry_msgs::msg::Twist类型的消息到/cmd_vel话题，用于控制机器人的线速度和角速度。我们设置了目标航向角为90度，并根据当前航向角与目标航向角之间的差异计算出需要调整的角度，进而控制机器人的转向。实验结果显示，机器人能够在遇到外部干扰导致航向偏移时，自动调整方向回到设定的目标航向上，实现了航向锁定功能。

​ 通过运行仿真环境wpr_simulation2，我们在仿真环境中观察到机器人能够实时显示其姿态数据，并且在手动改变机器人的姿态后，节点能够准确地检测到姿态变化并进行相应的调整。航向锁定功能的测试表明，机器人能够稳定地维持在目标航向90度，即使受到外部干扰也能迅速恢复到目标航向上，证明了算法的有效性和鲁棒性。

​ 我们使用了tf2库进行四元数到欧拉角的转换，确保了姿态数据的准确解析。通过geometry_msgs::msg::Twist消息类型控制机器人的运动，实现了平滑的转向和直线运动。代码结构清晰，模块化设计使得各个部分功能独立，便于维护和扩展。