从原理到实践：ROS 状态机在机器人竞赛中的实时控制设计

ROS状态机在机器人竞赛中的应用

原创于 2025-10-18 00:00:10 发布 · 677 阅读

12 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #学习 #机器人

ROS（Robot operating system) 是机器人的操作系统（Android、IOS），其中各节点主要通过话题、服务等机制进行实时消息的传输与通信，其核心要义可用下图较好的概括。

ROS实时通信的基础为话题通信（和其他），其运作机理为基于传感器信息的实时传输，以固定帧率执行回调函数（每次接受传感器数据就执行）。实时数据是天然的自变量，其基于时间的变化刚好可以作为状态机状态切换的标志，今天分享一下作为一个ROS初学者，在机器人竞赛中的状态机代码

一、ROS 实时通信的原理

ROS 的通信模型以节点（Node）为基本单元，节点之间通过话题（Topic）、服务（Service）与动作（Action）等机制交换信息。

话题（Topic）：用于连续数据流的发布与订阅，例如图像、激光雷达数据。
服务（Service）：适合请求-响应类型的通信，例如参数查询或单次任务触发。
动作（Action）：适用于需要持续反馈的任务控制，如导航或机械臂动作。

在该项目中，主节点负责状态机逻辑，而感知与控制模块各自以独立线程实时订阅话题，实现异步通信。例如：

rospy.Subscriber("/usb_cam_1/image",Image, update_camera_image) rospy.Subscriber("/scan", LaserScan, update_lidar_data) pub = rospy.Publisher('/ackermann_cmd', AckermannDriveStamped, queue_size=1)

📡 解读：
图像数据与激光雷达数据通过回调函数实时更新，而车辆的控制命令（速度、转角）则通过 AckermannDriveStamped 消息发布。
这种“非阻塞式”话题通信让机器人能够在毫秒级内响应环境变化，满足竞赛中对实时性的高要求。

二、状态机（State Machine）逻辑设计

状态机是整个系统的“决策大脑”，它根据感知模块传来的环境信息切换行为状态。

项目中共定义了六个状态：

IDLE = 'IDLE' WAITING_FOR_GREEN_LIGHT = 'WAITING_FOR_GREEN_LIGHT' LINE_FOLLOWING = 'LINE_FOLLOWING' OBSTACLE_AVOIDANCE = 'OBSTACLE_AVOIDANCE' STOPPED_AT_INTERSECTION = 'STOPPED_AT_INTERSECTION' INTERSECTION_MANEUVER = 'INTERSECTION_MANEUVER'

核心逻辑如下：

等待绿灯 → 绿灯亮起或超时 → 进入巡线状态
巡线行驶 → 检测障碍物 → 进入避障状态
巡线行驶 → 检测停止线 → 进入路口停止
停止 → 等待计时结束 → 执行路口动作
完成转向 → 回到 巡线状态

简化版伪代码如下：

if state == WAITING_FOR_GREEN_LIGHT and light_is_green(): state = LINE_FOLLOWING elif state == LINE_FOLLOWING and detect_obstacle(): state = OBSTACLE_AVOIDANCE elif state == OBSTACLE_AVOIDANCE and avoidance_done(): state = LINE_FOLLOWING elif state == STOPPED_AT_INTERSECTION and timeout(): state = INTERSECTION_MANEUVER

🧠 对比说明：
传统的“条件判断式控制”容易出现逻辑混乱，而使用状态机可以让每个行为逻辑独立清晰，便于调试和扩展。例如在后续加入“手动干预”或“交通标志识别”时，只需增加新的状态分支。

三、感知模块：实时输入的处理逻辑

感知模块（PerceptionModule）负责从相机与雷达中提取环境信息。核心任务包括：

红绿灯识别
车道线检测
停止线识别
障碍物检测

其中红绿灯检测核心函数如下：

deflight_detection(origin_img): hsv = cv2.cvtColor(origin_img, cv2.COLOR_BGR2HSV) red_mask = cv2.inRange(hsv, (0,127,127), (20,255,255)) green_mask = cv2.inRange(hsv, (35,50,50), (85,255,255)) count_red = cv2.countNonZero(red_mask) count_green = cv2.countNonZero(green_mask) return1if count_green > 15000else0

📸 原理解析：
通过 HSV 颜色空间提取特定颜色区域（红或绿），再统计像素数量判断灯色。
与深度学习识别不同，这种基于传统视觉算法的方案具有响应快、计算轻量的优势，更适合资源受限的嵌入式系统。

四、控制模块：从感知到运动

控制模块（ControlModule）将感知结果转化为车辆运动指令。其主要功能包括：

车道保持（Line Following）
避障（Obstacle Avoidance）
路口机动（Intersection Maneuver）

下面是巡线控制的核心逻辑：

lane_offset = top_centroid[0] - bottom_centroid[0] center_offset = lane_center - self.image_center angular_component = self.kp_angle * (-lane_offset/100.0) lateral_component = self.kp_lateral * (-center_offset/100.0) steering_angle = angular_component + 1.0 msg.drive.steering_angle = steering_angle msg.drive.speed = linear_x self.publisher.publish(msg)

🚗 原理解释：
通过图像识别到的车道中心点偏移计算舵机转角，并通过 PID 参数控制偏差修正。
与简单的“线性映射”不同，该系统将横向偏移与角度误差分离计算，使车辆在复杂曲线中行驶更平稳。

五、避障与状态切换机制

当雷达检测到前方障碍物时，系统进入避障状态。代码中通过时间控制确保动作执行完整：

if self.obstacle_avoidance_state == 'TURN_LEFT': msg.drive.steering_angle = self.obstacle_avoidance_steer_left if current_time - self.avoidance_start_time > 1.5: self.obstacle_avoidance_state = 'MOVE_FORWARD'

🕹 机制解析：
相比依赖路径规划算法的避障方式，这种基于状态时间控制的方案简单且实时性强。虽然精度较低，但在竞赛场地（如固定路线）中更可靠，调试成本更低。