2、PX4与视觉避障基础：PX4架构、避障模块概述、视觉避障的典型工作流程

视觉避障并非一个独立运行的“黑盒”，而是深度嵌入飞控系统（Flight Controller）与机载计算机（Companion Computer）协同体系中的一项高级感知功能。要理解PX4无人机如何利用摄像头实现“看见障碍物并绕开”，首先必须厘清PX4的软件架构分层、避障模块在其中的位置，以及数据从像素到控制指令的完整流转链路。本章将围绕PX4的模块化架构、避障系统的组成要素以及视觉避障的典型工作流程展开，为后续章节的算法实现与参数调优奠定基础。

2.1 PX4软件架构：从底层驱动到上层应用

PX4是一个运行在实时操作系统（RTOS，如NuttX）上的模块化飞行控制软件。其架构设计遵循“发布-订阅”的通信模式，各个模块（如传感器驱动、姿态估计、导航、控制）独立运行，通过uORB（Micro Object Request Broker）消息总线进行数据交换。理解这一架构对于视觉避障至关重要，因为视觉避障并非直接操作电机，而是通过向PX4的特定模块“注入”感知数据，并“请求”特定的飞行行为。

从功能分层来看，PX4架构可划分为四个主要层次：

• 驱动层（Driver Layer）：负责与硬件直接交互，包括IMU、气压计、GPS、以及通过MAVLink连接的外部传感器（如摄像头、激光雷达）。驱动层将原始数据转换为标准化的uORB消息，例如传感器加速度计数据发布到sensor_accel话题。
• 估计层（Estimator Layer）：核心是扩展卡尔曼滤波器（EKF2），融合IMU、GPS、气压计以及视觉里程计（VIO）数据，输出高频率、低延迟的飞行状态估计（位置、速度、姿态）。视觉避障中的深度信息并不直接进入EKF，但视觉里程计（用于定位）会。
• 控制层（Controller Layer）：包括位置控制器、姿态控制器和速率控制器。位置控制器接收期望位置或速度设定点，输出姿态和推力指令。视觉避障的“绕行”动作最终体现为对位置控制器输入的速度修正量。
• 应用层（Application Layer）：包括导航模式（如Offboard、Auto）、任务脚本以及避障模块。避障模块在此层运行，它订阅障碍物距离信息，结合当前飞行状态，计算出安全的期望速度，并发布给位置控制器。

架构层次	关键模块	与视觉避障的关系
驱动层	MAVLink驱动、摄像头驱动	接收机载计算机发来的障碍物距离消息
估计层	EKF2、VIO融合	提供精确的自身位置，用于将障碍物坐标转换到机体坐标系
控制层	位置/速度控制器	执行避障模块输出的速度指令，实现物理绕行
应用层	避障模块、导航模式	核心决策逻辑，判断是否刹车、绕行或继续前进

2.2 避障模块概述：功能边界与接口定义

PX4的避障功能并非一个单一的“避障节点”，而是一组协同工作的模块集合。在PX4 v1.13及之后的版本中，避障逻辑主要集成在Navigator和FlightTask中，尤其是FlightTaskAuto和FlightTaskOffboard。当无人机处于Offboard模式时，外部机载计算机可以直接发送速度设定点，但PX4内部的安全逻辑（如距离传感器触发）可以覆盖这些指令。

避障模块的核心接口是MAVLink消息，具体包括：

• OBSTACLE_DISTANCE：用于传输360°或特定扇区的障碍物距离数组。该消息包含一个距离数组（单位厘米）、传感器类型（如MAV_DISTANCE_SENSOR_ULTRASOUND或MAV_DISTANCE_SENSOR_LASER）、以及最小/最大探测距离。
• VISION_INFO：用于传递视觉里程计的状态（如是否正常、置信度），帮助飞控判断视觉数据是否可信。
• SET_POSITION_TARGET_LOCAL_NED：机载计算机通过该消息发送期望速度或位置给飞控，避障模块可以在此消息基础上进行修正或否决。

值得注意的是，PX4原生的避障模块（如obstacle_avoidance）在标准固件中默认是禁用的，需要开发者通过编译选项或参数（如COM_OBS_AVOID）启用。更常见的做法是，开发者使用PX4的Offboard模式，在机载计算机上运行如PX4-Avoidance（基于ROS）或QGroundControl的避障插件，这些外部程序订阅障碍物数据，计算安全路径，再通过MAVLink发送速度指令。

2.3 视觉避障的典型工作流程：从像素到绕行

一个完整的视觉避障工作流程可以拆解为五个阶段，每个阶段都有明确的输入输出和性能指标。以下以双目视觉避障为例进行说明。

