【ROS2 速成 - Day18】机器人底盘运动控制基础（速度指令开发）

本文为【ROS2 嵌入式实战速成系列】第 18 篇，专注于从 0 到 1 实现 ROS2 对机器人底盘的运动控制。我们将从标准速度消息格式讲起，手把手编写 C++ 控制节点，深入对接嵌入式端电机 PID 控制逻辑，完成软硬联动的完整闭环。所有代码均可直接复制运行，配套工程模板已开源。

一、为什么底盘运动控制是机器人的 "心脏"

机器人的所有高级功能（导航、避障、SLAM）最终都要落地到底盘的运动执行上。如果说 SLAM 是机器人的 "眼睛"，规划算法是 "大脑"，那么底盘运动控制就是 "四肢"—— 没有可靠的运动执行，再优秀的算法也只是空中楼阁。

在 ROS2 生态中，所有移动机器人的速度控制都遵循一个统一的标准协议：geometry_msgs/Twist消息。这意味着无论你使用的是两轮差速、四轮麦克纳姆轮还是阿克曼转向底盘，上层算法只需要发布标准的 Twist 消息，底层驱动负责将其转换为具体的电机转速，实现了软硬件解耦。

本文我们将实现：

• ✅ 彻底理解geometry_msgs/Twist消息的每一个字段
• ✅ 编写工业级 C++ 速度指令发布节点
• ✅ 设计通用的 ROS2 与嵌入式通信协议
• ✅ 实现嵌入式端电机 PID 闭环控制
• ✅ 完成软硬联动调试，让机器人动起来

二、核心概念：geometry_msgs/Twist 消息详解

2.1 消息格式与字段含义

geometry_msgs/Twist是 ROS2 定义的标准速度消息，包含线速度和角速度两个部分，每个部分都有 x、y、z 三个方向的分量：

# 标准Twist消息定义 Vector3 linear # 线速度，单位：m/s Vector3 angular # 角速度，单位：rad/s

其中Vector3的定义为：

float64 x float64 y float64 z

2.2 不同底盘类型的分量映射

注意：不是所有分量对所有底盘都有效！不同运动学模型的底盘，可用的速度分量不同：

最常用的两轮差速底盘只需要控制linear.x（前后速度）和angular.z（左右转向速度）两个参数，这也是我们本文的重点。

2.3 坐标系约定

• 线速度 linear.x：正值表示机器人向前运动，负值表示向后
• 角速度 angular.z：正值表示机器人向左旋转（逆时针），负值表示向右旋转（顺时针）
• 所有速度都是相对于机器人本体坐标系 (base_link)的，而不是世界坐标系

三、实战：编写 C++ 速度指令发布节点

我们将编写一个通用的底盘控制节点，支持通过参数动态调整速度，同时提供键盘控制接口。

3.1 工程结构与依赖配置

首先在你的 ROS2 工作空间中创建功能包：

cd ~/ros2_ws/src ros2 pkg create --build-type ament_cmake chassis_controller --dependencies rclcpp geometry_msgs std_srvs

修改package.xml，添加必要的依赖和描述：

<name>chassis_controller</name> <version>0.0.0</version> <description>ROS2底盘运动控制节点，支持速度指令发布与嵌入式对接</description> <maintainer email="your_email@example.com">Your Name</maintainer> <license>MIT</license> <buildtool_depend>ament_cmake</buildtool_depend> <depend>rclcpp</depend> <depend>geometry_msgs</depend> <depend>std_srvs</depend> <build_depend>ament_cmake_auto</build_depend> <exec_depend>rclcpp</exec_depend> <exec_depend>geometry_msgs</exec_depend> <exec_depend>std_srvs</exec_depend> <export> <build_type>ament_cmake</build_type> </export>

修改CMakeLists.txt：

cmake

cmake_minimum_required(VERSION 3.8) project(chassis_controller) 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(geometry_msgs REQUIRED) find_package(std_srvs REQUIRED) # 编译可执行文件 add_executable(twist_publisher src/twist_publisher.cpp) ament_target_dependencies(twist_publisher rclcpp geometry_msgs std_srvs ) # 安装可执行文件 install(TARGETS twist_publisher DESTINATION lib/${PROJECT_NAME} ) # 安装launch文件 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ ) ament_package()

