PX4 计算机视觉功能全解析：Optical Flow、Motion Capture、VIO 与防碰撞

PX4 计算机视觉功能全解析Optical Flow、Motion Capture、VIO 与防碰撞【免费下载链接】PX4-AutopilotPX4 Autopilot Software项目地址: https://gitcode.com/gh_mirrors/px/PX4-Autopilot计算机视觉Computer Vision技术让飞行器能够利用视觉数据理解周围环境从而在无 GNSS 或 GNSS 不可靠的环境中实现自主导航与安全飞行。PX4 通过伴机电脑Companion Computer上的视觉系统为飞控提供光流Optical Flow、动作捕捉Motion Capture、视觉惯性里程计VIO和防碰撞Collision Prevention四大能力。本文以 docs/en/advanced/computer_vision.md 为主线结合仓库内 EKF2 估计器、防碰撞库源码与 uORB 消息定义系统讲解每项技术的原理、配置方法、适用场景与实战选型建议。概述PX4 的四大视觉能力PX4 使用计算机视觉系统主要运行在伴机电脑上来支持以下功能光流Optical Flow使用朝下的摄像头和朝下的距离传感器提供 2D 速度估计用于 GNSS 缺失环境室内、地下等下的速度测量与定点悬停。动作捕捉Motion Capture通过飞行器外部的视觉系统如红外相机阵列提供 3D 位姿估计主要用于室内导航。视觉惯性里程计VIO使用机载视觉系统与 IMU 提供 3D 位姿和速度估计在 GNSS 位置信息缺失或不可靠时用于导航。防碰撞Collision Prevention在障碍物撞击前自动减速并停止飞行器主要应用于手动模式Position 模式。::: tip 官方推荐的视觉开发硬件平台是 PX4 Vision 自动驾驶开发套件Holybro 出品它是一套兼顾成本与可靠性的开发者套件出厂预装 Pixhawk 4 / Pixhawk 6C 飞控、UP Core 伴机电脑与 Structure Core 深度相机。 :::Motion Capture外部 3D 位姿估计动作捕捉MoCap是一种使用飞行器外部定位机制估计 3D位姿位置与姿态的技术。MoCap 系统最常使用红外相机检测运动也可使用其他类型的相机、Lidar 或超宽带UWB技术。::: info MoCap 常用于 GPS 缺失如室内环境下的导航提供相对于局部坐标系的位置信息。 :::核心特性与差异外部视角MoCap 由外部系统如 VICON、NOKOV、Optitrack告诉飞行器自身的位姿相机不在机上。支持消息位姿数据通过 MAVLink 的ATT_POS_MOCAP消息或frame_id MAV_FRAME_MOCAP_NED的ODOMETRY消息送入 PX4映射到vehicle_mocap_odometryuORB 主题。估计器差异EKF2 只订阅vehicle_visual_odometry主题因此使用 EKF2 时 MoCap 系统必须生成VISION_POSITION_ESTIMATE或ODOMETRY消息LPE 估计器两者都订阅但 LPE 已废弃推荐使用 EKF2。坐标系与轴交换技巧PX4 的机体坐标系为 FRDXForward 前、YRight 右、ZDown 下参考坐标系在磁航向对齐时称为 NEDXNorth 北、YEast 东、ZDown 下。外部位姿估计的参考系通常与此不一致需要在发送 MAVLink 消息前做坐标变换。以 Optitrack 为例其局部坐标系 x、z 位于水平面x 前、z 右y 轴竖直向上。通过简单的轴交换即可得到 NED 约定x_{mav} x_{mocap} y_{mav} z_{mocap} z_{mav} - y_{mocap}姿态四元数同理保持标量部分 w 不变将矢量部分 x、y、z 按同样方式交换。这一技巧适用于所有需要转换到 NED 帧的系统。参考文档使用视觉或动作捕捉系统进行位置估计含 ROS/MAVROS 完整配置使用动作捕捉飞行VICON、NOKOV、OptitrackEKF2 导航滤波器 外部视觉系统VIO机载视觉惯性里程计视觉惯性里程计VIO用于估计运动飞行器相对于局部起始位置的 3D位姿和速度。它常用于 GPS 缺失如室内或不可靠如桥下飞行的环境。VIO 使用视觉里程计技术从图像信息估计位姿并结合 IMU 的惯性测量来校正快速运动导致的图像模糊误差。::: info VIO 与 MoCap 的一个关键区别是VIO 的相机/IMU 位于飞行器上机载视角并且额外提供速度信息。 :::MAVLink 与 uORB 映射PX4 通过以下 MAVLink 消息接收外部位置信息并映射到 uORB 主题MAVLink 消息帧要求uORB 主题VISION_POSITION_ESTIMATE—vehicle_visual_odometryODOMETRYframe_id MAV_FRAME_LOCAL_FRDvehicle_visual_odometryATT_POS_MOCAP—vehicle_mocap_odometryODOMETRYframe_id MAV_FRAME_MOCAP_NEDvehicle_mocap_odometry重要限制EKF2默认估计器只订阅vehicle_visual_odometry只能处理前两种消息ODOMETRY是唯一能同时携带线速度的消息。消息应以 30 Hz含协方差时到 50 Hz 的速率流式发送速率过低时 EKF2 不会融合外部视觉消息。以下 MAVLink 视觉消息当前不支持GLOBAL_VISION_POSITION_ESTIMATE、VISION_SPEED_ESTIMATE、VICON_POSITION_ESTIMATE。EKF2 外部视觉配置参数在QGroundControlVehicle 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设置视觉传感器或 MoCap 标记点相对机体坐标系的位置EKF2_EV_CTRL的融合位定义参见 tuning_the_ecl_ekf.md 外部视觉系统一节位0水平位置数据位1垂直位置数据高度源可额外用EKF2_HGT_REF配置位2速度数据位3偏航数据启用后航向相对外部视觉坐标系否则相对北此外可用EKF2_GPS_CTRL、EKF2_BARO_CTRL、EKF2_RNG_CTRL分别关闭 GNSS、气压计和测距仪融合。修改参数后需要重启飞控生效。EKF2 会考虑视觉位姿估计的不确定性该不确定性可通过ODOMETRY消息的协方差字段发送也可通过EKF2_EVP_NOISE、EKF2_EVV_NOISE、EKF2_EVA_NOISE参数设定来源选择由EKF2_EV_NOISE_MD控制。调校 EKF2_EV_DELAYEKF2_EV_DELAY即视觉位置估计器相对 IMU 测量的延迟也就是视觉系统时间戳与 EKF2 基准时钟IMU 时钟记录实际采集时间之差。理论上若 MoCap 与 ROS 主机之间时间戳正确且时钟同步如 NTP可设为 0实际上整条 MoCap→PX4 链路的延迟高度依赖具体部署需要经验调校。粗略估计查看日志中 IMU 速率与 EV 速率的偏移。将 SDLOG_PROFILE 的位 7Computer Vision and Avoidance置 1 以记录 EV 速率。精细调校动态机动中变化该参数找到使 EKF 创新量innovations最低的值。T265 VIO 相机设置示例虽然 Intel RealSense T265 相机已停产但其设置流程仍是 VIO 部署的典型模板使用 Intel 提供的realsense-ros封装读取原始数据。相机默认镜头朝下安装需通过 ROS launch 文件发布base_link与camera_pose_frame之间的静态变换tf例如node pkgtf typestatic_transform_publisher nametf_baseLink_cameraPose args0 0 0 0 1.5708 0 base_link camera_pose_frame 1000/前三个args为从飞控中心到相机的平移量 x、y、z米后三个为旋转yaw, pitch, roll弧度[... 0, 1.5708, 0]表示俯仰向下 90°朝向地面。T265 对高频振动敏感应使用减振泡沫等软安装方式。Optical Flow2D 速度估计光流Optical Flow 使用朝下的摄像头与朝下的距离传感器进行速度估计可在 GNSS 缺失环境建筑物内、地下等中确定速度。硬件与连接光流传感器与距离传感器可集成于单个产品如 ARK Flow、ARK Flow MR、Holybro H-Flow也可分开部署。可通过 MAVLink、I2C 或其他支持外设的总线连接。若经 MAVLink 连接光流相机必须发布OPTICAL_FLOW_RAD消息距离传感器必须发布DISTANCE_SENSOR消息信息写入对应的 DistanceSensor 与 ObstacleDistance uORB 主题。距离传感器推荐使用 LiDAR 而非声纳因其鲁棒性与精度更好。输出方向约定光流在不同运动方向下的输出必须符合以下约定飞行器运动积分光流前进 Y后退- Y向右- X向左 X::: info 积分光流值是角度测量弧度表示图像绕传感器机体轴的旋转右手系而非沿轴的平移位移。因此飞行器前进沿 X出现在 Y 光流轴右移沿 Y出现在 X 光流轴。 :::比例因子Scale Factor纯旋转时OPTICAL_FLOW_RAD.integrated_xgyro与integrated_x以及integrated_ygyro与integrated_y应当相等。若不一致可通过 SENS_FLOW_SCALE 调整光流比例因子。::: tip 常见光流传感器分辨率较低在高于地面 20 m 悬停时可能引起缓慢振荡。适当减小光流比例因子可改善该情况。 :::EKF2 中的光流融合在 tuning_the_ecl_ekf.md 的光流一节中EKF2 使用光流数据需满足存在有效的测距仪数据设置 EKF2_OF_CTRL光流传感器返回的质量指标高于 EKF2_OF_QMIN 设定的最低要求。光流传感器相对飞行器中心的偏移通过以下参数设置默认均为 0.0 m参数说明EKF2_OF_POS_X光流焦点在机体坐标系中的 X 位置EKF2_OF_POS_Y光流焦点在机体坐标系中的 Y 位置EKF2_OF_POS_Z光流焦点在机体坐标系中的 Z 位置若低空 10 m可稳定悬停但更高空出现缓慢振荡可考虑调整光流比例因子见上。支持的传感器ARK Flow / ARK Flow MRDroneCAN 光流 测距 IMU 一体化模块分别采用 PAW3902 / PAA3905 光流传感器与 AFBR-S50LV85D / AFBR-S50LX85D 测距传感器。Holybro H-Flow紧凑型 DroneCAN 光流 测距模块集成 PAA3905 光流传感器、AFBR-S50LV85D 测距传感器与 ICM-42688-P 六轴 IMU自带红外 LED 增强低光环境可见性。PMW3901 系列类似电脑鼠标的追踪传感器可在 80 mm 至无穷远范围内工作被 Bitcraze、Tindie、Hex 等多家产品采用。其他方案也可使用带集成相机的板卡参考 PX4 的 Optical Flow 仓库与 snap_cam 项目。防碰撞Collision Prevention防碰撞 用于在飞行器撞上障碍物之前自动减速并停止。它适用于基于加速度的 Position 模式MPC_POS_MODE设为Acceleration based下的多旋翼或 MC 模式下的 VTOL数据源可以是伴机电脑、MAVLink 上的外部测距仪、飞控直连的测距仪或以上任意组合。::: warning 防碰撞可能限制飞行器的最大速度若传感器量程不足并禁止向无传感器数据的方向运动例如没有后向传感器就无法后退。若高速飞行很关键建议在不需要时关闭防碰撞。 :::PX4 软件配置在QGroundControl中设置以下参数参数说明CP_DIST设置最小允许距离飞行器可接近障碍物的最近距离。设为负值可禁用防碰撞。警告该距离是到传感器的距离而非到机身或螺旋桨外侧的距离务必留出安全余量CP_DELAY设置传感器与速度设定点跟踪延迟用于延迟调校。CP_GUIDE_ANG设置引导角指令方向两侧的角度飞行器在该角度内可偏离以寻找障碍更少的方向。CP_GO_NO_DATA设为 1 允许向无传感器覆盖的方向运动默认 0/False。MPC_POS_MODE必须设为Acceleration based。上述参数在仓库中的定义见 collisionprevention_params.yaml其默认值与范围CP_DIST默认 -1.0-1 ~ 15 m、CP_DELAY默认 0.4 s0 ~ 1 s、CP_GUIDE_ANG默认 30.0°0 ~ 90°、CP_GO_NO_DATA默认 0。算法原理防碰撞实现位于 src/lib/collision_prevention其核心数据结构由 CollisionPrevention.hpp 定义所有传感器数据融合为飞行器周围72 个扇区的内部障碍地图每个扇区包含传感器数据、最后观测时间或无数据标记BIN_COUNT 72BIN_SIZE 5°与 ObstacleDistance.msg 中uint16[72] distances的 72 元素数组一一对应。当飞行器被指令向某方向运动时检查该方向半球内的所有扇区判断运动会否使飞行器更接近障碍物若是则限制飞行器速度。速度限制同时考虑由MPC_XY_P调节的内环速度环以及经MPC_JERK_MAX与MPC_ACC_HOR配置的 jerk 最优速度控制器限制保证飞行器能及时停止以维持CP_DIST距离。若某方向无传感器数据该方向速度被限制为 0防止撞上未探测物体可通过CP_GO_NO_DATA 1允许向无覆盖方向运动。传感器数据与速度设定点跟踪的整体延迟通过CP_DELAY保守估计。若指令扇区相邻扇区明显更好请求输入方向最多可被修正CP_GUIDE_ANG指定的角度帮助引导飞行器绕过障碍。数据丢失行为若超过 0.5 s 未收到任何测距数据PX4 输出警告No range data received, no movement allowed并将 xy 速度设定点强制置零源码中对应RANGE_STREAM_TIMEOUT_US 500 ms超时。5 s 无数据后TIMEOUT_HOLD_US 5_s飞行器切换到 HOLD 模式。恢复运动需将CP_DIST设为负值禁用防碰撞或切换到非 Position 模式如 Altitude 或 Stabilized 模式。多传感器场景下丢失其中一个时故障传感器覆盖区域将按无数据处理无法向该区域运动。警告启用CP_GO_NO_DATA1后若多传感器之一失联其覆盖区域同样被视为无数据可无约束地运动过去需格外小心。延迟调校CP_DELAY两大延迟来源传感器延迟直连飞控的测距仪可视为 0外部视觉系统最高可达 0.2 s与飞行器速度设定点跟踪延迟在 Position 模式全速飞行后指令停车从日志测量实际速度与设定点速度的延迟通常 0.1 ~ 0.5 s。::: tip 若飞行器接近障碍物时速度振荡减速-加速-减速说明延迟设置过高。 :::引导角调校CP_GUIDE_ANG设为 0 禁用引导飞行器只严格沿指令方向运动。增大该值让飞行器自主选择避障最优方向更易穿越狭窄间隙、绕行物体时精确保持最小距离。过小会感觉卡住只有远离障碍的方向被允许过大会感觉飞行器向未指令方向滑动。经验上 30° 是较好的平衡点。引导功能永远不会将飞行器导向无传感器数据的方向只有单个前向测距仪时感觉卡住通常是因为缺少信息而无法安全调整方向。加速度约束与速度补偿防碰撞算法分为两部分约束操作者的加速度设定点以及补偿飞行器当前速度。加速度设定点被分解为平行于最近障碍距离的分量与垂直分量各自按比例缩放。速度补偿将当前速度与仍能按最大 jerk、加速度和延迟刹停的最大允许速度比较并利用加速度控制器的比例增益MPC_XY_VEL_P_ACC转换为加速度。传感器数据与扇区映射旋转式 LiDAR直接将数据写入obstacle_distanceuORB 主题。测距仪将数据发布到distance_sensoruORB 主题再映射到obstacle_distance所有与测距仪朝向orientation、q及水平视场h_fov有重叠的扇区都会被赋予该测量值。伴机电脑通过 ROS 2 或 MAVLinkOBSTACLE_DISTANCE消息更新obstacle_distance主题。消息发送频率取决于飞行速度——初始测试以 4 m/s 速度飞行、10 Hz 发送视觉系统支持的最大频率工作良好系统可能支持更高速度与更低频率。obstacle_distance主题的角度定义与obstacle_distance_fused主题均由 ObstacleDistance.msg 描述distances[0]对应局部北向increment为每个元素的角宽度angle_offset为 0 号元素的角度偏移。测试与开发工具单元测试运行make tests TESTFILTERCollisionPrevention见 CollisionPreventionTest.cpp测试覆盖默认关闭behaviorOff、无传感器数据时速度限制为零noSensorData、障碍消息下的行为等场景。代码集成位置防碰撞在 Position 模式的手动加速度设定点生成处生效见 StickAccelerationXY.cpp 中if (_collision_prevention.is_active()) _collision_prevention.modifySetpoint(...)类成员声明于 FlightTaskManualPosition.hpp。Gazebo 仿真使用 x500 带 2D LiDAR 模型测试防碰撞make px4_sitl gz_x500_lidar_2dPlotJuggler 实时可视化在dds_topics.yaml中加入obstacle_distance_fused主题导出使用 Reactive Lua 脚本将距离数据转换为笛卡尔坐标实时绘制障碍物距离与最小距离。支持的测距传感器Lanbao PSK-CM8JL65-CC5已停产开箱即用支持防碰撞配置 SENS_CM8JL65_R_0 设置传感器朝向。LightWare LiDAR SF45旋转式 LiDAR提供 320° 感知。Sony AS-DT1多点 LiDAR作为直连 UART 传感器支持防碰撞驱动以 5° 为 bin 发布到obstacle_distance前向水平视场约 35°仅填充被覆盖的扇区。其他测距仪需要修改驱动代码设置传感器朝向与视场模仿SENS_CM8JL65_R_0参数或在模块 yaml 中硬编码朝向并在distance_sensor_s.h_fov中设置视场。光流 vs VIO局部位置估计选型对比两种技术都使用相机并测量帧间差异光流使用朝下相机VIO 使用双目相机或 45° 追踪相机。在都校准良好的前提下光流的优势朝下光流提供经陀螺仪角速度修正的平面速度。需要精确的地面距离并假设平坦表面满足条件时精度/可靠性不亚于 VIO如室内飞行。状态更少比 VIO 更鲁棒。成本显著更低、部署更简单仅需光流传感器、测距仪和几个参数可直接连接飞控。VIO 的优势提供 3D 位姿 速度可扩展障碍物避让、建图等更多功能。灵活性更高。VIO 的劣势购买更贵、设置更难需要独立伴机电脑、标定、软件与配置若无可追踪特征点则效果变差实际世界通常有点特征。选型建议计划室外无 GPS 飞行或室内外兼有或需要支持避障等高级视觉功能 → 选择 VIO。仅在室内无 GPS飞行且成本敏感 → 选择光流。两者融合最可靠但多数实际场景并非必需。无 GNSS 环境下的自动模式所有 PX4 自动飞行模式Mission、Return、Land、Hold、Orbit都需要全局位置估计通常来自 GPS/GNSS。仅有局部位置估计来自 MoCap、VIO 等的系统可通过 SET_GPS_GLOBAL_ORIGIN MAVLink 消息将 EKF 原点设置为特定全局位置EKF 随后基于原点与局部位置提供全局位置估计从而支持室内任务规划与局部返航点设置。总结PX4 的计算机视觉体系覆盖了从测速光流到定位MoCap/VIO再到安全防碰撞的完整链路光流与 VIO 在无 GNSS 环境下提供速度/位姿状态通过 EKF2 的外部视觉、光流融合机制接入状态估计防碰撞则在 Position 模式下基于 72 扇区障碍地图实时约束速度设定点。实际部署时应根据运行环境室内/室外、所需功能避障、建图与成本约束在光流与 VIO 之间选择若追求最大可靠性可考虑二者融合。更多细节可继续阅读 EKF2 导航滤波器调校指南、外部位置估计ROS 与 光流传感器文档。【免费下载链接】PX4-AutopilotPX4 Autopilot Software项目地址: https://gitcode.com/gh_mirrors/px/PX4-Autopilot创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考