简介这是一份2017年暑假电赛期间在ROS平台上完成的四旋翼无人机飞行控制与视觉识别项目适合无人机开发者、ROS学习者和竞赛备赛人员。项目涵盖飞控基础、人脸检测、颜色识别、二维码识别等实用功能并兼容DJI Mobile SDK、STM32等软硬件环境。压缩包共1348个文件包含CMake构建脚本、Python/C源码、launch启动文件、消息定义及说明文档等整体仅3MB代码结构清晰便于快速定位模块。目前已有96人学习浏览项目内包含说明文档与分步教程对初学者友好可降低上手门槛。借助完整的代码结构和配置文件读者能够直接复用飞控与视觉联动方案缩短二次开发周期尤其适合作为电赛或无人机课题的参考实现。1. 为什么把飞控和视觉都放进ROS一个电赛四旋翼项目的模块化拆解2017年暑假的电赛时间紧、任务重四旋翼要飞得稳还要在人脸、颜色、二维码识别上拿分。如果飞控逻辑和视觉识别全堆在一个main.cpp里调试起来会非常痛苦。ROS的价值恰恰在这里把飞控驱动、串口转发、视觉识别拆成独立节点用话题和消息把数据流串起来任何一个模块出了问题用rostopic看消息就能定位不必把整架飞机拆下来重新烧录。这个项目就是典型的ROS化设计——apdriver负责连接飞控板uart-srv提供串口透传april_pro和arinfo处理视觉识别自定义msg包统一接口。适合正在做ROS无人机课设、准备电赛/毕设、或者想把旧飞控代码改造成模块化架构的人参考。2. apdriver、uart-srv 与自定义消息ROS 节点间如何传递飞控数据2.1 从文件清单看 ROS 包划分拿到这份代码包第一眼先看顶层的.asd文件apdriver、arinfo、april_pro、uart-srv以及各自的-msg包。从命名惯性上看这是一套典型的 ROS catkin 工作空间包名推断职责对外的 topic / serviceapdriver飞控板驱动读取 MAVLink 或串口遥测/apdriver/imu、/apdriver/attitudeuart-srv串口服务节点统一管理串口读写/uart/write、/uart/readapril_proAprilTag 二维码检测节点/april_tagsarinfo视觉识别信息汇总人脸、颜色、二维码/vision_info*-msg自定义消息定义供上述节点引用这里的关键设计是把硬件通信和业务逻辑解耦。uart-srv只负责按字节流收发apdriver从串口数据里解析飞控状态april_pro发布检测结果的几何位姿。各节点用消息传递数据任务分配时一个人写驱动、一个人写视觉、一个人写上层决策互不阻塞。2.2 用自定义 msg 统一数据接口自定义消息在这份代码里占了很大比重。apdriver-msg和arinfo-msg两个包的存在说明节点间的数据不是裸的std_msgs/String而是结构化的自定义类型。看下面这个模拟arinfo-msg的定义# arinfo/msg/VisionResult.msg uint8 object_type # 0person 1color_block 2april_tag string object_label # 例如 red, tag_12 float32 confidence # 置信度 geometry_msgs/PoseStamped pose # 物体在相机坐标系下的位姿把它放到arinfo-msg/msg/目录下修改CMakeLists.txtadd_message_files( FILES VisionResult.msg ) generate_messages( DEPENDENCIES geometry_msgs std_msgs )运行时用rosmsg show arinfo/VisionResult验证是否编译成功。为什么要自定义而不是直接发数组因为数组没法表达“这个值是哪个检测物体的置信度”而结构化消息后接收方一眼就能用msg-object_type做分支。这也是 2017 年电赛团队为了让视觉和飞控两个模块能并行开发的合理选择。2.3 连接飞控Apdriver 的驱动逻辑apdriver通常负责与 ArduPilot/PX4 飞控板通信。常见做法是通过 MAVLink 协议走串口或者直接用串口读 NMEA 格式的姿态数据。这里贴一个简化但能跑的 ROS 节点骨架#!/usr/bin/env python3 import rospy import serial from geometry_msgs.msg import Quaternion, Twist from std_msgs.msg import Header class APDriver: def __init__(self): rospy.init_node(apdriver) self.ser serial.Serial(/dev/ttyUSB0, 57600, timeout0.1) self.attitude_pub rospy.Publisher(/apdriver/attitude, Quaternion, queue_size10) self.cmd_sub rospy.Subscriber(/cmd_vel, Twist, self.cmd_callback) def cmd_callback(self, msg): # 将 Twist 转换成飞控自定义串口指令帧这里省略 crc 计算 frame b$CMD,%f,%f,%f* % (msg.linear.x, msg.linear.y, msg.angular.z) self.ser.write(frame) def loop(self): while not rospy.is_shutdown(): line self.ser.readline() if line.startswith(b$POS): parts parse(line) # 解析经纬高、四元数 self.attitude_pub.publish(parts) rospy.sleep(0.01) if __name__ __main__: ap APDriver() ap.loop()这段代码的关键参数串口波特率 57600超时 0.1 秒发布姿态信息到/apdriver/attitude订阅上位机速度指令/cmd_vel。写串口时注意Twist.linear.z如果没用到要传 0飞控端解析时不会因为字段缺失丢帧。这个节点的作用就像翻译官把 ROS 世界里的速度指令翻译成飞控串口听得懂的指令再把飞控的反馈翻译成 ROS 话题。3. 飞控链路的实现从 MAVLink 串口到四旋翼姿态控制3.1 姿态控制的 PID 增量计算放在哪一层很多人在 ROS 无人机项目里会误把整个姿态 PID 放在 ROS 节点里这是不安全的。飞控板上往往已经有硬件姿态解算和控制回路ROS 节点应该是慢速的高层控制位置环、航向环输出的是期望姿态角或速度。这份电赛代码里apdriver和飞控之间的通信协议如果支持 MAVLink那么姿态控制仍在飞控板 STM32 上完成ROS 只下发目标油门、横滚、俯仰、偏航角。比如订阅/apdriver/attitude得到当前四元数计算期望姿态的 PID 增量// 位置环 PID 输出发布成飞控指令 double dt (ros::Time::now() - last_time).toSec(); error target_height - current_height; integral error * dt; derivative (error - last_error) / dt; output kp * error ki * integral kd * derivative; geometry_msgs::Twist cmd; cmd.linear.z 0.5 output; // 基础油门 0.5 附近加上 PID 修正 cmd_pub.publish(cmd);注意integral要加限幅比如限制在[-0.3, 0.3]防止积分饱和导致起飞瞬间猛地窜高。derivative对高度计噪声很敏感如果用的超声波测距建议先做一阶低通滤波再求导。电赛现场最容易出现的问题是kp调大了以后低频振荡听到油门声音嗡嗡响这时候观察/apdriver/attitude的 z 轴是否在目标值附近正弦波动如果是就先把kd加到比kp小一个数量级而不是继续加kp。3.2 uart-srv串口读写封装的细节uart-srv在这个项目里的角色是串口服务节点。如果直接在每个节点里操作串口多个节点同时读写会造成波特率冲突和数据错乱。常见的做法是单独一个节点打开串口提供两个 service# uart-srv/srv/SendData.srv uint8[] data # 要发送的字节流 --- bool success # uart-srv/srv/ReadData.srv uint32 timeout_ms --- uint8[] data服务端处理逻辑def send_cb(req): self.ser.write(bytes(req.data)) return SendDataResponse(True) def read_cb(req): self.ser.timeout req.timeout_ms / 1000.0 resp ReadDataResponse(self.ser.readline()) return resp串口打开时注意非阻塞模式和rtscts不要乱开。在setup.bash之前先执行sudo usermod -a -G dialout $USER给普通用户串口权限否则 ROS 节点启动时serial.Serial会直接抛 PermissionError。电赛现场的摄像头和 GPS 可能同时占串口一条 USB 转串口线对应一个/dev/ttyUSB*用ls -l /dev/ttyUSB*看当前设备必要时加 udev 规则固定设备名不然开会时串口序号变了代码里写死/dev/ttyUSB0的节点就起来了。3.3 安全逻辑低电量、丢串口、视觉丢失飞行控制安全比控制精度更有优先级。这套代码里应该有的一个机制是看门狗如果apdriver在 500ms 内没有收到飞控板的心跳帧就要自动切到降落模式。实现上可以在 ROS 节点里维护一个时间戳rospy.Timer(rospy.Duration(0.2), watchdog_cb) last_heartbeat rospy.Time(0) def watchdog_cb(event): if (rospy.Time.now() - last_heartbeat.to_sec() 0.5): rospy.logwarn(FCU heartbeat lost, forcing landing!) cmd_pub.publish(land_cmd)需要注意的是这个看门狗只能作为软件层面的降级不能替代飞控板上的失控保护failsafe。ROS 节点挂掉不等于飞控板断电飞控板自己也要设置无遥控信号、无 GPS 信号时的自动返航或自稳。电赛场地人多万一 ROS 主机崩了至少要让四旋翼原地降落而不是自由漂移。这套代码的参数包里armed状态位和mode模式切换都要通过apdriver实时反馈回来测试时用rostopic echo确认逻辑在跑。4. 视觉识别AprilTag、人脸与颜色检测在 ROS 中的落地4.1 AprilTag 检测用april_pro拿到相对位姿AprilTag 在无人机视觉里常用来做精确降落和定点悬停。april_pro节点通常会订阅图像话题发布检测到的 tag 的 ID、角点、以及 tag 相对于相机坐标系的变换。这里的关键是相机内参矩阵。使用前需要标定相机得到 fx, fy, cx, cy。一份合理的 launch 参数如下launch node nameapril_pro pkgapril_pro typeapril_detector outputscreen param namepublish_tf valuetrue / param nametag_size value0.15 / !-- 单个 AprilTag 边长单位米 -- param namefamily valuetag36h11 / !-- tag 密度与鲁棒性平衡 -- param namemax_hamming_distance value2 / param namecamera_fx value615.0 / param namecamera_fy value615.0 / param namecamera_cx value320.0 / param namecamera_cy value240.0 / /node /launchtag_size如果写错计算出来的距离会线性偏移。比如实际边长是 15cm但你填了 5cm那么测得的距离是真实距离的三倍。电赛场地里打印 tag 时最好实际用尺子量不要按照纸面尺寸拍脑袋。max_hamming_distance是汉明距离容错调大能检测到更多破损的 tag但误检率也会上升。4.2 人脸和颜色检测OpenCV 在 ROS 里的标准写法人脸检测用 Haar 或 DNN颜色检测用 HSV 阈值。ROS 下常见的做法是单独写一个节点订阅图像然后发布检测框和类别。这里给一个颜色检测的节点片段import cv2 import numpy as np import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge from vision_msgs.msg import BoundingBox2D bridge CvBridge() green_lower np.array([35, 80, 46]) green_upper np.array([77, 255, 255]) def image_cb(msg): frame bridge.imgmsg_to_cv2(msg, bgr8) hsv cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) mask cv2.inRange(hsv, green_lower, green_upper) contours, _ cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) for cnt in contours: area cv2.contourArea(cnt) if area 500: continue x, y, w, h cv2.boundingRect(cnt) box BoundingBox2D() box.center.x x w / 2.0 box.center.y y h / 2.0 box.size_x w box.size_y h box_pub.publish(box) rospy.init_node(color_detect) rospy.Subscriber(/camera/rgb/image_raw, Image, image_cb) box_pub rospy.Publisher(/vision/color_box, BoundingBox2D, queue_size10)HSV 阈值里[35, 80, 46]和[77, 255, 255]是我常用的绿色范围。光照变化大的场地亮度分量 V 很容易超界最好在代码里加一个自动曝光限制或者用cv2.GaussianBlur先把图像模糊一下再转 HSV减少噪点。这个节点发布的是 2D 框如果只想检测颜色而不需要距离信息这个做法足够简单。4.3 视觉信息如何反馈给飞控话题还是 TF视觉结果要送给飞控通常有两种方式信息类型推荐通道理由检测到/没检测到、类别、置信度自定义话题语义信息需要逻辑判断AprilTag 位姿TFTransform坐标系变换关系便于后续运动规划目标在图像中的像素坐标话题用于视觉伺服如“让目标保持在画面中心”如果发的是 TFapril_pro会发布map→tag_frame的变换飞控节点通过tf2_buffer.lookupTransform查询相机坐标系里 tag 的位置。这在代码上比话题更简洁geometry_msgs::TransformStamped tag_pose; try { tag_pose tf_buffer.lookupTransform(camera_link, tag_12, ros::Time(0), ros::Duration(1.0)); double dx tag_pose.transform.translation.x; double dy tag_pose.transform.translation.y; double dz tag_pose.transform.translation.z; } catch (tf2::TransformException ex) { ROS_WARN_STREAM(ex.what()); }注意这里lookupTransform的源和目标顺序第一个参数是目标坐标系第二个是源坐标系含义是“tag_12 在 camera_link 坐标系下的坐标”。如果顺序反了得到的位姿方向和实际相反飞控会让四旋翼朝反方向飞。电赛项目里至少有一个人要专门盯 rviz看 TF 树里 tag 的坐标轴方向是否符合场景。5. 进阶用 AprilTag 相对位姿做定点悬停的一个可运行技巧打印机一个 15cm 见方的 AprilTag 贴在地面目标点让四旋翼在 tag 正上方 1 米处悬停。思路是视觉节点发布tag 相对机身的偏移量飞控节点用这个偏移量作为水平位置环的误差输入调整给飞控的滚转俯仰指令。这个玩法比 GPS 悬停更适合室内电赛。核心代码挂在飞控节点里订阅/tag_posevoid tagToCmd(const geometry_msgs::TransformStamped tf_msg) { double dx tf_msg.transform.translation.x; // 机体系下前方为正 double dy tf_msg.transform.translation.y; // 机体系下左侧为正 // 死区tag 在正下方 5cm 以内就不修正防止静止时来回抖 const double deadband 0.05; double cmd_x 0, cmd_y 0; if (fabs(dx) deadband) cmd_x 0.3 * dx; // P 控制比例因子 0.3 if (fabs(dy) deadband) cmd_y 0.3 * dy; std_msgs::Float64MultiArray target; target.data {cmd_x, cmd_y}; // 发布给底层姿态环的期望横滚/俯仰 set_attitude_target_pub.publish(target); }这里只用了 P 项因为水平速度很快积分容易造成超调。比例因子 0.3的含义是当 tag 偏移 0.3 米时给 0.09 弧度的期望横滚/俯仰角大约 5 度。这个数值从 0.1 开始逐步微调观察飞机是否逼近 tag 正上方。死区的设置非常关键没有死区时飞机悬浮会有微小漂移控制量在零附近快速切换机体发颤。加了死区后运动是间断式的偏了 10cm 才修正 3cm不会因为传感器噪声持续输出指令。调试时先用rostopic echo -r 5 /tag_pose看视觉数据是否稳定再看指令话题是否跟随。如果视觉节点输出毛刺严重可以在april_pro里增加时间滤波如取最近 5 帧位姿的中值而不是平均值因为中值能剔除误检跳变。把这个视觉伺服闭环跑通后换掉 PID 参数就能扩展到跟踪移动目标、自主降落等更多玩法。最后把电赛现场环境光照调整好AprilTag 一张打印纸就能撑起整个视觉项目的核心得分点。本文还有配套的精品资源点击获取
ROS模块化架构助力电赛四旋翼：飞控与视觉识别协同设计
简介这是一份2017年暑假电赛期间在ROS平台上完成的四旋翼无人机飞行控制与视觉识别项目适合无人机开发者、ROS学习者和竞赛备赛人员。项目涵盖飞控基础、人脸检测、颜色识别、二维码识别等实用功能并兼容DJI Mobile SDK、STM32等软硬件环境。压缩包共1348个文件包含CMake构建脚本、Python/C源码、launch启动文件、消息定义及说明文档等整体仅3MB代码结构清晰便于快速定位模块。目前已有96人学习浏览项目内包含说明文档与分步教程对初学者友好可降低上手门槛。借助完整的代码结构和配置文件读者能够直接复用飞控与视觉联动方案缩短二次开发周期尤其适合作为电赛或无人机课题的参考实现。1. 为什么把飞控和视觉都放进ROS一个电赛四旋翼项目的模块化拆解2017年暑假的电赛时间紧、任务重四旋翼要飞得稳还要在人脸、颜色、二维码识别上拿分。如果飞控逻辑和视觉识别全堆在一个main.cpp里调试起来会非常痛苦。ROS的价值恰恰在这里把飞控驱动、串口转发、视觉识别拆成独立节点用话题和消息把数据流串起来任何一个模块出了问题用rostopic看消息就能定位不必把整架飞机拆下来重新烧录。这个项目就是典型的ROS化设计——apdriver负责连接飞控板uart-srv提供串口透传april_pro和arinfo处理视觉识别自定义msg包统一接口。适合正在做ROS无人机课设、准备电赛/毕设、或者想把旧飞控代码改造成模块化架构的人参考。2. apdriver、uart-srv 与自定义消息ROS 节点间如何传递飞控数据2.1 从文件清单看 ROS 包划分拿到这份代码包第一眼先看顶层的.asd文件apdriver、arinfo、april_pro、uart-srv以及各自的-msg包。从命名惯性上看这是一套典型的 ROS catkin 工作空间包名推断职责对外的 topic / serviceapdriver飞控板驱动读取 MAVLink 或串口遥测/apdriver/imu、/apdriver/attitudeuart-srv串口服务节点统一管理串口读写/uart/write、/uart/readapril_proAprilTag 二维码检测节点/april_tagsarinfo视觉识别信息汇总人脸、颜色、二维码/vision_info*-msg自定义消息定义供上述节点引用这里的关键设计是把硬件通信和业务逻辑解耦。uart-srv只负责按字节流收发apdriver从串口数据里解析飞控状态april_pro发布检测结果的几何位姿。各节点用消息传递数据任务分配时一个人写驱动、一个人写视觉、一个人写上层决策互不阻塞。2.2 用自定义 msg 统一数据接口自定义消息在这份代码里占了很大比重。apdriver-msg和arinfo-msg两个包的存在说明节点间的数据不是裸的std_msgs/String而是结构化的自定义类型。看下面这个模拟arinfo-msg的定义# arinfo/msg/VisionResult.msg uint8 object_type # 0person 1color_block 2april_tag string object_label # 例如 red, tag_12 float32 confidence # 置信度 geometry_msgs/PoseStamped pose # 物体在相机坐标系下的位姿把它放到arinfo-msg/msg/目录下修改CMakeLists.txtadd_message_files( FILES VisionResult.msg ) generate_messages( DEPENDENCIES geometry_msgs std_msgs )运行时用rosmsg show arinfo/VisionResult验证是否编译成功。为什么要自定义而不是直接发数组因为数组没法表达“这个值是哪个检测物体的置信度”而结构化消息后接收方一眼就能用msg-object_type做分支。这也是 2017 年电赛团队为了让视觉和飞控两个模块能并行开发的合理选择。2.3 连接飞控Apdriver 的驱动逻辑apdriver通常负责与 ArduPilot/PX4 飞控板通信。常见做法是通过 MAVLink 协议走串口或者直接用串口读 NMEA 格式的姿态数据。这里贴一个简化但能跑的 ROS 节点骨架#!/usr/bin/env python3 import rospy import serial from geometry_msgs.msg import Quaternion, Twist from std_msgs.msg import Header class APDriver: def __init__(self): rospy.init_node(apdriver) self.ser serial.Serial(/dev/ttyUSB0, 57600, timeout0.1) self.attitude_pub rospy.Publisher(/apdriver/attitude, Quaternion, queue_size10) self.cmd_sub rospy.Subscriber(/cmd_vel, Twist, self.cmd_callback) def cmd_callback(self, msg): # 将 Twist 转换成飞控自定义串口指令帧这里省略 crc 计算 frame b$CMD,%f,%f,%f* % (msg.linear.x, msg.linear.y, msg.angular.z) self.ser.write(frame) def loop(self): while not rospy.is_shutdown(): line self.ser.readline() if line.startswith(b$POS): parts parse(line) # 解析经纬高、四元数 self.attitude_pub.publish(parts) rospy.sleep(0.01) if __name__ __main__: ap APDriver() ap.loop()这段代码的关键参数串口波特率 57600超时 0.1 秒发布姿态信息到/apdriver/attitude订阅上位机速度指令/cmd_vel。写串口时注意Twist.linear.z如果没用到要传 0飞控端解析时不会因为字段缺失丢帧。这个节点的作用就像翻译官把 ROS 世界里的速度指令翻译成飞控串口听得懂的指令再把飞控的反馈翻译成 ROS 话题。3. 飞控链路的实现从 MAVLink 串口到四旋翼姿态控制3.1 姿态控制的 PID 增量计算放在哪一层很多人在 ROS 无人机项目里会误把整个姿态 PID 放在 ROS 节点里这是不安全的。飞控板上往往已经有硬件姿态解算和控制回路ROS 节点应该是慢速的高层控制位置环、航向环输出的是期望姿态角或速度。这份电赛代码里apdriver和飞控之间的通信协议如果支持 MAVLink那么姿态控制仍在飞控板 STM32 上完成ROS 只下发目标油门、横滚、俯仰、偏航角。比如订阅/apdriver/attitude得到当前四元数计算期望姿态的 PID 增量// 位置环 PID 输出发布成飞控指令 double dt (ros::Time::now() - last_time).toSec(); error target_height - current_height; integral error * dt; derivative (error - last_error) / dt; output kp * error ki * integral kd * derivative; geometry_msgs::Twist cmd; cmd.linear.z 0.5 output; // 基础油门 0.5 附近加上 PID 修正 cmd_pub.publish(cmd);注意integral要加限幅比如限制在[-0.3, 0.3]防止积分饱和导致起飞瞬间猛地窜高。derivative对高度计噪声很敏感如果用的超声波测距建议先做一阶低通滤波再求导。电赛现场最容易出现的问题是kp调大了以后低频振荡听到油门声音嗡嗡响这时候观察/apdriver/attitude的 z 轴是否在目标值附近正弦波动如果是就先把kd加到比kp小一个数量级而不是继续加kp。3.2 uart-srv串口读写封装的细节uart-srv在这个项目里的角色是串口服务节点。如果直接在每个节点里操作串口多个节点同时读写会造成波特率冲突和数据错乱。常见的做法是单独一个节点打开串口提供两个 service# uart-srv/srv/SendData.srv uint8[] data # 要发送的字节流 --- bool success # uart-srv/srv/ReadData.srv uint32 timeout_ms --- uint8[] data服务端处理逻辑def send_cb(req): self.ser.write(bytes(req.data)) return SendDataResponse(True) def read_cb(req): self.ser.timeout req.timeout_ms / 1000.0 resp ReadDataResponse(self.ser.readline()) return resp串口打开时注意非阻塞模式和rtscts不要乱开。在setup.bash之前先执行sudo usermod -a -G dialout $USER给普通用户串口权限否则 ROS 节点启动时serial.Serial会直接抛 PermissionError。电赛现场的摄像头和 GPS 可能同时占串口一条 USB 转串口线对应一个/dev/ttyUSB*用ls -l /dev/ttyUSB*看当前设备必要时加 udev 规则固定设备名不然开会时串口序号变了代码里写死/dev/ttyUSB0的节点就起来了。3.3 安全逻辑低电量、丢串口、视觉丢失飞行控制安全比控制精度更有优先级。这套代码里应该有的一个机制是看门狗如果apdriver在 500ms 内没有收到飞控板的心跳帧就要自动切到降落模式。实现上可以在 ROS 节点里维护一个时间戳rospy.Timer(rospy.Duration(0.2), watchdog_cb) last_heartbeat rospy.Time(0) def watchdog_cb(event): if (rospy.Time.now() - last_heartbeat.to_sec() 0.5): rospy.logwarn(FCU heartbeat lost, forcing landing!) cmd_pub.publish(land_cmd)需要注意的是这个看门狗只能作为软件层面的降级不能替代飞控板上的失控保护failsafe。ROS 节点挂掉不等于飞控板断电飞控板自己也要设置无遥控信号、无 GPS 信号时的自动返航或自稳。电赛场地人多万一 ROS 主机崩了至少要让四旋翼原地降落而不是自由漂移。这套代码的参数包里armed状态位和mode模式切换都要通过apdriver实时反馈回来测试时用rostopic echo确认逻辑在跑。4. 视觉识别AprilTag、人脸与颜色检测在 ROS 中的落地4.1 AprilTag 检测用april_pro拿到相对位姿AprilTag 在无人机视觉里常用来做精确降落和定点悬停。april_pro节点通常会订阅图像话题发布检测到的 tag 的 ID、角点、以及 tag 相对于相机坐标系的变换。这里的关键是相机内参矩阵。使用前需要标定相机得到 fx, fy, cx, cy。一份合理的 launch 参数如下launch node nameapril_pro pkgapril_pro typeapril_detector outputscreen param namepublish_tf valuetrue / param nametag_size value0.15 / !-- 单个 AprilTag 边长单位米 -- param namefamily valuetag36h11 / !-- tag 密度与鲁棒性平衡 -- param namemax_hamming_distance value2 / param namecamera_fx value615.0 / param namecamera_fy value615.0 / param namecamera_cx value320.0 / param namecamera_cy value240.0 / /node /launchtag_size如果写错计算出来的距离会线性偏移。比如实际边长是 15cm但你填了 5cm那么测得的距离是真实距离的三倍。电赛场地里打印 tag 时最好实际用尺子量不要按照纸面尺寸拍脑袋。max_hamming_distance是汉明距离容错调大能检测到更多破损的 tag但误检率也会上升。4.2 人脸和颜色检测OpenCV 在 ROS 里的标准写法人脸检测用 Haar 或 DNN颜色检测用 HSV 阈值。ROS 下常见的做法是单独写一个节点订阅图像然后发布检测框和类别。这里给一个颜色检测的节点片段import cv2 import numpy as np import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge from vision_msgs.msg import BoundingBox2D bridge CvBridge() green_lower np.array([35, 80, 46]) green_upper np.array([77, 255, 255]) def image_cb(msg): frame bridge.imgmsg_to_cv2(msg, bgr8) hsv cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) mask cv2.inRange(hsv, green_lower, green_upper) contours, _ cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) for cnt in contours: area cv2.contourArea(cnt) if area 500: continue x, y, w, h cv2.boundingRect(cnt) box BoundingBox2D() box.center.x x w / 2.0 box.center.y y h / 2.0 box.size_x w box.size_y h box_pub.publish(box) rospy.init_node(color_detect) rospy.Subscriber(/camera/rgb/image_raw, Image, image_cb) box_pub rospy.Publisher(/vision/color_box, BoundingBox2D, queue_size10)HSV 阈值里[35, 80, 46]和[77, 255, 255]是我常用的绿色范围。光照变化大的场地亮度分量 V 很容易超界最好在代码里加一个自动曝光限制或者用cv2.GaussianBlur先把图像模糊一下再转 HSV减少噪点。这个节点发布的是 2D 框如果只想检测颜色而不需要距离信息这个做法足够简单。4.3 视觉信息如何反馈给飞控话题还是 TF视觉结果要送给飞控通常有两种方式信息类型推荐通道理由检测到/没检测到、类别、置信度自定义话题语义信息需要逻辑判断AprilTag 位姿TFTransform坐标系变换关系便于后续运动规划目标在图像中的像素坐标话题用于视觉伺服如“让目标保持在画面中心”如果发的是 TFapril_pro会发布map→tag_frame的变换飞控节点通过tf2_buffer.lookupTransform查询相机坐标系里 tag 的位置。这在代码上比话题更简洁geometry_msgs::TransformStamped tag_pose; try { tag_pose tf_buffer.lookupTransform(camera_link, tag_12, ros::Time(0), ros::Duration(1.0)); double dx tag_pose.transform.translation.x; double dy tag_pose.transform.translation.y; double dz tag_pose.transform.translation.z; } catch (tf2::TransformException ex) { ROS_WARN_STREAM(ex.what()); }注意这里lookupTransform的源和目标顺序第一个参数是目标坐标系第二个是源坐标系含义是“tag_12 在 camera_link 坐标系下的坐标”。如果顺序反了得到的位姿方向和实际相反飞控会让四旋翼朝反方向飞。电赛项目里至少有一个人要专门盯 rviz看 TF 树里 tag 的坐标轴方向是否符合场景。5. 进阶用 AprilTag 相对位姿做定点悬停的一个可运行技巧打印机一个 15cm 见方的 AprilTag 贴在地面目标点让四旋翼在 tag 正上方 1 米处悬停。思路是视觉节点发布tag 相对机身的偏移量飞控节点用这个偏移量作为水平位置环的误差输入调整给飞控的滚转俯仰指令。这个玩法比 GPS 悬停更适合室内电赛。核心代码挂在飞控节点里订阅/tag_posevoid tagToCmd(const geometry_msgs::TransformStamped tf_msg) { double dx tf_msg.transform.translation.x; // 机体系下前方为正 double dy tf_msg.transform.translation.y; // 机体系下左侧为正 // 死区tag 在正下方 5cm 以内就不修正防止静止时来回抖 const double deadband 0.05; double cmd_x 0, cmd_y 0; if (fabs(dx) deadband) cmd_x 0.3 * dx; // P 控制比例因子 0.3 if (fabs(dy) deadband) cmd_y 0.3 * dy; std_msgs::Float64MultiArray target; target.data {cmd_x, cmd_y}; // 发布给底层姿态环的期望横滚/俯仰 set_attitude_target_pub.publish(target); }这里只用了 P 项因为水平速度很快积分容易造成超调。比例因子 0.3的含义是当 tag 偏移 0.3 米时给 0.09 弧度的期望横滚/俯仰角大约 5 度。这个数值从 0.1 开始逐步微调观察飞机是否逼近 tag 正上方。死区的设置非常关键没有死区时飞机悬浮会有微小漂移控制量在零附近快速切换机体发颤。加了死区后运动是间断式的偏了 10cm 才修正 3cm不会因为传感器噪声持续输出指令。调试时先用rostopic echo -r 5 /tag_pose看视觉数据是否稳定再看指令话题是否跟随。如果视觉节点输出毛刺严重可以在april_pro里增加时间滤波如取最近 5 帧位姿的中值而不是平均值因为中值能剔除误检跳变。把这个视觉伺服闭环跑通后换掉 PID 参数就能扩展到跟踪移动目标、自主降落等更多玩法。最后把电赛现场环境光照调整好AprilTag 一张打印纸就能撑起整个视觉项目的核心得分点。本文还有配套的精品资源点击获取
