本文为【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 z2.2 不同底盘类型的分量映射注意不是所有分量对所有底盘都有效不同运动学模型的底盘可用的速度分量不同底盘类型有效线速度分量有效角速度分量说明两轮差速底盘linear.xangular.z只能前后移动和原地转向三轮全向底盘linear.x, linear.yangular.z可以实现平面内任意方向移动四轮麦克纳姆轮linear.x, linear.yangular.z全向移动机动性最强阿克曼转向底盘linear.xangular.z类似汽车不能原地转向最常用的两轮差速底盘只需要控制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添加必要的依赖和描述namechassis_controller/name version0.0.0/version descriptionROS2底盘运动控制节点支持速度指令发布与嵌入式对接/description maintainer emailyour_emailexample.comYour Name/maintainer licenseMIT/license buildtool_dependament_cmake/buildtool_depend dependrclcpp/depend dependgeometry_msgs/depend dependstd_srvs/depend build_dependament_cmake_auto/build_depend exec_dependrclcpp/exec_depend exec_dependgeometry_msgs/exec_depend exec_dependstd_srvs/exec_depend export build_typeament_cmake/build_type /export修改CMakeLists.txtcmakecmake_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_parameterdouble(max_linear_speed, 0.5); // 最大线速度 0.5m/s this-declare_parameterdouble(max_angular_speed, 1.0); // 最大角速度 1.0rad/s this-declare_parameterint(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_publishergeometry_msgs::msg::Twist(/cmd_vel, 10); // 创建紧急停止服务 stop_service_ this-create_servicestd_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::Publishergeometry_msgs::msg::Twist::SharedPtr twist_pub_; rclcpp::Servicestd_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_sharedTwistPublisher(); // 简单的键盘控制逻辑 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 与嵌入式之间的通信方式设计一个简单可靠的二进制协议表格字节偏移内容类型说明0帧头uint8_t固定为 0xAA1帧头uint8_t固定为 0x552数据长度uint8_t后续数据字节数固定为 83-6线速度float单位m/s小端模式7-10角速度float单位rad/s小端模式11校验和uint8_t所有数据字节的异或和12帧尾uint8_t固定为 0x0D13帧尾uint8_t固定为 0x0A总帧长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 调试前准备确保嵌入式端代码已烧录串口连接正常在 ROS2 端安装串口驱动sudo apt install ros-humble-serial-driver给串口赋予权限sudo chmod 666 /dev/ttyUSB05.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完整联调运行 ROS2 速度发布节点运行串口节点将/cmd_vel消息转换为串口数据发送给嵌入式按键盘按键观察机器人是否能按照指令运动5.3 常用调试工具rqt_plot实时绘制速度曲线观察 PID 调节效果ros2 run rqt_plot rqt_plot /cmd_vel/linear/x /cmd_vel/angular/zros2 topic hz查看话题发布频率ros2 topic hz /cmd_velrqt_graph查看节点间的通信关系ros2 run rqt_graph rqt_graph六、常见问题与解决方案问题 1机器人运动方向与预期相反原因电机接线反了或者编码器方向反了解决交换电机的两根接线或者在代码中反转速度符号问题 2机器人走直线时跑偏原因左右轮 PID 参数不一致或者机械结构有误差解决重新校准左右轮的 PID 参数在代码中加入偏航补偿检查机械结构确保轮子转动顺畅问题 3速度响应慢有明显延迟原因PID 参数过小或者发布频率太低解决适当增大 PID 的 Kp 和 Ki 参数提高速度指令的发布频率建议 50Hz 以上优化串口通信减少延迟问题 4电机抖动严重原因PID 参数过大特别是 Kd 参数解决减小 Kd 参数或者增加滤波环节七、总结与下期预告今天我们完成了 ROS2 底盘运动控制的基础开发从标准 Twist 消息格式到 C 控制节点再到嵌入式端的 PID 控制和协议对接形成了一个完整的运动控制闭环。核心知识点回顾geometry_msgs/Twist是 ROS2 标准速度控制消息两轮差速底盘只需要控制linear.x和angular.z速度指令发布频率建议在 50Hz 以上保证控制的实时性嵌入式端使用增量式 PID 算法实现电机转速闭环控制差速底盘需要通过运动学解算将整体速度转换为左右轮转速下期连载预告【ROS2 速成 - Day19~Day21】软硬件联调闭环全流程实战Day19编码器数据采集与速度反馈Day20里程计计算与 TF 坐标变换Day21底盘驱动节点封装与 ROS2 控制接口标准化码字不易如果这篇文章对你有帮助欢迎点赞 收藏 关注我会持续更新【ROS2 嵌入式实战速成系列】分享更多工业级 ROS2 开发经验和完整项目代码。有任何问题或者建议欢迎在评论区留言交流我会一一回复
【ROS2 速成 - Day18】机器人底盘运动控制基础（速度指令开发）
发布时间：2026/5/15 16:15:54
本文为【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 z2.2 不同底盘类型的分量映射注意不是所有分量对所有底盘都有效不同运动学模型的底盘可用的速度分量不同底盘类型有效线速度分量有效角速度分量说明两轮差速底盘linear.xangular.z只能前后移动和原地转向三轮全向底盘linear.x, linear.yangular.z可以实现平面内任意方向移动四轮麦克纳姆轮linear.x, linear.yangular.z全向移动机动性最强阿克曼转向底盘linear.xangular.z类似汽车不能原地转向最常用的两轮差速底盘只需要控制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添加必要的依赖和描述namechassis_controller/name version0.0.0/version descriptionROS2底盘运动控制节点支持速度指令发布与嵌入式对接/description maintainer emailyour_emailexample.comYour Name/maintainer licenseMIT/license buildtool_dependament_cmake/buildtool_depend dependrclcpp/depend dependgeometry_msgs/depend dependstd_srvs/depend build_dependament_cmake_auto/build_depend exec_dependrclcpp/exec_depend exec_dependgeometry_msgs/exec_depend exec_dependstd_srvs/exec_depend export build_typeament_cmake/build_type /export修改CMakeLists.txtcmakecmake_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_parameterdouble(max_linear_speed, 0.5); // 最大线速度 0.5m/s this-declare_parameterdouble(max_angular_speed, 1.0); // 最大角速度 1.0rad/s this-declare_parameterint(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_publishergeometry_msgs::msg::Twist(/cmd_vel, 10); // 创建紧急停止服务 stop_service_ this-create_servicestd_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::Publishergeometry_msgs::msg::Twist::SharedPtr twist_pub_; rclcpp::Servicestd_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_sharedTwistPublisher(); // 简单的键盘控制逻辑 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 与嵌入式之间的通信方式设计一个简单可靠的二进制协议表格字节偏移内容类型说明0帧头uint8_t固定为 0xAA1帧头uint8_t固定为 0x552数据长度uint8_t后续数据字节数固定为 83-6线速度float单位m/s小端模式7-10角速度float单位rad/s小端模式11校验和uint8_t所有数据字节的异或和12帧尾uint8_t固定为 0x0D13帧尾uint8_t固定为 0x0A总帧长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 调试前准备确保嵌入式端代码已烧录串口连接正常在 ROS2 端安装串口驱动sudo apt install ros-humble-serial-driver给串口赋予权限sudo chmod 666 /dev/ttyUSB05.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完整联调运行 ROS2 速度发布节点运行串口节点将/cmd_vel消息转换为串口数据发送给嵌入式按键盘按键观察机器人是否能按照指令运动5.3 常用调试工具rqt_plot实时绘制速度曲线观察 PID 调节效果ros2 run rqt_plot rqt_plot /cmd_vel/linear/x /cmd_vel/angular/zros2 topic hz查看话题发布频率ros2 topic hz /cmd_velrqt_graph查看节点间的通信关系ros2 run rqt_graph rqt_graph六、常见问题与解决方案问题 1机器人运动方向与预期相反原因电机接线反了或者编码器方向反了解决交换电机的两根接线或者在代码中反转速度符号问题 2机器人走直线时跑偏原因左右轮 PID 参数不一致或者机械结构有误差解决重新校准左右轮的 PID 参数在代码中加入偏航补偿检查机械结构确保轮子转动顺畅问题 3速度响应慢有明显延迟原因PID 参数过小或者发布频率太低解决适当增大 PID 的 Kp 和 Ki 参数提高速度指令的发布频率建议 50Hz 以上优化串口通信减少延迟问题 4电机抖动严重原因PID 参数过大特别是 Kd 参数解决减小 Kd 参数或者增加滤波环节七、总结与下期预告今天我们完成了 ROS2 底盘运动控制的基础开发从标准 Twist 消息格式到 C 控制节点再到嵌入式端的 PID 控制和协议对接形成了一个完整的运动控制闭环。核心知识点回顾geometry_msgs/Twist是 ROS2 标准速度控制消息两轮差速底盘只需要控制linear.x和angular.z速度指令发布频率建议在 50Hz 以上保证控制的实时性嵌入式端使用增量式 PID 算法实现电机转速闭环控制差速底盘需要通过运动学解算将整体速度转换为左右轮转速下期连载预告【ROS2 速成 - Day19~Day21】软硬件联调闭环全流程实战Day19编码器数据采集与速度反馈Day20里程计计算与 TF 坐标变换Day21底盘驱动节点封装与 ROS2 控制接口标准化码字不易如果这篇文章对你有帮助欢迎点赞 收藏 关注我会持续更新【ROS2 嵌入式实战速成系列】分享更多工业级 ROS2 开发经验和完整项目代码。有任何问题或者建议欢迎在评论区留言交流我会一一回复
