PX4 计算机视觉技术指南：光学流、动作捕捉、视觉惯性里程计与碰撞预防

嵌入式物联网机器人自动驾驶智能硬件【免费下载链接】PX4-AutopilotPX4 Autopilot Software项目地址https://gitcode.com/gh_mirrors/px/PX4-Autopilot点击查看免费下载PX4 通过运行在伴机电脑上的计算机视觉系统为飞控提供两大核心能力位姿/速度估计光学流、动作捕捉 MoCap、视觉惯性里程计 VIO与避障/路径规划碰撞预防 Collision Prevention。本文以 PX4-Autopilot 官方用户指南为主线结合仓库源码与参数定义系统讲解这四类视觉能力的原理、传感器选型、PX4 参数配置、EKF2 估计器调参与故障排查帮助开发者快速搭建可用的视觉导航与避障系统。PX4 计算机视觉能力总览计算机视觉Computer Vision技术使计算机能够利用视觉数据理解周围环境。在 PX4 中视觉系统主要运行在伴机电脑上为飞控提供以下两类功能功能类别子功能说明位姿/速度估计光学流 Optical Flow使用朝下的相机与朝下的测距传感器提供2D 速度估计位姿/速度估计动作捕捉 Motion Capture使用机体外部的视觉系统提供3D 位姿估计主要用于室内导航位姿/速度估计视觉惯性里程计 VIO使用机载视觉系统与 IMU 提供3D 位姿与速度估计用于无 GNSS 或 GNSS 不可靠场景避障/路径规划碰撞预防 Collision Prevention在多旋翼手动模式飞行时自动减速/停车避免撞上障碍物对于从事 PX4 计算机视觉开发的开发者官方推荐使用PX4 Vision Autonomy Development KitHolybro 出品作为开发套件它是一个稳健且低成本的入门方案详见完整机型文档。光学流Optical Flow无 GNSS 环境下的 2D 速度估计光学流使用一个朝下的相机和一个朝下的测距传感器进行速度估计可在室内、地下或其他 GNSS 拒止环境中确定飞行速度配合 Position 模式即可实现定点悬停。硬件安装与总线接入一套光学流方案需要一个朝下的相机和一个朝下的测距传感器推荐使用 LiDAR 而非声呐因为其更稳健、精度更高。两者可以集成为单一产品例如ARK FlowDroneCAN 光学流传感器 测距传感器 IMU集成 PAW3902 光学流芯片、Broadcom AFBR-S50LV85D 30 米测距传感器、Invensense ICM-42688-P 6 轴 IMUARK Flow MR中距离版本集成 PixArt PAA3905 光学流芯片、Broadcom AFBR-S50LX85D 50 米测距传感器、Invensense IIM-42653 6 轴 IMUHolybro H-Flow紧凑型 DroneCAN 光学流 测距模块集成 PAA3905、AFBR-S50LV85D 与 ICM-42688-P一体化设计简化安装自带红外 LED 提升低光环境可见度。也可以使用分离式传感器例如 PX4Flow 光学流传感器搭配 Lidar-Lite 测距传感器。此外基于 PMW3901 芯片的光学流传感器工作原理类似电脑鼠标适配 80 mm 至无限远被 Bitcraze、Tindie、Hex、Thone、Alientek 等多款产品采用也可以使用带集成相机的板卡参考 PX4-OpticalFlow 与 snap_cam 仓库。传感器可通过 MAVLink、I2C 或其他支持的外设总线连接。若通过 MAVLink 连接光学流相机必须发布OPTICAL_FLOW_RAD消息、测距传感器必须发布DISTANCE_SENSOR消息PX4 会将其写入对应的 uORB 主题DistanceSensor 与 ObstacleDistance。流向方向与缩放因子校验光学流传感器在车辆不同方向运动时的积分输出必须符合下表车辆运动积分流向前进 Y后退- Y右移- X左移 X说明积分流值是以右手法则表示的角测量值弧度表示图像绕传感器机体轴的旋转而非沿各轴的平移位移。因此车辆沿 X 轴前进表现为 Y 轴正方向流地面图像绕 Y 轴旋转车辆沿 Y 轴右移表现为 X 轴负方向旋转。缩放因子校验对于纯旋转运动OPTICAL_FLOW_RAD.integrated_xgyro与integrated_x同理integrated_ygyro与integrated_y应保持一致。若不一致可通过参数 SENS_FLOW_SCALE 调整光学流缩放因子。该参数定义于 src/modules/sensors/sensor_params_flow.yaml 中实际由src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp中的ParamFloatpx4::params::SENS_FLOW_SCALE读取使用。提示常见光学流传感器分辨率较低在高于地面 20 米以上悬停时可能引起缓慢振荡适当减小光学流缩放因子可改善此现象。估计器融合与 EKF2 配置光学流传感器数据会与其他速度数据源融合融合方式及相对机体中心的偏移量需要在所用估计器中配置。EKF2 融合开关设置 EKF2_OF_CTRL 启用光学流融合。从 src/modules/ekf2/params_optical_flow.yaml 可以看到该参数为布尔类型默认值为1启用。同一文件中还定义了相关配套参数参数说明默认值/范围EKF2_OF_DELAY光学流测量相对 IMU 测量的延迟ms默认按 DroneCAN 流节点的积分窗口关闭后约 7 ms 到达飞控设定7 ms0~300需重启EKF2_OF_GATE光学流融合的创新一致性检验门限标准差倍数3.0 SD1.0 起EKF2_OF_N_MIN传感器质量指标最高时的测量噪声0.15 rad/sEKF2_OF_N_MAX传感器质量指标最低时的测量噪声0.5 rad/sEKF2_OF_QMIN空中使用时光学流最低质量阈值0~2551EKF2_OF_QMIN_GND地面使用时光学流最低质量阈值0~2550EKF2_OF_GYR_SRC光学流角速率补偿来源0Auto优先用消息内陀螺、1Internal始终用内部陀螺0EKF2_OF_POS_X光学流焦点在机体坐标系中的 X 位置相对重心0.0 mEKF2_OF_POS_Y光学流焦点在机体坐标系中的 Y 位置0.0 mEKF2_OF_POS_Z光学流焦点在机体坐标系中的 Z 位置0.0 m偏移量均以机体朝向与重心为基准计算。光学流导航可由 EKF2 启用LPE 估计器已废弃。更多信息参见 使用 PX4 导航滤波器EKF2 光学流。动作捕捉MoCap外部视觉系统提供 3D 位姿动作捕捉Motion Capture通过机体外部的定位机制估计车辆的 3D 位姿位置与姿态常用于无 GPS 的室内场景提供局部坐标系下的位置。MoCap 系统最常见的是红外相机方案也可使用其他类型相机、LiDAR 或超宽带UWB技术。与 VIO 的区别MoCap 与视觉惯性里程计概念相似核心差异在于 VIO 的视觉系统运行在机载并额外利用机载 IMU 提供速度信息而 MoCap 的定位机制在机体外部。MoCap 相关资料可进一步参考使用视觉或动作捕捉系统进行位置估计讲解 PX4 如何通过 MAVLink 集成外部位姿数据以及如何配置 EKF2/LPE 估计器使用动作捕捉飞行VICON、NOKOV、Optitrack端到端的搭建教程使用 PX4 导航滤波器EKF2 外部视觉系统。视觉惯性里程计VIO机载视觉 IMU 的 3D 位姿与速度估计视觉惯性里程计VIO结合视觉里程计从相机图像估计位姿与 IMU 惯性测量修正快速运动导致图像质量差时的误差输出相对局部起始位置的 3D 位姿与速度适用于室内、桥下等无 GNSS 或 GNSS 不可靠的场景。推荐软硬件架构PX4 官方推荐的 VIO 方案为现成跟踪相机 运行 ROS 的伴机电脑相机安装默认镜头朝下安装相机对振动非常敏感建议采用软性安装如振动隔离泡沫。典型相机示例为 Intel® RealSense™ Tracking Camera T265详见相机文档伴机配置在伴机电脑上安装并配置 MAVROS实现并运行一个 ROS 节点读取相机数据通过 MAVROS 发布 VIO 里程计PX4 调参按下文 EKF2 参数配置外部位置估计连接验证可使用 QGroundControl 的 MAVLink Inspector 确认收到ODOMETRY或VISION_POSITION_ESTIMATE消息或检查 component id 为 197 的HEARTBEAT消息即MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY首飞前验证务必先完成下文的 VIO 验证检查。说明PX4 本身不关心消息来源只要通过合适的 MAVLink 接口提供即可ROS 只是官方推荐的示例路由方案。ROS VIO 节点要求ROS 节点需完成三件事与所选相机/传感器硬件交互生成包含位置估计的里程计消息并经 MAVROS 发送给 PX4消息类型为nav_msgs/Odometry发布到主题/mavros/odometry/out发布 VIO 系统状态消息类型为mavros_msgs/CompanionProcessStatus发布到/mavros/companion_process/status组件标识为MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY197并推荐以下状态值MAV_STATE_ACTIVEVIO 系统工作正常MAV_STATE_CRITICALVIO 系统工作但置信度低MAV_STATE_FLIGHT_TERMINATION系统故障或估计置信度低到不可接受。节点实现与所选相机强相关需根据相机的接口与驱动自行开发。EKF2 外部视觉调参使用 EKF2 融合外部位置信息需设置以下参数可在 QGroundControl 的Vehicle Setup Parameters EKF2中修改修改后需重启飞控生效参数说明EKF2_EV_CTRL根据所需融合模型分别启用水平位置融合、垂直视觉融合、速度融合与偏航融合EKF2_HGT_REF设为Vision使用视觉作为高度估计的参考传感器EKF2_EV_DELAY设为测量时间戳与实际采集时间的差值详见下文EKF2_EV_POS_X、EKF2_EV_POS_Y、EKF2_EV_POS_Z视觉传感器相对机体坐标系的位置EKF2_EV_DELAY 调优EKF2_EV_DELAY是视觉位置估计器相对 IMU 测量的延迟即视觉系统时间戳与 IMU 时钟EKF2 的基准时钟记录的实际采集时间之差。理论上若 MoCap 与 ROS 电脑之间存在正确的时间戳而非到达时间与时间同步如 NTP该值可设为 0但现实中通信链路延迟因环境而异通常需要经验性调参。可以从日志中检查 IMU 速率与 EV 速率之间的偏移得到粗略估计可用 FlightPlot 等工具绘制外部数据与机载估计的对比图再通过动态机动中使 EKF 创新值最低的方式微调。首飞前验证 VIO 估计注意下文涉及的MAV_ODOM_LP参数已在 PX4 v1.14 中移除此节内容有待更新。按以下步骤在首次飞行前验证 VIO 是否正常工作将 PX4 参数MAV_ODOM_LP设为1PX4 将把接收到的外部位姿以 MAVLinkODOMETRY消息流式回传可用 QGroundControl MAVLink Inspector 查看偏航车辆直到ODOMETRY消息的四元数非常接近单位四元数w1, xyz0——此时机体坐标系与外部位姿系统参考系对齐。若无法在不横滚/俯仰的情况下接近单位四元数说明机架仍有俯仰或横滚偏移不要继续请重新检查坐标系对齐后将车辆从地面拿起应看到位置 z 坐标减小车辆前移应使 x 坐标增大车辆右移应使 y 坐标增大确认消息中的线速度以FRD 机体坐标系表达将MAV_ODOM_LP设回0PX4 停止回传ODOMETRY消息。上述检查一致后即可尝试首飞将车辆置于地面并开始流式回传ODOMETRY油门杆放最低并解锁电机左杆最低时切到位置控制应看到绿灯表示位置反馈可用、位置控制已激活油门杆置于中位死区以维持高度推杆起飞后立即回到中位最后确认车辆能保持位置。VIO 常见故障排查飞行时漂移/飞走但卸桨手持移动正常若使用 T265尝试软性安装该相机对高频振动非常敏感开启 VIO 后出现马桶效应toilet-bowling确认相机朝向与 launch 文件中的坐标变换一致并用 MAVLink Inspector 核对 MAVROS 发出的ODOMETRY速度是否对齐 FRD 坐标系既想用视觉位置做回环闭合、又想同时运行 GPS两者冲突时会干扰 EKF测试表明仅使用视觉速度更可靠。开发者若想扩展此实现或编写不依赖 ROS 的替代方案可参考使用视觉或动作捕捉系统进行位置估计该文档也说明了如何在 LPE 估计器已废弃下配置 VIO。碰撞预防Collision Prevention基于传感器地图的自动减速与停车碰撞预防可在撞上障碍物前自动减速并停车适用于基于加速度的 Position 模式需将 MPC_POS_MODE 设为Acceleration based的多旋翼或 VTOL 的 MC 模式数据可来自伴机电脑、通过 MAVLink 接入的机外测距传感器、挂载在飞控上的测距传感器或其任意组合。重要提示若传感器量程不足碰撞预防会限制车辆最大速度在无传感器数据的方向上禁止运动例如没有后向传感器就无法后退。因此需确保想飞行的所有方向都有传感器覆盖并在不需要时考虑禁用碰撞预防。支持的距离传感器Lanbao PSK-CM8JL65-CC5已停产PX4 开箱即用支持先按传感器文档安装配置并启用碰撞预防CP_DIST再用 SENS_CM8JL65_R_0 设置传感器朝向LightWare LiDAR SF45 旋转激光雷达PX4 v1.14 及以后支持提供 320 度感知详见传感器文档Sony AS-DT1 LiDAR可作为直连 UART 传感器用于碰撞预防驱动按配置的偏航偏移以 5 度扇区将测量发布到obstacle_distance其前向水平视场约 35 度仅覆盖扇区有数据其余方向无数据。按传感器文档配置后启用 CP_DIST 即可其他测距传感器需要修改驱动代码——模仿SENS_CM8JL65_R_0参数设置朝向也可在传感器module.yaml中硬编码如sf0x start -d ${SERIAL_DEV} -R 2525 等价于ROTATION_DOWNWARD_FACING并在距离传感器 uORB 主题distance_sensor_s.h_fov中设置水平视场。PX4 软件配置参数在 QGroundControl 中设置以下参数启用碰撞预防参数说明CP_DIST车辆距障碍物的最小允许距离设为负值可禁用碰撞预防。警告该值指距传感器的距离而非机架/螺旋桨外侧务必留出安全余量。源码定义见 src/lib/collision_prevention/collisionprevention_params.yaml默认 -1.0 m禁用范围 -1~15 mCP_DELAY传感器与速度设定值跟踪延迟默认 0.4 s0~1 s仅 Position 模式使用CP_GUIDE_ANG允许偏离指令方向的引导角左右各多少度默认 30 度0~90 度仅 Position 模式使用CP_GO_NO_DATA设为 1 允许车辆驶向无传感器覆盖的方向默认 0/FalseMPC_POS_MODE必须设为Acceleration based算法原理与速度限制所有传感器数据被融合为车辆周围36一说 72个扇区的内部表示每个扇区记录传感器数据、最后观测时间或无数据标记。当指令车辆向某方向运动时检查该方向半球内的所有扇区若运动会接近障碍物则限制速度。从源码看碰撞预防集成在位置控制的加速度设定值链路中src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp的generateSetpoints()在生成水平加速度设定值后调用_collision_prevention.modifySetpoint(_acceleration_setpoint, _velocity_setpoint)见该文件 L119-L121从而对加速度设定值与速度设定值进行约束。速度限制同时考虑内环速度控制增益 MPC_XY_P加加速度最优速度控制器 的 MPC_JERK_MAX 与 MPC_ACC_HOR各扇区传感器量程。算法可拆分为两部分加速度约束把加速度设定值分解为平行于最近障碍物距离与垂直于它的两个分量分别按缩放因子缩放与速度补偿当前速度与最大允许速度比较基于最大加加速度、加速度与延迟确保能及时刹停再用加速度控制器的比例增益 MPC_XY_VEL_P_ACC 转换为加速度。注意若某方向无传感器数据该方向速度被限制为 0防止撞上未观测物体若希望自由驶向无覆盖方向可设置 CP_GO_NO_DATA 为 1。若相邻扇区明显更好算法可在 CP_GUIDE_ANG 指定角度内微调请求方向帮助引导车辆绕过障碍物而非卡死。引导功能永远不会将车辆引向无传感器数据的方向单前向测距传感器场景下感觉卡住很可能是因为缺乏信息导致引导无法安全改变方向。实测中 30 度是比较好的平衡值不同机型可能不同过小会感觉卡住仅允许沿最小距离方向后退过大会感觉车辆滑向操作员未指令的方向。距离数据丢失行为若超过0.5 s未收到任何传感器的距离数据PX4 输出警告No range data received, no movement allowed并将 xy 速度设定值强制归零若5 s未收到数据车辆切换到 HOLD 模式恢复运动需禁用碰撞预防将 CP_DIST 设为负值或切换到其他模式如 Altitude / Stabilized 模式多传感器场景下丢失其中一个时仍可在其余传感器视场FOV内飞行故障传感器数据会过期其覆盖区域被视为无覆盖无法向该方向运动。注意启用 CP_GO_NO_DATA1 后丢失传感器的区域同样被当作无覆盖可无约束地飞入请谨慎使用。CP_DELAY 延迟调优延迟主要来自两个来源传感器延迟与速度设定值跟踪延迟均用 CP_DELAY 调优。直连飞控的距离传感器延迟可视为 0外部视觉系统的传感器延迟可高达 0.2 s。速度设定值跟踪延迟可通过在 Position 模式全速飞行后指令停车、从日志中测量实际速度与速度设定值的延迟获得典型值 0.1~0.5 s取决于机型尺寸与调参。提示若车辆接近障碍物时速度振荡减速-加速-减速说明延迟设得过高。CP_GUIDE_ANG 引导调优设为 0 可禁用引导车辆只沿指令方向运动增大该值可让车辆选择更优的避障方向更易穿越狭窄间隙、绕行障碍时保持精确最小距离。若车辆接近障碍物时卡住通常是引导角太小若车辆向操作员未指令的方向滑动则是引导角太大。伴机电脑设置使用伴机电脑或外部传感器时需提供 OBSTACLE_DISTANCE 消息流反映障碍物被检测到的时间与位置。消息最小发送频率取决于飞行速度——频率越高车辆响应时间越长。系统初期测试使用 4 m/s 飞行速度配合 10 Hz 的OBSTACLE_DISTANCE消息视觉系统的最大支持频率系统在更高速度与更低频率下也可能良好工作。警告伴机实现/设置目前未经测试原伴机项目已停止维护并被归档。Gazebo 仿真测试碰撞预防可在 Gazebo 中使用 x500_lidar_2d 机型测试make px4_sitl gz_x500_lidar_2d随后调整相关参数到合适值并在仿真世界中添加任意障碍物验证碰撞预防功能。实时可视化PlotJuggler 绘制障碍物距离可以使用 PlotJuggler 实时监视并可视化障碍物距离含最近障碍物的最小距离。其原理是通过一个响应式 Lua 脚本在每个时间步提取obstacle_distance_fused数据、将距离值转换为笛卡尔坐标并推送给 PlotJuggler。步骤如下按照 实时绘制 uORB 主题数据PlotJuggler 的说明操作配置 PX4 发布障碍物距离数据将obstacle_distance_fuseduORB 主题加入src/modules/uxrce_dds_client/dds_topics.yaml关于 uXRCE-DDS 桥的dds_topics.yaml详见 DDS Topics YAML- topic: /fmu/out/obstacle_distance_fused type: px4_msgs::msg::ObstacleDistance打开 PlotJuggler进入Tools Reactive Script Editor在Script Editor标签页添加脚本全局代码只执行一次obs_dist_fused_xy ScatterXY.new(obstacle_distance_fused_xy) obs_dist_min Timeseries.new(obstacle_distance_minimum)function(tracker_time)obs_dist_fused_xy:clear() i 0 angle_offset TimeseriesView.find(/fmu/out/obstacle_distance_fused/angle_offset) increment TimeseriesView.find(/fmu/out/obstacle_distance_fused/increment) min_dist 65535 -- Cache increment and angle_offset values at tracker_time to avoid repeated calls local angle_offset_value angle_offset:atTime(tracker_time) local increment_value increment:atTime(tracker_time) if increment_value nil or increment_value 0 then print(Invalid increment value: .. tostring(increment_value)) return end local max_steps math.floor(360 / increment_value) while i max_steps do local str string.format(/fmu/out/obstacle_distance_fused/distances[%d], i) local distance TimeseriesView.find(str) if distance nil then print(No distance data for: .. str) break end local dist distance:atTime(tracker_time) if dist ~ nil and dist 65535 then -- Calculate angle and Cartesian coordinates local angle angle_offset_value i * increment_value local y dist * math.cos(math.rad(angle)) local x dist * math.sin(math.rad(angle)) obs_dist_fused_xy:push_back(x, y) -- Update minimum distance if dist min_dist then min_dist dist end end i i 1 end -- Push minimum distance once after the loop if min_dist 65535 then obs_dist_min:push_back(tracker_time, min_dist) else print(No valid minimum distance found) end在右上角为脚本命名并点击Save保存后脚本出现在 Active Scripts 区域按实时绘制 uORB 主题数据的方法开始流式传输数据即可在左侧看到obstacle_distance_fused_xy与obstacle_distance_minimum序列。注意加载新日志或清空数据后需再次点击Save重新启用脚本。传感器数据如何映射到内部地图碰撞预防内部将无人机周围平面划分为 72 个扇区数据存储在obstacle_distanceuORB 主题中新传感器数据与现有地图比较并更新变化区域。不同来源的数据处理方式不同旋转激光雷达数据直接加入obstacle_distanceuORB 主题测距传感器先发布到distance_sensoruORB 主题再映射到obstacle_distance——与传感器朝向orientation和q及水平视场h_fov有重叠的所有扇区都会被赋该测量值。例如测量范围为 9.99°~10.01° 的传感器数据会加入覆盖 2.5°~12.5° 弧段的 5° 与 10° 扇区。四元数q仅在orientation设为ROTATION_CUSTOM时使用伴机电脑通过 ROS 2 或 MAVLinkOBSTACLE_DISTANCE消息更新obstacle_distance主题。开发资源与下一步若希望进一步了解伴机电脑与 PX4 的交互细节可阅读伴机电脑章节使用视觉/动作捕捉进行位置估计的通用集成指南见外部位置估计EKF2 的完整外部视觉融合说明见使用 PX4 导航滤波器EKF2 外部视觉系统社区资源 XTDrone 提供基于 ROS PX4 v1.9 的计算机视觉仿真环境其配套手册包含完整的入门教程可作为无硬件条件下的视觉算法开发与验证平台。赞分享嵌入式物联网机器人自动驾驶智能硬件【免费下载链接】PX4-AutopilotPX4 Autopilot Software项目地址https://gitcode.com/gh_mirrors/px/PX4-Autopilot点击查看免费下载相关推荐无人机视觉惯性里程计PX4-Autopilot VIO系统集成指南无人机视觉惯性里程计PX4 Autopilot VIO系统集成指南 PX4 Autopilot视觉惯性里程计VIO系统是无人机在GPS信号缺失环境下实现精嵌入式物联网机器人自动驾驶智能硬件FAST-LIVO完整使用指南快速掌握激光视觉惯性里程计技术FAST LIVO完整使用指南快速掌握激光视觉惯性里程计技术 FAST LIVO是一款快速紧耦合的稀疏直接激光雷达 惯性 视觉里程计系统它通过融合激光雷达、机器人ROS自动驾驶科研计算机视觉FAST-LIVO终极指南快速掌握高精度激光视觉惯性里程计技术FAST LIVO终极指南快速掌握高精度激光视觉惯性里程计技术 在机器人导航和自动驾驶领域 激光视觉惯性里程计LIVO 技术正成为实现高精度定位与建图的机器人ROS自动驾驶科研计算机视觉上一篇终极指南3步保护你的AI声库SoftVC VITS模型加密全攻略下一篇Screenshot-to-code代码可维护性分析转换后代码的长期维护成本创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考