从第一视角无人机玩到行业级应用避障系统都是绕不开的坎。室内飞丢、树丛穿越炸机、走廊里侧滑撞墙这些问题本质都一样飞控只负责“稳住姿态和执行航线”对周围环境是完全盲的。想真正解决避障得在飞控之外加一双“眼睛”和一个“大脑”——这双眼睛用深度相机或激光雷达采集点云大脑跑避障算法把“该往哪躲”翻译成飞控能听懂的速度指令。我今天要分享的这套方案就是基于PX4 飞控加伴侣计算机Companion Computer跑3DVFH* 算法做三维避障的完整集成过程。这套方案适合手里已有 PX4 开发基础、想让无人机具备自主绕障能力的朋友也适合正准备从纯手动飞行向自主飞行过渡的开发者。3DVFH* 这个名字听起来吓人其实思路很好懂它就是在三维空间里把周围的障碍物点云投影成一系列“柱状障碍概率”再通过一个不断迭代扩展的搜索树找出一条避开障碍、朝向目标点、飞行代价最小的局部路径。相比传统二维 VFH 算法它把“前方有没有墙”升级成了“上方有没有树枝、斜前方有没有电线”既保留了实时性又不会对仅 8 公里左右算力的树莓派造成压力是伴侣计算机避障方案里性价比很高的选择。下面我会从整个系统的架构取舍讲起再把 3DVFH* 的算法细节拆开揉碎然后给出可落地的软硬件集成步骤和 PX4 参数配置最后把我在真实试飞中踩过的坑和排查思路一并列出来。整篇文章不追高深的数学推导一切以“能跑起来、能躲开障碍”为准。1. 为什么避障要放在伴侣计算机上飞控的算力边界与架构取舍1.1 飞控主控的能力上限PX4 通常跑在 STM32F7 或 STM32H7 这类 MCU 上主频最高的 H7 系列也不过 480MHzRAM 普遍在 1MB 上下。这个量级的算力要同时处理姿态解算、位置估计、卡尔曼滤波、电机混控输出还要兼顾遥控信号解析和日志记录CPU 负载常年徘徊在 60% 到 80% 之间。避障算法则完全不一样光是把一帧 640x480 的深度图转成点云再完成障碍物栅格投影和路径搜索计算量就是姿态控制的上百倍。强行塞进 MCU最直接的后果是控制环路被拖垮飞机姿态开始发飘这在飞控领域是最致命的问题。1.2 伴侣计算机承担什么角色伴侣计算机Companion Computer通常是一块运行 Linux 的小型开发板比如树莓派 4B、Jetson Nano 或更专业的 Up Board。它在整个系统里扮演的是“感知与决策层”角色一方面接收来自深度相机、激光雷达的原始数据另一方面运行 3DVFH* 等避障算法最后把计算得到的避障速度指令通过 MAVLink 协议发送给飞控。飞控保留原有的姿态控制和位置控制权限双方各司其职互不干扰。用更通俗的话说飞控是执行指令的手脚伴侣计算机是负责观察和思考的大脑两者通过一条串口线保持沟通。1.3 三种避障架构方案对比我调研了不少团队的做法主流的避障架构大致有三种各有适用范围。架构数据来源计算位置优点缺点适用场景纯机载视觉单目/双目摄像头飞控或专用视觉芯片成本低、功耗低对光照敏感、测距精度低室内定高简单避障伴侣计算机深度相机深度相机/激光雷达树莓派/Jetson精度高、算法可选范围大功耗略高、体积大室内外通用、动态障碍全自主导航平台多传感器融合SLAM高性能工控机功能最全开发周期长、成本高大型行业无人机我最后选了第二种也就是伴侣计算机加深度相机。原因很直接这套方案在开发灵活性和实时性之间取了平衡算法代码可以直接复用 ROS2 生态里的现成实现遇到问题还能单独在 PC 上仿真调试不用每次改代码都往真机上烧。1.4 推荐硬件配置参考说说我自己的搭配可以作为选型参考深度相机Intel RealSense D435i预算有限可以换 D405近距离精度更高但视场角小很多伴侣计算机树莓派 4B 4GB 版本理由是功耗低、社区资料多如果要做更重度的视觉处理直接上 Jetson Orin Nano飞控Pixhawk 6C 或 Holybro Durandal均支持最新 PX4 固件数传XTend 900MHz 数传一只就够了注意频段合规性电源模块5V/5A 的 UBEC保证树莓派和相机供电稳定这套配置总重量大约 350g挂在四轴或者六轴上完全没问题。记得在飞控 EKF2 里开启外部视觉位置估计这块稍后会在参数配置章节展开说。2. 3DVFH* 到底在算什么算法拆解与避障逻辑2.1 从 VFH 家族说起VFHVector Field Histogram算法最早是 1991 年由 Borenstein 和 Koren 提出核心思想是把环境地图用网格表示然后统计每个扇形区域里的障碍物密度以此生成一个“极坐标直方图”。机器人会根据这个直方图选出障碍物最少的方向前进。后来 VFH 加入了障碍物膨胀和代价函数VFH* 又引入 A* 式的启发式搜索解决了局部极值问题。3DVFH* 是这一族算法的三维扩展把二维极坐标升级成三维球坐标同时在倾斜和垂直方向上也做直方图投票。2.2 输入、中间表示和输出3DVFH* 的工作流程分为四步。第一步是点云接收从深度相机或激光雷达获取三维点云先经过直通滤波器去掉超出避障距离范围的点和地面点。第二步是栅格投影把点云映射到以无人机为中心的极坐标网格上每个网格单元记录两个关键量障碍物存在概率和最近障碍物距离。第三步是直方图构建对所有方向进行扇形划分对每个扇区累加障碍物概率得到候选方向的可通行度评分。第四步是搜索树扩展在可通行方向中采样若干候选路径模拟飞行一小段距离后再在新位置重新执行前三步如此迭代数层选择综合代价最小的候选轨迹。整个过程说白了就是在飞之前先“脑内预演”几米看哪条路最安全然后沿着这条路飞飞一点再重新预演。2.3 代价函数是怎么设计的3DVFH* 的路径选择不是简单看哪个方向障碍物最少而是设计了一个加权代价函数[ C \alpha \cdot C_{obstacle} \beta \cdot C_{target} \gamma \cdot C_{direction} \delta \cdot C_{path} ]C_obstacle表示候选方向附近障碍物密度的惩罚项占比最高保证飞行安全性C_target表示候选方向与目标点方向的偏差惩罚防止飞机原地打转C_direction表示与当前航向角的偏差惩罚让转向更平滑C_path表示路径长度的惩罚让飞机不会绕不必要的远路这四个系数需要根据机型动态特性反复调节。比如电机响应慢的大型机gamma就要调大否则搜索树会给出太激进的大角度转向指令飞控根本执行不了。2.4 搜索树的深度与分支数搜索树是 3DVFH* 区别于 3DVFH 的关键。每次迭代会从当前节点扩展出若干条候选轨迹每条轨迹又作为子节点继续往前扩展。树的深度决定了“远见”分支数决定了“思考宽度”两者都要付出计算时间。实测下来深度取 4 到 6 层、每层分支取 8 到 12 条是比较合适的范围再往上算力开销会呈指数增长而避障效果提升非常有限。关于投影分辨率我建议水平方向按 5 度一个扇区垂直方向按 10 度一个扇区划分默认收缩到 72 个水平扇区和 18 个垂直扇区。如果环境中细长障碍物比如电线、细树枝较多可以提高到 2 度一个扇区但 CPU 占用会高不少树莓派 4B 可能跑不满 20Hz。2.5 算法实时性的实测数据为了验证算法在目标硬件上的实时性能我专门在 ROS2 环境里做了一组基准测试。测试条件树莓派 4B深度图分辨率 640x480点云约 3 万点搜索树深度 5分支数 10。单帧处理时间分布如下模块耗时毫秒占比点云预处理降采样直通滤波1218%栅格投影与直方图构建2842%搜索树扩展与代价计算2233%速度指令生成与 MAVLink 封装57%总耗时约 67 毫秒即算法每秒能跑约 15 帧。对低速飞行的四轴来讲足够了但对高速穿越机来说还差得比较远需要更换 Jetson 或者降低点云分辨率。3. 集成实现从点云到速度指令的完整链路3.1 系统整体数据流整个集成方案的数据流可以画成一条清晰的主线深度相机RealSense D435i→ USB 3.0 → 树莓派 4B → PX4-Avoidance 或自研节点跑 3DVFH* → MAVLink 速度设定点 → UART 串口 → PX4 飞控 → 电机执行这条链路里两个最关键的接口是树莓派如何拿到点云数据相机驱动以及树莓派如何把避障结果交给飞控MAVLink/MAVSDK。下面分别展开。3.2 相机驱动与点云预处理我选择用 ROS2 的realsense2-camera插件直接发布点云话题。安装命令sudo apt install ros-$ROS_DISTRO-realsense2-camera装好后还要设置一个规则文件避免每次插拔 USB 设备都要 sudo。点云话题默认是/camera/depth/color/points发布频率默认 30Hz但实际能达到的稳定频率取决于 CPU 和 USB 控制器。帧率太高反而会挤占处理器资源我通常会用decimation滤波把点云降采样到原始分辨率的四分之一。点云预处理是很容易被忽视的环节。原始点云里往往包含大量相机自身位置的噪声点和地面反射点直接丢给 3DVFH* 会引发误避障。我的做法是直通滤波设定 x 方向 0.3m 到 8my 方向 -3m 到 3mz 方向 -1m 到 2.5m 的采集范围移除地面点用 RANSAC 平面分割把地面平面拟合出来并删掉体素网格降采样叶子大小取 0.05m既保留障碍物轮廓又不至于数据量过大3.3 用 MAVSDK 将速度指令发往飞控伴侣计算机与飞控通信最主流的方式是 MAVLink。你可以在 ROS2 里直接用mavros的/mavros/setpoint_velocity/cmd_vel话题发送速度设定点也可以绕过 ROS2 直接用 MAVSDK 写一个轻量级 C 节点。这里给一个 MAVSDK 的速度控制示例核心逻辑是设置 OFFBOARD 模式后持续以 50Hz 频率发送 Body 坐标系下的速度指令#include mavsdk/mavsdk.h #include mavsdk/plugins/offboard/offboard.h #include mavsdk/plugins/telemetry/telemetry.h #include chrono #include thread using namespace mavsdk; int main(int argc, char** argv) { Mavsdk mavsdk; ConnectionResult conn_result mavsdk.add_any_connection(serial:///dev/ttyS0:921600); if (conn_result ! ConnectionResult::Success) { return 1; } auto system mavsdk.first_autopilot(10); if (!system) { return 1; } auto offboard Offboard{system.value()}; auto telemetry Telemetry{system.value()}; // 设置速度设定点NED 坐标系下x 向前 2m/s Offboard::VelocityBodyYawspeed velocity_body_yawspeed{}; velocity_body_yawspeed.velocity_m_s {2.0f, 0.0f, 0.0f}; velocity_body_yawspeed.yawspeed_deg_s 0.0f; offboard.set_velocity_body_yawspeed(velocity_body_yawspeed); // 切换 OFFBOARD 模式 if (!offboard.start()) { return 1; } while (true) { offboard.set_velocity_body_yawspeed(velocity_body_yawspeed); std::this_thread::sleep_for(std::chrono::milliseconds(20)); } return 0; }重点说三点第一速度设定点必须按 20Hz 以上的频率持续发送飞控一旦超时收不到指令会立即退出 OFFBOARD 模式飞机进入悬停或失控保护第二我推荐发 Body 坐标系的velocity_body_yawspeed因为它最直观前方遇到障碍算法直接生成一个 y 轴方向的横向速度分量就能侧移避让第三避障节点和飞控之间最好加一条心跳检测比如连续 1 秒收不到避障指令就强制切入悬停防止树莓派死机导致飞行器乱飞。3.4 自研避障节点框架我不太建议直接使用 PX4-Avoidance 里现成的 local_planner虽然它确实封装好了 VFH 和相关模块但想调 3DVFH* 的内部参数会比较难受。我更推荐自己写一个精简 ROS2 节点框架如下import rclpy from rclpy.node import Node from sensor_msgs.msg import PointCloud2 from geometry_msgs.msg import TwistStamped from px4_msgs.msg import VehicleLocalPositionSetpoint class VFH3DPlanner(Node): def __init__(self): super().__init__(vfh3d_planner) self.sub self.create_subscription(PointCloud2, /camera/depth/color/points, self.cloud_callback, 10) self.pub self.create_publisher(TwistStamped, /mavros/setpoint_velocity/cmd_vel, 10) self.param_alpha 0.6 # 障碍物代价权重 self.param_beta 0.3 # 目标方向代价权重 self.param_gamma 0.1 # 航向平滑代价权重 def cloud_callback(self, msg): # 1. 点云预处理直通滤波、地面分割、降采样 preprocessed_cloud self.preprocess_cloud(msg) # 2. 构建柱状栅格并生成直方图 histogram self.build_histogram(preprocessed_cloud) # 3. 搜索树扩展得到最优候选轨迹 best_direction self.search_tree(histogram) # 4. 根据最优方向和当前速度生成速度设定点 cmd_vel self.direction_to_velocity(best_direction) # 5. 发布 MAVLink 速度指令 self.pub.publish(cmd_vel)这个框架的关键在于回调函数必须在 50ms 内完成否则积压的点云消息会拖垮系统。如果发现 CPU 占用过高优先降低点云分辨率而不是精简算法逻辑因为 VFH* 系列对输入点云的密度很敏感降得太狠会把细小的障碍物直接滤掉。3.5 OFFBOARD 模式切换时机这里有个经常被忽略的细节OFFBOARD 模式一旦开启飞控会以伴侣计算机的指令为最高优先级此时遥控器的摇杆输入是无效的。所以一定要给遥控器设置一个“紧急切换通道”比如 Aux2 拨杆映射到 PX4 的MAV_CMD_DO_SET_MODE这样在避障逻辑跑飞时还能手动切回 Position 模式接管飞机。我最初调试时没设置这个通道结果有一次速度设定点的坐标系写反了飞机直接朝地面加速全靠直接断电才保住机器。4. PX4 参数与调优让避障动作既灵敏又平滑4.1 必须开启的避障相关参数PX4 从 v1.9 开始就提供了对外部避障速度指令的支持但入口参数比较隐蔽。进 QGroundControl 的参数界面按下表逐项确认参数名推荐值作用COM_OBS_AVOID1主开关将外部避障速度指令纳入控制栈MPC_COL_PREV_D1.5开始减速的障碍物距离米MPC_COL_PREV_SD0.5紧急停止距离米MPC_VEL_MANUAL5.0OFFBOARD 模式下最大水平速度MPC_XY_P1.5水平位置控制 P 增益越大越激进MIS_DIST_1FP300第一个航点距离阈值MPC_COL_PREV_D和MPC_COL_PREV_SD这两个参数是联动的。当飞控检测到外部给出的速度指令和目标方向上有距离小于MPC_COL_PREV_D的障碍时会主动降低速度当距离小于MPC_COL_PREV_SD时直接停止前进。这两层保险对于算法偶尔的误判非常有用相当于给避障系统加了一道独立的硬件保护闸。4.2 EKF2 外部视觉位置估计如果你的避障方案依赖外部视觉定位比如没有 GPS 的室内环境还需要设置如下参数EKF2_AID_MASK 88启用视觉位置和视觉姿态 EKF2_EV_CTRL 4水平视觉位置融合 EKF2_EV_DELAY 0.0视觉传感器延迟补偿根据实际标定这套参数我调试了整整两天才稳定。关键在于EKF2_EV_DELAY如果深度相机的图像时间戳和飞控时钟不同步位置估计会出现漂移飞机低速下看起来一切正常稍微飞快一点就会突然修正姿态感觉像被什么东西拽了一下。4.3 参数调优的优先级很多新手上来就调避障相关参数却忽略基础飞控参数这是本末倒置的做法。我建议按这个顺序调先把 OFFBOARD 模式下 2m/s 恒定速度飞行调稳看飞机有没有抖动和偏移再调MPC_XY_P和MPC_XY_VEL_P等位置速度增益直到轻微推杆能快速响应但不超调然后接入避障速度指令但从 0.5m/s 开始测试逐步提速找上限最后才动 3DVFH* 内部的代价系数因为不同机型对相同障碍物的躲避效果差异很大一味照搬参数只会越调越乱4.4 速度指令的平滑处理直接给速度设定点会导致转向非常生硬这里我加入了一个斜坡滤波器模拟“从多大速度减到 0”的过程。简单来说当算法探测到前方 1.5 米处有障碍物不是瞬间把前向速度从 2m/s 降到 0而是按四轴最大减速度不超过 3m/s²的方式生成一条平滑的减速曲线。对应的伪代码如下def smooth_velocity(cmd_vel, current_vel, max_accel3.0, dt0.02): delta cmd_vel - current_vel delta_clipped np.clip(delta, -max_accel * dt, max_accel * dt) return current_vel delta_clipped实测对比很明显不加平滑时飞机每遇到一个障碍物都会猛点头加了之后虽然躲避轨迹半径变大了但整个飞行过程舒服得多姿态稳定性也更好飞控的 IMU 积分漂移都小了很多。5. 真实飞行中的坑坐标系、延迟与误检排查5.1 坐标系错位导致的“幽灵障碍”第一次室外试飞飞机在空旷草坪上空突然向左侧猛打方向好像左边有一堵看不见的墙。排查了很久最后定位到是相机坐标系和飞控 NED 坐标系没有对齐。深度相机通常以“镜头朝前”定义 x 轴但飞控的 x 轴指向机头正前方地平线方向如果安装时相机有一个小俯仰角且没有做外参标定点云投影进 3DVFH* 栅格时就会整体倾斜左边地面被投射成了前方障碍物。解决办法是在 ROS2 里发布一个静态坐标变换 TF把相机坐标系对齐到机体坐标系。用static_transform_publisher发布时注意三个角度的旋转顺序ros2 run tf2_ros static_transform_publisher \ --x 0.10 --y 0.0 --z 0.05 \ --roll 0.0 --pitch -0.15 --yaw 0.0 \ camera_link base_linkpitch的负号是重点相机往下俯仰了 8.6 度在 TF 里就要转回正方向。这个值可以用一张印有黑白棋盘格的标定板来测比目测靠谱得多。5.2 点云延迟与运动畸变深度相机和算法节点、MAVLink 传输都会引入延迟。一次测试中我从日志里看到算法输出的避障指令比实际画面滞后了 180ms。当时飞机以 4m/s 速度飞行180ms 的延迟相当于目标点已经前移了 0.72 米遇到近距离障碍物时避障反应整整慢了一拍最后靠飞控的MPC_COL_PREV_SD紧急停车才没撞上。解决延迟问题的思路有两个一是给算法节点挂载消息过滤器只处理新鲜度低于 100ms 的点云丢弃旧数据二是在 3DVFH* 的搜索树里对障碍物做速度补偿也就是在预测未来位置时把飞机当前速度投影到障碍物坐标上等效扩大障碍物边界。第二种方案需要对算法有一定修改能力但对高速飞行帮助极大。5.3 误检避障如何区分真障碍和噪点3DVFH* 对点云噪声非常敏感RealSense 在强阳光下或低纹理墙面上的深度值会产生大量飞点这些飞点会被当成障碍物导致飞机没事就急刹。后来我在预处理阶段加了一个“邻域一致性检测”对每个点检查它周围 20 厘米内是否有至少 5 个邻居点没有则视为离群点删除。这个滤波器在 PCL 里可以直接调用StatisticalOutlierRemoval效果立竿见影pcl::StatisticalOutlierRemovalpcl::PointXYZ sor; sor.setInputCloud(cloud); sor.setMeanK(20); sor.setStddevMulThresh(1.0); sor.filter(*cloud_filtered);但要注意这个方法在树木等边缘复杂的场景会误删真实障碍物的边缘点导致避障不充分。我的经验是只在环境比较干净的测试场地开这个滤波器在树丛中飞行时降低处理强度或者改用基于时间累积的动态点云建图来滤除噪点。5.4 搜索树陷入局部极值怎么办3DVFH* 比 2D VFH 强很多但它依然属于局部规划器没有全局地图在 U 形障碍比如两侧高墙、前方直道中很可能陷入局部震荡算法先左转几秒后发现左边也有障碍又急转向右来回横摆无人机看起来像在跳摇摆舞。解决办法有两条路。一条是在 3DVFH* 之上叠加一个全局路径规划器比如 A* 或 RRT先用全局规划给出一个目标方向再用 3DVFH* 在这个方向附近做局部细化解算另一条是给代价函数加上“历史惩罚项”当飞机在某个狭窄区域内停留超过 3 秒时降低该区域的候选方向得分强迫算法选择其他方向。两条路我都试过全局规划器的稳定性更好但需要额外维护一张代价地图对树莓派的存储和算力都有更高要求。5.5 日志分析与复现的手段避障系统出问题时第一件事是翻日志。我强烈建议在伴侣计算机上把以下内容同步落盘原始点云话题降采样到 1Hz 存一份就够3DVFH* 的直方图输出和搜索树剪枝记录最终发布的 MAVLink 速度指令PX4 端的日志通过 QGC 的 Analyze 工具导出有一次我怀疑是避障算法在特定方向失效但光看速度指令看不出所以然。后来把点云按时间戳对齐到 PX4 的飞行日志用plotJuggler同时绘制高度、速度指令和障碍物距离才发现是树莓派在某个角度上 OOM 导致避障节点短暂退出飞控自动切回 Position 模式。这种问题光靠猜永远定位不了把日志对齐、可视化这一步无论如何不能省。避障系统从“能跑”到“跑得稳”是一个漫长的迭代过程。我这边走通的 3DVFH* 集成方案大概花了一个多月的时间最花精力的反而不是算法本身而是每次试飞后日志里那些看起来随机出现、又无法稳定复现的坐标系偏差和延迟问题。如果你打算自己动手做一开始尽量在空旷的场地用小速度、近距离的简单障碍物做验证先确认“遇到障碍能不能停下来”再逐步尝试动态绕障和更高速度。对于 3DVFH* 的代价参数我建议从安全优先的姿态开始宁可让飞机飞得保守一点也不要因为追求“灵敏”而牺牲可靠性。这套方案目前我还在持续迭代下一步打算把全局路径规划加进去让飞机在复杂地形里能真正走出像样的绕障路线。
