ROS开发专栏---基于 IMU 实现机器人航向锁定实验---适配Ubuntu 22.04

🎬 渡水无言：个人主页渡水无言

❄专栏传送门： 《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门： 《freertos专栏》 《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门：《产品测评专栏》    《Ai智能体专栏）      《ROS开发专栏》 ⭐️流水不争先，争的是滔滔不绝

 📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP18 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

​

目录

前言

一、航向锁定原理

二、编写航向锁定节点代码

三、配置 CMakeLists.txt

四、配置 package.xml

五、编译与运行

总结

前言

上一期博客我们已经实现了 IMU 数据订阅、四元数转欧拉角，能够读取机器人 roll/pitch/yaw 姿态角。本节在此基础上，利用 IMU 的 yaw 航向角，实现机器人航向锁定功能：设定一个目标航向角度，机器人一边向前直行，一边实时对比当前航向与目标航向的偏差，自动发布纠偏角速度，就算人为掰动机器人改变朝向，也能自动转回设定方向，保持固定航向直行。

一、航向锁定原理

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

二、编写航向锁定节点代码

本次实验在上一期博客的imu_pkg软件包的基础上进行，我们在imu_pkg软件包的src目录下新建imu_behavior.cpp节点文件，在上一期博客的节点代码里增加一个航向锁定的操作

#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;
// 速度发布器：发布/cmd_vel控制机器人运动
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub;

// IMU回调函数：收到IMU数据自动执行
void IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
// 1. 提取IMU四元数
tf2::Quaternion tf_quat;
tf_quat.setX(msg->orientation.x);
tf_quat.setY(msg->orientation.y);
tf_quat.setZ(msg->orientation.z);
tf_quat.setW(msg->orientation.w);

// 2. 四元数转旋转矩阵
tf2::Matrix3x3 mat(tf_quat);

// 3. 转欧拉角 弧度制
double roll, pitch, yaw;
mat.getRPY(roll, pitch, yaw);

// 4. 弧度转角度
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);

// ========== 航向锁定核心逻辑 ==========
// 设定目标航向 90度
double target_yaw = 90.0;
// 计算角度偏差
double diff_angle = target_yaw – yaw;

// 定义速度消息
geometry_msgs::msg::Twist vel_msg;
// 固定前进速度
vel_msg.linear.x = 0.1;
// 根据偏差自动纠偏
vel_msg.angular.z = diff_angle * 0.01;

// 发布速度指令
vel_pub->publish(vel_msg);
}

int main(int argc, char** argv)
{
// 初始化ROS2
rclcpp::init(argc, argv);
// 创建节点
node = std::make_shared<rclcpp::Node>("imu_behavior_node");

// 创建速度发布者 话题/cmd_vel
vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

// 订阅IMU数据话题
auto sub = node->create_subscription<sensor_msgs::msg::Imu>(
"/imu/data", 10, IMUCallback);

// 循环阻塞 监听话题、触发回调
rclcpp::spin(node);

rclcpp::shutdown();
return 0;
}

三、配置 CMakeLists.txt

打开 imu_pkg/CMakeLists.txt，追加配置，完整内容如下：

cmake_minimum_required(VERSION 3.8)
project(imu_pkg)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

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

# 编译IMU数据读取节点
add_executable(imu_data src/imu_data.cpp)
ament_target_dependencies(imu_data
rclcpp
sensor_msgs
tf2
)

# 编译IMU航向锁定节点
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}
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()

四、配置 package.xml

在原有依赖基础上，补充 geometry_msgs，提供 Twist 速度消息类型，用来控制机器人运动。

完整依赖：

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>imu_pkg</name>
<version>0.0.0</version>
<description>IMU data read</description>
<maintainer email="todo@todo.todo">todo</maintainer>
<license>Apache-2.0</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<!– 依赖就加在这里！↓↓↓ –>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>geometry_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>

五、编译与运行

编译工作空间

cd ~/ros2_zice
colcon build
source install/setup.bash

启动仿真环境

ros2 launch wpr_simulation2 wpb_simple.launch.py

运行航向锁定节点

ros2 run imu_pkg imu_behavior

可以看到机器人一直往前走。

终端持续打印 roll、pitch、yaw 实时姿态角； 机器人以 0.1m/s 速度向前直行； 人为拖动、改变机器人朝向； 机器人检测到 yaw 偏离 90°，自动产生纠偏角速度，慢慢转回目标航向； 最终保持固定方向直行，航向锁定功能生效。

角度一直在修正，如下图所示：

总结

本期博客基于 IMU 姿态角，完成了机器人航向锁定功能开发。