1. 整体设计与方案选型思路1.1 为什么是“伴侣计算机飞控”的分工模式做PX4避障很多人第一个反应是在飞控固件里加逻辑但真正跑了几个版本你会发现这条路走起来非常别扭。飞控的实时性很强姿态环、位置环都在1kHz甚至更快的节奏上运转但它毕竟不是一台通用计算设备。避障需要处理点云数据、跑障碍物聚类、计算代价函数、做前瞻搜索这些任务对算力的要求远超STM32或者普通MCU的承受范围硬塞进飞控里只会带来两个后果要么主循环被打乱要么避障逻辑被砍得面目全非。所以我一直推荐用伴侣计算机的方案也就是把感知和决策从飞控里剥离出来交给一台运行Linux的小型计算机飞控只负责执行底层控制。这样避障算法可以跑在Python或者C里调试方便迭代也快。飞控和伴侣计算机之间通过MAVLink通信伴侣计算机以offboard模式向飞控发送速度指令或者位置设定点PX4负责把这些指令变成电机的转动。这个架构还有一个隐藏优势飞控固件基本不用改。意味着你可以一直用官方稳定版PX4固件升级、参数调整都不影响避障节点风险被隔离了。如果哪天想换避障算法也只动伴侣计算机上的代码不用动飞控。1.2 避障算法选型3DVFH*赢在哪避障算法有很多种常见的有传统的人工势场法APF、向量场直方图VFH/VFH、3DVFH*、RRT系列、以及基于深度学习的方法。我逐一排除之后选定了3DVFH*原因很直接它适合机载环境、算力需求可控、行为可预测而且不需要先验地图。先说人工势场法。它的思路是目标点产生引力、障碍物产生斥力合成就得到运动方向。原理很简单但工程上有个致命缺陷——容易陷入局部极小也就是无人机在一个凹形障碍物前面来回抖动永远绕不出去。飞行器不像地面小车可以原地转向空中悬停时一旦陷入局部极小不光是电量的浪费还有安全隐患。RRT系列算法在路径规划里非常流行能找到一条可行路径。但它在每次规划时都需要重新采样、构建搜索树计算量相对大而且生成路径是全局的适合在已知环境中做预规划不适合应对突然出现的动态障碍物。避障场景下我希望能对传感器实时感知到的障碍物做出即时反应RRT这个定位本身就有偏差。3DVFH是VFH算法族里针对三维空间扩展出来的一版。VFH系列最早是为地面机器人设计的核心思路是维护一个障碍物密度直方图把周围空间划分成扇区找到障碍物密度最低且尽量指向目标的方向作为行走方向。VFH引入了代价函数选方向时会同时考虑当前朝向和转向代价避免了剧烈抖动。VFH则在VFH的基础上加入了类似A的前瞻搜索减少局部最小的影响。3DVFH把直方图从二维极坐标扩展到三维球面坐标避障方向不再局限于一个平面内转圈而是可以上下绕行这对多旋翼来说非常重要。深度学习方法我也考虑过比如训练端到端的避障策略但实机部署时对数据集的覆盖度要求太高环境一变就容易失效而且出了问题很难解释为什么。3DVFH*的每一个决策都有明确的几何和物理含义哪个方向障碍物多、哪个方向近、代价如何算出来的这些都是可以调试和解释的。在可靠性优先的无人机系统里这种透明度极其宝贵。1.3 系统整体架构与数据流整条链路的架构其实可以归纳成一句话传感器感知环境伴侣计算机做决策PX4负责执行。具体的数据流是激光雷达或深度相机采集到的三维点云数据先进入伴侣计算机经过降采样、感兴趣区域ROI截取等预处理之后送入3DVFH*算法模块。算法模块会生成一个建议的飞行方向和速度再由一个转换模块把它封装成MAVLink的位置设定点或速度设定点通过串口或USB发送给PX4。PX4在offboard模式下接收这些设定点经过自身的位置控制器和姿态控制器最终输出到电机。这条链路里每个环节的延迟都会累积。传感器采集需要时间点云处理需要时间算法计算需要时间MAVLink传输需要时间PX4内部环路也需要时间。我实测下来整个闭环的反应时间通常在100ms到200ms之间对于低速飞行的无人机来说已经足够。所以方案选型和代码实现的首要目标就是把这个延迟压到可控范围同时保证指令稳定、不丢包、不超时。2. 3DVFH*算法原理与工程化适配2.1 从VFH到3DVFH*三维空间里的“找路”逻辑把3DVFH讲明白最好用一个生活化的类比。想象你站在一个拥挤的商场里手里拿着手机导航要去出口。你不可能完全按导航直线走因为面前可能有人、有柱子、有临时展台。3DVFH做的事情就是每隔一小段时间你环顾四周把所有挡路的东西按方向记录成一张“拥挤程度表”然后选一个既不撞东西、又尽量靠近导航方向的角度走过去。每隔半秒重复一次这个动作你的路径看起来就在绕来绕去但始终朝着目标前进。算法本身的核心是一个空间直方图。三维点云被投影到以无人机为中心的球面上球面被划分成很多个小格子每个格子代表一个方向区间。算法统计每个格子里有多少个障碍点、距离有多近转换成该方向的“通行代价”。障碍物密集的方向代价高空旷的方向代价低。最终选择代价最低且满足转向限制的方向。VFH相对VFH的改进在于引入了前瞻能力。VFH在做决策时只看当前一步VFH则会模拟未来几步的选择用类似A*的评估函数来提前判断哪条路更可能走得通。这在狭窄通道和复杂障碍布局下特别有价值能大幅减少“走了几步发现是死胡同又退回来”的情况。3DVFH*将坐标系从2D极坐标升级到3D球坐标后代价函数里的每个方向不仅有水平偏航角还有垂直俯仰角。这就意味着无人机可以选择从障碍物上方飞过而不仅仅是在水平面上绕路。对多旋翼来说这个自由度是天然的优势。2.2 代价函数设计权重才是核心3DVFH*的代价函数具体怎么设计工程实现上一般会考虑四项因素。第一项是障碍物密度项方向扇区里障碍物点越多、越近代价越高第二项是目标方向偏差项方向越偏离目标朝向代价越高第三项是转向代价项当前飞行方向和候选方向的差值越大代价越高因为大幅转向会让飞行不平稳、也会消耗更多动力第四项是历史一致性项也就是尽量保持和上一时刻选择的方向相近避免方向在相邻帧之间跳变。这四项通过加权求和得到最终的代价权重调起来很讲究。目标方向项权重设得太大飞机会往障碍物里钻设得太小飞机会绕远路甚至原地打转。转向代价项权重太大遇到突然出现的障碍物会反应迟缓权重太小飞机会左右狂摆姿态很难看。我实测的初始权重一般是障碍物密度项0.5、目标方向偏差项0.3、转向代价项0.15、历史一致性项0.05但这只是一个起点实际运行时要看你用的传感器、飞行速度和场景特点来微调。这里有个很重要的心得不要一上来就在真机上调参数。先在仿真环境里把权重扫一遍用几个有代表性的障碍场景跑比如单柱、窄通道、U形障碍记录每一组权重下的成功率、平均飞行时间和偏航角变化率形成一个表格。这样你能直观看到每个权重是偏激进还是偏保守上了真机之后只需要做少量微调。2.3 算法输出如何变成PX4能执行的指令3DVFH*算法输出的本质上是一个期望运动方向球面上一个点包含偏航角和俯仰角和一个期望速度标量。要让PX4执行这个指令需要做一个坐标系转换并明确无人机的控制模式。PX4在offboard模式下支持位置设定点和速度设定点。位置设定点就是直接告诉飞控“飞到世界坐标系下的某个点”速度设定点则是告诉飞控“以某个速度矢量运动”。避障场景下我更推荐速度设定点因为3DVFH*本身就是输出运动方向天然匹配速度控制而且速度控制模式下飞控不会因为避障节点给出的目标点频繁变化而产生陡峭的加减速飞行姿态更顺滑。坐标系转换必须特别注意。3DVFH*计算时使用的一般是机体坐标系下的传感器数据但PX4的速度设定点使用的是NED北东地或ENU坐标系的地球固定坐标系。这意味着需要把机体系下的期望速度旋转到世界系下还需要同时提供合理的偏航角设定。如果这个转换做错飞机会往完全无关的方向飞而且日志里看不出明显的报错排查起来非常痛苦。后面第三节我会专门讲坐标系标定的细节。3. 硬件选型与通信链路搭建3.1 传感器选择深度相机与激光雷达的取舍避障系统的感知质量直接决定算法上限。3DVFH*吃的是三维点云所以传感器的本质任务就是提供可靠、及时的点云数据。目前常用的两类传感器是深度相机和激光雷达。我做了个对比表格方便参考特性深度相机如RealSense D435i激光雷达如Livox Mid-360点云密度高像素级稠密点云中等但可融合多帧有效距离一般3~10米受光照影响大40米以上几乎不受光照影响视角范围宽水平约87度、垂直约58度360度水平视场适合全向避障户外强光表现差强光下深度值大量缺失优秀重量与功耗轻约70g功耗1.5W左右重一些约265g功耗10W左右价格相对便宜贵不少如果你主要做室内避障场景光照可控深度相机完全够用而且省电、轻便对小机架很友好。如果做户外低速巡航避障激光雷达几乎是必须的否则下午太阳一斜深度相机就开始“瞎”了。还有一类选择是双目相机比如Mynt Eye系列它的优势是不依赖红外投射户外抗光性强一些劣势是对计算资源要求更高需要机载设备跑立体匹配算法会增加端到端延迟。我自己在样机上用的是RealSense D435i加一台Livox Mid-360双传感器方案。平时室内测试用D435i拿到户外之前切到Livox点云。代码上做了传感器抽象层点云来源不同但下游处理流程一致切换成本很低。3.2 伴侣计算机的算力评估伴侣计算机的核心指标有三个CPU单核性能、内存带宽和USB/以太网带宽。GPU很重要但优先级没有想象中高因为3DVFH*本身是CPU友好的算法主要瓶颈在点云预处理和数据结构构建上不是深度神经网络推理。我的配置建议是室内避障用Raspberry Pi 58GB版起步它能轻松处理D435i的点云并跑3DVFH*总延迟可以控制在150ms以内。如果要上Livox雷达或者要跑额外的视觉里程计我建议直接上Jetson Orin Nano 8GB或者Orin NX前者的CUDA核心可以用来做点云降采样和滤波加速后者的性能余量更大可以同时跑避障、VIO和多路传感器。有个容易被忽略的问题存储设备。伴侣计算机最好用SSD或者高速TF卡不要用普通SD卡。避障系统运行时会产生日志、点云缓存和MAVLink日志普通SD卡在持续写入下容易掉速严重时会导致系统卡顿、避障节点被kill掉。我有一块TF卡就是这么废掉的写入一小时后IO等待飙到90%以上整个系统像死机一样。3.3 飞控与伴侣计算机的通信配置通信是整套系统里看起来简单、实际坑最多的地方。PX4和伴侣计算机之间最常用的连接方式是串口接在飞控的TELEM1或TELEM2端口上。如果你用的是Pixhawk 6C之类的飞控TELEM端口通常是JST-GH 1.25mm接口需要用配套的转接线接到伴侣计算机的USB口或者UART口。硬件接好之后PX4侧需要做三个配置。第一是把TELEM端口的波特率改成921600默认115200在传大流量MAVLink消息时会严重丢包。第二是把MAV_0_CONFIG或SER_TEL2_BAUD这类参数与你实际使用的端口对应起来具体参数名取决于固件版本。第三是确认MAV_PROTO_VER是1还是2建议用2消息号空间大支持更高的数据率。伴侣计算机侧如果你是走USB虚拟串口需要检查设备权限。Linux下通常要给串口设备添加udev规则否则普通用户无法打开设备。我之前在这上面卡了半天每次都要sudo才能跑避障节点后来加了规则文件一劳永逸在/etc/udev/rules.d/99-pixhawk.rules里写上设备对应的ATTRS信息然后udevadm control --reload并重新插拔USB。通信协议我使用的是MAVSDK-Python。它比直接操作pymavlink更友好抽象程度更高可以少写很多底层代码。避障主循环里只需要调用mavsdk.offboard.set_velocity_ned()就能把速度设定点发出去。但要注意MAVSDK-Python内部有一个接收线程和发送缓冲如果主循环里处理点云耗时过长可能导致设定点发送不及时PX4会因此退出offboard模式所以一定要把点云处理和MAVLink发送放在不同的线程里。3.4 供电、减震与安装布局供电是整个系统里最无聊但又最容易翻车的部分。伴侣计算机和传感器需要5V或者12V供电绝对不能直接从飞控的5V引脚取大电流。飞控的BEC通常是给接收机、舵机这类小电流设备设计的带不动Jetson这类负载。正确做法是使用独立的稳压模块从电池直接取电或者使用带BEC的分电板。减震问题也值得专门说。深度相机和激光雷达对震动都敏感特别是深度相机震动会导致点云出现大量飞点、深度值抖动。安装传感器时尽量使用减震球或者减震泡沫垫并且把传感器固定部分和飞控的减震系统分开避免共振。还有就是传感器的安装位置。机架振动最明显的地方是电机臂附近传感器的安装位置尽量靠近机体中心这样能减少高频震动影响也能让点云视角更对称。如果传感器装在机头一侧机尾方向会有较大盲区3DVFH*输出的路径就可能在侧向和后向出现“瞎走”的情况。4. 软件栈与核心代码实现4.1 运行环境与软件栈梳理我用的是Ubuntu 22.04 LTS系统ROS2 Humble作为中间件但说实话ROS2在这里不是必需的。如果你只是单机运行避障节点直接用Python加几行MAVSDK就能跑通全链路。ROS2的价值在于模块化管理比如点云采集节点、避障决策节点、状态监控节点可以独立运行、独立重启而且可以方便地用rosbag记录数据做回放分析。完整软件栈是这样的系统层是Ubuntu 22.04驱动层包括librealsense深度相机SDK或Livox SDK雷达驱动通信层是MAVSDK-Python或者pymavlink算法层是自己写的3DVFH*模块上层再加一个简单的状态机控制无人机在不同飞行阶段的行为比如起飞后进入避障模式、收到急停标志后立即悬停。这里不建议把避障逻辑和飞控逻辑混在同一个进程里。避障节点应当是一个独立进程职责单一接收点云、计算方向、输出速度指令。就算它崩溃了飞控还有最后的安全兜底至少不会出现“程序一起挂掉、飞机完全失联”的极端场景。4.2 3DVFH*主循环的核心代码框架主循环的伪代码大致是这样的# 伪代码主循环结构 while not shutdown: cloud sensor.get_point_cloud() # 获取点云 cloud preprocess(cloud) # ROI截取、降采样、去离群点 vfh ThreeDVFHStar() vfh.set_point_cloud(cloud, pose) vfh.set_target_direction(target_bearing) best_direction, speed vfh.search() velocity_ned convert_to_ned(best_direction, speed) offboard.send_velocity_ned(velocity_ned) time.sleep(0.05) # 20Hz控制频率关键参数需要预先赋值扇区sector的划分精度、障碍物影响半径、最大允许转向角、速度上限、前瞻步数等。我常用的扇区水平分辨率是2度垂直分辨率是5度这个精度在20Hz频率下既能保证方向精度又不会让计算量失控。障碍物影响半径设在3米到5米之间小于3米的方向直接判为不可通行大于5米的障碍点不参与代价计算可以省掉大量无效计算。ROI截取这一步特别关键。如果不对点云做预处理算法会把地面和无人机自身结构也当成障碍物。我的做法是先做一个以机体为中心的立方体ROI水平范围正负5米、垂直范围从机身上方3米到下方1.5米超出这个范围的点全部丢弃。再做一个地面分割或者平面拟合把安装在无人机正下方的深度点云对应的地面点剔除。有机会的话可以读一下VFH系列原始论文里关于极性直方图密度函数的定义并且在你的代码里实现时有意识地把极坐标下相近角度、相近距离的点做合并这样能大幅减少数据处理量。4.3 与PX4 offboard模式的衔接细节PX4的offboard模式是避障系统正常工作的前提同时也是最容易出问题的环节。核心规则只有一条飞控必须在1Hz以内持续收到设定点消息否则会立刻退出offboard模式切回你预设的失控保护模式。如果你从offboard控制切换回了位置模式或着陆模式飞机可能突然悬停或降落这个行为在低空问题不大但高空测试时容易吓人一跳。所以安全设计上除了保证发送频率稳定还要在代码里加“超时巡逻”和“信号丢失检测”逻辑。每发送一帧设定点就记录一个时间戳主循环每次检查时间差一旦超过300ms没有新的设定点就进入本地降级策略比如发送悬停指令或者直接退出offboard。这个逻辑放在伴侣计算机侧的独立线程里不依赖避障主流程是否正常运行。初始化offboard时PX4要求先收到一定量的一致性设定点消息才能解锁offboard。比较稳妥的做法是在无人机起飞前先用位置模式起飞到安全高度再切换到offboard而不是解锁后直接offboard。遇到GPS信号不佳的室内环境你可能需要视觉里程计或光流传感器提供位置反馈否则PX4无法在offboard模式下正常控制位置只能进入无位置的速度控制这种情况更危险建议先把位置反馈问题解决掉再跑避障。4.4 TF树与坐标系校准最容易翻车的环节前面提到坐标系转换不能错这里展开说。传感器输出的点云通常在传感器自身坐标系下深度相机是右-下-前方RGB-D摄像头惯例Livox雷达是前-左-上的雷达坐标系。而3DVFH*需要把点云转换到机体坐标系中再去计算机体系的障碍物方向最后输出的期望速度还要从机体系旋转到世界系才能发给PX4。我的做法是在程序启动时通过ROS2的TF树把传感器坐标系下的点云变换到“机体坐标系”过程中只使用静态变换也就是传感器安装的相对位姿在飞行过程中不变。这里有个经验不要为了省事跳过标定。哪怕传感器只是歪了两三度远距离点云就会偏移几十厘米避障方向会整体偏移。标定方法可以用棋盘格或者特征点匹配也可以直接观察地面点云的平整度来手动微调外参。至于机体坐标系到世界系的转换建议直接订阅PX4通过MAVLink发布的姿态四元数或者使用VIO/RTK设备提供的位姿信息。MAVLink的ATTITUDE_QUATERNION消息可以直接从MAVSDK里获取然后用四元数旋转矩阵把速度向量从机体系转到世界系。这里有一个常见误区PX4官方常用NED坐标系东北地ROS2常用ENU坐标系东北上两者虽然前两个轴相同但Z轴方向相反、航向角的零点和方向也相反。如果你不处理好这个差异飞机会在水平方向飞得很好但在垂直方向完全反着来。5. 实测过程、常见问题与参数调优5.1 从仿真到真机的分阶段测试流程真机测试之前我强烈建议在仿真环境里把整个链路跑通。PX4官方支持Gazebo仿真你可以在Gazebo中搭建一个带障碍物的场景然后让避障节点直接运行在宿主机或容器里通过MAVLink连接仿真PX4。这个阶段能发现绝大多数逻辑问题尤其是坐标系转换、offboard时序、代价函数权重这些。仿真无论如何比不上真机的气动和传感器噪声但至少能省下不少炸机的风险。仿真跑通之后真机测试要分阶段降级。第一步先做桌面测试桨叶拆掉电机不供电只验证避障节点能正常收发MAVLink消息检查PX4日志里有没有反复进出offboard的警告。第二步做系留测试把无人机用绳索固定在地面或者安全带绑好让避障节点控制无人机做一个简单的前飞-悬停-后退动作。这一步看起来土但能帮你验证姿态控制响应、点云实时性和延迟是否在合理范围内。第三步做真机飞行并且先从低速、低空、大障碍物的场景开始逐渐增加难度。我自己的经验是每一次真机测试前都要把同一套参数在仿真里跑一遍每一次真机测试后都要把飞行日志导出分析重点看避障节点的决策是否合理而不是只看有没有撞到障碍物。3DVFH*的决策逻辑经常会“疑神疑鬼”在明明空旷的地方也绕了很大弯这种问题只有分析日志才能发现。5.2 常见问题与排查方法表格现象可能原因排查方法飞控频繁退出offboard设定点发送间隔超过1s或MAVLink链路不通畅检查发送线程是否有阻塞观察PX4日志里的offboard状态点云出现大量飞点传感器震动过大或深度相机曝光异常检查减震安装查看传感器自诊断信息避障方向整体偏移传感器外参标定不准或坐标系转换错误重新标定外参打印中间变换矩阵检查飞行中飞机原地画圈目标方向有误或代价函数里目标项权重过小检查目标输入是否正确查看代价分布日志动态障碍物避不开控制频率太低或速度上限太高提高主循环频率到20Hz降低最大速度增加扇区分辨率室内无GPS时位置漂移缺少位置反馈源增加VIO或光流模块确保offboard模式有可用位置估计遇到问题不要急着改代码。先通过MAVLink日志、点云录制、以及PX4的ulog日志三份数据联合分析通常就能定位到具体环节。最忌讳的是东改一个参数、西改一个阈值最后不知道自己怎么修好的下次出了问题又抓瞎。5.3 参数调优的经验心得参数调优我奉行“单变量原则”一次只动一个参数记录前后效果。三个最值得先调的参数分别是最大速度、扇区分辨率、障碍物影响半径。最大速度决定了系统的“紧张程度”。速度越大留给算法反应的时间越短避障成功率越低。我测试时的基准是先设一个很低的值比如1m/s保证避障成功率无限接近100%然后逐步提高到1.5m/s、2m/s找到失败率开始明显上升的临界点再将最大值设在临界点的80%左右。扇区分辨率则直接影响方向选择的细腻程度太粗会导致飞行路径呈锯齿状太细会增加计算量和抖动。障碍物影响半径也不能一刀切室内狭小空间和户外开阔空间需要不同的值建议做成动态参数通过遥控器通道来切换。还有个容易忽视的细节3DVFH*的方向输出和速度输出需要做平滑。直接用上一帧输出的方向会形成高频抖动不仅难看还会增加飞控负担。我是在输出端加了一个一阶低通滤波器对速度向量做平滑时间常数设为0.2秒左右。这样避免了不少姿态振荡的问题。6. 避障系统的扩展方向与个人体会6.1 从避障到局部路径规划3DVFH本质上解决的是“下一步往哪走”的局部问题它不做全局路径规划。如果要让无人机从A点飞到B点期间还要穿过一个复杂的环境我建议在伴侣计算机上叠加一个全局规划器比如A或者RRT*先在稀疏地图上规划出一条全局路径然后再用3DVFH*做局部避障跟踪。两个模块可以这样配合全局规划器每隔几秒输出一串航路点局部避障节点始终以当前航路点为目标方向如果前方有障碍物就绕开后再回到航路点方向。这个架构下全局规划负责“大方向”3DVFH*负责“小细节”分工清晰代码也容易维护。6.2 多传感器融合与动态目标跟随扩展一下3DVFH*的输入并不局限于点云。你可以把毫米波雷达的目标列表、超声波的近距读数一并转换成障碍点源融入直方图统计。多传感器融合的核心在于统一坐标和统一时间戳否则数据打架会严重影响避障判断。动态目标跟随也是不错的应用方向。你可以把目标检测节点输出的目标位置转换成“目标方向”喂给3DVFH*作为目标项。这样无人机不仅能避开障碍物还能始终对着目标方向跟踪飞行。这里要注意的是目标位置更新频率远低于避障决策频率需要做目标位置预测否则跟随会有一拍滞后。6.3 写在实际踩坑之后整个项目做完我最深的一个体会是避障系统的难点从来不只是算法而是全链路的稳定集成。传感器标定、通信配置、坐标系转换、线程安全、安全兜底这些看似不起眼的细节任何一个出了问题都会表现为避障失败。3DVFH*算法本身反而相对成熟网上的论文和开源实现都不少关键是要沉下心来把每一环的边界条件都验证过。先仿真再系留最后才真机这个顺序千万别跳。希望这篇文章能帮你少走一些弯路让PX4避障项目从“能跑”变成“真的能飞”。