3.2 完整 C++ 控制节点代码

创建src/twist_publisher.cpp文件：

#include <rclcpp/rclcpp.hpp> #include <geometry_msgs/msg/twist.hpp> #include <std_srvs/srv/trigger.hpp> #include <chrono> #include <thread> using namespace std::chrono_literals; class TwistPublisher : public rclcpp::Node { public: TwistPublisher() : Node("twist_publisher"), linear_speed_(0.0), angular_speed_(0.0) { // 声明参数 this->declare_parameter<double>("max_linear_speed", 0.5); // 最大线速度 0.5m/s this->declare_parameter<double>("max_angular_speed", 1.0); // 最大角速度 1.0rad/s this->declare_parameter<int>("publish_rate", 50); // 发布频率 50Hz // 获取参数 max_linear_speed_ = this->get_parameter("max_linear_speed").as_double(); max_angular_speed_ = this->get_parameter("max_angular_speed").as_double(); publish_rate_ = this->get_parameter("publish_rate").as_int(); // 创建Twist消息发布者 twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10); // 创建紧急停止服务 stop_service_ = this->create_service<std_srvs::Trigger>( "emergency_stop", std::bind(&TwistPublisher::emergency_stop_callback, this, std::placeholders::_1, std::placeholders::_2) ); // 创建定时器，定时发布速度指令 timer_ = this->create_wall_timer( std::chrono::milliseconds(1000 / publish_rate_), std::bind(&TwistPublisher::timer_callback, this) ); RCLCPP_INFO(this->get_logger(), "底盘速度发布节点已启动"); RCLCPP_INFO(this->get_logger(), "最大线速度: %.2f m/s", max_linear_speed_); RCLCPP_INFO(this->get_logger(), "最大角速度: %.2f rad/s", max_angular_speed_); RCLCPP_INFO(this->get_logger(), "发布频率: %d Hz", publish_rate_); RCLCPP_INFO(this->get_logger(), "紧急停止服务: /emergency_stop"); } // 设置速度 void set_speed(double linear, double angular) { // 速度限幅 linear_speed_ = std::clamp(linear, -max_linear_speed_, max_linear_speed_); angular_speed_ = std::clamp(angular, -max_angular_speed_, max_angular_speed_); } private: // 定时器回调函数，发布速度指令 void timer_callback() { auto twist_msg = geometry_msgs::msg::Twist(); twist_msg.linear.x = linear_speed_; twist_msg.angular.z = angular_speed_; twist_pub_->publish(twist_msg); } // 紧急停止服务回调 void emergency_stop_callback( const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response ) { (void)request; // 未使用参数 linear_speed_ = 0.0; angular_speed_ = 0.0; response->success = true; response->message = "紧急停止已触发，底盘速度已置零"; RCLCPP_WARN(this->get_logger(), "紧急停止！"); } rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_; rclcpp::Service<std_srvs::Trigger>::SharedPtr stop_service_; rclcpp::TimerBase::SharedPtr timer_; double linear_speed_; double angular_speed_; double max_linear_speed_; double max_angular_speed_; int publish_rate_; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = std::make_shared<TwistPublisher>(); // 简单的键盘控制逻辑 std::thread keyboard_thread([&node]() { char key; RCLCPP_INFO(node->get_logger(), "==================== 键盘控制 ===================="); RCLCPP_INFO(node->get_logger(), "w: 前进 s: 后退 a: 左转 d: 右转 空格: 停止"); RCLCPP_INFO(node->get_logger(), "q: 退出"); RCLCPP_INFO(node->get_logger(), "=================================================="); while (rclcpp::ok()) { std::cin >> key; switch (key) { case 'w': node->set_speed(0.3, 0.0); break; case 's': node->set_speed(-0.3, 0.0); break; case 'a': node->set_speed(0.0, 0.5); break; case 'd': node->set_speed(0.0, -0.5); break; case ' ': node->set_speed(0.0, 0.0); RCLCPP_INFO(node->get_logger(), "停止"); break; case 'q': RCLCPP_INFO(node->get_logger(), "退出键盘控制"); rclcpp::shutdown(); return; default: RCLCPP_WARN(node->get_logger(), "无效按键"); break; } } }); rclcpp::spin(node); keyboard_thread.join(); rclcpp::shutdown(); return 0; }

