★ FEATURED ARTICLE 简介一套基于ROS与PX4的无人机物体识别与精准投放工程面向无人机竞赛、智能物流投放课程设计及ROS开发者解决从YOLO目标检测到无人机自主悬停对准、舵机投放的完整闭环。工程已实际跑通经实测效果良好可直接下载使用作者另附详细教程与视频演示便于对照学习。压缩包内共125个文件体积约1.22MB文件类型覆盖头文件、C与Python节点源、launch启动脚本、消息定义、参数配置及XML包描述等其中头文件用于接口声明节点完成图像识别、状态估计与指令生成launch文件可一键启动多模块便于快速调试与部署。目录结构清晰适合作为ROS功能包引入现有工作空间坐标发布与指令转换采用松耦合设计方便替换检测算法或飞行平台。目前已有1180人学习资源热度较高。通过该资源可掌握视觉识别结果生成控制指令的思路理解最小误差判定与舵机投放策略并复现一套可扩展的无人机投放原型适合具备一定ROS基础、希望完成识别与动作联动项目的开发者。1. 基于ROS无人机的物体识别与精准投放这套参考工程把识别到投放的“最后一跳”补齐了做无人机识别投放的人最容易卡住的不是YOLO模型训练而是“识别到了”和“投放得准”之间那一段链路像素坐标怎么换成机体坐标、置信度波动怎么处理、投放指令什么时机给、给了之后无人机往哪偏。这套基于ROS无人机、采用YOLO做物体识别的工程参考把Gazebo仿真环境、机载识别节点、坐标变换、投放执行机构控制串成了完整闭环拿来就能改、能跑、能复现。适合正在做竞赛方案、课程设计、毕业设计或想入门无人机视觉抓取的工程师新手照着走能把整条链路跑通熟手可以直接抄坐标变换与时延补偿那部分实现省掉一个月的踩坑时间。2. 仿真与机载环境搭建GazeboMAVROS先把识别投放链路跑起来2.1 仿真环境基线摄像头视角、分辨率与视场角决定识别上限我一般先搭建仿真环境不是因为实机买不起而是因为识别到投放这条链路的调试非常依赖“可重复性”。实机飞一次要充电、要等GPS收敛、要防炸机而在Gazebo里同样的场景可以反复跑这对调坐标变换和投放时机至关重要。工程参考里默认用的是PX4固件 MAVROS Gazebo的组合这套组合在无人机视觉类课程设计和竞赛里最常见资料也最好找。搭建基线的核心不是把无人机飞起来而是把机载摄像头仿真视角调“像那么回事”。默认的无人机模型一般不带摄像头需要手动在sdf模型定义里加一个摄像插件。参考工程里给了一个可以直接用的摄像头配置片段我简化后放在下面sensor typecamera namemain_camera pose0.10 0 0.05 0 0 0/pose topicmain_camera/image_raw/topic camera horizontal_fov1.0472/horizontal_fov image width1280/width height720/height /image clip near0.1/near far100.0/far /clip /camera update_rate30.0/update_rate /sensor这段配置的作用是给无人机加一个前视摄像头pose里0.10是摄像头在机体前方10厘米处的安装位置水平视场角1.0472弧度也就是60度图像分辨率1280x720更新频率30帧。分辨率与视场角这两个参数直接决定模型能看清多小的目标比如在3米高度识别一个10厘米的物体1280x720加60度视场角能留下足够像素如果降到640x480就会变得很勉强。更新频率影响的是后续时延补偿的精度30帧意味着每帧间隔约33毫秒这个值后面计算投放提前量时要用到。摄像头插件配好后用roslaunch启动整套仿真环境。参考工程里提供了一个launch文件模板核心内容是同时拉起Gazebo世界、无人机模型、MAVROS和摄像头驱动launch arg nameworld default$(find ros_uav_project)/worlds/object_drop.world/ node namegazebo pkggazebo_ros typegazebo args$(arg world) outputscreen/ node namemavros pkgmavros typemavros_node outputscreen param namefcu_url valueudp://:14550127.0.0.1:14550/ /node node namecamera_repub pkgros_uav_project typecamera_repub.py outputscreen/ /launch需要注意fcu_url参数它指定MAVROS与飞控通信的地址和端口。仿真环境下用UDP连本机14550端口实机则是串口地址和波特率。如果这里没配对MAVROS会一直连不上飞控无人机的状态估计和位置控制全部失效识别节点再看清也没有意义。这一层属于“看不见但决定生死”的配置新手通常在这里卡一晚熟手看到udp://字样的第一反应也是先检查端口是不是被占用。2.2 YOLO识别模块怎么接入ROS检测结果不要直接发图像要发结构化消息仿真环境起来之后第二步是让YOLO识别节点跑起来。很多初学者会把识别与投放做成“全部放在一个节点里识别完就调用投放函数”这在单次实验里没问题但一上真机就暴露问题图像处理线程和飞控控制线程耦合在一起图像卡顿会导致投放时序错乱。参考工程的做法是把识别结果转成独立的话题投放决策节点单独订阅这个话题这样两边可以独立调试也方便用rosbag记录和回放分析。自定义一个检测结果消息类型是第一步。工程里定义了一个Detection.msg字段如下string class_name float32 confidence float32 u float32 v float32 width float32 height builtin_interfaces/Time headeru、v是检测框在图像像素坐标系下的中心坐标width和height是检测框的像素宽高。把像素坐标单独拎出来而不是直接丢给后续逻辑是因为投放决策真正依赖的是这个中心点在机体坐标系下的位置而像素坐标还要经过一步坐标变换单独成字段便于排查到底是识别错了还是变换错了。YOLO识别节点的代码骨架如下import rclpy from rclpy.node import Node from sensor_msgs.msg import Image from vision_msgs.msg import Detection2DArray from ros_uav_project.msg import Detection import cv2 import numpy as np class YoloDetector(Node): def __init__(self): super().__init__(yolo_detector) self.publisher self.create_publisher(Detection, /detection_result, 10) self.subscription self.create_subscription( Image, /main_camera/image_raw, self.image_callback, 10) # 这里加载训练好的YOLO权重与类别文件 self.net cv2.dnn.readNet(yolov5s.weights, yolov5s.cfg) self.net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV) def image_callback(self, msg): frame self.cv_image(msg) detections self.infer(frame) for det in detections: detection_msg Detection() detection_msg.class_name det[name] detection_msg.confidence float(det[confidence]) detection_msg.u float(det[center_x]) detection_msg.v float(det[center_y]) detection_msg.width float(det[width]) detection_msg.height float(det[height]) self.publisher.publish(detection_msg) def cv_image(self, msg): # 用numpy把ROS图像消息转成OpenCV图像 return np.frombuffer(msg.data, dtypenp.uint8).reshape(msg.height, msg.width, 3)逻辑说明节点订阅仿真相机发布的图像话题每一帧先转成OpenCV格式再用cv2.dnn加载YOLO权重推理最后把推理得到的类别、置信度、像素中心点、宽高打包成自定义Detection消息发布出去。这里没有把整张图像发出去只发结构化数据话题负载小后续节点处理快。参数说明有两个值得注意。第一个是cv2.dnn.readNet的权重路径工程模板里带的是YOLOv5的格式如果你用的是YOLOv8的onnx模型需要改成cv2.dnn.readNet(best.onnx)并在infer函数里调整输出层的解析方式。第二个是置信度阈值默认没写全实际使用时要根据场景设一个经验值比如0.35到0.5之间太低会频繁误检太高会漏检小目标。2.3 坐标变换从像素坐标到机体坐标TF树少一环都不行识别结果拿到的是像素坐标u、v而投放需要的是目标相对无人机的三维位置。这中间要经过两段坐标变换第一段是像素坐标转相机坐标系下的归一化坐标第二段是相机坐标系转机体坐标系。第二段变换在工程里往往被忽略很多人直接把相机坐标当成机体坐标用导致投放点在机身前方偏了十几厘米。像素坐标转相机系归一化坐标用到了针孔相机模型。已知相机内参fx、fy、cx、cy目标在相机系下的方向向量可以这样求import numpy as np fx 600.0 # x方向焦距单位像素来自相机标定 fy 600.0 # y方向焦距 cx 640.0 # 主点x坐标 cy 360.0 # 主点y坐标 def pixel_to_camera(u, v, depth): x (u - cx) / fx * depth y (v - cy) / fy * depth z depth return np.array([x, y, z])逻辑说明这个函数把检测框中心像素坐标还原成相机系下的三维坐标点depth是对应像素的深度值。在仿真环境里可以直接读取深度相机话题实机上如果没有测距传感器最常用的做法是假设目标在地面上、用无人机当前高度减去一个经验落差来估算depth这个估算值会有误差因此后面投放要留容忍范围。相机系到机体系的变换标准做法是监听TF树。通常在无人机模型里有一个从机体坐标系base_link到相机坐标系camera_link的静态变换需要先发布这个静态变换再用TF监听器实时拿到变换矩阵。参考工程里发布的静态TF如下node pkgtf2_ros typestatic_transform_publisher namecamera_static_tf args0.10 0 0.05 0 0 0 base_link camera_link/这段配置的含义是camera_link相对于base_link的位置是前方10厘米、上方5厘米姿态无旋转。数值必须和Gazebo里摄像头插件的pose完全一致否则识别结果经过坐标变换后会整体偏移。很多工程里摄像头装好了但TF没发布或者TF发布了但pose对不上这种“检查不出来的玄学误差”很难排查最好的习惯是做一个节点每隔一秒打印一次目标在base_link下的坐标看数值是否合理。我在排查这类问题时会在目标正前方放一个已知位置的标记物飞过去识别然后输出机体系坐标与真值对比。偏差在5厘米以内说明坐标变换链路正常超过这个数优先检查静态TF的pose和相机内参。3. 精准投放怎么做投放机构选型、判决逻辑与执行代码3.1 投放机构选型电磁释放、舵机推杆、气压式怎么选识别链路打通后投放机构是另一大块。很多人在仿真里跑通识别就以为完成了实际上机构的机械响应时间、控制信号类型都会影响投放精度。参考工程里没有指定唯一机构而是给出了三种常见方案的对比方便根据负载和成本选型。机构类型控制信号典型响应时间适用负载改装难度电磁释放器GPIO高低电平10-50ms50g以下小物体低只需一个MOS管舵机推杆PWM 50Hz100-200ms200g以下中需要固定底座气压推杆电磁阀气缸30-80ms500g以下高需要气瓶或气泵从投放精度角度看电磁释放器响应最快但只能放不含铁磁性外壳的物体舵机推杆通用性强PWM控制代码最好写但响应时间最长比较适合对投放时机要求不高的场景气压式响应速度和负载能力都居中代价是机构复杂度高。我在课程设计里最推荐先做舵机推杆因为PWM控制可以直接复用飞控的AUX口而且舵机动作的起点和终点都容易用限位块固定便于做重复实验。舵机推杆的PWM控制本身不复杂但要先确认飞控的AUX口是否已经映射到MAVROS的actuator控制话题。常见的控制方式是在MAVROS里发送MAV_CMD_DO_SET_ACTUATOR命令或者直接连接一个独立的串口舵机控制板后者更简单可靠与飞控完全隔离不容易因为误发指令导致飞控异常。3.2 投放判决逻辑置信度、触发保持时间与投放窗口有了识别结果和投放机构剩下最关键的逻辑是“什么时候投”。我见过最多的错误写法是检测到目标置信度超过阈值马上触发投放。这种做法在静止画面里看着没问题一旦无人机悬停有姿态波动检测框中心点会在目标附近来回跳视频流某一帧误检到别的东西就直接把物体投出去了等飞控反应过来已经晚了。参考工程里的判决逻辑是“两段式确认”第一段是置信度平滑用指数移动平均抑制单帧抖动第二段是连续帧确认连续N帧检测到同一类目标且中心点像素位移小于阈值才认为目标稳定。最后还有一个投放窗口只有无人机当前水平位置与目标位置的距离小于规定值才允许发送投放指令。class DropDecision: def __init__(self): self.ema_confidence 0.0 self.confirm_count 0 self.required_confirms 5 self.confidence_threshold 0.45 self.enabled False def update(self, detection): # 置信度做EMA平滑alpha取0.3 self.ema_confidence 0.7 * self.ema_confidence 0.3 * detection.confidence if self.ema_confidence self.confidence_threshold: self.confirm_count 0 return False self.confirm_count 1 if self.confirm_count self.required_confirms: self.enabled True return True return False逻辑说明先把当前帧置信度与历史平滑值做指数移动平均再和阈值比较。这样单帧误检的尖峰会被平滑掉不会立刻触发。连续5帧平滑置信度都达标才把enabled置位。置位后还要经过投放窗口判断——查看当前无人机位置与目标位置的水平距离距离小于设定值才真正执行投放。这里有个参数值得注意required_confirms设成5对应30帧视频流里的约0.17秒确认时间。通常0.15到0.3秒的确认窗口比较合适太长会让无人机飞过目标头顶太短又滤不掉抖动。3.3 参考实现识别到投放的状态机代码投放执行节点采用状态机思路状态依次是STANDBY待命、TRACKING追踪、ARMED瞄准、RELEASE投放、RECOVERY复位。参考工程里这个节点的核心代码片段如下import serial import time from enum import Enum class DropState(Enum): STANDBY 0 TRACKING 1 ARMED 2 RELEASE 3 RECOVERY 4 class DropController: def __init__(self, serial_port/dev/ttyUSB0, baudrate115200): self.state DropState.STANDBY self.serial serial.Serial(serial_port, baudrate, timeout0.1) self.drop_pwm_open 1100 # 舵机打开位置 self.drop_pwm_close 1900 # 舵机闭合位置 def send_pwm(self, pwm_value): # 串口协议帧头 0xAA 0x55 通道 高字节 低字节 校验 data bytearray([0xAA, 0x55, 0x01, (pwm_value 8) 0xFF, pwm_value 0xFF, (0xAA 0x55 0x01 (pwm_value 8) pwm_value) 0xFF]) self.serial.write(data) def execute_drop(self): if self.state DropState.ARMED: self.send_pwm(self.drop_pwm_open) time.sleep(0.5) self.send_pwm(self.drop_pwm_close) self.state DropState.RECOVERY逻辑说明这个节点从决策节点拿到enabled信号状态从ARMED切到RELEASE时通过串口给舵机控制板发送PWM指令。串口协议加了两层保护一是帧头0xAA 0x55二是末尾的校验和任何一个字节不对舵机控制板直接丢弃避免因为串口误码造成误投放。参数说明PWM值1100微秒对应舵机打开位置1900微秒对应闭合位置这两个值需要根据实际舵机型号和安装位置标定不能照搬。如果舵机打开和闭合方向反了把两个值互换即可。串口波特率115200是舵机控制板最常见的默认值如果你的板子是9600记得同步改。我提醒一点执行投放时state从ARMED切到RELEASE之后要等舵机动作完成再切回STANDBY或RECOVERY否则下一次识别还没来舵机就提前复位了物体半路滑出去。这个等待时间就设在send_pwm之后的time.sleep里0.5秒对大多数舵机足够换大舵机要加到1秒以上。4. 参数怎么调从相机标定到投放时延的四个实测要点4.1 相机标定与图像坐标误差一个像素偏差能变几厘米投放偏差识别模块在仿真环境下通常直接用理想内参但同一套代码上真机时相机内参必须重新标定。原因很简单仿真摄像头的fx、fy、cx、cy是精确已知的实机镜头因为加工和安装误差主点不在画面正中心畸变系数也不是零直接用理想内参做像素反算误差会随视场角边缘进一步放大。参考工程的标定流程用的是ROS标准做法启动相机标定工具用棋盘格在相机前变换姿态采集样本rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.025 \ image:/main_camera/image_raw camera:/main_camera--size 8x6指定棋盘格内角点数量--square 0.025是每个棋盘格方块的边长单位米必须用尺子量准量错会导致深度估计整体偏小或偏大。标定完成后会得到相机内参矩阵和畸变系数需要把结果更新到前面的pixel_to_camera函数里。一个像素误差到底会造成多大投放偏差这取决于飞行高度和相机分辨率。以1280x720分辨率、60度水平视场角、3米飞行高度为例单个像素对应的地面尺寸大约在0.5厘米左右看起来很小但目标在画面边缘时畸变可能让坐标偏差到3到5个像素对应的地面误差就是1.5到2.5厘米。这还不算深度估算误差如果深度差5厘米机体系坐标也会跟着偏几厘米。叠加起来投放点偏移超过5厘米很常见。4.2 悬停控制与PID参数位置环和速度环怎么约束投放精度很大程度取决于投放瞬间无人机是否够稳。很多人在Gazebo仿真里用默认PID参数直接跑感觉悬停精度还行但仿真环境里没有风、没有GPS飘移实机放飞时完全不是一回事。参考工程里整理了一组适用于视觉悬停的PID初值方向是先调速度环、再调位置环、最后限幅。控制环PID输出限幅水平速度环0.80.20.1最大倾斜角15度水平位置环1.20.050.3最大水平速度1.5 m/s高度速度环1.00.10.2最大垂直速度0.8 m/s高度位置环1.50.00.4高度误差阈值0.1m参数说明位置环P值一般比速度环大因为位置误差要先转成速度指令速度环再转成姿态指令。I值不要给大悬停控制里积分的意义是消除静差但太大容易引起低频振荡尤其在视觉定位有跳变时积分会积累出一个错误的速度指令。D值的作用是抑制超调位置环0.3是比较稳妥的起点。我调PID的顺序习惯是先把高度稳住再调水平。因为高度误差会直接影响深度估算深度错了投放点必偏。一种做法是先遥控飞到目标上方切位置模式悬停观察高度波动波动超过0.15米就先调高度速度环不要动位置环。4.3 投放时延与提前量计算检测到落地的数学账识别投放系统里从“看到目标”到“物体落地”之间有五个延时分量图像采集曝光时间、YOLO推理时间、决策确认时间、舵机响应时间、物体从挂载点下落到地面时间。前四个是固定系统时延第五个随高度变化。如果不做补偿无人机在往前飞时投放物体会落在机身位置的后方。提前量公式很简单水平提前距离 无人机水平速度 × 总时延。参考工程里给了一个计算示例def compute_lead_distance(horizontal_speed, total_delay, drop_height, gravity9.8): # 下落时间只取决于高度和重力加速度忽略空气阻力 fall_time (2 * drop_height / gravity) ** 0.5 # 总时延中舵机响应和推理时延要计入但在下落过程中无人机还会继续前飞 lead_distance horizontal_speed * (total_delay fall_time) return lead_distance逻辑说明下落时间按自由落体公式估算总时延包括图像处理、决策和机构响应。最终提前量等于水平速度乘以总时延加下落时间结果是无人机在距离目标正上方还有多远时就要提前释放。如果你用30帧相机、YOLO推理200毫秒、确认5帧160毫秒、舵机响应150毫秒总时延大约0.5秒。在1 m/s水平速度下提前量就是0.5米这个量级在3米高度已经非常可观不补偿会直接投到目标外。空气阻力要不要算我的经验是投放高度3米以内、物体重量20克以上时空气阻力对水平位移影响小于1厘米可以忽略如果投轻飘飘的泡沫球或者投放高度超过5米就必须做风洞测试或弹道修正。这种情况下自由落体模型会明显偏小不能直接用。4.4 数据记录与回放把每次投放变成可复现实验调试投放精准度最怕的是“这次偏了不知道偏在哪”。参考工程强烈建议在每次实验时记录rosbag把相机图像、识别结果、TF变换、无人机位姿、投放指令全部录下来rosbag record -O drop_experiment_001.bag \ /main_camera/image_raw /detection_result /tf /mavros/local_position/pose /drop_cmd回放时用下面命令可以慢速回放逐帧观察投放瞬间的坐标偏差rosbag play -r 0.5 drop_experiment_001.bag-r参数是回放倍率0.5表示半速回放适合逐帧看识别框和无人机位姿的关系。我通常会在投放指令话题里额外打一个时间戳回放时对比时间戳对应的位姿和检测结果能快速定位问题出在识别、坐标变换还是决策时机。5. 避坑与常见问题六条实测记录5.1 投放乱触发识别框抖动导致误投现象无人机悬停在目标上方没有主动发送投放指令舵机却自己打开了物体掉下来。 原因识别节点的置信度阈值设得太低比如0.25画面里某个背景纹理被误检成目标单帧置信度超过阈值就直接触发了投放。后续没有加平滑和连续帧确认误检尖峰没有被过滤掉。 解决把置信度阈值提高到0.45以上加上前面第3章提到的EMA平滑和连续5帧确认。我从那次之后所有投放判决逻辑都强制要求“连续帧确认加投放窗口”双条件缺一个都不执行。5.2 空投偏移坐标变换少了一环现象识别框在画面中央无人机机头也正对目标但物体落到目标前方约十几厘米处。 原因投放逻辑直接用了像素坐标算出来的相机系坐标没有做相机系到机体系的TF变换。相机安装在机体前方识别到的目标在相机系下是正前方但在机体系下还要叠加一个相机安装偏移投放时舵机挂在机体腹部两套坐标混用自然偏。 解决查看TF树确认base_link到camera_link的静态变换存在且数值与摄像头安装位置一致在状态机里先做完整坐标变换再计算投放点。从那以后我每次都会在投放前打印机体系下目标位置和挂载点的距离做一次“坐标体检”。5.3 悬停漂移位置控制用了GPS现象无人机在目标上方悬停水平位置慢慢漂移投放瞬间和目标的水平距离忽大忽小。 原因位置控制默认使用GPS定位在室内或树荫下GPS精度差定位噪声直接造成悬停漂移。视觉识别给的是机体相对目标的像素偏差没有融合进位置环。 解决换成视觉定位或光流感加激光测距的组合把识别目标位置作为位置环的外部反馈。如果条件有限至少把水平速度限幅降到1.0 m/s以下从控制层减少漂移幅度。这个问题在Gazebo室外场景里不明显上真机室内飞行几乎是必现。5.4 实机识别率骤降仿真与实机图像风格差异现象同一个YOLO权重在Gazebo仿真里识别率很高上实机后同一类目标频繁漏检。 原因仿真渲染的图像色彩分布、光照方向、纹理细节和实机相机拍到的画面差异很大。特别是目标表面反光程度和阴影形状仿真里往往过度理想化。 解决训练阶段加数据增强把亮度扰动、对比度扰动、高斯模糊、仿射变换加进去另一个实用技巧是在实机采集几十张目标照片做测试集专门检验权重在真实光线下的表现。如果差异实在太大考虑用domain randomization思路在仿真环境里随机化光照和纹理后重新训练。5.5 串口丢帧波特率与校验和问题现象投放指令偶发不执行舵机没反应但用串口调试助手手动发同样的协议又正常。 原因串口波特率不匹配或校验和计算错误。最常见的坑是舵机控制板默认波特率是9600或115200而代码里写的是19200数据能发出去但板子不认。另一个坑是校验和累加时忘了把帧头也算进去部分板子的协议要求校验和覆盖全部字节部分只覆盖数据位。 解决先把波特率全部统一到一块板子标称值再对照协议文档逐字节核对校验和算法。工程参考里用的校验和是把帧头加通道加数据全部累加取低8位如果你换了一块控制板第一件事就是确认它按哪种协议解析。5.6 相机反光造成漏检曝光时间要固定现象目标表面是金属或塑料材质实机悬停时偶发漏检但同一位置偏一下机身又能检到。 原因相机自动曝光导致目标表面过曝反光区域把物体的纹理细节完全吃掉YOLO卷积特征提取不到有效信息。尤其当无人机朝向变化时太阳角度变化会让反光忽强忽弱。 解决固定相机曝光时间不要用自动曝光。设置曝光后还要锁白平衡否则色彩偏移会影响模型输入分布。我在实际项目里把相机配置脚本写进launch文件每飞必设一次避免飞到后半程曝光被某个亮背景带偏。6. 用仿真回放验证识别与投放链路一个能复现的命中率统计脚本仿真和实机都跑完之后最后一步是把整条链路的命中率量化。凭眼睛看投放准不准不够要拿到数字才能证明“精准投放”真的准。我一般会录一段包含多次投放实验的rosbag然后用一个离线统计脚本回放计算每次投放指令发出时无人机位置与目标真值的水平距离偏差。目标真值在仿真里可以直接从Gazebo拿实机则依赖地面标记点或视觉标签。统计脚本的核心逻辑是订阅投放指令话题触发时记录当前无人机位姿从检测结果话题里找到对应时刻的目标位置计算两者的水平距离偏差最后统计偏差小于设定阈值的次数占比。#!/usr/bin/env python3 import rosbag import math def evaluate_drop(bag_file, threshold0.2): bag rosbag.Bag(bag_file) drop_pose None target_pose None hit_count 0 total_count 0 for topic, msg, t in bag.read_messages( topics[/drop_cmd, /mavros/local_position/pose, /target_position]): if topic /drop_cmd: if drop_pose is not None and target_pose is not None: total_count 1 dx drop_pose.pose.position.x - target_pose.position.x dy drop_pose.pose.position.y - target_pose.position.y distance math.sqrt(dx * dx dy * dy) if distance threshold: hit_count 1 elif topic /mavros/local_position/pose: drop_pose msg elif topic /target_position: target_pose msg if total_count 0: print(f有效投放次数: {total_count}, 命中次数: {hit_count}, f命中率: {hit_count / total_count * 100:.1f}%) bag.close()逻辑说明回调里同时记录最新位姿和目标位置每次投放指令到来时取这两个值计算水平距离偏差小于阈值记为一次命中。threshold设0.2米是考虑舵机响应一致性和定位噪声之后比较合理的判定标准如果投放物本身直径就有10厘米阈值可以放宽到0.25米。参数说明/mavros/local_position/pose是MAVROS发布的机体在局部坐标系下的位姿/target_position是参考工程里额外发布的目标真值话题仿真环境直接从Gazebo模型状态读取实机则需要换成视觉标签定位结果。我用这个脚本在仿真里连续跑了二十次投放实验命中率从最初的不足30%提升到85%以上主要修正的就是第4章提到的提前量计算和第5章的坐标变换问题。后来养成了一个习惯每次改参数后都强制走一遍录包、回放、统计的流程哪怕只是微调置信度阈值也不跳过。这套流程虽然慢但每一次改动都有数据支撑不会出现“上次明明好了这次又歪了”的玄学问题。希望帮到你。本文还有配套的精品资源点击获取
阅读完成 · 觉得有帮助？
