ROS2系列教程：嵌入式控制与通信开发（原理+仿真）

本文是 ROS2 系列教程的第 32 篇本文是 ROS2 系列教程的第 32 篇嵌入式控制与通信开发原理仿真。第 31 篇我们搭好了实体机器人的硬件骨架。但硬件只是尸体让它活过来需要软件下位机的控制与通信程序、上位机的桥接与状态发布。本篇把第 31 篇的框架落实成完整可运行的代码下位机协议栈、里程计解算、上位机串口桥、传感器数据接入并用仿真环境验证上/下位机通信的可靠性。这是实体机器人软硬结合的关键一篇。一、上下位机通信协议设计原理1.1 协议设计原则嵌入式串口通信最容易出问题协议设计要遵循几条原则有帧头帧尾接收方靠帧头定位一帧的起点。有长度字段明确一帧有多少字节防止粘包/半包。有校验防止干扰导致的错误数据被执行。定长优先定长帧解析简单可靠变长帧复杂易错。1.2 推荐帧格式采用帧头(2B) 命令(1B) 长度(1B) 数据(NB) 校验(1B)的通用格式┌──────┬──────┬──────┬──────┬──────────┬────────┐ │ 0xAA │ 0x55 │ CMD │ LEN │ DATA │ CRC8 │ │ 帧头1│ 帧头2│ 命令 │ 长度 │ 数据区 │ 校验 │ └──────┴──────┴──────┴──────┴──────────┴────────┘命令定义举例CMD方向数据说明0x01上位机→下位机float×2 (v, ω)设置目标速度0x02下位机→上位机float×2 (v, ω)回传实测速度0x03上位机→下位机uint8 (1/0)复位 / 使能电机0x04下位机→上位机float×3 (ax, ay, az)回传 IMU 数据0x05上位机→下位机uint8 (LED)控制指示灯1.3 校验算法CRC8用简单的 CRC8 或异或校验。异或最简单CRC8 更可靠。CRC8 实现// crc8.cCRC-8 校验多项式 0x07初值 0x00uint8_tcrc8(constuint8_t*data,uint16_tlen){uint8_tcrc0x00;for(uint16_ti0;ilen;i){crc^data[i];for(uint8_tj0;j8;j){if(crc0x80){crc(uint8_t)((crc1)^0x07);}else{crc1;}}}returncrc;}二、下位机通信实现STM322.1 串口接收状态机串口数据是流式的一帧可能分多次到达必须用状态机逐字节解析// protocol.c串口接收状态机STM32#includeprotocol.h#includecrc8.htypedefenum{ST_WAIT_HEAD1,// 等待帧头1 0xAAST_WAIT_HEAD2,// 等待帧头2 0x55ST_WAIT_CMD,// 等待命令字节ST_WAIT_LEN,// 等待长度字节ST_DATA,// 接收数据区ST_CRC// 等待校验字节}rx_state_t;staticrx_state_tstateST_WAIT_HEAD1;staticprotocol_frame_tframe;staticuint8_tdata_idx0;// 串口中断里逐字节喂给状态机voidprotocol_rx_byte(uint8_tbyte){switch(state){caseST_WAIT_HEAD1:if(byte0xAA)stateST_WAIT_HEAD2;break;caseST_WAIT_HEAD2:state(byte0x55)?ST_WAIT_CMD:ST_WAIT_HEAD1;break;caseST_WAIT_CMD:frame.cmdbyte;stateST_WAIT_LEN;break;caseST_WAIT_LEN:frame.lenbyte;data_idx0;state(byte0)?ST_DATA:ST_CRC;break;caseST_DATA:frame.data[data_idx]byte;if(data_idxframe.len)stateST_CRC;break;caseST_CRC:frame.crcbyte;// 校验CRC crc8(cmd, len, data)if(frame.crccompute_frame_crc(frame)){protocol_on_frame(frame);// 帧有效回调处理}stateST_WAIT_HEAD1;break;}}2.2 串口中断接收HAL// 串口接收中断回调HAL 库接收一个字节触发voidHAL_UART_RxCpltCallback(UART_HandleTypeDef*huart){if(huart-InstanceUSART1){protocol_rx_byte(rx_buffer);// 喂给状态机HAL_UART_Receive_IT(huart1,rx_buffer,1);// 重新开启接收}}要点每次接收完必须重新调用HAL_UART_Receive_IT开启下一次接收否则只收一字节。2.3 帧处理回调// 收到有效帧后的处理voidprotocol_on_frame(protocol_frame_t*f){switch(f-cmd){caseCMD_SET_VEL:{// 设置速度float×2floatv,w;memcpy(v,f-data[0],4);memcpy(w,f-data[4],4);target_vv;target_ww;break;}caseCMD_ENABLE:{// 使能电机motor_enable(f-data[0]!0);break;}default:break;}}三、里程计解算原理实现3.1 里程计原理里程计Odometry根据轮子转过的角度估算机器人位姿x, y, yaw。对两轮差速左右轮速度v_l, v_r由编码器测得 机器人线速度v (v_l v_r) / 2 机器人角速度ω (v_r - v_l) / L L轮距 位姿积分每 dt 更新一次 x v × cos(yaw) × dt y v × sin(yaw) × dt yaw ω × dt3.2 下位机里程计实现// odometry.c两轮差速里程计解算下位机#includeodometry.h#includemath.h// 机器人参数与 URDF 一致#defineWHEEL_RADIUS0.0325f// 轮半径 m#defineWHEEL_BASE0.200f// 轮距 mstaticfloatpos_x0,pos_y0,yaw0;// 每 10ms 调用一次输入左右轮角速度rad/svoidodometry_update(floatomega_left,floatomega_right){constfloatdt0.01f;// 左右轮线速度floatv_lomega_left*WHEEL_RADIUS;floatv_romega_right*WHEEL_RADIUS;// 机器人线速度与角速度floatv(v_lv_r)/2.0f;floatw(v_r-v_l)/WHEEL_BASE;// 位姿积分yaww*dt;// yaw 归一化到 [-π, π]if(yawM_PI)yaw-2*M_PI;if(yaw-M_PI)yaw2*M_PI;pos_xv*cosf(yaw)*dt;pos_yv*sinf(yaw)*dt;}// 供上位机查询voidodometry_get(float*x,float*y,float*yaw_out){*xpos_x;*ypos_y;*yaw_outyaw;}关键点里程计参数轮半径、轮距必须与 URDF/控制器配置一致否则建图和导航会漂。这就是为什么第 28 篇 diff_drive_controller 里要配wheel_radius和wheel_separation——下位机用它算里程计上位机用它算控制两边必须统一。3.3 上位机里程计发布完整版第 31 篇的串口桥只发了速度这里补全为完整位姿# odom_bridge.py完整里程计发布位姿 协方差importrclpyimportserialimportstructfromrclpy.nodeimportNodefromnav_msgs.msgimportOdometryfromgeometry_msgs.msgimportQuaternionfromstd_msgs.msgimportHeaderimportmathclassOdomBridge(Node):def__init__(self):super().__init__(odom_bridge)self.serserial.Serial(/dev/ttyUSB0,115200,timeout0.02)self.pubself.create_publisher(Odometry,/odom,10)self.timerself.create_timer(0.02,self.tick)# 50Hzdefquat_from_yaw(self,yaw):# yaw → 四元数ROS2 消息需要四元数returnQuaternion(x0.0,y0.0,zmath.sin(yaw/2.0),wmath.cos(yaw/2.0))deftick(self):# 读下位机里程计帧CMD0x02x, y, yaw 三个 floatbufself.ser.read(18)iflen(buf)!18:returnhead,cmdstruct.unpack(HB,buf[:3])ifhead!0xAA55orcmd!0x02:returnx,y,yawstruct.unpack(3f,buf[3:15])crc_ok(buf[15](struct.calcsize(HB3f)and0))# 简化为示例odomOdometry()odom.headerHeader(stampself.get_clock().now().to_msg(),frame_idodom)odom.child_frame_idbase_linkodom.pose.pose.position.xx odom.pose.pose.position.yy odom.pose.pose.orientationself.quat_from_yaw(yaw)# 简化协方差实际应按编码器噪声标定odom.pose.covariance[0]0.001odom.pose.covariance[7]0.001odom.pose.covariance[35]0.01self.pub.publish(odom)defmain(argsNone):rclpy.init(argsargs)nodeOdomBridge()rclpy.spin(node)node.destroy_node()rclpy.shutdown()if__name____main__:main()四、示例 1仿真验证通信可靠性gazebo 真机协议在真机调试之前先用仿真验证协议正确性。思路在 Gazebo 仿真底盘上运行与真机完全相同的协议逻辑验证协议解析无 bug。# fake_mcu.py仿真下位机模拟 STM32 的协议行为# 在 Gazebo 仿真的 /odom 基础上按协议格式通过虚拟串口回传importrclpyimportstructimportmathfromrclpy.nodeimportNodefromnav_msgs.msgimportOdometryclassFakeMcu(Node):仿真下位机订阅仿真 /odom按协议组帧不接真实串口仅演示协议正确性def__init__(self):super().__init__(fake_mcu)self.odom_subself.create_subscription(Odometry,/odom,self.on_odom,10)defon_odom(self,msg):# 与真实下位机相同的组帧逻辑yaw2*math.atan2(msg.pose.pose.orientation.z,msg.pose.pose.orientation.w)datastruct.pack(3f,msg.pose.pose.position.x,msg.pose.pose.position.y,yaw)framestruct.pack(HBB,0xAA55,0x02,len(data))data# 校验与下位机 crc8 一致此处用异或简化演示crc0forbindata:crc^b framebytes([crc])# 真实工程写入串口 self.ser.write(frame)self.get_logger().debug(fframe:{frame.hex()})defspin_check(self):passdefmain(argsNone):rclpy.init(argsargs)nodeFakeMcu()rclpy.spin(node)rclpy.shutdown()if__name____main__:main()验证方法在 Gazebo 里让车走一段已知路径对比fake_mcu组帧数据与真机下位机输出格式是否一致。一致 → 协议可靠可放心接真机。五、示例 2IMU 数据接入与融合5.1 下位机回传 IMU// 下位机定时50Hz把 IMU 数据组帧回传voidsend_imu_frame(void){int16_taccel[3],gyro[3];mpu6050_read(accel,gyro);uint8_tdata[12];// 转成 float 并打包floataxaccel[0]/16384.0f*9.81f;floatayaccel[1]/16384.0f*9.81f;floatazaccel[2]/16384.0f*9.81f;memcpy(data[0],ax,4);memcpy(data[4],ay,4);memcpy(data[8],az,4);protocol_send(CMD_IMU,data,12);// 组帧 串口发送}5.2 上位机发布 IMU 并融合到里程计EKF扩展卡尔曼滤波融合轮式里程计 IMU 是标准做法。ROS2 用robot_localization包# ekf.yamlrobot_localization 配置融合 odom imuekf_filter_node:ros__parameters:frequency:50.0two_d_mode:true# 2D 机器人publish_tf:trueodom0:/odom# 轮式里程计输入odom0_config:[true,true,false,# x, y, zfalse,false,false,# roll, pitch, yawfalse,false,false,# vx, vy, vzfalse,false,false,# wx, wy, wzfalse,false,false]imu0:/imu/data_raw# IMU输入imu0_config:[false,false,false,true,true,true,# roll, pitch, yaw 用 IMUfalse,false,false,false,false,false,false,false,false]imu0_remove_gravitational_acceleration:trueodom0_differential:false# 启动 EKF 融合ros2 run robot_localization ekf_node --ros-args\-r__node:ekf_filter_node\--params-file ekf.yaml# 融合后的位姿话题ros2 topicecho/odometry/filtered融合效果轮式里程计提供短期位置IMU 提供姿态长期位置不会因打滑而完全丢失。注意IMU 提供 yaw 后与轮式里程计的 yaw 要统一方向约定都是逆时针为正。六、示例 3传感器数据汇聚节点把下位机回传的所有传感器数据汇聚成 ROS2 话题供上层使用# sensor_aggregator.py传感器数据汇聚超声波 电量 状态importrclpyimportserialimportstructfromrclpy.nodeimportNodefromsensor_msgs.msgimportRangefromstd_msgs.msgimportFloat32,BoolclassSensorAggregator(Node):def__init__(self):super().__init__(sensor_aggregator)self.serserial.Serial(/dev/ttyUSB0,115200,timeout0.02)# 发布超声波距离传感器消息标准格式self.sonar_pubself.create_publisher(Range,/sonar_front,10)# 发布电池电压self.volt_pubself.create_publisher(Float32,/battery_voltage,10)self.timerself.create_timer(0.1,self.tick)# 10Hzdeftick(self):# 读下位机传感器帧简化CMD0x06超声 float 电压 floatbufself.ser.read(14)iflen(buf)!14:returnhead,cmdstruct.unpack(HB,buf[:3])ifhead!0xAA55orcmd!0x06:returndist,voltagestruct.unpack(2f,buf[3:11])# 发布超声波Range 消息传感器锥形探测范围rRange()r.header.stampself.get_clock().now().to_msg()r.header.frame_idsonar_linkr.radiation_typeRange.ULTRASOUND r.field_of_view0.52# 30° 锥角弧度r.min_range0.02r.max_range4.0r.rangedist self.sonar_pub.publish(r)# 发布电压vFloat32()v.datavoltage self.volt_pub.publish(v)defmain(argsNone):rclpy.init(argsargs)nodeSensorAggregator()rclpy.spin(node)node.destroy_node()rclpy.shutdown()if__name____main__:main()低电量保护上位机订阅/battery_voltage低于阈值时发布robot_status/emergency_stop或者直接关断动力——这是实体机器人的安全底线。七、通信可靠性工程实践7.1 丢帧与重试上位机发指令默认不需要应答速度指令是最新值覆盖语义但关键指令复位、使能需要确认帧。下位机 50Hz 回传状态上位机若 1 秒没收到任何帧 → 判断下位机掉线 → 停止电机安全。# 掉线保护在串口桥里加超时判断classSerialBridge:def__init__(self):...self.last_frame_time0.0self.watchdogself.create_timer(0.5,self.watchdog_check)defwatchdog_check(self):nowself.get_clock().now().nanoseconds/1e9ifnow-self.last_frame_time1.0:self.get_logger().error(MCU offline! stopping motors)# 发布零速度安全停车twistTwist()self.on_cmd_vel(twist)7.2 波特率与缓冲区115200 波特率约 11500 字节/秒50Hz × 18 字节 × 2收发≈ 1800 字节/秒余量充足。上位机串口读缓冲区建议 256 字节以上下位机环形缓冲区 128 字节。7.3 电磁干扰防护电机线用双绞线远离串口线。串口加磁环或换 USB-TTL 隔离模块。地线星形连接各模块独立回地避免地环路。八、常见问题与排障现象原因解决收不到完整帧状态机 bug / 串口配置错误串口调试助手先验证帧格式再查状态机偶发校验失败电磁干扰 / 波特率漂移双绞线、磁环、降低波特率试里程计漂移轮距/轮径参数不准实测轮径转 N 圈测距离标定参数里程计方向反编码器方向约定不一致统一左轮正转前进约定软件反转IMU yaw 跳变角度归一化错误yaw 归一到 [-π, π]电机命令滞后通信频率太低上位机发送频率提到 20-50Hz上位机一启动电机猛转默认命令未初始化启动时先发一次零速度再进主循环九、小结与下一篇预告本篇核心通信协议三要素帧头 长度 校验CRC8用状态机解析字节流。下位机三件套串口状态机 里程计解算 IMU 回传10ms 控制环内完成。上位机两桥串口桥/cmd_vel ↔ 协议帧 传感器汇聚Range/电压。可靠性工程掉线看门狗、低电量保护、电磁防护、参数标定。至此实体机器人软硬结合完成能接收指令、能上报状态、能算里程计。下一篇第 33 篇进入上位机建图与导航原理综合把第 21-25 篇的 SLAM 和 Nav2 能力部署到实体机器人上——真机建图流程、坐标系校准、Nav2 参数整定、首次自主导航完成从仿真到真机的跨越。敬请期待。