3.3 编译与运行

cd ~/ros2_ws colcon build --packages-select chassis_controller source install/setup.bash # 运行节点 ros2 run chassis_controller twist_publisher

在另一个终端中，你可以查看发布的速度指令：

ros2 topic echo /cmd_vel

四、嵌入式端：电机 PID 控制与协议对接

ROS2 发布的/cmd_vel消息只是抽象的速度指令，最终需要由嵌入式控制器（如 STM32）接收并转换为电机的 PWM 控制信号。

4.1 通信协议设计

我们使用串口通信作为 ROS2 与嵌入式之间的通信方式，设计一个简单可靠的二进制协议：

表格

总帧长：14 字节

4.2 嵌入式端协议解析代码（STM32 示例）

#include "stm32f4xx_hal.h" #include <string.h> #include <math.h> // 串口接收缓冲区 #define BUFFER_SIZE 256 uint8_t rx_buffer[BUFFER_SIZE]; uint16_t rx_index = 0; // 速度变量 float target_linear_speed = 0.0f; float target_angular_speed = 0.0f; // 串口接收中断回调函数 void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (huart->Instance == USART1) { uint8_t data = rx_buffer[rx_index]; rx_index++; // 检查帧头 if (rx_index == 1 && data != 0xAA) { rx_index = 0; } else if (rx_index == 2 && data != 0x55) { rx_index = 0; } // 接收完整一帧 if (rx_index >= 14) { // 检查帧尾 if (rx_buffer[12] == 0x0D && rx_buffer[13] == 0x0A) { // 计算校验和 uint8_t checksum = 0; for (int i = 2; i < 11; i++) { checksum ^= rx_buffer[i]; } // 校验和正确 if (checksum == rx_buffer[11]) { // 解析速度 memcpy(&target_linear_speed, &rx_buffer[3], sizeof(float)); memcpy(&target_angular_speed, &rx_buffer[7], sizeof(float)); } } rx_index = 0; } // 继续接收下一个字节 HAL_UART_Receive_IT(&huart1, &rx_buffer[rx_index], 1); } }

4.3 电机 PID 闭环控制

解析出目标速度后，我们需要通过 PID 算法将其转换为电机的 PWM 输出。这里使用增量式 PID 算法：

// PID结构体定义 typedef struct { float kp; float ki; float kd; float target; float current; float error; float last_error; float prev_error; float output; float max_output; } PID_TypeDef; // 左右电机PID实例 PID_TypeDef left_motor_pid; PID_TypeDef right_motor_pid; // PID初始化 void PID_Init(PID_TypeDef *pid, float kp, float ki, float kd, float max_output) { pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->target = 0.0f; pid->current = 0.0f; pid->error = 0.0f; pid->last_error = 0.0f; pid->prev_error = 0.0f; pid->output = 0.0f; pid->max_output = max_output; } // 增量式PID计算 float PID_Calculate(PID_TypeDef *pid, float target, float current) { pid->target = target; pid->current = current; pid->error = pid->target - pid->current; // 增量式PID公式 float increment = pid->kp * (pid->error - pid->last_error) + pid->ki * pid->error + pid->kd * (pid->error - 2 * pid->last_error + pid->prev_error); pid->output += increment; // 输出限幅 if (pid->output > pid->max_output) { pid->output = pid->max_output; } else if (pid->output < -pid->max_output) { pid->output = -pid->max_output; } // 更新误差 pid->prev_error = pid->last_error; pid->last_error = pid->error; return pid->output; }

4.4 差速底盘运动学解算

对于两轮差速底盘，我们需要将 ROS2 发布的线速度和角速度转换为左右轮的目标转速：

// 底盘参数 #define WHEEL_RADIUS 0.035f // 轮子半径，单位：m #define WHEEL_BASE 0.2f // 轮距，单位：m #define REDUCTION_RATIO 30.0f // 减速比 // 将线速度和角速度转换为左右轮转速（单位：RPM） void chassis_kinematics(float linear_speed, float angular_speed, float *left_rpm, float *right_rpm) { // 计算左右轮线速度 float left_speed = linear_speed - (angular_speed * WHEEL_BASE) / 2.0f; float right_speed = linear_speed + (angular_speed * WHEEL_BASE) / 2.0f; // 转换为电机转速(RPM) *left_rpm = (left_speed * 60.0f) / (2.0f * M_PI * WHEEL_RADIUS) * REDUCTION_RATIO; *right_rpm = (right_speed * 60.0f) / (2.0f * M_PI * WHEEL_RADIUS) * REDUCTION_RATIO; }

