企业官网建设流程全解析

本文是 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.c：STM32 HAL 库控制舵机（TIM1 CH1，PE9）#include"main.h"// 初始化：定时器频率 1MHz（计数周期），ARR=19999 → 20ms 周期（50Hz）voidservo_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(angle>180)angle=180;// 脉宽(us) = 500 + angle * (2500-500)/180 = 500 + angle * 11.11// CCR 值 = 脉宽(us)（因为定时器是 1MHz，1 计数 = 1us）uint32_tpulse_us=500+(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 库）#include<ESP32Servo.h>Servo myservo;voidsetup(){myservo.attach(13,500,2500);// GPIO13，脉宽 0.5~2.5msmyservo.write(90);// 回中位delay(1000);}voidloop(){// 0° → 180° → 0° 来回摆动for(intangle=0;angle<=180;angle+=2){myservo.write(angle);delay(15);}delay(500);for(intangle=180;angle>=0;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）#include"main.h"#defineMPU6050_ADDR0xD0// I2C 地址（AD0=0 时）#definePWR_MGMT_10x6B#defineACCEL_XOUT_H0x3B#defineGYRO_XOUT_H0x43// 初始化：退出休眠模式voidmpu6050_init(void){uint8_tdata=0x00;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(inti=0;i<6;i++){int16_tv=(int16_t)((buf[i*2]<<8)|buf[i*2+1]);if(i<3)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.pub=self.create_publisher(Imu,'/imu/data_raw',10)self.ser=serial.Serial('/dev/ttyUSB1',115200,timeout=0.1)self.timer=self.create_timer(0.02,self.tick)# 50Hzdeftick(self):# 读串口帧（简化：固定格式 6 个 int16）line=self.ser.readline()iflen(line)<12:returnax,ay,az,gx,gy,gz=struct.unpack('<6h',line[:12])msg=Imu()msg.header=Header()msg.header.stamp=self.get_clock().now().to_msg()msg.header.frame_id='base_link'# 加速度单位换算：原始值 / 16384 (±2g 量程)msg.linear_acceleration.x=ax/16384.0*9.81msg.linear_acceleration.y=ay/16384.0*9.81msg.linear_acceleration.z=az/16384.0*9.81# 角速度：原始值 / 131 (±250°/s 量程)，转 rad/smsg.angular_velocity.x=gx/131.0*3.14159/180.0msg.angular_velocity.y=gy/131.0*3.14159/180.0msg.angular_velocity.z=gz/131.0*3.14159/180.0self.pub.publish(msg)defmain(args=None):rclpy.init(args=args)node=ImuPublisher()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.c：STM32 读取距离（溢出捕获测回波脉宽）#include"main.h"floathc_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_us=measure_echo_pulse_us();// 输入捕获实现// 3. 换算距离：1us ≈ 0.017cm（声速 340m/s）returnpulse_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 驱动（发布 /scan）ros2 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.rviz

LaserScan 消息核心字段：

header.frame_id: laser（坐标系） angle_min / angle_max / angle_increment：角度范围与步进 range_min / range_max：有效距离范围 ranges[]：每个角度的距离值（米）

四、摄像头接入（原理）

4.1 摄像头在 ROS2 中的角色

摄像头是机器人最重要的感知传感器：目标检测、视觉导航、人机交互。在 ROS2 中通过usb_cam（USB 摄像头）或v4l2_camera（Linux 原生）驱动，发布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_raw

4.2 图像话题与 YOLO 视觉示例

订阅图像做目标检测是经典应用：

# detect.py：订阅摄像头图像做简单颜色检测（OpenCV）importrclpyfromrclpy.nodeimportNodefromsensor_msgs.msgimportImagefromcv_bridgeimportCvBridgeimportcv2importnumpyasnpclassColorDetect(Node):def__init__(self):super().__init__('color_detect')self.bridge=CvBridge()self.sub=self.create_subscription(Image,'/image_raw',self.on_image,10)defon_image(self,msg):cv_img=self.bridge.imgmsg_to_cv2(msg,'bgr8')hsv=cv2.cvtColor(cv_img,cv2.COLOR_BGR2HSV)# 红色范围检测mask=cv2.inRange(hsv,(0,100,100),(10,255,255))contours,_=cv2.findContours(mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)ifcontours:largest=max(contours,key=cv2.contourArea)x,y,w,h=cv2.boundingRect(largest)self.get_logger().info(f'red target at ({x},{y}) size{w}x{h}')defmain(args=None):rclpy.init(args=args)node=ColorDetect()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];intidx=0;floatsum=0;intfilled=0;floatmoving_average(floatsample){if(filled==WINDOW)sum-=buf[idx];buf[idx]=sample;sum+=sample;idx=(idx+1)%WINDOW;filled=(filled<WINDOW)?filled+1:WINDOW;returnsum/filled;}

滤波后的数据再进 ROS2，能显著提升后续算法稳定性。

六、常见问题与排障

七、硬件控制阶段总结（26-30 收官）

本阶段（第 26-30 篇）我们打通了机器人硬件全链路：

至此，从"电"的层面（GPIO、PWM、串口）到"信息"的层面（ROS2 话题、控制框架、传感器数据），机器人硬件部分全部打通。

八、小结与下一篇预告

本篇核心：

1. 舵机：PWM 脉宽映射角度（0.5-2.5ms → 0-180°），内部自带闭环，总线舵机可级联。
2. IMU：加速度计+陀螺仪，互补滤波融合姿态，ROS2 里发布 sensor_msgs/Imu。
3. 测距三件套：超声波（时间测距）、红外（开关量）、激光雷达（极坐标点云，SLAM 核心）。
4. 摄像头：usb_cam 驱动 → /image_raw → OpenCV 视觉处理。

至此硬件控制阶段（26-30）全部收官。下一篇开始进入实体机器人阶段（31-34）：第 31 篇「实体机器人硬件搭建（原理）」——把前面所有的"零件"（底盘、电机、传感器、上位机、电池）组装成一台能跑的真机器人，讲解硬件选型、电气连接、电源管理、车架结构等实战知识。从"纸上谈兵"到"真机实战"，最激动人心的阶段即将开始！