1. 图像采集与校正：双目摄像头同步采集左右图像，经过畸变校正和立体校正，输出对齐后的图像对。此阶段的关键指标是帧率（至少15fps）和同步精度（左右帧时间差小于1ms）。
2. 深度估计：通过立体匹配算法（如SGM、BM）计算视差图，再根据相机内参和基线距离转换为深度图。深度图以像素坐标表示每个点的距离，通常以16位灰度图存储。
3. 障碍物检测与栅格化：将深度图投影到机体坐标系下的3D点云，并降采样到2D占据栅格地图（Occupancy Grid）。栅格地图以机体为中心，例如前方10米×10米区域，分辨率0.1米/格。每个栅格标记为“空闲”、“占用”或“未知”。
4. 路径规划与速度生成：在栅格地图上运行局部规划算法（如VFH+、TEB），生成一条避开障碍物的局部路径，并转化为期望速度向量（Vx, Vy, Vz, Yaw_rate）。此速度向量通过MAVLink发送给飞控。
5. 飞控执行与安全监控：PX4位置控制器接收速度指令，将其转换为姿态指令，最终驱动电机。同时，飞控内部的避障安全逻辑（如距离传感器触发）持续监控，若检测到障碍物距离小于紧急刹车阈值（如0.5米），则直接覆盖速度指令，执行急停。

阶段	典型延迟预算	关键参数示例	失败后果
图像采集	10ms	分辨率640×480, 30fps	图像模糊导致深度错误
深度估计	30ms	最小探测距离0.3m, 最大15m	视差空洞导致漏检
栅格化	5ms	栅格尺寸0.1m, 范围10m×10m	栅格冲突导致路径规划失败
路径规划	20ms	最大绕行速度1.5m/s	规划超时导致无人机悬停
飞控执行	10ms	速度控制频率50Hz	通信延迟导致震荡

2.4 案例拆解：基于树莓派与PX4的室内避障实现

为了将上述理论落地，我们拆解一个典型的低成本视觉避障原型系统。该案例使用树莓派4B作为机载计算机，Intel RealSense D435i作为深度相机，飞控为Pixhawk 6C，运行PX4 v1.13固件。

系统约束与目标：在5m×5m的室内飞行区域，无人机需以0.5m/s的速度自主飞行至目标点，并避开随机放置的纸箱（高度0.5m）。通信链路为USB（树莓派与飞控），深度相机通过USB3.0连接树莓派。

初始现象与问题：首次测试时，无人机在距离纸箱1.2米处才触发避障，且绕行轨迹出现明显震荡。通过日志分析发现，OBSTACLE_DISTANCE消息的频率仅为8Hz，远低于预期的20Hz，导致飞控接收到的距离信息滞后。

分析过程：使用top命令查看树莓派CPU占用，发现立体匹配算法（SGM）占用了85%的CPU。进一步使用perf工具定位到瓶颈在视差计算函数。此外，MAVLink消息封装使用了Python的pymavlink库，其序列化开销较大。

优化操作：

1. 将深度分辨率从640×480降至424×240，帧率保持30fps，SGM的CPU占用降至45%。
2. 将MAVLink消息发送逻辑从Python改为C++（使用libmavlink），消息频率提升至25Hz。
3. 在PX4参数中设置COM_OBS_AVOID=1，并调整NAV_LOITER_RADIUS为1.0m，使无人机在检测到障碍物时提前进入绕行半径。

结果对比：优化后，避障触发距离提前至2.0米，绕行轨迹平滑，震荡幅度从±0.4米降至±0.1米。整个流程的端到端延迟从约180ms降至约80ms。

风险与代价：降低分辨率导致对小尺寸障碍物（<0.2m）的检测能力下降，但本场景中纸箱尺寸较大，影响可接受。此外，C++重写增加了约200行代码，开发时间增加2小时。

复盘经验：视觉避障的性能瓶颈往往不在算法本身，而在数据通路。优先优化消息频率和延迟，比优化算法精度更能提升整体避障效果。同时，务必在飞控端设置合理的避障安全阈值，作为最后一道防线。

2.5 常见误区与总结复盘

在PX4视觉避障的工程实践中，开发者常陷入以下误区，导致系统可靠性不足：

• 误区一：将视觉避障视为独立功能。实际上，避障效果高度依赖VIO（视觉惯性里程计）的精度。如果VIO漂移严重，即使障碍物检测准确，无人机也无法准确绕行。因此，必须先确保EKF2中的视觉位置估计稳定。
• 误区二：忽略传感器失效场景。摄像头被遮挡、光线过暗或过曝都会导致深度数据异常。PX4提供了SENS_FLOW_MINHGT等参数，但更关键的是在机载计算机中实现数据有效性检查，并在失效时主动降级为“无避障”模式，而不是发送错误的速度指令。
• 误区三：盲目追求高帧率。高帧率意味着更短的曝光时间和更高的ISO，可能导致图像噪声增大，反而降低深度精度。通常，室内飞行15fps足够，室外高速飞行才需要30fps。
• 误区四：忽视坐标系对齐。深度相机输出的障碍物坐标是在相机坐标系下，而飞控的速度指令是在机体坐标系（NED）下。如果相机安装有俯仰角或偏航角，必须进行外参标定，否则避障方向会偏移。