PX4+Gazebo动态目标追踪：视觉控制从仿真到实战

1. 从“看得见”到“追得上”动态目标追踪到底难在哪很多人第一次接触无人机视觉追踪脑子里想的都是“摄像头识别目标飞控控制飞机跟过去”听起来就是两个模块拼在一起的事。但真正上手做过的人都知道静态目标追踪和动态目标追踪完全是两个难度量级的东西。静态目标你只要让飞机飞到目标上方悬停就行位置环调好参数剩下的交给GPS和光流。可一旦目标动起来尤其是目标运动方向不确定、速度有变化的时候整个系统的响应链路就会被拉长延迟、抖动、丢失目标这些问题会一个接一个冒出来。我这次要聊的是在PX4加Gazebo这套仿真环境里怎么把动态目标追踪的视觉控制方案跑通。选仿真而不是直接上真机原因很实在真机炸一次的成本太高而且视觉追踪涉及相机标定、图像传输延迟、控制频率匹配这些环节在真机上调试效率极低。Gazebo的好处是你能把整个感知到控制的链路完整复现出来还能反复重来不用担心硬件损坏。这套方案适合谁看如果你已经搭好了PX4开发环境能编译固件、能跑SITL仿真但不知道怎么把视觉信息接入控制回路那这篇内容就是给你准备的。如果你连PX4和Gazebo的基本通信机制都还不清楚建议先把基础仿真跑通再来看不然中间很多环节会卡住。核心思路其实不复杂Gazebo里放一个移动的目标模型无人机上挂一个下视相机相机图像经过视觉算法提取目标在画面中的位置把这个位置误差转换成速度或者位置指令通过MAVROS或者uORB发给PX4PX4控制无人机去追踪。但每一步都有坑下面我按实际搭建的顺序把关键环节拆开讲。2. 仿真环境搭建别急着写代码先把这几个配置确认清楚2.1 PX4与Gazebo的版本匹配问题版本不匹配是新手最容易踩的第一个坑。PX4的固件版本和Gazebo的版本之间有严格的对应关系尤其是从PX4 v1.13之后Gazebo的启动方式发生了比较大的变化。如果你用的是PX4 v1.14.x它默认使用的是Gazebo Classic 11而如果你系统里装的是Gazebo Harmonic或者Fortress那启动脚本会直接报错。我的建议是如果你没有特别的版本需求直接用PX4 v1.14.3配合Ubuntu 22.04和Gazebo Classic 11这套组合的社区资料最全遇到问题容易搜到答案。安装方式推荐用PX4官方的一键脚本git clone https://github.com/PX4/PX4-Autopilot.git --recursive cd PX4-Autopilot bash Tools/setup/ubuntu.sh这个脚本会自动帮你装好Gazebo、依赖库和编译工具链。注意如果你之前已经装过ROS2脚本可能会和ROS2的Gazebo包产生冲突这时候要么用Docker隔离环境要么手动指定Gazebo的安装路径。装完之后验证一下make px4_sitl gazebo-classic如果能看到Gazebo界面弹出来并且飞机停在跑道上说明基础环境没问题。如果Gazebo界面一直在闪或者黑屏大概率是显卡驱动的问题试试用软件渲染export LIBGL_ALWAYS_SOFTWARE1这个命令强制使用CPU渲染虽然帧率会低一些但至少能跑起来。2.2 相机模型的添加与参数配置PX4自带的机型模型里默认是没有下视相机的。你需要自己往模型文件里加相机。以iris机型为例模型文件在Tools/sitl_gazebo/models/iris/iris.sdf.jinja你需要在合适的位置插入相机标签sensor typecamera namedownward_camera update_rate30/update_rate camera namedown_cam horizontal_fov1.047/horizontal_fov image width640/width height480/height formatR8G8B8/format /image clip near0.1/near far100/far /clip /camera plugin namecamera_controller filenamelibgazebo_ros_camera.so robotNamespace//robotNamespace cameraNamedown_cam/cameraName imageTopicNameimage_raw/imageTopicName cameraInfoTopicNamecamera_info/cameraInfoTopicName frameNamedown_camera_link/frameName /plugin /sensor这里有几个参数需要特别注意。update_rate设成30Hz是权衡后的结果设太高会让Gazebo的仿真步长被迫减小影响实时性设太低则视觉追踪的响应会明显滞后。horizontal_fov我设的是60度这个视场角在追踪场景下比较合适太窄容易丢目标太宽则目标在画面中的像素占比太小检测精度下降。相机的安装位置也很关键。如果你把相机放在机体正下方朝下看那目标在画面中的位置直接对应地面上的相对位置计算最简单。但实际飞行中飞机会有俯仰和横滚画面会跟着倾斜所以后续还需要用姿态信息做补偿。我建议在仿真阶段先用正下视方案把链路跑通再考虑倾斜安装或者云台方案。2.3 动态目标模型的创建Gazebo里创建一个会动的目标最简单的方式是写一个简单的ROS节点通过set_model_state服务来周期性地更新目标模型的位置。你可以先用Gazebo自带的简单几何体比如一个红色的立方体方便视觉算法做颜色阈值分割。在world文件里添加目标模型model nametarget pose5 0 0.1 0 0 0/pose link namelink visual namevisual geometry boxsize0.5 0.5 0.2/size/box /geometry material ambient1 0 0 1/ambient diffuse1 0 0 1/diffuse /material /visual collision namecollision geometry boxsize0.5 0.5 0.2/size/box /geometry /collision /link /model然后写一个Python脚本让目标做圆周运动或者正弦运动import rospy from gazebo_msgs.srv import SetModelState from gazebo_msgs.msg import ModelState import math rospy.init_node(target_mover) set_state rospy.ServiceProxy(/gazebo/set_model_state, SetModelState) rate rospy.Rate(50) t 0.0 while not rospy.is_shutdown(): state ModelState() state.model_name target state.pose.position.x 5 3 * math.sin(0.5 * t) state.pose.position.y 3 * math.cos(0.5 * t) state.pose.position.z 0.1 state.pose.orientation.w 1.0 set_state(state) t 0.02 rate.sleep()这个脚本让目标在水平面上做圆周运动半径3米角速度0.5rad/s线速度大约1.5m/s。这个速度对无人机追踪来说不算快适合初期调试。等链路跑通之后你可以把速度提上去测试系统的跟踪上限。注意set_model_state服务在Gazebo Classic里默认是开启的但如果你用的是Gazebo Sim新版服务名称和消息类型都不一样需要改用/world/world_name/set_pose服务。3. 视觉检测环节颜色阈值不是万能的但它是起步最快的3.1 为什么先用颜色阈值而不是深度学习一提到视觉检测很多人第一反应是上YOLO或者SSD。但在仿真环境里做动态目标追踪我强烈建议先用颜色阈值把整个控制链路跑通。原因有三个第一颜色阈值的计算量极小在CPU上就能跑到几百帧不会成为整个系统的瓶颈第二仿真环境的光照条件可控颜色阈值的效果非常稳定不会像真实场景那样受光照变化影响第三调试直观你可以实时看到二值化之后的图像哪里出了问题一眼就能看出来。等你把控制链路调通了再换深度学习检测器也不迟。到时候你只需要把检测模块的输出从“颜色块中心坐标”换成“检测框中心坐标”后面的控制逻辑完全不用改。这种模块化的思路能帮你省下大量重复调试的时间。3.2 从图像到目标位置的完整计算链假设相机图像是640x480目标是一个红色立方体。处理流程是这样的第一步把图像从BGR转到HSV空间。BGR空间下红色和暗红色容易混淆HSV空间下用色调通道做阈值更稳定。import cv2 import numpy as np def detect_target(image): hsv cv2.cvtColor(image, cv2.COLOR_BGR2HSV) lower_red1 np.array([0, 100, 100]) upper_red1 np.array([10, 255, 255]) lower_red2 np.array([160, 100, 100]) upper_red2 np.array([180, 255, 255]) mask1 cv2.inRange(hsv, lower_red1, upper_red1) mask2 cv2.inRange(hsv, lower_red2, upper_red2) mask mask1 mask2 mask cv2.erode(mask, None, iterations2) mask cv2.dilate(mask, None, iterations2) contours, _ cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if len(contours) 0: return None c max(contours, keycv2.contourArea) M cv2.moments(c) if M[m00] 0: return None cx int(M[m10] / M[m00]) cy int(M[m01] / M[m00]) return cx, cy这段代码返回的是目标在图像中的像素坐标。但飞控需要的是物理世界中的位置偏差所以还需要做一步转换。第二步像素坐标转物理坐标。假设相机内参已知焦距为f像素单位相机高度为h那么目标相对于相机光轴的物理偏移为def pixel_to_ground(cx, cy, img_width, img_height, f, h): dx (cx - img_width / 2.0) * h / f dy (cy - img_height / 2.0) * h / f return dx, dy这里的f可以通过相机标定得到也可以在仿真里直接根据视场角算f img_width / (2 * math.tan(hfov / 2))以640像素宽、60度视场角为例f大约是554像素。如果飞行高度是3米目标在画面中心偏右100像素那么实际横向偏移大约是100 * 3 / 554约0.54米。第三步把物理偏移转换成速度指令。最简单的做法是用比例控制kx 0.5 ky 0.5 vx kx * dx vy ky * dy这里vx和vy是无人机在机体坐标系下的速度指令。注意如果你的相机是正下视安装的那dx对应的是机体右方向dy对应的是机体前方向或者反过来取决于相机的安装朝向。这个对应关系一定要在仿真里验证清楚搞反了飞机会越追越远。3.3 姿态补偿为什么飞机一倾斜目标就偏了上面那个计算假设相机光轴始终垂直向下。但实际飞行中飞机为了产生横向速度必须倾斜倾斜之后相机也跟着倾斜画面中的目标位置就不能直接按垂直投影来算了。补偿的方法是用飞机的姿态角对像素坐标做旋转。假设飞机横滚角为φ俯仰角为θ那么修正后的地面偏移为def pixel_to_ground_with_attitude(cx, cy, img_width, img_height, f, h, roll, pitch): x_cam (cx - img_width / 2.0) / f y_cam (cy - img_height / 2.0) / f x_body x_cam * math.cos(pitch) math.sin(pitch) y_body y_cam * math.cos(roll) math.sin(roll) dx x_body * h dy y_body * h return dx, dy这个公式是简化版忽略了高阶小量和耦合项。在小角度倾斜小于15度的情况下精度足够。如果你要做大机动追踪建议用完整的旋转矩阵来做变换。我在实际调试中发现如果不做姿态补偿飞机在加速追踪的时候会出现明显的振荡飞机倾斜导致目标在画面中偏移控制器以为目标跑了加大速度指令飞机更倾斜目标偏得更多形成正反馈。加了补偿之后这个振荡基本消失。4. 控制接口的选择MAVROS还是uORB差别比你想的大4.1 两种通信方式的本质区别把视觉计算出来的速度指令送给PX4有两条路可以走。一条是通过MAVROS把指令封装成MAVLink消息发给飞控另一条是直接在PX4内部写一个模块通过uORB消息总线发布指令。MAVROS的好处是开发快你在ROS环境里用Python就能写不用碰PX4的C代码。缺点是延迟大MAVLink消息要经过串口或者UDP传输再加上ROS的话题发布订阅机制整个链路的延迟可能在20到50毫秒。对于慢速目标追踪这个延迟可以接受但如果目标机动比较剧烈延迟会导致追踪滞后甚至失稳。uORB的好处是快消息在PX4内部直接传递延迟在毫秒级别。缺点是开发门槛高你需要写C模块编译进固件调试也没那么方便。我的建议是先用MAVROS把算法验证一遍确认控制逻辑没问题再把核心部分移植到uORB模块里。这样你能在前期快速迭代后期又能保证性能。4.2 MAVROS速度指令的发布细节用MAVROS发布速度指令话题是/mavros/setpoint_raw/local消息类型是PositionTarget。关键是要把坐标系设对from mavros_msgs.msg import PositionTarget setpoint PositionTarget() setpoint.coordinate_frame PositionTarget.FRAME_BODY_NED setpoint.type_mask (PositionTarget.IGNORE_PX | PositionTarget.IGNORE_PY | PositionTarget.IGNORE_PZ | PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ | PositionTarget.IGNORE_YAW | PositionTarget.IGNORE_YAW_RATE) setpoint.velocity.x vx setpoint.velocity.y vy setpoint.velocity.z 0FRAME_BODY_NED表示速度是在机体坐标系下定义的x朝前y朝右z朝下。如果你用的是FRAME_LOCAL_NED那速度就是在地理坐标系下需要你自己把视觉偏差旋转到地理系。我推荐用机体坐标系因为视觉计算出来的偏差天然就是机体坐标系下的。type_mask的设置很容易出错。这个掩码的作用是告诉飞控“哪些字段我要用哪些字段忽略”。上面这个掩码的意思是位置忽略、加速度忽略、偏航角和偏航角速率忽略只用速度。如果你不小心把速度也忽略了飞控会收到一个全零的指令飞机就悬停了。还有一个坑是坐标系的方向。PX4的机体坐标系是前左上FRD而ROS常用的机体坐标系是前右下FLU。MAVROS在中间做了一层转换但如果你直接发FRAME_BODY_NEDMAVROS会认为你发的是NED系下的速度也就是x前、y右、z下。而你的视觉计算如果用的是图像坐标系y轴朝下那dy的正方向对应的是机体后方还是前方一定要在仿真里实际验证。4.3 控制频率与仿真步长的匹配控制频率不是越高越好。Gazebo的默认仿真步长是1毫秒但相机的更新频率是30Hz视觉处理再快也要等新图像。所以控制回路的频率应该和相机频率匹配设在30到50Hz之间比较合理。如果你把控制频率设到200Hz但相机只有30Hz那大部分控制周期里你用的都是旧图像算出来的指令相当于在重复执行同一个动作没有意义还会增加计算负担。在MAVROS里你可以用一个定时器来控制发布频率rospy.Timer(rospy.Duration(0.033), control_loop)0.033秒对应大约30Hz。这个频率下即使视觉处理有10到20毫秒的延迟整个闭环的相位裕度也还够用。5. 调试过程中那些让人抓狂的现象与排查思路5.1 飞机追着追着就飞远了这是最典型的问题通常有三个原因。第一个是速度指令的方向搞反了。视觉算出目标在画面右侧你应该往右飞结果代码里符号写反了往左飞目标越来越远偏差越来越大飞机就越飞越快。排查方法很简单在代码里把dx和dy打印出来手动把目标放在画面右侧看dx是正还是负然后确认速度指令的方向对不对。第二个原因是比例系数太大。kx设成2.0甚至更高偏差0.5米就产生1m/s的速度指令飞机响应不过来冲过头然后反向修正来回振荡。我的经验是kx从0.3开始试逐步加到0.8左右再大就容易振荡了。第三个原因是姿态补偿没做或者做错了。前面说过飞机倾斜会让目标在画面中偏移如果不补偿控制器会把这个偏移当成真实偏差产生错误的速度指令。5.2 目标在画面中抖动严重如果目标检测出来的像素坐标一直在跳速度指令就会跟着抖飞机也会抖。这个问题要从检测环节找原因。颜色阈值分割出来的轮廓如果边缘不平滑质心坐标就会跳。解决办法是对mask做形态学操作先腐蚀再膨胀把噪点去掉把轮廓平滑一下。另一个办法是对像素坐标做低通滤波alpha 0.3 cx_filtered alpha * cx (1 - alpha) * cx_prevalpha越小滤波效果越强但延迟也越大。0.3到0.5之间是比较好的折中。如果抖动还是很大检查一下Gazebo的相机噪声设置。默认情况下Gazebo的相机是没有噪声的但如果你在sensor标签里加了noise那图像本身就有随机扰动检测结果自然会抖。5.3 仿真跑着跑着就发散了“仿真发散”是Gazebo里常见的问题表现为飞机突然乱飞、穿模、或者直接消失。原因可能有很多控制指令超出了飞控的安全限制、仿真步长太小导致数值积分不稳定、或者模型之间的碰撞检测出了问题。先检查你的速度指令有没有限幅。PX4默认的最大水平速度是5m/s如果你发的指令超过这个值飞控会截断但截断之后的指令和你的预期不一致可能导致控制逻辑混乱。在代码里加一句限幅vx max(min(vx, 3.0), -3.0) vy max(min(vy, 3.0), -3.0)然后检查Gazebo的实时因子。如果实时因子远小于1说明仿真跑得比真实时间慢很多这时候物理引擎的积分误差会累积导致发散。解决办法是减小仿真步长或者降低场景复杂度。还有一个容易被忽略的点是模型的惯性参数。如果你自己建的无人机模型质量分布不合理转动惯量太小飞控稍微给一点力矩飞机就剧烈旋转也会导致发散。建议直接用PX4自带的iris或者typhoon模型这些模型的参数是调好的。5.4 视觉延迟导致追踪滞后即使你的算法很快图像从Gazebo传到ROS再到你的处理节点中间也有延迟。如果你发现飞机总是追着目标的“影子”跑目标已经转弯了飞机才反应过来那就是延迟太大了。测量延迟的方法是在图像里加一个时间戳在处理节点里算当前时间和时间戳的差。如果延迟超过50毫秒就要考虑优化。优化的方向包括降低图像分辨率从640x480降到320x240、用压缩图像传输、把处理节点和Gazebo放在同一台机器上。如果延迟实在降不下来可以在控制端做预测。用目标当前的速度估计它的未来位置然后追那个预测位置而不是当前位置。最简单的预测是一阶外推cx_pred cx vx_target * dt其中vx_target是目标在图像中的像素速度可以通过前后两帧的差分来估计。dt是预测时间一般取延迟的一半左右。6. 从仿真到真机中间还差哪些东西仿真跑通之后很多人会迫不及待地想上真机。但仿真和真机之间的差距比你想象的要大。首先是光照仿真里光照恒定真机上太阳角度变化、阴影、反光都会影响颜色阈值的效果。其次是相机仿真相机的内参是理想的真机相机有畸变需要标定。再次是振动真机飞行时相机画面会有高频抖动检测算法要能抗抖。我的建议是在仿真里把控制逻辑和参数调到一个比较满意的程度然后换一个更鲁棒的检测器比如基于轮廓形状的检测或者轻量级的神经网络再上真机。上真机之前先在室内用光流定位把低速追踪跑通确认视觉链路没问题再逐步放开速度。另外真机上的安全策略一定要做好。设置一个地理围栏飞机飞出一定范围就自动悬停或者返航。视觉丢失目标超过一定时间也触发悬停。这些保护逻辑在仿真里也要加上养成习惯。7. 一些让调试效率翻倍的小技巧Gazebo的界面一直在闪这个问题很多人遇到过。除了前面说的软件渲染还有一个办法是把Gazebo的渲染引擎从ogre换成ogre2或者直接用无头模式跑用RViz看图像和状态。无头模式的启动方式make px4_sitl gazebo-classic_iris__empty HEADLESS1这样Gazebo不弹界面只跑物理仿真省下的GPU资源可以用来跑视觉算法整体帧率会高很多。还有一个技巧是把视觉处理节点和Gazebo分开跑在不同的终端里这样你可以单独重启视觉节点而不用重启整个仿真。用roslaunch把节点组织好改代码之后只需要重新编译和重启对应的节点。最后记录数据非常重要。把图像、检测结果、速度指令、飞机位置都录成bag包出问题的时候回放分析比盯着屏幕猜原因高效得多。尤其是那种偶发的发散或者振荡不回放很难定位。我在实际调试中最大的体会是不要试图一次性把所有环节都做到完美。先把最简单的颜色阈值加比例控制跑通看到飞机能跟着目标动起来哪怕追得歪歪扭扭也算成功了。然后再一个环节一个环节地优化每次只改一个参数记录变化。这样即使出了问题你也能快速定位是哪个改动导致的。动态目标追踪本身就是一个需要反复迭代的活仿真环境给了你低成本试错的机会用好它。