从上一篇文章把单目视觉检测和相对位姿解算跑通之后这周我把整套链路接到了mavros和Pixhawk上真正让飞机朝着视觉标志飞过去。中间踩了不少坑尤其是坐标系和OFFBOARD切换这两个环节几乎每个第一次做的人都会在这里卡上两三天。这篇文章就专门讲清楚视觉节点算出来的结果到底通过什么消息、什么频率、什么坐标约定发给PX4px4才会老老实实照着执行。这篇东西适合两类人看一类是把仿真里的视觉降落搬到真机上的朋友另一类是已经会飞PX4但一直没搞懂mavros控制链路的同学。我会把mavros在整条数据链中的位置、坐标系变换的来龙去脉、OFFBOARD模式下的setpoint发布方式以及EKF2参数配置的真实踩坑过程全部讲一遍。所有内容都是我在室内飞行、没有GPS环境下反复验证过的方案可以直接抄作业但建议先理解再动手。1. 先把任务拆清楚一串视觉坐标是怎么变成电机转速的1.1 机载电脑、飞控和底层控制器各干各的活很多刚接触这个项目的人对机载电脑和飞控的分工是模糊的。他们以为视觉算出来的目标位置可以直接发给飞控飞控就能自己飞过去。实际上这套系统里至少有三层在协作。最上层是机载电脑一般是一块树莓派、Jetson Tx2/Nano或者Intel NUC。它跑的是Ubuntu、ROS、视觉算法和mavros节点。视觉算法负责从相机图像里识别降落标志解算出标志相对相机的位姿mavros负责把算好的位姿和控制指令翻译成MAVLink消息通过串口或USB发给飞控。第二层是Pixhawk飞控跑PX4固件。PX4内部又分了好几层底层的状态估计模块EKF2负责把IMU、气压计、磁力计、外部视觉等数据融合成完整的姿态和位置估计中层的位置控制器和姿态控制器根据机载电脑下发的期望位置计算油门、俯仰、滚转指令最底层是混控器和电机执行器。飞控本身不关心视觉算法怎么实现的它只接收两类东西一类是我现在在哪里的外部观测另一类是我希望飞机到哪个位置的期望值。这两个东西很容易被搞混。视觉节点既可能作为观测源vision_pose给飞控提供位置信息也可能作为决策器直接给飞控下发目标点setpoint。这两种方式在mavros里对应完全不同的消息话题用错了飞机根本不会理你。1.2 mavros在链路里到底扮演什么mavros对于ROS开发者来说就是一个MAVLink协议的转换节点。它订阅ROS话题转换成MAVLink消息发往飞控同时把飞控发来的MAVLink消息转换成ROS话题供上层调用。你不用关心MAVLink字节流里那些字段怎么填只需要知道几个关键话题和服务的用法方向话题/服务消息类型作用上位机→飞控/mavros/setpoint_raw/localmavros_msgs/PositionTarget下发期望位置/速度/加速度上位机→飞控/mavros/vision_pose/posegeometry_msgs/PoseStamped将视觉位姿作为外部位置观测发送上位机→飞控/mavros/set_modemavros_msgs/SetMode切换飞行模式比如OFFBOARD上位机→飞控/mavros/cmd/armingmavros_msgs/CommandBool解锁/锁定电机飞控→上位机/mavros/statemavros_msgs/State当前飞行模式、解锁状态飞控→上位机/mavros/local_position/posegeometry_msgs/PoseStamped飞控内部估计的位置调试用这里最大的认知误区是把视觉算出的目标位置直接丢到setpoint里然后发出去以为飞机就会追着这个点飞。方向对但坐标基准必须和飞控内部的位置估计一致。换句话说你命令飞机飞到(0, 0, -3)飞控会把它当成本地坐标系中的三维点如果飞控本身不知道自己在哪这个指令就是空中楼阁。1.3 一眼看懂这条数据链哪些消息最常被搞错我画不出流程图但可以用一个时间序列来描述整条链路相机采集图像 → 视觉检测节点识别标志 → 解算标志相对相机的位姿 → 坐标变换变成机体在标志坐标系下的位置姿态 → 通过/mavros/vision_pose/pose发给PX4 → EKF2融合视觉和IMU得到平滑的位置估计 → 状态机判断当前该用什么期望值 → 通过/mavros/setpoint_raw/local下发期望位置 → 飞控控制电机飞行。在这条链路上最容易搞错的是第三到第五步。视觉解算输出的是相机看到的标志位姿直接发到vision_pose会被PX4当成机体在世界系中的位姿。如果坐标系变换没做干净飞控对自身位置的估计就是错的表现就是飞机往反方向飞、绕圈、或者高度失控。2. 坐标系对齐为什么视觉说目标在前面飞控却往后飞2.1 三个坐标系、两套惯用手做视觉降落的人迟早会被坐标系逼疯pass这个坎之后基本就通透了。这套系统至少要面对三个坐标系相机坐标系、机体坐标系Body系和导航坐标系世界系。相机坐标系使用OpenCV和ROS的视觉惯例往往定义为x向右、y向下、z向前。机体坐标系以飞控为中心x指向机头、y指向机身右侧、z向下FRD约定这是PX4内部使用的。导航坐标系用NEDx指北、y指东、z向下。而ROS生态里更常用ENUx指东、y指北、z向上。mavros这个中间层会帮你做一部分NED和ENU的转换但不会帮你做相机系到机体系的转换——这部分必须自己在代码里完成。姿态角的方向约定也容易乱。同一个向前飞在NED里是x正方向在ENU里是y正方向。同一个高度3米在NED里是z-3在ENU里是z3。我在代码里把所有从视觉节点出去的量都统一成NED再发给mavros调试时省掉一大半困惑。2.2 从像素到机体坐标一套标准的变换链假设你已经从PnP算法得到了降落标志中心相对于相机光心的平移向量记作t_cam_marker表示标志原点在相机坐标系下的位置单位是米。但PX4需要的是机体在世界系下的位置和姿态。要完成这一步需要级联几个变换。第一步是相机系到机体系的变换。这个变换由相机安装位置和安装角度决定是固定的。比如你的相机水平朝向正前方安装、位于机体中心前10cm处那么平移向量就是[0.1, 0, 0]旋转变换取决于相机坐标系轴与机体系轴的差异。像OpenCV相机坐标系z轴朝前而机体坐标系x轴朝前这就需要一个绕y轴旋转的变换。实践中我建议用tf2_ros发布一个静态变换把相机的安装外参固定下来之后所有变换都交给tf处理。第二步是标志系到世界系的转换。在定点降落任务里最聪明的做法是直接把降落标志当成世界原点这样一切就简单了。如果标志在相机坐标系下的位姿是T_marker_cam相机在机体坐标系下的位姿是T_cam_body那么标志在机体系下的位姿就是T_marker_body T_cam_body * T_marker_cam然后机体在标志坐标系下的位姿也就是世界坐标系中的机体位姿就是对上面结果求逆T_body_marker inv(T_marker_body)这个T_body_marker就是你要通过mavros的vision_pose/pose发送给飞控的量。很多教程直接把这个省略掉只讲发位置就行但姿态部分也很关键EKF会同时融合视觉姿态和IMU姿态如果你只发位置不发姿态或者姿态乱跳融合结果会非常不稳定。2.3 最省心的方案把降落标志当作临时世界原点为什么我要反复强调把标志当世界原点因为这样期望点就变得非常简单。你希望飞机悬停在标志正上方3米那么视觉坐标系中的目标位置就是(0, 0, -3)NEDz向下负z是高度。你用视觉定位得到机体在标志系下的位置用这个位置作为EKF的外部观测再下发一个全局期望点这套逻辑天然自洽不需要GPS参与。以前有人这么干视觉检测到目标在图像中的像素偏差然后把它当作偏差量直接加进当前setpoint里。这在原理上可行但问题很多。图像像素偏差是二维的要换算成三维偏差就得知道深度深度估计稍有误差换算出来的水平偏差就错了。而且这种方案没有任何滤波视觉抖动会直接传递到位置环飞机很容易抖振。把视觉位姿发到EKF里融合好处是飞控内部会结合IMU高频预测和视觉低频观测得到平滑、稳定且低延迟的位姿估计。这本质上是把视觉当成一颗低速GPS只不过GPS的坐标系是经纬度视觉的坐标系是标志原点。飞控不关心你的世界原点在哪只关心你给的观测和期望是否在同一个坐标系内。2.4 被忽略的时间戳和延迟问题坐标系对齐只是第一步时间戳的问题同样关键。视觉解算需要时间相机曝光、图像传输、检测算法推理、坐标变换这些加起来可能有50到200毫秒的延迟。如果你发送的vision_pose带着一个老旧的位姿EKF会认为这个观测是当前的融合出的位置就会有滞后甚至过冲。解决方案有两个层面。代码层面给发布的PoseStamped消息里的header.stamp填上图像采集时刻的时间戳而不是发布时刻的时间戳。这样mavros把消息转发给PX4时PX4的EKF可以结合EKF2_EV_DELAY参数补偿延迟。如果实在拿不到图像时间戳就把当前时间作为时间戳然后在PX4参数里设置合适的EKF2_EV_DELAY。第二个方案是尽可能压短视觉处理流水线比如把检测算法放到GPU上跑分辨率不要盲目调高检测帧率尽量稳定在30Hz以上。有个很隐蔽的坑如果视觉节点发布频率不稳定有时候20Hz有时候5HzEKF会很难受。它不知道下一次观测什么时候来融合权重没法配平。宁可把帧率稳定在15Hz也不要急一阵缓一阵。实践中我在视觉节点里加了一个自适应的帧率控制如果算法处理速度变慢就跳过当前帧保证输出节拍均匀。3. OFFBOARD模式实战设定点怎么发、模式怎么切才算稳3.1 为什么定点降落必须依赖OFFBOARDPX4的飞行模式很多POSCTL、ALTCTL、AUTO、OFFBOARD各有各的用途。定点降落任务必须用OFFBOARD原因是它允许外部计算机直接周期性下发期望位置、速度而且可以是任意的、动态变化的轨迹不依赖地面站预定义航点。AUTO模式虽然也可以飞航点但你很难在飞行中动态修改下一个航点视觉伺服跟踪的任务需要实时响应AUTO模式根本玩不转。POSCTL模式下飞控帮你做位置控制但它是靠遥控器摇杆输入产生期望位置变化而不是接收外部setpoint所以对视觉引导来说也不合适。OFFBOARD模式本质上是把导航决策权交给外部计算机飞控只负责姿态和速度的内环控制这对软件开发者是最友好的接口。要用OFFBOARD也有门槛PX4要求必须在切入OFFBOARD之前至少持续收到2Hz以上的setpoint否则会拒绝切换。工程上这个频率往往不够用我建议至少10Hz稳一点就30Hz后面会解释为什么。3.2 最简实现先发布setpoint再切模式给出一套可以直接跑的思路。在正式代码里建议先订阅/mavros/local_position/pose拿到当前飞机位置把它作为初始期望值发布一段事件再切换OFFBOARD避免从地面一下子跳到某个远点导致飞机猛冲。顺序是这样的启动节点后先不断发布当前悬停点的setpoint持续两秒左右等PX4确认收到了。然后调用/mavros/set_mode服务把模式切到OFFBOARD。等模式切换成功后再慢慢把setpoint往目标点(0, 0, -3)靠。过程中ROS节点必须保持发布不能在切完模式后就停下来。#!/usr/bin/env python3 import rospy from geometry_msgs.msg import PoseStamped from mavros_msgs.srv import SetMode, CommandBool def main(): rospy.init_node(offboard_position_publisher) pub rospy.Publisher(/mavros/setpoint_position/local, PoseStamped, queue_size10) set_mode_srv rospy.ServiceProxy(/mavros/set_mode, SetMode) # 假设飞机当前悬停在(0, 0, -3) target PoseStamped() target.pose.position.x 0.0 target.pose.position.y 0.0 target.pose.position.z -3.0 target.pose.orientation.w 1.0 rate rospy.Rate(30) # 先发90帧悬停指令确保PX4收到再切换OFFBOARD for _ in range(90): pub.publish(target) rate.sleep() resp set_mode_srv(custom_modeOFFBOARD) if resp.mode_sent: rospy.loginfo(OFFBOARD mode accepted) else: rospy.logwarn(OFFBOARD mode rejected) while not rospy.is_shutdown(): pub.publish(target) rate.sleep() if __name__ __main__: main()这里用的是/mavros/setpoint_position/local消息类型是PoseStamped只发位置和姿态mavros会自动补全其他字段。对于入门来说这个足够了。但如果你想在接近降落点后切换成恒定下降速度控制或者需要水平方向位置控制但垂直方向速度控制的混合模式就需要用更底层的PositionTarget。3.3 PositionTarget的进阶控制type_mask用什么mavros_msgs/PositionTarget是mavros里最灵活的下发接口可以同时携带位置、速度、加速度、yaw和yaw_rate。要用好它关键是理解type_mask字段。这个字段告诉飞控哪些字段有效、哪些字段忽略。放一个通用的位置控制maskfrom mavros_msgs.msg import PositionTarget def make_position_target(x, y, z, yaw): t PositionTarget() t.coordinate_frame PositionTarget.FRAME_LOCAL_NED t.type_mask ( PositionTarget.IGNORE_VX | PositionTarget.IGNORE_VY | PositionTarget.IGNORE_VZ | PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ | PositionTarget.IGNORE_YAW_RATE ) t.position.x x t.position.y y t.position.z z t.yaw yaw return t如果你要混合控制比如水平位置保持但垂直方向以-0.3m/s匀速下降可以把z相关的速度字段从mask里去掉填上期望的垂直速度t.type_mask ( PositionTarget.IGNORE_VX | PositionTarget.IGNORE_VY | PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ | PositionTarget.IGNORE_YAW_RATE ) t.position.x 0.0 t.position.y 0.0 t.velocity.z -0.3 # 下降速度0.3m/s t.yaw 0.0这种混合模式在最后降落阶段非常有用。因为视觉在低高度时Z方向估计误差会变大位置控制反而容易让高度震荡速度控制更平滑。3.4 安全保护逻辑setpoint断流会发生什么PX4对于OFFBOARD的安全机制很严格。如果它超过一定时间没有收到外部setpoint会自动退出OFFBOARD模式切回之前的模式通常是手动的STABILIZED或POSCTL。这个机制本来是防止链路断开后飞机失控但很多新手会误以为我切了OFFBOARD就万事大吉结果发射端一断飞机就掉高度或晃动机。我的建议是加一个看门狗逻辑。在mavros节点里如果30Hz的循环因为异常中断了整体代码要能主动呼出遥控器的安全模式。另外真机调试时手指永远不要离开遥控器并保持遥控器有独立的STABILIZE切换通道。这条建议在你第一次测试OFFBOARD时尤为重要。更隐蔽的一个坑是setpoint跳变过大。比如飞机当前在(2, 3, -3)你一下把目标点设成(0, 0, -3)水平距离差了3.6米PX4的位置控制会以最大水平加速度猛拉。这可不是你想要的效果。实际中我用了一个斜坡生成器每次循环把期望位置向目标点靠近限制单步变化不超过0.2米这样飞机飞过去又稳又不容易超调。4. 从发现标志到落地状态机是定点降落的大脑4.1 没有状态机的后果如果你把所有逻辑写在一个大while循环里随时检测视觉结果然后跟踪目标点多数情况下会出问题视觉一帧检测丢失飞机不知道往哪飞高度还没降到合适位置水平方向还在大范围调整标志刚从图像边缘出现但置信度不高飞机就开始莽撞地追踪。最后要么失控退出要么在目标周围来回乱窜。定点降落任务的本质是多阶段决策问题唯一的正确做法是设计一个有限状态机把任务分解成几个互斥的阶段每个阶段有明确的进入条件、控制策略和退出条件。4.2 四段式状态机设计我的状态机分四个状态SEARCH、TRACK、DESCEND、LANDED。SEARCH阶段是起始状态。此时视觉可能还没找到标志或者标志太小、太远信息不够可靠。控制策略是悬停在搜索点上方通常也就是任务开始时飞机的位置。等视觉系统连续N帧检测到标志并且标志在图像中的像素尺寸超过一个阈值才切换进入TRACK。这里加连续N帧判断很关键避免单帧误检导致状态抖动。TRACK阶段是视觉伺服阶段。飞机要飞行到标志正上方同时保持一定高度。期望水平位置是(0, 0)期望高度保持在进入该状态时的高度比如-5米。控制逻辑用上面说的斜坡生成器逐步逼近。当水平误差收敛到0.2米以内并且标志的像素面积足够大说明飞机已经位于标志正上方进入DESCEND。DESCEND阶段是最终降落。水平方向继续保持视觉跟踪垂直方向切换到恒定下降速度比如-0.3m/s到-0.5m/s。这个阶段视觉检测的作用已经从水平定位退化为确认标志还在视野内高度信息主要依赖距离传感器或气压计。4.3 视觉丢失的兜底策略TRACK和DESCEND阶段都可能发生视觉丢失。最常见的原因是飞机下降到太低标志已经超出相机视场角范围或者某个角度的反光让检测算法失效。如果直接宕机不动飞机会失去水平修正能力严重时会飘走。我的策略是在状态机里维护一个连续丢失帧计数器。如果连续小于10帧丢标志可以认为只是瞬时误检继续沿用最后一个可靠的目标位置并同时加大期望位置的平滑系数避免漂移。如果连续超过10帧还是没有就果断回到SEARCH状态把目标点恢复到当前估计位置让飞机先悬停稳不再追踪旧目标。这里有个很有用的技巧TRACK阶段并不是一直无脑把期望点设到(0,0)的。因为相机的视场角一般是有限的如果你期望点太偏飞机会先横移再对准而横移过程中标志可能跑出视野。我做的做法是设置一个限制圈当横向误差大于某个阈值时只朝标志方向移动目标位置的一部分让标志始终保持在画面中心附近。4.4 着陆检测什么时候停电机很多人在前几个阶段都处理得好好的最后卡在什么时候切掉电机。如果你只是让飞机一路下降到期望z0一旦触地PX4的位置控制还会尝试维持高度电机会顶着地面推最后可能侧翻。所以必须在DESCEND阶段之后新增LANDED检测。常用的触地检测信号有两个一是气压计或距离传感器显示高度小于某个极小值比如0.1米且持续零点几秒二是IMU的Z轴加速度出现明显冲击峰值这个信号很灵敏但也容易在快速下降中被误触。最稳妥的是把高度信号和加速度信号做一个与逻辑高度已经很低同时检测到加速度冲击才认为触地。触发触地后让状态机进入LANDED然后调用mavros的/mavros/cmd/arming服务发送disarm指令锁电机。注意要延时几百毫秒等飞机完全稳定下来再锁不然在机身还在晃动的时候锁电机桨叶容易被地面弹起来的力打坏。5. EKF2参数配置与真机调试视觉融合的坑都踩在哪5.1 先给一份能用的参数组合视觉位姿发到PX4之后EKF2并不会默认启用。你不改参数飞行器会把vision_pose当成无效数据丢在一边。要让PX4信任并融合视觉数据关键是EKF2_AID_MASK参数。这个参数按位控制哪些传感器可以参与融合。常用位含义如下位数值含义bit01GPS位置bit12气压计高度bit24磁力计bit38光流速度bit416视觉位置bit532视觉偏航这套配置在我的室内无人机上验证过多次。如果你完全不用GPS希望用视觉作为主要外部定位源那么把bit0关掉bit1和bit2作为高度和航向的补充bit4开启视觉位置融合bit5建议开启视觉偏航融合。最终EKF2_AID_MASK设为54241632。如果你担心视觉偏航融合会引入跳变可以先只开视觉位置也就是222416然后再慢慢加。另外还有几个参数会直接影响融合质量参数名推荐值说明EKF2_EV_DELAY0.0~0.1视觉数据延迟补偿先从小值标定EKF2_EV_POS_X按实测相机在机体系下的安装位置xEKF2_EV_POS_Y0.0相机在机体系下的安装位置yEKF2_EV_POS_Z按实测相机在机体系下的安装位置zEKF2_EV_POS_X/Y/Z特别容易被忽略。它告诉EKF视觉传感器相对机体重心的物理位置如果你的相机装在机头前方15cm处这里填0.15如果装在机头下方还要填相应的负z。漏填这个偏移会让融合出来的位置有一个恒定偏差水平或垂直方向都偏而且你很难从现象上判断是标定问题还是算法问题。关于延迟补偿我的经验是从0.0开始用PlotJuggler对比视觉原始位置和飞控估计位置的相位如果飞控估计明显滞后于视觉逐步增大EKF2_EV_DELAY直到曲线重合度最好。一般不会超过0.1秒如果视觉链路实在延迟过大还是要先优化代码而不是硬调参数。5.2 上真机前的SITL仿真验证如果你的项目还在室内或者没有真机条件强烈建议先在PX4的SITLSoftware In The Loop仿真里把mavros完整链路跑通。SITL的意义不是验证PX4能不能控制仿真器而是验证你自己的mavros节点、状态机、坐标变换、参数配置是否合理。等仿真里飞机能稳定地降到标志正上方再搬到真机可以省下大量炸机成本。SITL启动方式很简单在PX4固件目录下启动Gazebo仿真然后用一个mavros的sitl launch文件连接。仿真里你可以发布一个模拟的/mavros/vision_pose/pose消息直接用gazebo的地面真值位置加一点高斯噪声来模拟视觉输出验证OFFBOARD链路和状态机逻辑。这个过程不需要启动真实相机也不需要视觉算法先把控制链路验证唯一再换真实视觉。顺带说一句在更早的设计阶段我也会先用Simulink把位置控制律调一遍。比如有些同学在降落任务里想用滑模控制代替PID提高对地面效应和风扰的鲁棒性。这件事完全可以在Simulink里先做模型验证把四旋翼动力学模型建出来跑滑模控制器的跟踪效果确认没毛病了再往PX4的mavros链路上搬。这一步不是必须的但能帮你把算法问题和工程问题分开来排查。5.3 常见现象与排查表这里整理一份我在实际调试中踩过的坑对应现象和解决思路现象可能原因解决思路切OFFBOARD后飞机原地掉高setpoint没有持续发布或z方向符号反了检查setpoint的z是否为负值表示高度飞机水平方向突然猛冲vision_pose坐标系没转换干净EKF跳变用PlotJuggler对比vision_pose和local_position飞机在标志上方来回晃动视觉位置噪声大EKF融合权重过高适当降低vision_pose频率检查延迟参数视觉显示稳定但飞控位置持续偏移EKF2_EV_POS_X/Y/Z设置错误核实相机安装位置并修改参数视觉一启动EKF就报警时间戳或坐标系不在合理范围确认PoseStamped坐标系、时间戳单调递增降落时飞机蹭地往前窜水平误差未收敛就进入DESCEND把状态机的切换阈值收紧提高水平收敛要求排查时最推荐的工具是PlotJuggler和QGroundControl。QGroundControl的MAVLink Inspector可以看飞控是否收到VISION_POSITION_ESTIMATE消息PlotJuggler里导入/mavros/vision_pose/pose、/mavros/local_position/pose、/mavros/imu/data三个话题在同一个时间轴上对比绝大部分坐标变换和时间戳问题一眼就能看出来。5.4 几点个人经验这套调试我最想强调的一点是千万不要刚拿到真机就全链路联调。先把视觉节点离线跑一遍把输出录成rosbag再用rosbag播放让mavros节点和状态机全程执行一遍观察指令是否合理确认没问题后拆掉桨叶把飞机放在桌面上让飞控上电切换OFFBOARD看电机是否按照预期响应。这四步走完才轮到装桨起飞。还有一个容易被忽略的小细节机载电脑和飞控之间建议用高质量的USB线很多丢包问题其实都是线材质量差、供电不稳造成的。如果调试中出现mavros连接经常断开先换线和独立供电不要急着怀疑代码。另外一个建议是关于降落标志的物理尺寸。单目视觉的尺度本来是缺失的你完全依赖标志的已知物理尺寸来解算距离所以标志尺寸必须量准确差1厘米都会导致里距离估计出现可观的百分比误差。我还习惯在标志四周加一个亮色边框检测鲁棒性提升很明显尤其在地面纹理复杂的情况下漏检率会低很多。