简介vision_to_mavros 是一组面向无人机视觉定位与飞控通信的转换工具定位在外部视觉系统与飞控堆栈之间主要服务于使用 ROS 或非 ROS 环境、希望将 AprilTag、VIO、SLAM、Realsense T265 以及深度图像等视觉数据接入 MAVLink 或 MAVROS 的开发者。压缩包共 38 个文件约 94KB以 Python 脚本、launch 启动文件和 JSON/C 配置为主覆盖鱼眼图像去畸变、tf 位姿到 NED 坐标系转换、深度图像过滤、相机参数配置、外参校准、原点设置、MAVLink 消息封装等环节并且提供可直接运行的启动文件和 ArduPilot 飞行控制器配合示例。资源中还包含多组视觉定位与控制的 Python 工具脚本区分 ROS 与非 ROS 两条使用路径并附有安装配置与 ArduPilot 示例说明便于在仿真或真机上验证精确定位、自主导航与降落等任务。已有 1628 人浏览学习适合具备基础 ROS 与无人机开发经验、需要快速打通视觉定位到飞控链路的开发者参考。1. vision_to_mavros把视觉位姿“翻译”成飞控能听懂的语言做无人机视觉定位的人大多会在同一个地方卡壳算法侧明明已经算出了位姿飞控却不认账。ArduPilot 这类飞控不关心你用的是 ArUco 基准标签还是 VIO、SLAM 算法它只接收自己约定好的消息——ROS 环境下是 mavros 的 /mavros/vision_pose/pose 话题非 ROS 环境下是 VISION_POSITION_ESTIMATE 报文。vision_to_mavros 就是用来补齐这段“最后一公里”的代码集合把基准标签、VIO、SLAM、深度图像等视觉源的数据统一整理成带正确时间戳、正确坐标系的飞控消息。适合正在做无人机视觉导航定位的开发者尤其是算法能跑通、但一直没搞定飞控消息格式的人。2. 架构拆解从相机像素到 MAVLink 消息的完整链路2.1 数据链路总览视觉消息怎么被飞控消化掉的把整个链路拉直了看vision_to_mavros 处在视觉源和飞控之间的中间层做的事情可以压缩成五个环节视觉源 → 位姿或距离提取 → 坐标变换与时间戳整理 → 消息封装 → 飞控 EKF 融合视觉源可以是 ArUco 基准标签、能做 VIO 的双目相机或专用硬件、ORB-SLAM 这类纯算法也可以是深度相机输出的深度图。vision_to_mavros 不关心你前面用了什么它只把后面三步做扎实。首先是坐标归一化。VIO 和 SLAM 的输出坐标五花八门有 ENU、有 NEU、也有相机坐标系而 ArduPilot 的 EKF 期望的是 NED 坐标系北-东-地。如果把这些数据直接发给飞控飞控会默认它们是 NED结果飞机一起飞就往奇怪的方向冲。代码里每个输入源都配了一个坐标映射函数把 ENU 转 NED、相机系转机体系这一步是整个项目的核心价值也是后文会反复提到的“翻车点”。其次是时间戳统一。ROS 侧的时间戳是秒为单位的浮点数MAVLink 侧要求的是微秒整数。如果直接把 ROS 的 time 塞进 MAVLink 的 time_usec飞控会因为时间戳跳变直接把数据过滤掉。这里在桥接层把时间统一换算成微秒并允许配置一个时钟偏移量来对齐两套系统避免视觉系统和飞控之间因为启动时间不同带来的时间基准偏差。最后是消息形态分离。ROS 场景走 mavros 的 vision_pose 话题非 ROS 场景走 pymavlink 直发 MAVLink。两套出口共用同一份坐标变换逻辑避免改了一个地方忘了另一个。这套设计看起来简单但拆过几个类似项目之后你会发现很多人恰恰是把顺序搞反了先写消息发布再回头补坐标变换最后才发现时间戳不对整个链路推倒重来。vision_to_mavros 的价值不是代码量多大而是把“先归一化、再封装”这个顺序立住了。2.2 目录结构ROS 与非 ROS 两套入口的分工拆包之后的目录结构非常清晰一眼就能看出作者把两套使用场景刻意分开vision_to_mavros/ ├── ros_ws/ │ └── src/ │ └── vision_to_mavros/ │ ├── launch/ # 各视觉源的启动文件 │ ├── config/ # 坐标映射与相机参数 │ ├── nodes/ # Python 节点脚本 │ └── scripts/ # 标定与日志辅助脚本 ├── non_ros/ │ ├── aruco_loc.py # ArUco 基准标签位姿估计 │ ├── vio_bridge.py # VIO/SLAM 数据转发 │ ├── depth_obstacle.py # 深度图转障碍物距离 │ ├── mavlink_common.py # 公共 MAVLink 封装 │ └── config/ └── README.mdROS 侧适合已经在跑 ROS 的团队比如 Gazebo 仿真、已有相机话题、想用 rviz 实时看位姿的情况。launch 文件把数据流整理成一条链启动一个文件就能拉起相机驱动、坐标变换和 mavros 转发三个节点。非 ROS 侧适合边缘设备。树莓派、NX 板子上没有 ROS也不想为了发几个消息装一套 ROS 环境直接跑 Python 脚本就行。这一侧的依赖只有 pymavlink、numpy、opencv 三个库一个 venv 就能跑起来。两套入口的消息形态虽然不同但底层逻辑一致raw 数据进去、NED 消息出来。实际对比两个入口的代码会发现真正有区别的主回路只在“消息构造”那一层坐标变换部分几乎可以逐行复用。这也是我推荐二次开发时先读 mavlink_common.py 的原因——它承载了整个项目里最通用的那部分逻辑。2.3 消息接口约定vision_pose 话题与 MAVLink 报文的对应关系这里需要澄清一个最常见的困惑vision_to_mavros 并不直接跟飞控对话它把视觉数据转换后交给 mavros或者直接发送给飞控。既然有两条道就需要两组消息接口。ROS 侧的核心话题是 /mavros/vision_pose/pose类型是 geometry_msgs/PoseStamped。mavros 订阅这个话题后把内容封装成 MAVLink 的 VISION_POSITION_ESTIMATE 消息再通过串口或 UDP 发送给飞控。如果你要同步发速度估计可以用 /mavros/vision_speed/speed_topic避障场景对应 /mavros/obstacle_distance类型是 mavros_msgs/ObstacleDistance。非 ROS 侧直接调用 pymavlink 的 vision_position_estimate_send 和 vision_speed_estimate_send把同样的数据以 MAVLink 二进制格式发出。两条链路的差异只在序列化层数据内容完全等价。设计上还有个值得借鉴的点消息接口和坐标系转换是解耦的。即便后续飞控固件或 mavros 改了话题名只要改 launch 文件里的 remap 参数不需要动 Python 代码。我第二次读这套代码时才注意到这个细节但正是这一点让它在不同 ArduPilot 版本之间迁移时省了很多事。2.4 坐标系约定ENU、NED 与机体坐标系的爱恨情仇坐标系问题值得单独一节因为它是这个项目里最玄学也最容易翻车的部分。视觉系统的常见输出约定有三种ENUx 东、y 北、z 天、相机坐标系x 右、y 下、z 前、以及 VIO 里程计常用的 NEUx 北、y 东、z 天。ArduPilot 的 EKF 是 NED 体系所以最终消息的坐标轴约定必须落到 NED 上。视觉源输出到 NED 的核心处理注意点ENUx东 y北 z天x 和 y 交换z 取反位置要换姿态角也要换NEUx北 y东 z天只翻转 z速度向量同样要处理相机系x右 y下 z前先用 R_body_cam 转到机体系还要再走一次 ENU→NED深度图距离不需要坐标变换但要注意单位是米还是毫米这里有个细节经常被无视桥接节点返回的 yaw 角也应该做同样的轴变换。如果只换了位置向量没换欧拉角飞控会拿到“位置朝北、机头朝西”的怪异组合悬停时还能勉强用一给航线就失控。我一般会在接入新视觉源时先在桌面上用一张已知大小的 ArUco 标签做静态测试把标签放在机体的正北方看输出的 x 是正还是负、y 是不是零。这个测试十分钟就能做完能省下后文会讲到的至少半天排查时间。3. 核心代码实现从基准标签、VIO 到 mavros 的完整拆解3.1 ArUco 基准标签像素坐标到位姿消息的完整流程ArUco 这一路是 vision_to_mavros 里最完整的示例因为它覆盖了从图像输入到位姿发布的全过程。核心思路是先用 OpenCV 检测标签角点再用 PnP 求解出标签在相机坐标系下的位姿最后经过坐标变换发出去。import cv2 import numpy as np import rospy from cv_bridge import CvBridge from geometry_msgs.msg import PoseStamped bridge CvBridge() pub rospy.Publisher(/mavros/vision_pose/pose, PoseStamped, queue_size1) # 相机内参与标签物理边长来自 config 文件 camera_matrix np.load(config/camera_matrix.npy) dist_coeffs np.load(config/dist_coeffs.npy) marker_size 0.18 # 标签边长单位米 aruco_dict cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) aruco_params cv2.aruco.DetectorParameters_create() def image_callback(msg): frame bridge.imgmsg_to_cv2(msg, desired_encodingbgr8) corners, ids, _ cv2.aruco.detectMarkers( frame, aruco_dict, parametersaruco_params) if ids is None: return rvecs, tvecs, _ cv2.aruco.estimatePoseSingleMarkers( corners, marker_size, camera_matrix, dist_coeffs) # tvec 是相机坐标系下的位置先转到机体系再转 ENU-NED pos_cam tvecs[0].flatten() pos_body R_body_cam pos_cam # 相机系 - 机体系 pos_enu pos_body body_offset # 补偿相机安装偏移 pos_ned np.array([pos_enu[1], pos_enu[0], -pos_enu[2]]) pose_msg PoseStamped() pose_msg.header.stamp rospy.Time.now() pose_msg.header.frame_id map pose_msg.pose.position.x pos_ned[0] pose_msg.pose.position.y pos_ned[1] pose_msg.pose.position.z pos_ned[2] pub.publish(pose_msg)这段代码的逻辑是先用 aruco.detectMarkers 检测四角点得到 corners 和 ids再用 estimatePoseSingleMarkers 得到旋转向量 rvecs 和平移向量 tvecs。tvecs 是从相机光心指向标签中心的向量坐标系是相机坐标系R_body_cam 是相机安装矩阵由安装朝向决定把相机系转到机体系最后把机体系坐标从 ENU 转成 NED 发布出去。飞控看到的永远是一个干净的 NED 坐标。参数说明一下marker_size 是基准标签的物理边长单位是米直接影响距离估计的绝对尺度camera_matrix 和 dist_coeffs 来自相机标定如果标定不准标签在 2 米距离上的位置误差会被放大到 5 厘米以上这直接影响飞控贴近时的精度。还有一个容易忽略的点R_body_cam 和 body_offset 必须对应相机实际安装位置否则视角差会在近距离产生明显的定位偏差。3.2 VIO/SLAM 数据转发一个通用的话题桥VIO/SLAM 接入的代码更薄本质是一个“话题到话题”的转发器。它把上游视觉里程计输出的位姿话题接进来做坐标变换和时间戳整理再发布到 mavros 的视觉话题上。#!/usr/bin/env python3 import rospy from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped class VioBridge: def __init__(self): self.source_topic rospy.get_param(~source_topic, /camera/pose) self.output_topic rospy.get_param(~output_topic, /mavros/vision_pose/pose) self.coord_mode rospy.get_param(~coord_mode, ENU) self.yaw_offset rospy.get_param(~yaw_offset, 0.0) self.sub rospy.Subscriber(self.source_topic, PoseWithCovarianceStamped, self.cb) self.pub rospy.Publisher(self.output_topic, PoseStamped, queue_size1) def cb(self, msg): p msg.pose.pose # 简化版 ENU - NED 变换实际代码会从 config 读安装参数 ned PoseStamped() ned.header.stamp rospy.Time.now() ned.header.frame_id map ned.pose.position.x p.position.y ned.pose.position.y p.position.x ned.pose.position.z -p.position.z # 注意这里只处理了平移yaw_offset 的修正需要单独实现 self.pub.publish(ned)之所以能写这么薄是因为 VIO/SLAM 在业务上已经把位姿算完了这里的桥只需要做两件事坐标系归一化加时间戳刷新。VIO 输出的 header.stamp 可能来自自己的时钟和飞控时钟存在漂移简单的做法是用本机时间覆盖更严谨的可以做时钟偏移估计但大多数仿真和短程飞行场景下直接覆盖已经够用。几个参数值得关注source_topic 是上游 VIO 发布的位姿话题coord_mode 用来指定上游坐标约定支持 ENU/NED/body 三种取值yaw_offset 用于修正 VIO 初始航向和真实航向之间的夹角。VIO 里程计启动时通常会给自己一个任意航向基准这个 offset 如果不配飞控融合出来的机头朝向就会差一个固定角度。有一个容易被忽略的点VIO 的输出频率通常能到 30~60Hz但 ArduPilot 的 EKF 并不需要那么高频20Hz 左右就够。桥接节点里可以做一个简单的降采样逻辑用余数方式每 N 帧发一条能有效降低串口和 rosbag 的流量压力对长时间飞行的稳定性有帮助。3.3 深度图像转障碍物距离从深度图到 OBSTACLE_DISTANCE深度图这一路不走位姿消息走避障消息。它把深度相机的 2D 距离信息按方位角划分成若干扇区每个扇区取最近障碍物距离交给飞控的避障逻辑去用。import numpy as np def depth_to_sectors(depth_image, sector_num8, max_range5.0, camera_configNone): depth_image: 深度图单位可能是毫米或米 sector_num: 扇区数量一般 8 或 12 max_range: 最大感知距离超过视为无障碍 h, w depth_image.shape cx, cy camera_config[cx], camera_config[cy] fx, fy camera_config[fx], camera_config[fy] # 投影到相机坐标系得到每个像素对应的方向向量 u (np.arange(w) - cx) / fx v (np.arange(h) - cy) / fy x np.tile(u, (h, 1)) y np.tile(v.reshape(-1, 1), (1, w)) z depth_image / 1000.0 # 如果深度图单位是毫米则转成米 mask (z 0.1) (z max_range) if not mask.any(): return [max_range] * sector_num angle np.arctan2(x, -y) # 以机头方向为 0 度 sector_idx ((angle / (2 * np.pi)) * sector_num).astype(int) % sector_num sectors [max_range] * sector_num for i in range(sector_num): candidates z[mask (sector_idx i)] if candidates.size 0: sectors[i] np.min(candidates) return sectors这段代码的核心是扇区化把深度图按相机视角对应的方位角切分每个扇区里找最小深度这个值就是飞控避障时使用的“该方向最近障碍距离”。sector_num 越多避障方向越精细但消息长度也越长串口带宽有限时需要做取舍max_range 是最大感知距离超过这个距离的像素会被忽略防止远处的噪点干扰。深度的单位是个容易翻车的点。相当多的深度驱动输出的是 16 位毫米也有输出 float 米的代码里 /1000.0 那一步必须跟驱动文档对清楚。如果你用的相机直接输出米再除以 1000避障距离就会整体缩小一千倍飞控会以为障碍物近在咫尺。这种错误在仿真环境里看不出问题真机上会非常惊悚。3.4 非 ROS 出口pymavlink 直发 MAVLink 的两条函数非 ROS 侧的出口就简单直接了pymavlink 封装好了 MAVLink 消息构造我们只需要填数据。import time from pymavlink import mavutil mav mavutil.mavlink_connection(udp:127.0.0.1:14550, source_system1) mav.wait_heartbeat() # 发送视觉位姿估计坐标为 NED单位米欧拉角单位弧度 mav.mav.vision_position_estimate_send( int(time.time() * 1e6), # usec 时间戳微秒 pos_ned[0], pos_ned[1], pos_ned[2], roll, pitch, yaw ) # 如果视觉系统还能给出速度一起发对 EKF 更友好 mav.mav.vision_speed_estimate_send( int(time.time() * 1e6), vel_ned[0], vel_ned[1], vel_ned[2] )pymavlink 这条线的好处是没有 ROS 依赖在 NX 板上单独跑一个进程就能把消息送到飞控。发送前需要 wait_heartbeat() 确认与飞控的链路已经建立否则前几帧数据会被当作陌生系统丢弃。如果飞控要求接收端带系统 IDsource_system 必须与地面站配置一致这里填 1 是常见约定。这段代码虽然短但它引出一个重要事实非 ROS 和 ROS 两种出口的核心目标完全一致区别只在帧格式。所以不要把这两套当成两个系统而要当成“一套转换逻辑两个传输通道”。写代码时把坐标变换函数做成公共模块两边 import 同一个文件就不会出现一边改了坐标系、另一边忘了改的尴尬。4. 跑起来ArduPilot SITL 环境下的最小联调流程与验证方法4.1 环境准备依赖清单与最小安装路径在真机之前先在 ArduPilot SITL 仿真里把链路跑通是最稳妥的做法。环境准备按“ROS 侧”和“非 ROS 侧”两条线来装这里以 ROS 侧为例。# 安装 mavros 与 mavros_msgs按你自己的 ROS 发行版选择包名 sudo apt install ros-distro-mavros ros-distro-mavros-msgs # 安装 python 依赖 pip install numpy opencv-python pymavlink装完之后确认 mavros 的 launch 文件在。mavros 装好后会自带 apm.launch它负责把 MAVLink 数据转成 ROS 话题。如果你用的是 ArduPilot SITL不需要额外硬件电脑上直接起仿真就能测。非 ROS 侧更简单只要 python3 环境加 pymavlink、numpy、opencv-python 三个库。我一般会用 venv 单独建一个环境避免和系统的依赖冲突。4.2 最短启动路径三个终端三行命令SITL 联调的最小启动路径是三个终端分别跑仿真、mavros 和视觉桥。# 终端1启动 ArduPilot SITL并把 MAVLink 输出到本机 14550 端口 sim_vehicle.py -v ArduCopter --map --console --out 127.0.0.1:14550 # 终端2启动 mavros连接 SITL 输出的端口 roslaunch mavros apm.launch fcu_url:udp://127.0.0.1:14550 # 终端3启动视觉桥接节点订阅上游视觉话题 rosrun vision_to_mavros vio_bridge.py _source_topic:/camera/pose启动顺序有讲究先起 SITL再起 mavros最后起视觉桥。SITL 的 --out 参数把 MAVLink 打包发到 UDP 14550 端口mavros 的 fcu_url 也指向这个端口两者构成一条双向链路。如果顺序反了mavros 会一直重连虽然不影响最终结果但会在日志里刷一大片错误干扰排查。视觉桥节点的 source_topic 参数要指向你实际视觉源的话题。如果没有现成的视觉源可以先做一个 fake_pose.py 定时发布 PoseStamped用来验证 mavros 链路本身是通的。这一步能帮你区分“问题在视觉算法”还是“问题在桥接配置”。4.3 链路验证从话题频率到 EKF 状态的四步检查链路起来后不要急着让飞机起飞先做四步验证。# 第一步确认话题有数据 rostopic echo /mavros/vision_pose/pose -n 5 # 第二步确认话题频率达标 rostopic hz /mavros/vision_pose/pose第一步看数据内容x、y、z 值是否在你的视觉源的合理量程内有没有出现 NaN。第二步看频率ArduPilot 的 EKF 融合视觉源一般要求不低于 10Hz稳定在 20Hz 以上比较理想。如果频率只有 5Hz飞控大概率会忽略这部分数据。频率不达标时检查上游视觉源是不是本身就很慢或者桥接节点里是不是多了不必要的耗时操作。第三步是动动飞机在 SITL 界面里手动拖动机体位置看视觉位姿话题是否跟随。这个动作能最快暴露坐标系反了的问题——你往北拖结果 x 在减少那肯定是坐标映射不对。第四步是看飞控日志或地面站里的 EKF 状态。在 QGC 或 Mission Planner 里观察 EKF 状态确认视觉源被列为参与融合的传感器之一。如果视觉数据一直在发但状态里根本没有这一项多半是 EKF 参数没开启视觉源这个要回到飞控参数里去配。5. 避坑指南视觉定位上机常见的翻车现场与排查思路5.1 现象飞机一起飞就往一个方向猛冲推杆也拉不住原因基本可以锁定在坐标系方向反了。飞控按 NED 解算你给的视觉位姿如果是 ENU 或相机系坐标等于告诉飞控“北在那边”它当然往错误方向加速。这种情况在静止悬停时可能看不出来因为位置没变化但一给速度指令就原形毕露。解决方法是先做静态坐标验证把视觉源放在机体的正北方查看发布出去的 x 值是不是大于 0。如果 x 为负交换一下映射关系再试。切记位置向量和姿态角要一起验证只修一个往往会带歪另一个。5.2 现象话题有数据但飞控日志里完全没有视觉融合记录先别怀疑飞控先查频率和时间戳。ArduPilot 的 EKF 对视觉源的更新频率有最低要求常见的阈值在 10Hz 左右低于这个会被丢弃。另一个常见原因是时间戳问题ROS time 是秒MAVLink 要求微秒如果差了一千倍飞控会认为数据来自未来直接拒绝。解决分两步一是用 rostopic hz 确认频率达标不达标就在桥里做缓存锁存最新一帧并按固定频率推送二是检查 time_usec 的换算确保单位是微秒且没有溢出。5.3 现象标签静止时位置稳定但相机稍微动一下位置就剧烈跳变原因大概率是 ArUco 检测在运动模糊帧上出了误检。estimatePoseSingleMarkers 对 corner 的亚像素精度很敏感模糊帧会让角点偏移几个像素近距离下这就会变成几厘米的跳变。解决方式有三层第一层是在检测端做质量过滤把角点分布方差过大的帧丢掉第二层是在桥接端对位姿做低通滤波比如一阶滞后滤波让输出平滑第三层是提高相机帧率或缩短曝光时间从根源上减少运动模糊。如果连续多帧检测不到标签我一般会让桥接节点主动停发视觉位置让飞控切回 GPS 模式而不是把最后一次检测结果一直怼给飞控。5.4 现象VIO 跑了半小时开始慢慢漂但飞控还在继续信任它VIO 漂移是必然的飞控自己无法判断视觉数据变坏了多少桥接层的责任是及时发现漂移并触发重置。解决思路是加一个“视觉/GPS 一致性检查”定期比较视觉位置和 GPS 位置的偏差当偏差超过阈值时给飞控发一个视觉位置重置指令或者直接停发视觉数据让 EKF 回到 GPS 融合。从实际效果看加了这个逻辑之后长航时任务的可靠性提升非常明显。很多人忽略这一层只做了坐标变换结果就是前半段飞得好好的后半段开始莫名偏航。5.5 现象近距离定位精度差得离谱标签明明在正前方位置却偏了十几厘米这类问题八成出在标定和物理尺寸上。marker_size 每偏 1 厘米2 米距离上的位置误差就会放大到 5 厘米以上相机内参如果是从同型号相机“复制”来的而不是实际标定畸变修正一错边缘区域的角点坐标就全歪了。解决方式是回到源头用棋盘格重新标定相机用卡尺实测标签边长把这两个值填进 config 之后再做一次静态验证。这个坑最坑的地方在于它不会报错所有参数看起来都合理就是精度差那么一点。我自己的习惯是每次换相机或者换标签型号都会强制重新走一遍标定流程。6. 二次开发接入一个新的视觉定位源的完整套路6.1 代码复用写一个最小适配器接新视觉源时不需要从头读全仓库只需要仿照 vio_bridge.py 写一个适配器。核心就一个方法把上游的原始位姿转换成 NED然后发布出去。坐标系变换逻辑从 mavlink_common.py 里 import消息发布直接复用现有函数。# 新视觉源接入的最小模板 class CustomVisionAdapter: def __init__(self, coord_modeENU): self.coord_mode coord_mode self.pub rospy.Publisher(/mavros/vision_pose/pose, PoseStamped, queue_size1) def handle_pose(self, raw_pose): ned to_ned(raw_pose, self.coord_mode) # 从 mavlink_common import self.pub.publish(ned)6.2 接入前强制自查的三项清单第一坐标系约定写清楚。上游输出到底是 ENU 还是相机系姿态角用什么表示全都要在适配器里显式转换不要靠猜。第二时间戳对齐方式。上游时间基准是什么和飞控差多少统一在入口处处理。第三频率策略。上游是 30Hz 还是 5Hz降采样还是锁存重发提前定好。这三项确认完再谈别的。6.3 从“能飞”到“好飞”的参数调整链路通了之后剩下的就是调信任度。EKF 融合视觉源的时候参数里通常会有一个控制视觉位置置信度的系数数值越大飞控越信任视觉。稳定悬停时可以把置信度调高让位置保持更紧环境光线变化大、视觉容易丢的时候置信度要降下来避免视觉一个跳变把整个姿态带飞。VIO 的漂移补偿也要持续观察。我习惯在每次长航时测试后看一眼飞控日志里的定位误差曲线如果误差随时间单调增大说明视觉源在缓慢漂移需要在桥接层加强一致性检查逻辑而不是无脑提高信任系数。6.4 我的习惯强制先跑坐标系检查再谈上天接新视觉源最容易翻车的时刻就是你觉得“这个输出应该是 ENU应该没错”的时候。我被这种“应该”坑过不止一次。某次在仿真里调好了所有参数真机上飞机一起飞就朝反方向冲原因就是真实相机安装角度跟仿真的差了 90 度而我偷懒没做静态坐标验证就直接切了自动模式。从那以后我每次接入新的视觉源都强制先在 SITL 里做一遍固定标签测试确认 x、y、z 三个轴的方向和位移跟随全部正确才允许自己碰真机。这套流程救过我至少三次希望你也能少踩几个我踩过的坑。希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/10/19 2:53:43 · 阅读 1.2k vision_to_mavros：无人机视觉定位到飞控MAVLink消息的桥接实践
简介vision_to_mavros 是一组面向无人机视觉定位与飞控通信的转换工具定位在外部视觉系统与飞控堆栈之间主要服务于使用 ROS 或非 ROS 环境、希望将 AprilTag、VIO、SLAM、Realsense T265 以及深度图像等视觉数据接入 MAVLink 或 MAVROS 的开发者。压缩包共 38 个文件约 94KB以 Python 脚本、launch 启动文件和 JSON/C 配置为主覆盖鱼眼图像去畸变、tf 位姿到 NED 坐标系转换、深度图像过滤、相机参数配置、外参校准、原点设置、MAVLink 消息封装等环节并且提供可直接运行的启动文件和 ArduPilot 飞行控制器配合示例。资源中还包含多组视觉定位与控制的 Python 工具脚本区分 ROS 与非 ROS 两条使用路径并附有安装配置与 ArduPilot 示例说明便于在仿真或真机上验证精确定位、自主导航与降落等任务。已有 1628 人浏览学习适合具备基础 ROS 与无人机开发经验、需要快速打通视觉定位到飞控链路的开发者参考。1. vision_to_mavros把视觉位姿“翻译”成飞控能听懂的语言做无人机视觉定位的人大多会在同一个地方卡壳算法侧明明已经算出了位姿飞控却不认账。ArduPilot 这类飞控不关心你用的是 ArUco 基准标签还是 VIO、SLAM 算法它只接收自己约定好的消息——ROS 环境下是 mavros 的 /mavros/vision_pose/pose 话题非 ROS 环境下是 VISION_POSITION_ESTIMATE 报文。vision_to_mavros 就是用来补齐这段“最后一公里”的代码集合把基准标签、VIO、SLAM、深度图像等视觉源的数据统一整理成带正确时间戳、正确坐标系的飞控消息。适合正在做无人机视觉导航定位的开发者尤其是算法能跑通、但一直没搞定飞控消息格式的人。2. 架构拆解从相机像素到 MAVLink 消息的完整链路2.1 数据链路总览视觉消息怎么被飞控消化掉的把整个链路拉直了看vision_to_mavros 处在视觉源和飞控之间的中间层做的事情可以压缩成五个环节视觉源 → 位姿或距离提取 → 坐标变换与时间戳整理 → 消息封装 → 飞控 EKF 融合视觉源可以是 ArUco 基准标签、能做 VIO 的双目相机或专用硬件、ORB-SLAM 这类纯算法也可以是深度相机输出的深度图。vision_to_mavros 不关心你前面用了什么它只把后面三步做扎实。首先是坐标归一化。VIO 和 SLAM 的输出坐标五花八门有 ENU、有 NEU、也有相机坐标系而 ArduPilot 的 EKF 期望的是 NED 坐标系北-东-地。如果把这些数据直接发给飞控飞控会默认它们是 NED结果飞机一起飞就往奇怪的方向冲。代码里每个输入源都配了一个坐标映射函数把 ENU 转 NED、相机系转机体系这一步是整个项目的核心价值也是后文会反复提到的“翻车点”。其次是时间戳统一。ROS 侧的时间戳是秒为单位的浮点数MAVLink 侧要求的是微秒整数。如果直接把 ROS 的 time 塞进 MAVLink 的 time_usec飞控会因为时间戳跳变直接把数据过滤掉。这里在桥接层把时间统一换算成微秒并允许配置一个时钟偏移量来对齐两套系统避免视觉系统和飞控之间因为启动时间不同带来的时间基准偏差。最后是消息形态分离。ROS 场景走 mavros 的 vision_pose 话题非 ROS 场景走 pymavlink 直发 MAVLink。两套出口共用同一份坐标变换逻辑避免改了一个地方忘了另一个。这套设计看起来简单但拆过几个类似项目之后你会发现很多人恰恰是把顺序搞反了先写消息发布再回头补坐标变换最后才发现时间戳不对整个链路推倒重来。vision_to_mavros 的价值不是代码量多大而是把“先归一化、再封装”这个顺序立住了。2.2 目录结构ROS 与非 ROS 两套入口的分工拆包之后的目录结构非常清晰一眼就能看出作者把两套使用场景刻意分开vision_to_mavros/ ├── ros_ws/ │ └── src/ │ └── vision_to_mavros/ │ ├── launch/ # 各视觉源的启动文件 │ ├── config/ # 坐标映射与相机参数 │ ├── nodes/ # Python 节点脚本 │ └── scripts/ # 标定与日志辅助脚本 ├── non_ros/ │ ├── aruco_loc.py # ArUco 基准标签位姿估计 │ ├── vio_bridge.py # VIO/SLAM 数据转发 │ ├── depth_obstacle.py # 深度图转障碍物距离 │ ├── mavlink_common.py # 公共 MAVLink 封装 │ └── config/ └── README.mdROS 侧适合已经在跑 ROS 的团队比如 Gazebo 仿真、已有相机话题、想用 rviz 实时看位姿的情况。launch 文件把数据流整理成一条链启动一个文件就能拉起相机驱动、坐标变换和 mavros 转发三个节点。非 ROS 侧适合边缘设备。树莓派、NX 板子上没有 ROS也不想为了发几个消息装一套 ROS 环境直接跑 Python 脚本就行。这一侧的依赖只有 pymavlink、numpy、opencv 三个库一个 venv 就能跑起来。两套入口的消息形态虽然不同但底层逻辑一致raw 数据进去、NED 消息出来。实际对比两个入口的代码会发现真正有区别的主回路只在“消息构造”那一层坐标变换部分几乎可以逐行复用。这也是我推荐二次开发时先读 mavlink_common.py 的原因——它承载了整个项目里最通用的那部分逻辑。2.3 消息接口约定vision_pose 话题与 MAVLink 报文的对应关系这里需要澄清一个最常见的困惑vision_to_mavros 并不直接跟飞控对话它把视觉数据转换后交给 mavros或者直接发送给飞控。既然有两条道就需要两组消息接口。ROS 侧的核心话题是 /mavros/vision_pose/pose类型是 geometry_msgs/PoseStamped。mavros 订阅这个话题后把内容封装成 MAVLink 的 VISION_POSITION_ESTIMATE 消息再通过串口或 UDP 发送给飞控。如果你要同步发速度估计可以用 /mavros/vision_speed/speed_topic避障场景对应 /mavros/obstacle_distance类型是 mavros_msgs/ObstacleDistance。非 ROS 侧直接调用 pymavlink 的 vision_position_estimate_send 和 vision_speed_estimate_send把同样的数据以 MAVLink 二进制格式发出。两条链路的差异只在序列化层数据内容完全等价。设计上还有个值得借鉴的点消息接口和坐标系转换是解耦的。即便后续飞控固件或 mavros 改了话题名只要改 launch 文件里的 remap 参数不需要动 Python 代码。我第二次读这套代码时才注意到这个细节但正是这一点让它在不同 ArduPilot 版本之间迁移时省了很多事。2.4 坐标系约定ENU、NED 与机体坐标系的爱恨情仇坐标系问题值得单独一节因为它是这个项目里最玄学也最容易翻车的部分。视觉系统的常见输出约定有三种ENUx 东、y 北、z 天、相机坐标系x 右、y 下、z 前、以及 VIO 里程计常用的 NEUx 北、y 东、z 天。ArduPilot 的 EKF 是 NED 体系所以最终消息的坐标轴约定必须落到 NED 上。视觉源输出到 NED 的核心处理注意点ENUx东 y北 z天x 和 y 交换z 取反位置要换姿态角也要换NEUx北 y东 z天只翻转 z速度向量同样要处理相机系x右 y下 z前先用 R_body_cam 转到机体系还要再走一次 ENU→NED深度图距离不需要坐标变换但要注意单位是米还是毫米这里有个细节经常被无视桥接节点返回的 yaw 角也应该做同样的轴变换。如果只换了位置向量没换欧拉角飞控会拿到“位置朝北、机头朝西”的怪异组合悬停时还能勉强用一给航线就失控。我一般会在接入新视觉源时先在桌面上用一张已知大小的 ArUco 标签做静态测试把标签放在机体的正北方看输出的 x 是正还是负、y 是不是零。这个测试十分钟就能做完能省下后文会讲到的至少半天排查时间。3. 核心代码实现从基准标签、VIO 到 mavros 的完整拆解3.1 ArUco 基准标签像素坐标到位姿消息的完整流程ArUco 这一路是 vision_to_mavros 里最完整的示例因为它覆盖了从图像输入到位姿发布的全过程。核心思路是先用 OpenCV 检测标签角点再用 PnP 求解出标签在相机坐标系下的位姿最后经过坐标变换发出去。import cv2 import numpy as np import rospy from cv_bridge import CvBridge from geometry_msgs.msg import PoseStamped bridge CvBridge() pub rospy.Publisher(/mavros/vision_pose/pose, PoseStamped, queue_size1) # 相机内参与标签物理边长来自 config 文件 camera_matrix np.load(config/camera_matrix.npy) dist_coeffs np.load(config/dist_coeffs.npy) marker_size 0.18 # 标签边长单位米 aruco_dict cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) aruco_params cv2.aruco.DetectorParameters_create() def image_callback(msg): frame bridge.imgmsg_to_cv2(msg, desired_encodingbgr8) corners, ids, _ cv2.aruco.detectMarkers( frame, aruco_dict, parametersaruco_params) if ids is None: return rvecs, tvecs, _ cv2.aruco.estimatePoseSingleMarkers( corners, marker_size, camera_matrix, dist_coeffs) # tvec 是相机坐标系下的位置先转到机体系再转 ENU-NED pos_cam tvecs[0].flatten() pos_body R_body_cam pos_cam # 相机系 - 机体系 pos_enu pos_body body_offset # 补偿相机安装偏移 pos_ned np.array([pos_enu[1], pos_enu[0], -pos_enu[2]]) pose_msg PoseStamped() pose_msg.header.stamp rospy.Time.now() pose_msg.header.frame_id map pose_msg.pose.position.x pos_ned[0] pose_msg.pose.position.y pos_ned[1] pose_msg.pose.position.z pos_ned[2] pub.publish(pose_msg)这段代码的逻辑是先用 aruco.detectMarkers 检测四角点得到 corners 和 ids再用 estimatePoseSingleMarkers 得到旋转向量 rvecs 和平移向量 tvecs。tvecs 是从相机光心指向标签中心的向量坐标系是相机坐标系R_body_cam 是相机安装矩阵由安装朝向决定把相机系转到机体系最后把机体系坐标从 ENU 转成 NED 发布出去。飞控看到的永远是一个干净的 NED 坐标。参数说明一下marker_size 是基准标签的物理边长单位是米直接影响距离估计的绝对尺度camera_matrix 和 dist_coeffs 来自相机标定如果标定不准标签在 2 米距离上的位置误差会被放大到 5 厘米以上这直接影响飞控贴近时的精度。还有一个容易忽略的点R_body_cam 和 body_offset 必须对应相机实际安装位置否则视角差会在近距离产生明显的定位偏差。3.2 VIO/SLAM 数据转发一个通用的话题桥VIO/SLAM 接入的代码更薄本质是一个“话题到话题”的转发器。它把上游视觉里程计输出的位姿话题接进来做坐标变换和时间戳整理再发布到 mavros 的视觉话题上。#!/usr/bin/env python3 import rospy from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped class VioBridge: def __init__(self): self.source_topic rospy.get_param(~source_topic, /camera/pose) self.output_topic rospy.get_param(~output_topic, /mavros/vision_pose/pose) self.coord_mode rospy.get_param(~coord_mode, ENU) self.yaw_offset rospy.get_param(~yaw_offset, 0.0) self.sub rospy.Subscriber(self.source_topic, PoseWithCovarianceStamped, self.cb) self.pub rospy.Publisher(self.output_topic, PoseStamped, queue_size1) def cb(self, msg): p msg.pose.pose # 简化版 ENU - NED 变换实际代码会从 config 读安装参数 ned PoseStamped() ned.header.stamp rospy.Time.now() ned.header.frame_id map ned.pose.position.x p.position.y ned.pose.position.y p.position.x ned.pose.position.z -p.position.z # 注意这里只处理了平移yaw_offset 的修正需要单独实现 self.pub.publish(ned)之所以能写这么薄是因为 VIO/SLAM 在业务上已经把位姿算完了这里的桥只需要做两件事坐标系归一化加时间戳刷新。VIO 输出的 header.stamp 可能来自自己的时钟和飞控时钟存在漂移简单的做法是用本机时间覆盖更严谨的可以做时钟偏移估计但大多数仿真和短程飞行场景下直接覆盖已经够用。几个参数值得关注source_topic 是上游 VIO 发布的位姿话题coord_mode 用来指定上游坐标约定支持 ENU/NED/body 三种取值yaw_offset 用于修正 VIO 初始航向和真实航向之间的夹角。VIO 里程计启动时通常会给自己一个任意航向基准这个 offset 如果不配飞控融合出来的机头朝向就会差一个固定角度。有一个容易被忽略的点VIO 的输出频率通常能到 30~60Hz但 ArduPilot 的 EKF 并不需要那么高频20Hz 左右就够。桥接节点里可以做一个简单的降采样逻辑用余数方式每 N 帧发一条能有效降低串口和 rosbag 的流量压力对长时间飞行的稳定性有帮助。3.3 深度图像转障碍物距离从深度图到 OBSTACLE_DISTANCE深度图这一路不走位姿消息走避障消息。它把深度相机的 2D 距离信息按方位角划分成若干扇区每个扇区取最近障碍物距离交给飞控的避障逻辑去用。import numpy as np def depth_to_sectors(depth_image, sector_num8, max_range5.0, camera_configNone): depth_image: 深度图单位可能是毫米或米 sector_num: 扇区数量一般 8 或 12 max_range: 最大感知距离超过视为无障碍 h, w depth_image.shape cx, cy camera_config[cx], camera_config[cy] fx, fy camera_config[fx], camera_config[fy] # 投影到相机坐标系得到每个像素对应的方向向量 u (np.arange(w) - cx) / fx v (np.arange(h) - cy) / fy x np.tile(u, (h, 1)) y np.tile(v.reshape(-1, 1), (1, w)) z depth_image / 1000.0 # 如果深度图单位是毫米则转成米 mask (z 0.1) (z max_range) if not mask.any(): return [max_range] * sector_num angle np.arctan2(x, -y) # 以机头方向为 0 度 sector_idx ((angle / (2 * np.pi)) * sector_num).astype(int) % sector_num sectors [max_range] * sector_num for i in range(sector_num): candidates z[mask (sector_idx i)] if candidates.size 0: sectors[i] np.min(candidates) return sectors这段代码的核心是扇区化把深度图按相机视角对应的方位角切分每个扇区里找最小深度这个值就是飞控避障时使用的“该方向最近障碍距离”。sector_num 越多避障方向越精细但消息长度也越长串口带宽有限时需要做取舍max_range 是最大感知距离超过这个距离的像素会被忽略防止远处的噪点干扰。深度的单位是个容易翻车的点。相当多的深度驱动输出的是 16 位毫米也有输出 float 米的代码里 /1000.0 那一步必须跟驱动文档对清楚。如果你用的相机直接输出米再除以 1000避障距离就会整体缩小一千倍飞控会以为障碍物近在咫尺。这种错误在仿真环境里看不出问题真机上会非常惊悚。3.4 非 ROS 出口pymavlink 直发 MAVLink 的两条函数非 ROS 侧的出口就简单直接了pymavlink 封装好了 MAVLink 消息构造我们只需要填数据。import time from pymavlink import mavutil mav mavutil.mavlink_connection(udp:127.0.0.1:14550, source_system1) mav.wait_heartbeat() # 发送视觉位姿估计坐标为 NED单位米欧拉角单位弧度 mav.mav.vision_position_estimate_send( int(time.time() * 1e6), # usec 时间戳微秒 pos_ned[0], pos_ned[1], pos_ned[2], roll, pitch, yaw ) # 如果视觉系统还能给出速度一起发对 EKF 更友好 mav.mav.vision_speed_estimate_send( int(time.time() * 1e6), vel_ned[0], vel_ned[1], vel_ned[2] )pymavlink 这条线的好处是没有 ROS 依赖在 NX 板上单独跑一个进程就能把消息送到飞控。发送前需要 wait_heartbeat() 确认与飞控的链路已经建立否则前几帧数据会被当作陌生系统丢弃。如果飞控要求接收端带系统 IDsource_system 必须与地面站配置一致这里填 1 是常见约定。这段代码虽然短但它引出一个重要事实非 ROS 和 ROS 两种出口的核心目标完全一致区别只在帧格式。所以不要把这两套当成两个系统而要当成“一套转换逻辑两个传输通道”。写代码时把坐标变换函数做成公共模块两边 import 同一个文件就不会出现一边改了坐标系、另一边忘了改的尴尬。4. 跑起来ArduPilot SITL 环境下的最小联调流程与验证方法4.1 环境准备依赖清单与最小安装路径在真机之前先在 ArduPilot SITL 仿真里把链路跑通是最稳妥的做法。环境准备按“ROS 侧”和“非 ROS 侧”两条线来装这里以 ROS 侧为例。# 安装 mavros 与 mavros_msgs按你自己的 ROS 发行版选择包名 sudo apt install ros-distro-mavros ros-distro-mavros-msgs # 安装 python 依赖 pip install numpy opencv-python pymavlink装完之后确认 mavros 的 launch 文件在。mavros 装好后会自带 apm.launch它负责把 MAVLink 数据转成 ROS 话题。如果你用的是 ArduPilot SITL不需要额外硬件电脑上直接起仿真就能测。非 ROS 侧更简单只要 python3 环境加 pymavlink、numpy、opencv-python 三个库。我一般会用 venv 单独建一个环境避免和系统的依赖冲突。4.2 最短启动路径三个终端三行命令SITL 联调的最小启动路径是三个终端分别跑仿真、mavros 和视觉桥。# 终端1启动 ArduPilot SITL并把 MAVLink 输出到本机 14550 端口 sim_vehicle.py -v ArduCopter --map --console --out 127.0.0.1:14550 # 终端2启动 mavros连接 SITL 输出的端口 roslaunch mavros apm.launch fcu_url:udp://127.0.0.1:14550 # 终端3启动视觉桥接节点订阅上游视觉话题 rosrun vision_to_mavros vio_bridge.py _source_topic:/camera/pose启动顺序有讲究先起 SITL再起 mavros最后起视觉桥。SITL 的 --out 参数把 MAVLink 打包发到 UDP 14550 端口mavros 的 fcu_url 也指向这个端口两者构成一条双向链路。如果顺序反了mavros 会一直重连虽然不影响最终结果但会在日志里刷一大片错误干扰排查。视觉桥节点的 source_topic 参数要指向你实际视觉源的话题。如果没有现成的视觉源可以先做一个 fake_pose.py 定时发布 PoseStamped用来验证 mavros 链路本身是通的。这一步能帮你区分“问题在视觉算法”还是“问题在桥接配置”。4.3 链路验证从话题频率到 EKF 状态的四步检查链路起来后不要急着让飞机起飞先做四步验证。# 第一步确认话题有数据 rostopic echo /mavros/vision_pose/pose -n 5 # 第二步确认话题频率达标 rostopic hz /mavros/vision_pose/pose第一步看数据内容x、y、z 值是否在你的视觉源的合理量程内有没有出现 NaN。第二步看频率ArduPilot 的 EKF 融合视觉源一般要求不低于 10Hz稳定在 20Hz 以上比较理想。如果频率只有 5Hz飞控大概率会忽略这部分数据。频率不达标时检查上游视觉源是不是本身就很慢或者桥接节点里是不是多了不必要的耗时操作。第三步是动动飞机在 SITL 界面里手动拖动机体位置看视觉位姿话题是否跟随。这个动作能最快暴露坐标系反了的问题——你往北拖结果 x 在减少那肯定是坐标映射不对。第四步是看飞控日志或地面站里的 EKF 状态。在 QGC 或 Mission Planner 里观察 EKF 状态确认视觉源被列为参与融合的传感器之一。如果视觉数据一直在发但状态里根本没有这一项多半是 EKF 参数没开启视觉源这个要回到飞控参数里去配。5. 避坑指南视觉定位上机常见的翻车现场与排查思路5.1 现象飞机一起飞就往一个方向猛冲推杆也拉不住原因基本可以锁定在坐标系方向反了。飞控按 NED 解算你给的视觉位姿如果是 ENU 或相机系坐标等于告诉飞控“北在那边”它当然往错误方向加速。这种情况在静止悬停时可能看不出来因为位置没变化但一给速度指令就原形毕露。解决方法是先做静态坐标验证把视觉源放在机体的正北方查看发布出去的 x 值是不是大于 0。如果 x 为负交换一下映射关系再试。切记位置向量和姿态角要一起验证只修一个往往会带歪另一个。5.2 现象话题有数据但飞控日志里完全没有视觉融合记录先别怀疑飞控先查频率和时间戳。ArduPilot 的 EKF 对视觉源的更新频率有最低要求常见的阈值在 10Hz 左右低于这个会被丢弃。另一个常见原因是时间戳问题ROS time 是秒MAVLink 要求微秒如果差了一千倍飞控会认为数据来自未来直接拒绝。解决分两步一是用 rostopic hz 确认频率达标不达标就在桥里做缓存锁存最新一帧并按固定频率推送二是检查 time_usec 的换算确保单位是微秒且没有溢出。5.3 现象标签静止时位置稳定但相机稍微动一下位置就剧烈跳变原因大概率是 ArUco 检测在运动模糊帧上出了误检。estimatePoseSingleMarkers 对 corner 的亚像素精度很敏感模糊帧会让角点偏移几个像素近距离下这就会变成几厘米的跳变。解决方式有三层第一层是在检测端做质量过滤把角点分布方差过大的帧丢掉第二层是在桥接端对位姿做低通滤波比如一阶滞后滤波让输出平滑第三层是提高相机帧率或缩短曝光时间从根源上减少运动模糊。如果连续多帧检测不到标签我一般会让桥接节点主动停发视觉位置让飞控切回 GPS 模式而不是把最后一次检测结果一直怼给飞控。5.4 现象VIO 跑了半小时开始慢慢漂但飞控还在继续信任它VIO 漂移是必然的飞控自己无法判断视觉数据变坏了多少桥接层的责任是及时发现漂移并触发重置。解决思路是加一个“视觉/GPS 一致性检查”定期比较视觉位置和 GPS 位置的偏差当偏差超过阈值时给飞控发一个视觉位置重置指令或者直接停发视觉数据让 EKF 回到 GPS 融合。从实际效果看加了这个逻辑之后长航时任务的可靠性提升非常明显。很多人忽略这一层只做了坐标变换结果就是前半段飞得好好的后半段开始莫名偏航。5.5 现象近距离定位精度差得离谱标签明明在正前方位置却偏了十几厘米这类问题八成出在标定和物理尺寸上。marker_size 每偏 1 厘米2 米距离上的位置误差就会放大到 5 厘米以上相机内参如果是从同型号相机“复制”来的而不是实际标定畸变修正一错边缘区域的角点坐标就全歪了。解决方式是回到源头用棋盘格重新标定相机用卡尺实测标签边长把这两个值填进 config 之后再做一次静态验证。这个坑最坑的地方在于它不会报错所有参数看起来都合理就是精度差那么一点。我自己的习惯是每次换相机或者换标签型号都会强制重新走一遍标定流程。6. 二次开发接入一个新的视觉定位源的完整套路6.1 代码复用写一个最小适配器接新视觉源时不需要从头读全仓库只需要仿照 vio_bridge.py 写一个适配器。核心就一个方法把上游的原始位姿转换成 NED然后发布出去。坐标系变换逻辑从 mavlink_common.py 里 import消息发布直接复用现有函数。# 新视觉源接入的最小模板 class CustomVisionAdapter: def __init__(self, coord_modeENU): self.coord_mode coord_mode self.pub rospy.Publisher(/mavros/vision_pose/pose, PoseStamped, queue_size1) def handle_pose(self, raw_pose): ned to_ned(raw_pose, self.coord_mode) # 从 mavlink_common import self.pub.publish(ned)6.2 接入前强制自查的三项清单第一坐标系约定写清楚。上游输出到底是 ENU 还是相机系姿态角用什么表示全都要在适配器里显式转换不要靠猜。第二时间戳对齐方式。上游时间基准是什么和飞控差多少统一在入口处处理。第三频率策略。上游是 30Hz 还是 5Hz降采样还是锁存重发提前定好。这三项确认完再谈别的。6.3 从“能飞”到“好飞”的参数调整链路通了之后剩下的就是调信任度。EKF 融合视觉源的时候参数里通常会有一个控制视觉位置置信度的系数数值越大飞控越信任视觉。稳定悬停时可以把置信度调高让位置保持更紧环境光线变化大、视觉容易丢的时候置信度要降下来避免视觉一个跳变把整个姿态带飞。VIO 的漂移补偿也要持续观察。我习惯在每次长航时测试后看一眼飞控日志里的定位误差曲线如果误差随时间单调增大说明视觉源在缓慢漂移需要在桥接层加强一致性检查逻辑而不是无脑提高信任系数。6.4 我的习惯强制先跑坐标系检查再谈上天接新视觉源最容易翻车的时刻就是你觉得“这个输出应该是 ENU应该没错”的时候。我被这种“应该”坑过不止一次。某次在仿真里调好了所有参数真机上飞机一起飞就朝反方向冲原因就是真实相机安装角度跟仿真的差了 90 度而我偷懒没做静态坐标验证就直接切了自动模式。从那以后我每次接入新的视觉源都强制先在 SITL 里做一遍固定标签测试确认 x、y、z 三个轴的方向和位移跟随全部正确才允许自己碰真机。这套流程救过我至少三次希望你也能少踩几个我踩过的坑。希望帮到你。本文还有配套的精品资源点击获取