简介面向无人机视觉定位与自主降落场景这份基于V2版本的ROS二维码降落方案着重解决降落阶段位置持续修正的问题常见做法往往在识别到二维码后一次性对准、随即下降此版本则在下落至约0.6米高度时仍依据二维码反馈持续调整机体的水平偏移高度阈值还可根据摄像头盲区自行设定对学习ROS、二维码识别以及无人机精准降落的开发者很有参考价值。资源包共包含27个文件大小约970KB以launch、yaml、xml等启动与配置文件为主另有cpp源码、lua脚本、pgm地图和txt说明文档可支撑完整的无人机视觉降落实验搭建包内还附带robot_bringup、usb_cam、robot_vision等子模块的zip压缩包便于直接整合到已有ROS工作空间复用。目前已有797人学习下载作者在配套博客中给出了具体使用流程和参数调整思路拿到手后可对照搭建摄像头、地图与降落任务适合作为毕业设计、竞赛方案或移动机器人二次开发的基础参考。1. 为什么二维码降落需要一个 V2 版本自动飞行中最容易出问题的环节不在巡航段多数在最后十米。GPS 在地面附近的多径效应、气压计的低空漂移、视觉里程计在无纹理停机坪上的失锁这些干扰在降落阶段会叠加在一起。二维码降落方案把已知尺寸的视觉标记作为绝对参考在 ROS 环境中用下视相机一帧帧地解算无人机相对降落点的三维位姿。V2 版本的核心变化是把位姿解算从降落前的一次测量变成整个降落过程的连续反馈在落到地面之前一边修正水平偏差一边限制下降速度。它的适用对象是物流自动换电、巡检返航、定点回收这类对落点精度有硬指标的场景而不是只求“能落下来就行”的演示代码。2. 二维码降落的技术底座坐标系、位姿估计与 V2 的控制思路2.1 为什么 ArUco 比普通二维码更适合做降落基准普通 QR 码的定位能力取决于三个定位角点扫出来的是一个“内容”不是“位姿”。虽然也能通过角点做透视解算但 QR 的编码密度高、角点特征不够锐利在运动模糊和低照度下很容易丢失角点恢复出来的三维位置噪声很大。ArUco 标记从设计上就服务于位姿估计内部是二进制网格外面一圈黑色边框解码后拿到 ID同时提取四个角点的亚像素坐标。四个角点配合已知的物理尺寸就能解出相机相对标记的旋转和平移。选择 ArUco 还有两个实际原因。第一位姿精度直接由码的物理尺寸和相机内参决定不需要像 QR 那样先识别内容再换算坐标少一层误差。第二ArUco 有“字典”机制每个字典规定了码点数量和校验位错用字典时检测率会断崖式下跌这反而是一种保护——至少你能确认自己用的是哪套码。工程上常用 DICT_6X6_250码点多且抗遮挡能力中等偏上适合作为降落码和周边辅助码混用。字典码数量网格尺寸抗遮挡能力适合场景DICT_4X4_50504x4一般小尺寸打印、近距离识别DICT_6X6_2502506x6较好无人机降落、导航基准DICT_7X7_100010007x7好大场景多标记、远距离识别选择字典时不要只看“码多不多”。DICT_4X4_50 打印出来确实小同样的物理尺寸下它的网格单元比 6x6 更大近距离识别更轻松但误检率也更高。我一般用 6x6 做降落主码用 4x4 做场地周围的辅助码这样主码和辅助码不会互相干扰。2.2 位姿估计的原理PnP 解算与相机-二维码相对坐标ArUco 检测完成后的核心动作是解 PnPPerspective-n-Point。已知二维码四个角点在标记坐标系中的三维坐标以及它们在图像中的像素坐标加上相机内参和畸变系数就能解出标记坐标系到相机坐标系的旋转向量和平移向量。整个过程只需要四个点因为二维码本身刚性四个角点已经足以约束旋转和平移的六个自由度OpenCV 内部用 EPnP 或迭代法求解。这里要理解 tvec 的物理含义。estimatePoseSingleMarkers返回的tvec是二维码原点在相机坐标系中的位置单位是米。对于下视相机你可以把它理解成“二维码在我相机前方多远处、偏左还是偏右”。如果相机内参标定准确、二维码尺寸量得准tvec 的精度可以达到厘米级。尺寸量错是新手最容易犯的问题MARKER_SIZE 写的是 0.25实际打印出来量一下是 0.245高度误差就会按比例放大降落点跟着偏移。2.3 V2 版本的核心变化从定点下降到动态调整V1 版本的降落逻辑通常是三段式先飞到二维码正上方悬停检测一次二维码确认 ID 正确后开始垂直下降。问题在于“正上方”是靠 GPS 或视觉里程计确定的这两种定位源在近地段的误差都很大。飞控认为自己在正上方实际上可能偏了半米一降下去就落在二维码边缘。V2 版本把视觉解算从“降落前的一次确认”改成“整个降落过程的连续反馈”。下降过程中飞控每一帧都从下视相机拿到二维码的相对位置计算水平偏差然后输出水平速度修正指令。也就是说无人机不是沿着预设轨迹砸下去而是像人盯着停车位倒车一样一边下降一边调整方向。这个控制结构的改变才是 V2 真正要解决的问题。3. 把 V2 版本跑起来标定、检测与位姿输出3.1 相机标定怎么拿下视相机的内参和畸变系数位姿解算的精度上限由相机内参决定。下视相机不一定需要很高分辨率但畸变一定要标定到位。常见做法是用棋盘格标定板在不同角度、不同距离拍 20 张左右再用 OpenCV 的calibrateCamera得到内参矩阵和畸变系数。棋盘格可以在打印店打一张贴在硬纸板上关键是格子尺寸要量准。import cv2 import numpy as np import glob square_size 0.025 # 每格边长单位 m pattern_size (9, 6) objp np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32) objp[:, :2] np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) objp * square_size obj_points, img_points [], [] for fname in sorted(glob.glob(calib_images/*.jpg)): img cv2.imread(fname) gray cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners cv2.findChessboardCorners(gray, pattern_size, None) if not ret: print(fskip {fname}) continue obj_points.append(objp) img_points.append(corners) ret, camera_matrix, dist_coeffs, rvecs, tvecs cv2.calibrateCamera( obj_points, img_points, gray.shape[::-1], None, None) np.save(cam_K.npy, camera_matrix) np.save(cam_D.npy, dist_coeffs)代码里square_size是关键参数如果标定板实际格子是 30mm 而这里写成 25mm内参的尺度会整体偏移导致后续所有位姿解算的距离都不对。另外要注意角点的亚像素精度拍照时标定板要尽量占满画面并且每个位置姿态要有明显变化不要只在一个角度平移。标定完成后验证重投影误差小于 0.5 像素算合格大于这个数基本是角点提取不准删掉几帧重拍。mean_error 0 for i in range(len(obj_points)): imgpoints2, _ cv2.projectPoints( obj_points[i], rvecs[i], tvecs[i], camera_matrix, dist_coeffs) error cv2.norm(img_points[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2) mean_error error print(mean reproj error:, mean_error / len(obj_points))重投影误差是标定质量的直接度量。误差偏大不要急着调算法先把标定板的帧拿出来一张张看通常是有模糊帧或者标定板反光的帧混进去了。3.2 用 Python 写一个可用的 ArUco 检测与位姿节点ROS 环境下下视相机的图像通常以sensor_msgs/Image话题发布。我们需要写一个节点订阅图像检测 ArUco解算位姿再把位姿发布出去。下面是常用的实现方式适配 ROS1 和 OpenCV 4.x。#!/usr/bin/env python3 import rospy import cv2 import numpy as np from sensor_msgs.msg import Image from geometry_msgs.msg import PoseStamped from cv_bridge import CvBridge ARUCO_DICT cv2.aruco.DICT_6X6_250 MARKER_SIZE 0.25 class ArucoDownNode: def __init__(self): self.bridge CvBridge() self.K np.load(cam_K.npy) self.D np.load(cam_D.npy) self.aruco_dict cv2.aruco.Dictionary_get(ARUCO_DICT) self.params cv2.aruco.DetectorParameters_create() self.pose_pub rospy.Publisher(aruco_pose, PoseStamped, queue_size1) rospy.Subscriber(camera/down/image_raw, Image, self.image_callback) def image_callback(self, msg): frame self.bridge.imgmsg_to_cv2(msg, bgr8) corners, ids, _ cv2.aruco.detectMarkers( frame, self.aruco_dict, parametersself.params) if ids is None: return idx np.where(ids.flatten() 0)[0] if len(idx) 0: return rvec, tvec, _ cv2.aruco.estimatePoseSingleMarkers( corners[idx[0]], MARKER_SIZE, self.K, self.D) self.publish_pose(tvec[0][0]) def publish_pose(self, tvec): msg PoseStamped() msg.header.stamp rospy.Time.now() msg.header.frame_id camera_down msg.pose.position.x float(tvec[0]) msg.pose.position.y float(tvec[1]) msg.pose.position.z float(tvec[2]) self.pose_pub.publish(msg) if __name__ __main__: rospy.init_node(aruco_down_node) ArucoDownNode() rospy.spin()这个节点的核心逻辑在image_callback里。detectMarkers返回的corners是每个标记的四个角点坐标ids是对应的 ID。这里只处理 ID 为 0 的标记如果你的降落场有多个标记可以改成遍历ids把每个标记的 ID 和位姿一起发布出去。MARKER_SIZE必须和实际打印尺寸一致单位是米这个参数错了后面所有距离信息都会按比例偏移。注意 OpenCV 的 API 版本差异。OpenCV 4.7 之后cv2.aruco的接口有调整老代码在新版本上可能直接报错。如果遇到AttributeError: module cv2.aruco has no attribute Dictionary_get先确认 OpenCV 版本再按照对应版本调整接口名称。3.3 坐标变换从相机系到机体系的链路飞控和视觉节点通常不使用同一套坐标系。相机拿到的位姿是相机坐标系下的飞控需要的是机体坐标系下的偏差。这一步必须做扎实否则前面检测和位姿解算全都白费。以最常见的下视安装为例相机镜头向下光轴与机体 z 轴方向相反相机 x 轴与机体 x 轴同向。# tvec: 二维码在相机坐标系中的位置 # 本例约定相机 x 轴朝机头相机 y 轴朝机体下方相机 z 轴朝向机体左侧 bx tvec[0] # 机体系 x 方向机头 by -tvec[2] # 机体系 y 方向机体左侧 bz -tvec[1] # 机体系 z 方向机体上方即高度这段映射不是固定的。如果你的相机安装时绕光轴转了 90 度或者相机 x 轴朝机体后方符号就必须相应调整。我见过有人在仿真里跑得好好的真机上镜像翻转原因就是相机安装方向没有对齐机体轴。最稳妥的做法是在真机验证阶段故意把二维码向左移动 0.5 米看bx的变化方向如果符号反了直接翻转。不要指望一次就把坐标变换写对这个环节几乎每个人都翻过车。如果项目里已经维护了 TF 树更规范的做法是把相机到机体的静态变换发布到 TF然后用tf2做位姿变换代码里就不用写死坐标映射。但对小项目来说一个坐标变换函数加一个真机方向验证已经够用了。4. 降落过程中的动态位置调整控制回路与参数设计4.1 控制回路高度环与水平修正环怎么配合动态调整不是一个独立的控制环而是嵌在飞控原有的级联控制里。外环是视觉定位的水平偏差输出期望水平速度内环是飞控自带的速度环和姿态环负责跟踪这个速度指令。高度通道要单独处理通常由激光或超声波测距传感器主导视觉高度只做参考。控制逻辑可以这样理解飞行器在较高高度时先做水平对齐不要急着下降。水平偏差在设定阈值以内时才开始下降。下降过程中一旦发现水平偏移超出安全范围立即停止下降继续修正。这个“先对齐再下降偏离就停”的约束比任何 PID 参数都重要因为没有一个控制器能在二维码已经出视野的情况下还把位置拽回来。MAX_SPEED_X 0.5 MAX_SPEED_Y 0.5 LAND_XY_THRESHOLD 0.10 DESCENT_SPEED 0.3 KP_XY 0.8 def compute_setpoint(tvec): # 坐标变换得到机体系偏差 bx tvec[0] by -tvec[2] bz -tvec[1] # 水平速度指令P 控制 vx np.clip(KP_XY * bx, -MAX_SPEED_X, MAX_SPEED_X) vy np.clip(KP_XY * by, -MAX_SPEED_Y, MAX_SPEED_Y) # 水平偏差在阈值内才开始下降否则悬停修正 horizontal_err np.hypot(bx, by) if horizontal_err LAND_XY_THRESHOLD and bz 0.05: vz -DESCENT_SPEED else: vz 0.0 return vx, vy, vz这里KP_XY 0.8的含义是水平偏差 1 米时期望修正速度为 0.8 米/秒。这个值看起来不大但注意是持续修正无人机从偏 1 米到偏 0.1 米只需要一两秒。np.clip不只是保护飞控也保护视觉节点——如果修正速度过大飞行器倾角变大二维码很容易滑出相机视野修正就中断了。下降速度DESCENT_SPEED 0.3也是安全边界。动态调整的核心不是降得快而是降得稳。最后半米宁可放慢到 0.15 米/秒也不要追求速度。近地段的电机下洗气流会把地面杂物吹起来气压计高度值也会漂这时候慢就是快。4.2 PID 参数怎么设先改哪个、用多大动态调整控制回路最常见的问题不是算法复杂而是 PID 参数一上来就乱调。我建议按下面的顺序来每一步都只动一个参数。第一步把 I 设为 0D 设为 0只调 P。从 0.3 开始依次试 0.6、1.0、1.5观察水平偏差曲线。第二步如果出现围绕二维码来回振荡的情况说明 P 偏大先减小 P而不是急着加 D。第三步只有出现明显超调并且用减小 P 无法解决时才加 DD 系数不要超过 P 的百分之二十。第四步把最大修正速度从 0.5 降到 0.3看收敛是否变慢。如果变慢但稳定就可以接受因为降落场景下一两秒的快慢差别感知不明显。参数初始范围调整建议kp_xy0.3 ~ 1.5从 0.3 起每次加 0.2~0.3观察振荡kd_xy0.0 ~ 0.2仅在超调明显时加且不超过 P 的 20%max_speed0.3 ~ 0.8过大会导致二维码出视野上限优先压低land_threshold0.08 ~ 0.15越小落点越准但收敛时间越长参数调好后的验证流程也很有讲究。先在离地 1.5 米的高度悬停人为把二维码偏移一米左右让飞行器主动去追。只观察水平修正不打开下降逻辑。等水平修正稳定、没有振荡之后再打开下降开关观察整个降落过程。如果有人帮你拿一根长杆挑着二维码板慢速移动就能很快验证动态跟踪能力。5. 二维码降落避坑指南5 个让飞机摔下来的细节5.1 ArUco 字典不匹配一个都识别不出来现象地面电脑上跑检测程序一切正常上机后在 ROS 节点里同一张图像一个标记都检测不到ids永远是空数组。原因生成二维码时用的字典和检测时用的字典不一致。比如打印时用 DICT_6X6_250 生成的图片检测代码里却写成了 DICT_4X4_50两个字典的编码规则完全不同自然匹配不上。这个错误在代码里表现得非常隐蔽因为 OpenCV 不会报错只是返回空结果。解决把打印时使用的字典和 ID 记录成一份参数文件与相机内参放在同一个配置目录。检测程序启动时打印当前字典名称方便和实际码核对。在上机之前先用一张真机拍摄的图像离线跑一遍检测确认能检测到之后再启动降落程序。5.2 二维码反光导致位姿跳变现象晴朗天气下普通 A4 纸打印的二维码在太阳光下局部反光飞行器悬停时 tvec 数值偶尔会跳变二三十厘米水平修正指令跟着乱抖。原因反光区域导致角点提取的位置偏差亚像素角点被拉到高光中心位姿解算时这个误差被几何关系放大。尤其是二维码边缘出现反光时影响更大。解决打印二维码不要用光面纸用哑光纸或覆哑膜。控制侧也不要对原始 tvec 直接做控制加一个窗口为 3 到 5 帧的中值滤波能滤掉明显的跳变。如果跳变仍然频繁把检测参数里minMarkerPerimeterRate调大一点滤掉由于高光产生的错误轮廓。5.3 下降过程中二维码出了视野现象水平对齐时一切正常下降到一米左右高度时二维码突然从图像中消失。此时飞控手里已经没有位姿信息只能盲降最终落偏。原因下降过程中机体会因为侧风和自身姿态调整产生摆动下视相机的视野中心也随之偏移。如果相机视场角不够大二维码在低空时正好溜出画面边缘。另一个原因是水平修正指令让飞行器侧倾侧倾角度过大时相机的光轴不再垂直于地面。解决下降触发条件里除了水平偏差阈值还要检查姿态倾角超过 15 度就禁止下降。同时设置丢帧保护连续 5 帧以上没有检测到二维码立即停止下降并悬停等待重新识别。最后这个问题可以这样兜底在接近地面时不再依赖二维码而是靠超声波或激光测距触发停机让视觉只负责水平修正。5.4 视觉高度与气压计高度打架现象下降过程中给飞控同时喂了视觉解算的高度和气压计高度两个数值在 0.5 米附近相差越来越大飞控突然跳变修正整个机身猛抖一下。原因气压计在近地面受电机下洗气流影响很大数值漂移明显。视觉高度和气压计在切换时没有平滑过渡直接从一个值跳到另一个值。解决高度控制在 1.5 米以上以气压计为主1.5 米以下切换为视觉高度或激光测距切换带内做线性插值不要硬切。我见过有人想省事把两个高度取平均结果在两种传感器误差方向相反时反而更差。正确的做法是明确谁主导另一个只做参考和校验。5.5 风场里修正过度飞行器荡秋千现象户外 2 到 3 级风的环境下飞行器水平修正方向是对的但每次逼近二维码都会过冲绕着一个点来回摆幅度越来越大。原因P 参数偏大且没有限制修正速度上限。风本身就在不断推偏飞行器修正速度又被限制得过高形成了振荡的正反馈。解决把最大水平修正速度限制到 0.3 米/秒同时把 P 降到 0.5 左右。如果振荡还在加小的 D 系数。另一个有效做法是变增益远离二维码时 P 大一点靠近后 P 减小让收敛过程更柔顺。这个功能在 PX4 的 offboard 模式里自己写就行不需要改飞控底层的参数。6. 验证 V2 版本稳定性的落地方法与进阶技巧动态降落系统的验证不能一上来就真机全流程。我习惯把验证分成三层。第一层是离线回放把一次完整降落过程中的下视相机视频和飞控姿态日志一起录下来在电脑上离线跑一遍检测和位姿解算对比控制指令和实际轨迹。这一步能帮你发现坐标变换错误、参数异常这类问题不需要反复上机试错。第二层是静态跟踪飞行器悬停在 1.5 米高度人为移动二维码确认水平修正方向正确无振荡。第三层才是完整降落并且第一次完整降落时不要期望一次成功可以把二维码放在一块软垫子附近即使落偏也不会摔坏。验证过程中一个容易被忽略的点是记录时间戳。视觉位姿话题和飞控状态话题的时间戳必须对齐到同一时钟域才能准确判断“二维码检测到偏差之后飞控用了多少毫秒响应”。如果时间戳错位你看到的就是一个无法解释的滞后现象排查起来很痛苦。进阶技巧里最有实用价值的一条是“丢帧保持”策略。视觉检测不可能做到每一帧都成功偶尔丢几帧是正常的。不要一丢帧就悬停否则二维码稍微一低角度就会导致系统反复中止下降。正确的做法是丢帧少于 3 帧时保持上一帧的控制指令丢帧超过 5 帧再进入悬停恢复逻辑。这个策略能把降落成功率提升一大截。另一个进阶方向是放大降落场的覆盖范围。单块 ArUco 在 10 米高度就已经很难检测了如果想让无人机在更高的高度就开始对准降落点可以用多块二维码拼成一个降落垫。每块码大小一致通过离线标定确认彼此的相对位置统一换算到降落垫中心坐标系。视觉节点在远距离时任意检测到其中一块码就能提供粗略对准近地面后再用中心码做精细修正。这样做之后降落距离和成功率都会有明显提升。我第一次做这个功能时把水平修正速度上限设得太大结果飞行器在半米高度被吹得左右摆最后靠“丢帧保持”这个策略才把降落稳下来。从那以后我养成一个习惯所有视觉降落参数都写成配置文件每次改参数都记录日期和效果方便翻回去查。希望帮到你。本文还有配套的精品资源点击获取
