本文是 ROS2 系列教程的第 30 篇本文是 ROS2 系列教程的第 30 篇控制硬件实战舵机与传感器原理。第 29 篇我们打通了电机驱动全链路H 桥、PWM、编码器、PID、ros2_control 硬件接口。但机器人除了电机还需要关节舵机和感知传感器。本篇讲透舵机的控制原理PWM 与角度映射、总线舵机和常用传感器的接入原理IMU、超声波、红外、激光雷达、摄像头给出代码示例并完成硬件控制阶段26-30的收官。一、舵机控制原理1.1 舵机是什么舵机Servo是能精确控制角度的执行器广泛用于机械臂关节、云台、转向机构。它内部集成了直流电机 减速齿轮组 电位器角度反馈 控制电路。给一个 PWM 信号它就转到指定角度并保持。PWM 信号 → 控制电路 → 比较角度误差 → 驱动电机 → 减速齿轮 → 输出轴 ↑_________________电位器反馈_______________|舵机内部是一个天然的闭环系统你给目标角度它自己闭环跟踪。这就是为什么舵机只需要一根信号线而电机需要编码器PID。1.2 角度与 PWM 的映射普通模拟舵机如 SG90用 20ms 周期50Hz的 PWM 控制占空比决定角度0.5ms → 0°最小角度 1.0ms → 45° 1.5ms → 90°中位 2.0ms → 135° 2.5ms → 180°最大角度关键点频率固定50Hz周期 20ms变化的是高电平脉宽0.5-2.5ms。不同舵机脉宽范围不同需按型号校准。舵机不能长时间堵转到极限位置还持续给信号会烧毁。1.3 STM32 定时器 PWM 控制舵机用 STM32 定时器产生 50Hz PWM通过调节 CCR 值改变脉宽// servo.cSTM32 HAL 库控制舵机TIM1 CH1PE9#includemain.h// 初始化定时器频率 1MHz计数周期ARR19999 → 20ms 周期50Hzvoidservo_init(void){HAL_TIM_PWM_Start(htim1,TIM_CHANNEL_1);servo_set_angle(90);// 先回中位}// 设置舵机角度0~180 度 → 脉宽 0.5~2.5msvoidservo_set_angle(uint8_tangle){if(angle180)angle180;// 脉宽(us) 500 angle * (2500-500)/180 500 angle * 11.11// CCR 值 脉宽(us)因为定时器是 1MHz1 计数 1usuint32_tpulse_us500(uint32_t)(angle*11.11f);__HAL_TIM_SET_COMPARE(htim1,TIM_CHANNEL_1,pulse_us);}1.4 ESP32 Arduino 控制舵机ESP32 用 Servo 库或 LEDC 实现代码更简洁// ESP32 Arduino控制舵机使用内置 Servo 库#includeESP32Servo.hServo myservo;voidsetup(){myservo.attach(13,500,2500);// GPIO13脉宽 0.5~2.5msmyservo.write(90);// 回中位delay(1000);}voidloop(){// 0° → 180° → 0° 来回摆动for(intangle0;angle180;angle2){myservo.write(angle);delay(15);}delay(500);for(intangle180;angle0;angle-2){myservo.write(angle);delay(15);}delay(500);}1.5 总线舵机进阶传统舵机每路一个 PWM 引脚舵机多了引脚不够。总线舵机如串行总线舵机 LX-16A、SCS15通过串口/半双工总线级联用 ID 寻址一根线控制多个舵机总线舵机特点 - 串口通信UART可级联 253 个 - 反馈角度、温度、电压还能感知自己 - 可设置速度、模式这是机械臂第 35-40 篇 MoveIt2 实战常用的舵机方案ROS2 侧通过dynamixel_sdk类库封装控制。二、IMU 传感器接入原理2.1 IMU 是什么IMU惯性测量单元集成了加速度计 陀螺仪 磁力计测量传感器测量用途加速度计三轴加速度计算倾角、检测运动陀螺仪三轴角速度计算姿态变化率磁力计可选三轴磁场航向角指南针常见芯片MPU6050入门、ICM-20948高性能、BMI088机器人主流。通信接口I2C 或 SPI。2.2 姿态解算原理陀螺仪积分得角度但会漂移误差累积加速度计能直接得倾角但不适合动态振动干扰大。所以要用数据融合互补滤波或卡尔曼滤波。互补滤波简单有效角度 0.98 × (上一角度 陀螺仪角速度 × dt) 0.02 × 加速度计角度陀螺仪负责短期准确加速度计负责长期纠偏。2.3 MPU6050 读取示例STM32 I2C// mpu6050.c读取加速度和陀螺仪原始数据I2C#includemain.h#defineMPU6050_ADDR0xD0// I2C 地址AD00 时#definePWR_MGMT_10x6B#defineACCEL_XOUT_H0x3B#defineGYRO_XOUT_H0x43// 初始化退出休眠模式voidmpu6050_init(void){uint8_tdata0x00;HAL_I2C_Mem_Write(hi2c1,MPU6050_ADDR,PWR_MGMT_1,1,data,1,100);}// 读三轴加速度 三轴陀螺仪voidmpu6050_read(int16_t*accel,int16_t*gyro){uint8_tbuf[14];HAL_I2C_Mem_Read(hi2c1,MPU6050_ADDR,ACCEL_XOUT_H,1,buf,14,100);for(inti0;i6;i){int16_tv(int16_t)((buf[i*2]8)|buf[i*21]);if(i3)accel[i]v;elsegyro[i-3]v;}}2.4 ROS2 侧IMU 数据发布Python下位机通过串口把 IMU 原始数据发给上位机上位机用imu_filter_madgwick或直接发布 sensor_msgs/Imu# imu_publisher.py把串口 IMU 数据发布为 ROS2 Imu 消息importrclpyimportserialimportstructfromrclpy.nodeimportNodefromsensor_msgs.msgimportImufromstd_msgs.msgimportHeaderclassImuPublisher(Node):def__init__(self):super().__init__(imu_publisher)self.pubself.create_publisher(Imu,/imu/data_raw,10)self.serserial.Serial(/dev/ttyUSB1,115200,timeout0.1)self.timerself.create_timer(0.02,self.tick)# 50Hzdeftick(self):# 读串口帧简化固定格式 6 个 int16lineself.ser.readline()iflen(line)12:returnax,ay,az,gx,gy,gzstruct.unpack(6h,line[:12])msgImu()msg.headerHeader()msg.header.stampself.get_clock().now().to_msg()msg.header.frame_idbase_link# 加速度单位换算原始值 / 16384 (±2g 量程)msg.linear_acceleration.xax/16384.0*9.81msg.linear_acceleration.yay/16384.0*9.81msg.linear_acceleration.zaz/16384.0*9.81# 角速度原始值 / 131 (±250°/s 量程)转 rad/smsg.angular_velocity.xgx/131.0*3.14159/180.0msg.angular_velocity.ygy/131.0*3.14159/180.0msg.angular_velocity.zgz/131.0*3.14159/180.0self.pub.publish(msg)defmain(argsNone):rclpy.init(argsargs)nodeImuPublisher()rclpy.spin(node)node.destroy_node()rclpy.shutdown()if__name____main__:main()IMU 数据在机器人中的作用里程计融合轮式里程计 IMU 姿态、机器人直立平衡、AMCL 初始姿态。三、距离传感器接入原理3.1 超声波传感器HC-SR04原理发射超声波测回波时间距离 声速 × 时间 / 2。触发引脚TRIG给 10us 高电平 回波引脚ECHO输出高电平宽度 往返时间 距离(cm) 回波时间(us) / 58// hc_sr04.cSTM32 读取距离溢出捕获测回波脉宽#includemain.hfloathc_sr04_read_distance_cm(void){// 1. TRIG 给 10us 高电平触发HAL_GPIO_WritePin(TRIG_GPIO_Port,TRIG_Pin,GPIO_PIN_SET);delay_us(10);HAL_GPIO_WritePin(TRIG_GPIO_Port,TRIG_Pin,GPIO_PIN_RESET);// 2. 测量 ECHO 高电平时间用定时器输入捕获// 伪代码启动捕获等待上升沿开始计数下降沿停止uint32_tpulse_usmeasure_echo_pulse_us();// 输入捕获实现// 3. 换算距离1us ≈ 0.017cm声速 340m/sreturnpulse_us/58.0f;}局限测量范围 2-400cm精度受温度影响对斜面/软表面不稳定。3.2 红外传感器避障红外避障传感器如 TCRT5000、红外光电开关原理发射红外光障碍物反射后接收管检测。输出是数字开关量有/无障碍适合简单的沿墙避障不提供距离数值。// 读取红外避障传感器返回 1有障碍0无障碍uint8_tir_obstacle_detect(void){// 传感器输出接 GPIO开漏需要上拉returnHAL_GPIO_ReadPin(IR_GPIO_Port,IR_Pin)GPIO_PIN_RESET?1:0;}3.3 激光雷达LiDAR激光雷达如 RPLIDAR A1、YDLIDAR X4通过旋转激光束扫描周围环境输出极坐标距离点集。它比超声波/红外精确得多是 SLAM第 21 篇和导航第 24 篇的核心传感器。接入方式USB 串口 官方驱动rplidar_ros发布sensor_msgs/LaserScan# 启动 RPLIDAR 驱动发布 /scanros2 launch rplidar_ros view_rplidar_a1_launch.py# 查看激光数据ros2 topicecho/scan--once# 输出 angle_min/angle_max/ranges 数组# RViz 可视化rviz2-drplidar_ros/rviz/view_rplidar_a1.rvizLaserScan 消息核心字段header.frame_id: laser坐标系 angle_min / angle_max / angle_increment角度范围与步进 range_min / range_max有效距离范围 ranges[]每个角度的距离值米四、摄像头接入原理4.1 摄像头在 ROS2 中的角色摄像头是机器人最重要的感知传感器目标检测、视觉导航、人机交互。在 ROS2 中通过usb_camUSB 摄像头或v4l2_cameraLinux 原生驱动发布sensor_msgs/Image和sensor_msgs/CameraInfo。# 启动 USB 摄像头驱动ros2 run usb_cam usb_cam_node_exe --ros-args\-pvideo_device:/dev/video0\-pimage_width:640-pimage_height:480\-pframerate:30# 查看图像话题ros2 topic list|grepcamera# /image_raw /camera_info ...# RViz 查看实时画面rviz2# 添加 Image 显示选择 /image_raw4.2 图像话题与 YOLO 视觉示例订阅图像做目标检测是经典应用# detect.py订阅摄像头图像做简单颜色检测OpenCVimportrclpyfromrclpy.nodeimportNodefromsensor_msgs.msgimportImagefromcv_bridgeimportCvBridgeimportcv2importnumpyasnpclassColorDetect(Node):def__init__(self):super().__init__(color_detect)self.bridgeCvBridge()self.subself.create_subscription(Image,/image_raw,self.on_image,10)defon_image(self,msg):cv_imgself.bridge.imgmsg_to_cv2(msg,bgr8)hsvcv2.cvtColor(cv_img,cv2.COLOR_BGR2HSV)# 红色范围检测maskcv2.inRange(hsv,(0,100,100),(10,255,255))contours,_cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)ifcontours:largestmax(contours,keycv2.contourArea)x,y,w,hcv2.boundingRect(largest)self.get_logger().info(fred target at ({x},{y}) size{w}x{h})defmain(argsNone):rclpy.init(argsargs)nodeColorDetect()rclpy.spin(node)rclpy.shutdown()if__name____main__:main()视觉在实体机器人第 31-34 篇和机械臂第 35-40 篇中会大量使用。五、传感器数据汇总与滤波原理5.1 传感器融合思想单传感器都有缺陷轮式里程计会打滑漂移、IMU 会积分漂移、激光雷达有噪点。机器人用传感器融合取长补短轮式里程计短期准 IMU姿态稳 激光雷达定位准 ↓ 融合算法EKF / 粒子滤波 机器人位姿估计x, y, yaw第 23 篇的 AMCL 就是基于粒子滤波的融合定位第 31-34 篇实体机器人会实战组合这些传感器。5.2 简单滤波滑动平均对传感器原始数据做滑动平均抑制噪声// moving_average.c滑动平均滤波窗口 10#defineWINDOW10floatbuf[WINDOW];intidx0;floatsum0;intfilled0;floatmoving_average(floatsample){if(filledWINDOW)sum-buf[idx];buf[idx]sample;sumsample;idx(idx1)%WINDOW;filled(filledWINDOW)?filled1:WINDOW;returnsum/filled;}滤波后的数据再进 ROS2能显著提升后续算法稳定性。六、常见问题与排障现象原因解决舵机抖动/乱转频率不对 / 供电不足确认 50Hz舵机独立供电不能和 MCU 共电源舵机转不到 180°脉宽范围不准按型号调 0.5-2.5ms 范围用attach(pin, 500, 2500)IMU 数据乱跳量程/换算不对 / 未滤波检查量程配置加互补滤波IMU 角度缓慢漂移陀螺仪零偏静止校准零偏定期重置超声波读数为 0回波测量超时增加超时判断38ms 视为无效摄像头画面卡顿USB 带宽 / 分辨率太高降低分辨率或帧率640×48030/scan 数据为空雷达被遮挡 / 串口权限检查遮挡、sudo chmod 666 /dev/ttyUSB*七、硬件控制阶段总结26-30 收官本阶段第 26-30 篇我们打通了机器人硬件全链路篇主题核心收获26嵌入式点灯GPIO 本质嵌入式开发流程27MicroROSMCU 直接成为 ROS2 节点28ROS2-Control关节控制的标准框架29电机驱动H桥/PWM/编码器/PID/硬件接口30舵机与传感器舵机、IMU、测距、视觉接入至此从电的层面GPIO、PWM、串口到信息的层面ROS2 话题、控制框架、传感器数据机器人硬件部分全部打通。八、小结与下一篇预告本篇核心舵机PWM 脉宽映射角度0.5-2.5ms → 0-180°内部自带闭环总线舵机可级联。IMU加速度计陀螺仪互补滤波融合姿态ROS2 里发布 sensor_msgs/Imu。测距三件套超声波时间测距、红外开关量、激光雷达极坐标点云SLAM 核心。摄像头usb_cam 驱动 → /image_raw → OpenCV 视觉处理。至此硬件控制阶段26-30全部收官。下一篇开始进入实体机器人阶段31-34第 31 篇「实体机器人硬件搭建原理」——把前面所有的零件底盘、电机、传感器、上位机、电池组装成一台能跑的真机器人讲解硬件选型、电气连接、电源管理、车架结构等实战知识。从纸上谈兵到真机实战最激动人心的阶段即将开始 
长河君 · 2026/10/19 12:01:09 · 阅读 1.2k ROS2系列教程：控制硬件实战（舵机与传感器，原理）
本文是 ROS2 系列教程的第 30 篇本文是 ROS2 系列教程的第 30 篇控制硬件实战舵机与传感器原理。第 29 篇我们打通了电机驱动全链路H 桥、PWM、编码器、PID、ros2_control 硬件接口。但机器人除了电机还需要关节舵机和感知传感器。本篇讲透舵机的控制原理PWM 与角度映射、总线舵机和常用传感器的接入原理IMU、超声波、红外、激光雷达、摄像头给出代码示例并完成硬件控制阶段26-30的收官。一、舵机控制原理1.1 舵机是什么舵机Servo是能精确控制角度的执行器广泛用于机械臂关节、云台、转向机构。它内部集成了直流电机 减速齿轮组 电位器角度反馈 控制电路。给一个 PWM 信号它就转到指定角度并保持。PWM 信号 → 控制电路 → 比较角度误差 → 驱动电机 → 减速齿轮 → 输出轴 ↑_________________电位器反馈_______________|舵机内部是一个天然的闭环系统你给目标角度它自己闭环跟踪。这就是为什么舵机只需要一根信号线而电机需要编码器PID。1.2 角度与 PWM 的映射普通模拟舵机如 SG90用 20ms 周期50Hz的 PWM 控制占空比决定角度0.5ms → 0°最小角度 1.0ms → 45° 1.5ms → 90°中位 2.0ms → 135° 2.5ms → 180°最大角度关键点频率固定50Hz周期 20ms变化的是高电平脉宽0.5-2.5ms。不同舵机脉宽范围不同需按型号校准。舵机不能长时间堵转到极限位置还持续给信号会烧毁。1.3 STM32 定时器 PWM 控制舵机用 STM32 定时器产生 50Hz PWM通过调节 CCR 值改变脉宽// servo.cSTM32 HAL 库控制舵机TIM1 CH1PE9#includemain.h// 初始化定时器频率 1MHz计数周期ARR19999 → 20ms 周期50Hzvoidservo_init(void){HAL_TIM_PWM_Start(htim1,TIM_CHANNEL_1);servo_set_angle(90);// 先回中位}// 设置舵机角度0~180 度 → 脉宽 0.5~2.5msvoidservo_set_angle(uint8_tangle){if(angle180)angle180;// 脉宽(us) 500 angle * (2500-500)/180 500 angle * 11.11// CCR 值 脉宽(us)因为定时器是 1MHz1 计数 1usuint32_tpulse_us500(uint32_t)(angle*11.11f);__HAL_TIM_SET_COMPARE(htim1,TIM_CHANNEL_1,pulse_us);}1.4 ESP32 Arduino 控制舵机ESP32 用 Servo 库或 LEDC 实现代码更简洁// ESP32 Arduino控制舵机使用内置 Servo 库#includeESP32Servo.hServo myservo;voidsetup(){myservo.attach(13,500,2500);// GPIO13脉宽 0.5~2.5msmyservo.write(90);// 回中位delay(1000);}voidloop(){// 0° → 180° → 0° 来回摆动for(intangle0;angle180;angle2){myservo.write(angle);delay(15);}delay(500);for(intangle180;angle0;angle-2){myservo.write(angle);delay(15);}delay(500);}1.5 总线舵机进阶传统舵机每路一个 PWM 引脚舵机多了引脚不够。总线舵机如串行总线舵机 LX-16A、SCS15通过串口/半双工总线级联用 ID 寻址一根线控制多个舵机总线舵机特点 - 串口通信UART可级联 253 个 - 反馈角度、温度、电压还能感知自己 - 可设置速度、模式这是机械臂第 35-40 篇 MoveIt2 实战常用的舵机方案ROS2 侧通过dynamixel_sdk类库封装控制。二、IMU 传感器接入原理2.1 IMU 是什么IMU惯性测量单元集成了加速度计 陀螺仪 磁力计测量传感器测量用途加速度计三轴加速度计算倾角、检测运动陀螺仪三轴角速度计算姿态变化率磁力计可选三轴磁场航向角指南针常见芯片MPU6050入门、ICM-20948高性能、BMI088机器人主流。通信接口I2C 或 SPI。2.2 姿态解算原理陀螺仪积分得角度但会漂移误差累积加速度计能直接得倾角但不适合动态振动干扰大。所以要用数据融合互补滤波或卡尔曼滤波。互补滤波简单有效角度 0.98 × (上一角度 陀螺仪角速度 × dt) 0.02 × 加速度计角度陀螺仪负责短期准确加速度计负责长期纠偏。2.3 MPU6050 读取示例STM32 I2C// mpu6050.c读取加速度和陀螺仪原始数据I2C#includemain.h#defineMPU6050_ADDR0xD0// I2C 地址AD00 时#definePWR_MGMT_10x6B#defineACCEL_XOUT_H0x3B#defineGYRO_XOUT_H0x43// 初始化退出休眠模式voidmpu6050_init(void){uint8_tdata0x00;HAL_I2C_Mem_Write(hi2c1,MPU6050_ADDR,PWR_MGMT_1,1,data,1,100);}// 读三轴加速度 三轴陀螺仪voidmpu6050_read(int16_t*accel,int16_t*gyro){uint8_tbuf[14];HAL_I2C_Mem_Read(hi2c1,MPU6050_ADDR,ACCEL_XOUT_H,1,buf,14,100);for(inti0;i6;i){int16_tv(int16_t)((buf[i*2]8)|buf[i*21]);if(i3)accel[i]v;elsegyro[i-3]v;}}2.4 ROS2 侧IMU 数据发布Python下位机通过串口把 IMU 原始数据发给上位机上位机用imu_filter_madgwick或直接发布 sensor_msgs/Imu# imu_publisher.py把串口 IMU 数据发布为 ROS2 Imu 消息importrclpyimportserialimportstructfromrclpy.nodeimportNodefromsensor_msgs.msgimportImufromstd_msgs.msgimportHeaderclassImuPublisher(Node):def__init__(self):super().__init__(imu_publisher)self.pubself.create_publisher(Imu,/imu/data_raw,10)self.serserial.Serial(/dev/ttyUSB1,115200,timeout0.1)self.timerself.create_timer(0.02,self.tick)# 50Hzdeftick(self):# 读串口帧简化固定格式 6 个 int16lineself.ser.readline()iflen(line)12:returnax,ay,az,gx,gy,gzstruct.unpack(6h,line[:12])msgImu()msg.headerHeader()msg.header.stampself.get_clock().now().to_msg()msg.header.frame_idbase_link# 加速度单位换算原始值 / 16384 (±2g 量程)msg.linear_acceleration.xax/16384.0*9.81msg.linear_acceleration.yay/16384.0*9.81msg.linear_acceleration.zaz/16384.0*9.81# 角速度原始值 / 131 (±250°/s 量程)转 rad/smsg.angular_velocity.xgx/131.0*3.14159/180.0msg.angular_velocity.ygy/131.0*3.14159/180.0msg.angular_velocity.zgz/131.0*3.14159/180.0self.pub.publish(msg)defmain(argsNone):rclpy.init(argsargs)nodeImuPublisher()rclpy.spin(node)node.destroy_node()rclpy.shutdown()if__name____main__:main()IMU 数据在机器人中的作用里程计融合轮式里程计 IMU 姿态、机器人直立平衡、AMCL 初始姿态。三、距离传感器接入原理3.1 超声波传感器HC-SR04原理发射超声波测回波时间距离 声速 × 时间 / 2。触发引脚TRIG给 10us 高电平 回波引脚ECHO输出高电平宽度 往返时间 距离(cm) 回波时间(us) / 58// hc_sr04.cSTM32 读取距离溢出捕获测回波脉宽#includemain.hfloathc_sr04_read_distance_cm(void){// 1. TRIG 给 10us 高电平触发HAL_GPIO_WritePin(TRIG_GPIO_Port,TRIG_Pin,GPIO_PIN_SET);delay_us(10);HAL_GPIO_WritePin(TRIG_GPIO_Port,TRIG_Pin,GPIO_PIN_RESET);// 2. 测量 ECHO 高电平时间用定时器输入捕获// 伪代码启动捕获等待上升沿开始计数下降沿停止uint32_tpulse_usmeasure_echo_pulse_us();// 输入捕获实现// 3. 换算距离1us ≈ 0.017cm声速 340m/sreturnpulse_us/58.0f;}局限测量范围 2-400cm精度受温度影响对斜面/软表面不稳定。3.2 红外传感器避障红外避障传感器如 TCRT5000、红外光电开关原理发射红外光障碍物反射后接收管检测。输出是数字开关量有/无障碍适合简单的沿墙避障不提供距离数值。// 读取红外避障传感器返回 1有障碍0无障碍uint8_tir_obstacle_detect(void){// 传感器输出接 GPIO开漏需要上拉returnHAL_GPIO_ReadPin(IR_GPIO_Port,IR_Pin)GPIO_PIN_RESET?1:0;}3.3 激光雷达LiDAR激光雷达如 RPLIDAR A1、YDLIDAR X4通过旋转激光束扫描周围环境输出极坐标距离点集。它比超声波/红外精确得多是 SLAM第 21 篇和导航第 24 篇的核心传感器。接入方式USB 串口 官方驱动rplidar_ros发布sensor_msgs/LaserScan# 启动 RPLIDAR 驱动发布 /scanros2 launch rplidar_ros view_rplidar_a1_launch.py# 查看激光数据ros2 topicecho/scan--once# 输出 angle_min/angle_max/ranges 数组# RViz 可视化rviz2-drplidar_ros/rviz/view_rplidar_a1.rvizLaserScan 消息核心字段header.frame_id: laser坐标系 angle_min / angle_max / angle_increment角度范围与步进 range_min / range_max有效距离范围 ranges[]每个角度的距离值米四、摄像头接入原理4.1 摄像头在 ROS2 中的角色摄像头是机器人最重要的感知传感器目标检测、视觉导航、人机交互。在 ROS2 中通过usb_camUSB 摄像头或v4l2_cameraLinux 原生驱动发布sensor_msgs/Image和sensor_msgs/CameraInfo。# 启动 USB 摄像头驱动ros2 run usb_cam usb_cam_node_exe --ros-args\-pvideo_device:/dev/video0\-pimage_width:640-pimage_height:480\-pframerate:30# 查看图像话题ros2 topic list|grepcamera# /image_raw /camera_info ...# RViz 查看实时画面rviz2# 添加 Image 显示选择 /image_raw4.2 图像话题与 YOLO 视觉示例订阅图像做目标检测是经典应用# detect.py订阅摄像头图像做简单颜色检测OpenCVimportrclpyfromrclpy.nodeimportNodefromsensor_msgs.msgimportImagefromcv_bridgeimportCvBridgeimportcv2importnumpyasnpclassColorDetect(Node):def__init__(self):super().__init__(color_detect)self.bridgeCvBridge()self.subself.create_subscription(Image,/image_raw,self.on_image,10)defon_image(self,msg):cv_imgself.bridge.imgmsg_to_cv2(msg,bgr8)hsvcv2.cvtColor(cv_img,cv2.COLOR_BGR2HSV)# 红色范围检测maskcv2.inRange(hsv,(0,100,100),(10,255,255))contours,_cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)ifcontours:largestmax(contours,keycv2.contourArea)x,y,w,hcv2.boundingRect(largest)self.get_logger().info(fred target at ({x},{y}) size{w}x{h})defmain(argsNone):rclpy.init(argsargs)nodeColorDetect()rclpy.spin(node)rclpy.shutdown()if__name____main__:main()视觉在实体机器人第 31-34 篇和机械臂第 35-40 篇中会大量使用。五、传感器数据汇总与滤波原理5.1 传感器融合思想单传感器都有缺陷轮式里程计会打滑漂移、IMU 会积分漂移、激光雷达有噪点。机器人用传感器融合取长补短轮式里程计短期准 IMU姿态稳 激光雷达定位准 ↓ 融合算法EKF / 粒子滤波 机器人位姿估计x, y, yaw第 23 篇的 AMCL 就是基于粒子滤波的融合定位第 31-34 篇实体机器人会实战组合这些传感器。5.2 简单滤波滑动平均对传感器原始数据做滑动平均抑制噪声// moving_average.c滑动平均滤波窗口 10#defineWINDOW10floatbuf[WINDOW];intidx0;floatsum0;intfilled0;floatmoving_average(floatsample){if(filledWINDOW)sum-buf[idx];buf[idx]sample;sumsample;idx(idx1)%WINDOW;filled(filledWINDOW)?filled1:WINDOW;returnsum/filled;}滤波后的数据再进 ROS2能显著提升后续算法稳定性。六、常见问题与排障现象原因解决舵机抖动/乱转频率不对 / 供电不足确认 50Hz舵机独立供电不能和 MCU 共电源舵机转不到 180°脉宽范围不准按型号调 0.5-2.5ms 范围用attach(pin, 500, 2500)IMU 数据乱跳量程/换算不对 / 未滤波检查量程配置加互补滤波IMU 角度缓慢漂移陀螺仪零偏静止校准零偏定期重置超声波读数为 0回波测量超时增加超时判断38ms 视为无效摄像头画面卡顿USB 带宽 / 分辨率太高降低分辨率或帧率640×48030/scan 数据为空雷达被遮挡 / 串口权限检查遮挡、sudo chmod 666 /dev/ttyUSB*七、硬件控制阶段总结26-30 收官本阶段第 26-30 篇我们打通了机器人硬件全链路篇主题核心收获26嵌入式点灯GPIO 本质嵌入式开发流程27MicroROSMCU 直接成为 ROS2 节点28ROS2-Control关节控制的标准框架29电机驱动H桥/PWM/编码器/PID/硬件接口30舵机与传感器舵机、IMU、测距、视觉接入至此从电的层面GPIO、PWM、串口到信息的层面ROS2 话题、控制框架、传感器数据机器人硬件部分全部打通。八、小结与下一篇预告本篇核心舵机PWM 脉宽映射角度0.5-2.5ms → 0-180°内部自带闭环总线舵机可级联。IMU加速度计陀螺仪互补滤波融合姿态ROS2 里发布 sensor_msgs/Imu。测距三件套超声波时间测距、红外开关量、激光雷达极坐标点云SLAM 核心。摄像头usb_cam 驱动 → /image_raw → OpenCV 视觉处理。至此硬件控制阶段26-30全部收官。下一篇开始进入实体机器人阶段31-34第 31 篇「实体机器人硬件搭建原理」——把前面所有的零件底盘、电机、传感器、上位机、电池组装成一台能跑的真机器人讲解硬件选型、电气连接、电源管理、车架结构等实战知识。从纸上谈兵到真机实战最激动人心的阶段即将开始