浙江省机器人竞赛“空中机器人”赛项系列文章（五）：颜色识别和精准抓投

经历了自主飞行、动态避障和高速穿门的重重考验你的空中机器人已抵达省赛任务的最后环节——精准抓取与投放。这是对无人机稳定悬停精度、视觉识别鲁棒性和任务级决策能力的终极考核。无人机必须在A区识别并抓取指定颜色的球体然后在B区将其精准投放至对应颜色的得分框最后自主降落在相应颜色的H区。任何一个环节的失误都可能导致前功尽弃。灵智实验室的系列收官之作将为你彻底解析“颜色识别-定位-抓取-投放”的全链路技术。本文将以rgb_landing.py代码为核心深入讲解如何从下视相机图像中稳定识别颜色块并解算其精确三维坐标并在此基础上构建完整的抓取与投放任务决策框架。第一部分任务分解与系统总览省赛的抓投任务可分解为以下关键步骤其核心是视觉感知与位姿计算的闭环1.A区悬停与搜索无人机飞抵A区上方保持稳定悬停。2.颜色识别与定位利用下视单目相机识别红、绿、蓝三种颜色的球体或色块并计算目标在全局坐标系NED下的精确三维坐标。这是整个任务的技术基石。3.精准下降与抓取基于计算出的坐标控制无人机下降至抓取高度触发抓取机构。4.携物飞行至B区携带目标球体飞向B区。5.投放区识别与定位在B区再次使用视觉识别投放框的颜色。6.精准悬停与投放将目标球体投放至颜色匹配的得分框中。7.自主返航与颜色降落最后根据得分框颜色自主飞向并降落在对应颜色的H区。rgb_landing.py代码正是解决了其中最核心的第2步颜色识别与全局定位。它不仅仅是一个视觉检测节点更是一个完整的视觉-惯性定位系统能将图像中的一个像素点映射为无人机可以飞抵的全局坐标。第二部分代码深度解析——从图像像素到全局坐标ColorBlockDetector类是整个系统的核心。让我们深入其关键部分理解其如何实现高精度的视觉定位。2.1 传感器数据同步状态估计的基础无人机在任何时刻的位置和姿态是进行视觉定位的先决条件。代码通过两个订阅者获取这些信息# 订阅PX4发布的无人机姿态四元数表示从NED系到机体FRD系的旋转 self.att_sub self.create_subscription(VehicleAttitude, /fmu/out/vehicle_attitude, self.vehicle_attitude_callback, qos) # 订阅PX4发布的无人机局部位置NED坐标系原点为Home点 self.pos_sub self.create_subscription(VehicleLocalPosition, /fmu/out/vehicle_local_position_v1, self.vehicle_local_position_callback, qos)self.latest_q存储当前机体坐标系相对于NED坐标系的旋转四元数。这是将机体坐标系下的向量转换到全局坐标系的关键。self.latest_pos存储无人机在NED坐标系中的当前位置 [x, y, z]。注意在NED坐标系中高度 z向下为正因此无人机离地高度为 -z。2.2 视觉感知核心颜色块的检测与提取在 image_callback函数中系统对每一帧下视图像进行处理颜色空间转换与阈值分割hsv cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) mask cv2.inRange(hsv, lower, upper)代码将图像从BGR转换到HSV颜色空间这比RGB空间对光照变化更鲁棒。通过预先定义好的HSV上下阈值例如红色为[0,100,100]到[10,255,255]和[170,100,100]到[180,255,255]生成一个二值掩膜其中白色区域即目标颜色区域。2. 轮廓查找与中心计算contours, _ cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) largest_contour max(contours, keycv2.contourArea) M cv2.moments(largest_contour) cX int(M[m10] / M[m00]) cY int(M[m01] / M[m00])在掩膜上查找轮廓并选取面积最大的轮廓假设一个色块通过计算图像矩Moments得到其像素中心坐标 (cX, cY)。此坐标是后续几何计算的输入。2.3 几何计算的精髓compute_target_position函数详解这是整个代码最核心、最精妙的部分。它解决了“如何通过一个2D像素点计算出目标在3D世界中的位置”这一经典问题。其原理是基于已知高度的平面地面进行射线与平面求交。下图清晰地展示了从像素到全局坐标的完整坐标变换链条关键参数与步骤1. (fx, fy, cx, cy)这些参数描述了相机的成像几何通过标定得到。它们用于将像素坐标转换到相机坐标系下的归一化射线方向向量d_cam。2. 相机到机体的外参(R_cam_to_body, trans_body_to_cam)R_cam_to_body相机坐标系假设为RDF: Right-Down-Forward到机体坐标系FRD: Forward-Right-Down的旋转矩阵。此矩阵取决于相机的安装朝向。代码中的矩阵对应相机绕Z轴旋转了-90度。trans_body_to_cam相机光学中心在机体坐标系下的平移量。这是一个重要的杆臂值忽略它会导致定位系统误差。机体到NED的旋转(R_body_to_ned)由当前姿态四元数 self.latest_q计算得到的旋转矩阵。它将机体坐标系下的向量转换到北东地NED全局坐标系。射线平面求交计算相机光学中心在NED系中的位置cam_pos drone_pos R_body_to_ned * t_b2c。将射线方向转换到NED系d_ned R_body_to_ned * d_body。建立射线参数方程P(t) cam_pos t * d_ned。假设目标位于地面NED系中 Z0的平面代入方程求解参数 t -cam_pos.z / d_ned.z。将 t代回方程即得到目标点的完整NED坐标 target_pos。最终代码为每个识别到的颜色块发布一个 TrajectorySetpoint消息到对应话题如 /rgb_block/red其中包含了计算出的目标点平面坐标 [x, y, 0]高度设置为0地面。第三部分从“看到”到“抓到”——构建抓取与投放任务框架有了精确的目标坐标我们就可以设计上层的抓取与投放任务逻辑。这通常由一个状态机State Machine来驱动。以下是一个基于 offboard_control_lib和 rgb_landing.py的集成示例import rclpy import math import time from enum import Enum from geometry_msgs.msg import PoseStamped from offboard_control_lib import Vehicle class TaskState(Enum): SEARCH_A 1 DESCEND_TO_GRAB 2 GRAB 3 ASCEND_AND_FLY_TO_B 4 SEARCH_B 5 DESCEND_TO_DROP 6 DROP 7 RETURN_AND_LAND 8 class AerialManipulationTask: def __init__(self): self.vehicle Vehicle() # 假设我们有一个节点订阅 /rgb_block/red 等话题获取目标位置 self.target_position {red: None, green: None, blue: None} self.current_color_target red # 假设任务要求抓取红色 self.state TaskState.SEARCH_A self.grab_height 0.3 # 抓取高度距离地面0.3米 self.cruise_height 1.5 # 巡航高度 def run(self): self.vehicle.drone.arm() self.vehicle.drone.takeoff(self.cruise_height) try: while rclpy.ok(): if self.state TaskState.SEARCH_A: print(状态: 在A区搜索目标...) # 飞往A区中心 self.vehicle.drone.fly_to_trajectory_setpoint(1.0, 1.0, self.cruise_height, 0) # 在此悬停等待视觉识别结果 time.sleep(3) if self.target_position[self.current_color_target] is not None: target self.target_position[self.current_color_target] print(f找到{self.current_color_target}目标坐标: {target}) self.state TaskState.DESCEND_TO_GRAB else: print(未找到目标执行搜索模式...) # 可加入小范围搜索路径 continue elif self.state TaskState.DESCEND_TO_GRAB: print(状态: 下降至抓取高度...) target self.target_position[self.current_color_target] # 飞向目标正上方然后下降 self.vehicle.drone.fly_to_trajectory_setpoint(target[0], target[1], self.cruise_height, 0) self.vehicle.drone.fly_to_trajectory_setpoint(target[0], target[1], self.grab_height, 0) self.state TaskState.GRAB elif self.state TaskState.GRAB: print(状态: 执行抓取...) # 触发抓取机构例如通过发送服务请求或发布话题 # self.gripper_control(grabTrue) time.sleep(1) # 等待抓取完成 print(抓取完成。) self.state TaskState.ASCEND_AND_FLY_TO_B elif self.state TaskState.ASCEND_AND_FLY_TO_B: print(状态: 爬升并飞向B区...) # 先爬升至安全高度 self.vehicle.drone.fly_to_trajectory_setpoint(target[0], target[1], self.cruise_height, 0) # 飞往B区中心 self.vehicle.drone.fly_to_trajectory_setpoint(4.0, 3.0, self.cruise_height, 0) self.state TaskState.SEARCH_B elif self.state TaskState.SEARCH_B: print(状态: 在B区搜索对应颜色投放框...) # 悬停等待视觉识别B区颜色 time.sleep(3) # 假设我们通过另一个视觉节点获得了B区对应颜色框的位置 drop_target self.get_drop_position(self.current_color_target) if drop_target: self.state TaskState.DESCEND_TO_DROP else: # 处理异常 pass # ... 其余状态类似处理 elif self.state TaskState.RETURN_AND_LAND: print(状态: 返回并降落在对应颜色H区...) # 根据self.current_color_target确定降落H区坐标 landing_spot self.get_landing_spot(self.current_color_target) self.vehicle.drone.fly_to_trajectory_setpoint(landing_spot[0], landing_spot[1], self.cruise_height, 0) self.vehicle.drone.land() print(任务完成) break time.sleep(0.1) # 主循环延迟 except KeyboardInterrupt: print(任务被中断紧急降落。) self.vehicle.drone.land() finally: self.vehicle.close() def get_drop_position(self, color): 模拟获取B区对应颜色投放框的位置 # 在实际系统中这里会订阅另一个视觉节点发布的消息 drop_zones {red: (4.2, 3.2), green: (4.0, 3.0), blue: (3.8, 3.2)} return (*drop_zones[color], 0.0) # 返回 (x, y, 0) def get_landing_spot(self, color): 根据颜色返回H区降落点坐标 landing_spots {red: (0.2, 0.2), green: (0.0, 0.0), blue: (-0.2, 0.2)} return (*landing_spots[color], 0.0)第四部分实战优化与挑战应对在实际比赛中仅有基础功能是不够的。你需要考虑以下进阶优化点多目标处理与选择A区可能同时存在多个同色球体。代码目前选择面积最大的轮廓。更优策略是结合任务上下文如抓取最靠近中心的、或未抓取过的进行选择并需要为目标分配唯一ID进行跟踪。视觉伺服与动态调整在下降抓取过程中目标在图像中可能移动。应采用视觉伺服在下降的同时根据图像反馈实时微调无人机位置确保始终对准目标。3.抓取与投放的鲁棒性a抓取确认在触发抓取指令后应通过力传感器或视觉反馈确认抓取成功。如果失败需重新尝试。b投放确认同样投放后可通过视觉判断球体是否已离开抓取机构或通过舱内传感器确认。异常处理与状态恢复状态机必须包含完备的异常处理。例如在抓取过程中目标丢失应悬停并重新搜索通信中断应执行紧急降落程序。传感器融合提升精度在近距离抓取时单目视觉的深度估计误差会被放大。可考虑融合激光雷达或超声波的精确高度信息或使用AprilTag等人工标记来提供更稳定的相对位姿。总结从单点技术到系统集成至此灵智实验室的“浙江省大学生机器人竞赛‘空中机器人’赛项”系列文章已全部完结。我们从仿真环境搭建启航历经自主飞行控制、动态避障决策、竞速门高速穿越直至本篇的颜色识别与精准抓投系统性地拆解了夺冠之路上的每一个核心技术环节。回顾整个系列我们希望你掌握的不仅是五个独立的技能点更是一套完整的机器人系统集成思维感知是眼睛通过3D激光雷达、单目/下视相机让机器人理解环境。决策是大脑通过状态机、DWA、路径规划等算法让机器人知道“下一步该做什么”。控制是四肢通过PX4 Offboard模式、精准的位置与速度指令让机器人稳定、准确地执行动作。仿真与调试是练兵场在虚拟世界中以“零成本、零风险”的方式进行无数次的迭代与优化。将这四者无缝融合你的空中机器人便不再是简单的飞行器而是一个具备高度自主性的智能体足以应对省赛乃至更复杂场景下的所有挑战。最后预祝你在浙江省大学生机器人竞赛的舞台上代码凌云一飞冲天灵智实验室始终致力于成为技术创新者背后的坚实力量。本系列所有涉及的开源代码、仿真模型与详细文档均可在我们的开源社区获取。竞赛有终点但探索无止境。期待与你在更广阔的机器人技术领域相遇、合作、共创未来。