经历了自主飞行、动态避障和高速穿门的重重考验，你的空中机器人已抵达省赛任务的最后环节——精准抓取与投放。这是对无人机稳定悬停精度、视觉识别鲁棒性和任务级决策能力的终极考核。无人机必须在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函数中，系统对每一帧下视图像进行处理：

1. 颜色空间转换与阈值分割：

   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, key=cv2.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系中 Z=0的平面），代入方程求解参数 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(grab=True)
                    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)

第四部分：实战优化与挑战应对 

在实际比赛中，仅有基础功能是不够的。你需要考虑以下进阶优化点：

1. 多目标处理与选择：A区可能同时存在多个同色球体。代码目前选择面积最大的轮廓。更优策略是结合任务上下文（如抓取最靠近中心的、或未抓取过的）进行选择，并需要为目标分配唯一ID进行跟踪。
2. 视觉伺服与动态调整：在下降抓取过程中，目标在图像中可能移动。应采用视觉伺服，在下降的同时根据图像反馈实时微调无人机位置，确保始终对准目标。

3.抓取与投放的鲁棒性：

a   抓取确认：在触发抓取指令后，应通过力传感器或视觉反馈确认抓取成功。如果失败，需重新尝试。

b   投放确认：同样，投放后可通过视觉判断球体是否已离开抓取机构，或通过舱内传感器确认。

1. 异常处理与状态恢复：状态机必须包含完备的异常处理。例如，在抓取过程中目标丢失，应悬停并重新搜索；通信中断应执行紧急降落程序。
2. 传感器融合提升精度：在近距离抓取时，单目视觉的深度估计误差会被放大。可考虑融合激光雷达或超声波的精确高度信息，或使用AprilTag等人工标记来提供更稳定的相对位姿。

总结：从单点技术到系统集成 

至此，灵智实验室的“浙江省大学生机器人竞赛‘空中机器人’赛项”系列文章已全部完结。我们从仿真环境搭建启航，历经自主飞行控制、动态避障决策、竞速门高速穿越，直至本篇的颜色识别与精准抓投，系统性地拆解了夺冠之路上的每一个核心技术环节。

回顾整个系列，我们希望你掌握的不仅是五个独立的技能点，更是一套完整的机器人系统集成思维：

感知是眼睛：通过3D激光雷达、单目/下视相机，让机器人理解环境。

决策是大脑：通过状态机、DWA、路径规划等算法，让机器人知道“下一步该做什么”。

控制是四肢：通过PX4 Offboard模式、精准的位置与速度指令，让机器人稳定、准确地执行动作。

仿真与调试是练兵场：在虚拟世界中以“零成本、零风险”的方式，进行无数次的迭代与优化。

将这四者无缝融合，你的空中机器人便不再是简单的飞行器，而是一个具备高度自主性的智能体，足以应对省赛乃至更复杂场景下的所有挑战。

最后，预祝你在浙江省大学生机器人竞赛的舞台上，代码凌云，一飞冲天！

灵智实验室，始终致力于成为技术创新者背后的坚实力量。本系列所有涉及的开源代码、仿真模型与详细文档，均可在我们的开源社区获取。竞赛有终点，但探索无止境。期待与你在更广阔的机器人技术领域相遇、合作、共创未来。