五、软硬联动调试全流程

5.1 调试前准备

1. 确保嵌入式端代码已烧录，串口连接正常
2. 在 ROS2 端安装串口驱动：sudo apt install ros-humble-serial-driver
3. 给串口赋予权限：sudo chmod 666 /dev/ttyUSB0

5.2 分步调试步骤

步骤 1：单独测试 ROS2 速度发布节点

# 运行速度发布节点 ros2 run chassis_controller twist_publisher # 新开终端，查看/cmd_vel话题 ros2 topic echo /cmd_vel # 按w/s/a/d键，观察速度值是否正确变化

步骤 2：测试串口通信

使用minicom或screen工具查看串口数据：

sudo apt install minicom minicom -D /dev/ttyUSB0 -b 115200

步骤 3：测试电机 PID 控制

在嵌入式端编写一个简单的测试函数，直接给电机设定目标转速，观察电机是否能平稳运行：

// 电机测试函数 void motor_test() { float left_pwm = PID_Calculate(&left_motor_pid, 100.0f, get_left_motor_speed()); float right_pwm = PID_Calculate(&right_motor_pid, 100.0f, get_right_motor_speed()); set_motor_pwm(left_pwm, right_pwm); }

步骤 4：完整联调

1. 运行 ROS2 速度发布节点
2. 运行串口节点，将/cmd_vel消息转换为串口数据发送给嵌入式
3. 按键盘按键，观察机器人是否能按照指令运动

5.3 常用调试工具

• rqt_plot：实时绘制速度曲线，观察 PID 调节效果

  ros2 run rqt_plot rqt_plot /cmd_vel/linear/x /cmd_vel/angular/z

• ros2 topic hz：查看话题发布频率

  ros2 topic hz /cmd_vel

• rqt_graph：查看节点间的通信关系

  ros2 run rqt_graph rqt_graph

六、常见问题与解决方案

问题 1：机器人运动方向与预期相反

• 原因：电机接线反了，或者编码器方向反了
• 解决：交换电机的两根接线，或者在代码中反转速度符号

问题 2：机器人走直线时跑偏

• 原因：左右轮 PID 参数不一致，或者机械结构有误差
• 解决：
  1. 重新校准左右轮的 PID 参数
  2. 在代码中加入偏航补偿
  3. 检查机械结构，确保轮子转动顺畅

问题 3：速度响应慢，有明显延迟

• 原因：PID 参数过小，或者发布频率太低
• 解决：
  1. 适当增大 PID 的 Kp 和 Ki 参数
  2. 提高速度指令的发布频率（建议 50Hz 以上）
  3. 优化串口通信，减少延迟

问题 4：电机抖动严重

• 原因：PID 参数过大，特别是 Kd 参数
• 解决：减小 Kd 参数，或者增加滤波环节

七、总结与下期预告

今天我们完成了 ROS2 底盘运动控制的基础开发，从标准 Twist 消息格式到 C++ 控制节点，再到嵌入式端的 PID 控制和协议对接，形成了一个完整的运动控制闭环。

核心知识点回顾：

1. geometry_msgs/Twist是 ROS2 标准速度控制消息，两轮差速底盘只需要控制linear.x和angular.z
2. 速度指令发布频率建议在 50Hz 以上，保证控制的实时性
3. 嵌入式端使用增量式 PID 算法实现电机转速闭环控制
4. 差速底盘需要通过运动学解算将整体速度转换为左右轮转速

📢下期连载预告：【ROS2 速成 - Day19~Day21】软硬件联调闭环全流程实战

• Day19：编码器数据采集与速度反馈
• Day20：里程计计算与 TF 坐标变换
• Day21：底盘驱动节点封装与 ROS2 控制接口标准化

码字不易，如果这篇文章对你有帮助，欢迎点赞 + 收藏 + 关注！我会持续更新【ROS2 嵌入式实战速成系列】，分享更多工业级 ROS2 开发经验和完整项目代码。

有任何问题或者建议，欢迎在评论区留言交流，我会一一回复！