做四旋翼自主避障最怕的还不是算法效果差而是还没等代码验证完飞机就已经撞在墙上了。我在没有实物平台的情况下靠PX4、GAZEBO和ROS这套开源仿真环境把四旋翼自主避障完整跑通了从PX4 SITL仿真起飞到Gazebo里布置障碍物再到ROS里写避障决策节点最后无人机自主绕开正前方障碍物并回到原定航线。整个过程没有一块飞控硬件也没承担任何炸机风险。这篇文章是给准备做PX4二次开发、四旋翼避障算法验证或者正在纠结ROS与Gazebo联合仿真怎么落地的人写的。我会把版本搭配、环境搭建、避障思路、核心代码、联调过程和排坑经验一次性讲清楚文中所有命令都是我在实际项目里验证过的你可以直接照着抄。1. 为什么选择PX4GAZEBOROS这套仿真栈做避障开发仿真和实物之间的差别非常大。真机上遇到问题要么摔机要么飞丢一次试错成本可能就是几千块。仿真环境的优势不只是省钱更重要的是可复现同样的障碍物位置、同样的飞行速度你可以反复跑一百遍把所有参数边界都试出来。这就是我坚持用PX4、GAZEBO、ROS搭建避障验证环境的核心原因。1.1 仿真的核心价值先让算法在虚拟世界“炸”一遍避障算法最怕的是小概率边界情况比如无人机正前方突然出现一个半透明障碍物、传感器丢帧、障碍物距离恰好卡在安全阈值附近。真实飞行中你很难为了测试这些情况去故意撞树但仿真环境里可以。我在Gazebo中会故意布置几个尺寸、位置都不对称的障碍物然后调整避障安全距离反复测试无人机是在什么条件下会擦碰什么条件下能安全绕开。这种“先炸后修”的开发模式能让算法稳定度提高一个数量级。否则你带着一个只测过两三次的避障程序上真机心里根本没底。1.2 PX4、GAZEBO、ROS各自扮演什么角色这三者不是竞争关系而是分工明确的一条流水线PX4负责无人机底层控制。在仿真环境下运行的是SITLSoftware In The Loop也就是把飞控固件当成一个普通进程跑在电脑上所有姿态控制、状态估计、导航逻辑都和真机一致。GAZEBO提供物理世界和传感器模型。无人机在仿真世界里飞行Gazebo计算重力、碰撞、推力并模拟激光雷达、IMU、GPS等传感器数据。ROS相当于连接大脑和小脑的神经。它从Gazebo/PX4读取无人机状态和传感器数据运行避障算法再把决策结果通过MAVROS送回PX4。整个闭环是Gazebo给PX4虚拟的传感器数据PX4解算出飞行状态后ROS读取状态并运行避障计算最后通过MAVROS把速度指令发给PX4PX4再输出电机控制量作用于Gazebo中的虚拟飞机。1.3 版本搭配与踩坑预判版本是这套环境里最让人头疼的地方我见过太多人因为版本不匹配卡在编译阶段好几天。目前最稳的组合是Ubuntu 20.04 ROS Noetic Gazebo 11 PX4 v1.14.3。组件推荐版本说明Ubuntu20.04LTSROS Noetic官方支持ROSNoeticROS1的最后一个长期支持版本生态最成熟Gazebo11与Noetic绑定的默认版本PX4v1.14.3带完整的Gazebo仿真支持MAVROSNoetic对应版本与PX4的MAVLink通信桥梁个人建议初学者直接选这套网上教程多、问题好搜。不要一上来就尝试ROS2Gazebo Classic或其他组合虽然现在ROS2越来越普及但避障相关的很多经典例程仍然都是围绕ROS1写的先用成熟栈把算法闭环跑通再迁移不迟。2. 环境搭建把PX4仿真跑起来这个阶段的目标不是立刻写避障算法而是先让虚拟无人机能正常起飞和降落。如果连这一步都做不到后面所有东西都无从谈起。2.1 安装ROS与MAVROS第一步安装ROS Noetic直接用官方源安装完整桌面版sudo apt install ros-noetic-desktop-full安装完成后初始化环境echo source /opt/ros/noetic/setup.bash ~/.bashrc source ~/.bashrc sudo apt install python3-rosdep python3-catkin-tools sudo rosdep init rosdep update这里有个容易踩的坑rosdep update经常因为网络原因失败。如果遇到不要反复重试先检查是否能正常访问域名然后配置合适的软件源再继续。搭建这事欲速则不达。接着安装MAVROSsudo apt install ros-noetic-mavros ros-noetic-mavros-extrasMAVROS还需要装地理数据包不装的话启动会报错sudo /opt/ros/noetic/lib/mavros/install_geographiclib_datasets.sh这个脚本会下载一些地理数据如果下载失败可以手动从源地址下载后放到指定目录细节不在这里展开记住装完必须要验证。2.2 编译PX4源码并启动Gazebo仿真拿到PX4源码时要特别注意分支和子模块很多编译问题都是子模块没拉全造成的git clone --recursive https://github.com/PX4/PX4-Autopilot.git --branch v1.14.3 cd PX4-Autopilot git submodule update --init --recursive然后执行官方工具链安装脚本它会自动安装PX4编译需要的所有依赖bash ./Tools/setup/ubuntu.sh这一步时间比较长安装编译工具链时一定要让脚本完整跑完不要中途CtrlC。我有一次因为嫌等待时间太长提前中断结果后面编译PX4时不断报缺头文件白白排查了两个小时。编译并启动带激光雷达的四旋翼仿真模型这是避障实验最常用的模型make px4_sitl gazebo_iris_lidar如果一切正常会跳出Gazebo窗口里面有一架四旋翼控制台会打印PX4的启动日志。看到类似INFO [commander] Ready for takeoff的日志就说明PX4已经在虚拟世界里准备好了。这里再强调一次默认启动的是iris不带传感器的机型做避障必须指定iris_lidar否则后面你根本订阅不到激光雷达数据。我最初没指定机型折腾了半天才发现Gazebo里压根没挂雷达传感器。2.3 配置MAVROS连接PX4 SITLPX4 SITL启动后会通过UDP端口向外发送MAVLink数据。MAVROS需要以UDP方式连接。常用方式是roslaunch mavros px4.launch fcu_url:udp://:14540localhost:14557这条命令的意思是MAVROS在本地14540端口接收数据同时向localhost的14557端口发送数据。PX4默认配置里会向14557端口发送MAVLink数据因此这个组合最常见。连接成功后新开一个终端验证rostopic echo /mavros/state如果能看到connected: True说明通信链路已经打通。如果一直是False多半是端口号不对或者MAVROS启动时机早于PX4启动。解决办法是先启动PX4仿真再启动MAVROS顺序不要颠倒。3. 四旋翼自主避障的感知与决策实现环境通了以后就进入正题怎么让无人机在飞行过程中自己发现障碍并绕开。3.1 避障传感器选型2D激光雷达最省心自主避障第一步是感知。Gazebo中能为四旋翼挂载很多种传感器但做避障入门我强烈建议先用2D激光雷达。原因有三点2D激光数据是平面上均匀分布的距离信息消息格式简单处理逻辑直观。PX4自带的iris_lidar模型已经把雷达挂好了启动即用。避障决策算法在二维平面上计算理解起来最顺适合先跑通闭环。如果你做的是三维空间的避障也可以换成深度相机订阅/camera/depth/points点云数据但处理复杂度和运算量会明显上升。我建议先把2D方案做明白再向3D扩展。3.2 在Gazebo中布置障碍物Gazebo本身提供了一些简单模型可以直接从左侧“Insert”面板拖入立方体、圆筒等障碍物。但这种方式每次启动都要手动拖不适合做自动化实验。持久化的做法是编辑world文件。PX4的Gazebo仿真会加载一个默认world你可以自己定义一个新的world文件。下面是一个最简单的障碍物模型片段放在world节点下就能在指定坐标放置一个立方体障碍物model nameobstacle_box pose5 0 0.5 0 0 0/pose statictrue/static link namelink visual namevisual geometry boxsize0.8 0.8 1.0/size/box /geometry /visual collision namecollision geometry boxsize0.8 0.8 1.0/size/box /geometry /collision /link /model发布world文件后在启动PX4 SITL时通过环境变量指定export PX4_WORLD_NAMEirislidar_obstacle.world make px4_sitl gazebo_iris_lidar这样你就能反复用同一场景做实验了。我一般会在起点和目标点之间放2到3个不对称的障碍物专门考验避障算法的鲁棒性。3.3 避障算法选型从向量法到路径规划避障算法有很多种势场法、VFH、DWA、A*、RRT还有基于OctoMap的三维路径规划。仿真环境的好处就是你可以把不同算法都跑一遍看效果再决定用哪个。我第一次实现时用的是最直观的“人工势场-向量法”思路把激光雷达扫描到的每个近距离障碍物看作一个“压力源”。按距离做加权距离越近压力越大。将障碍方向的加权平均反向作为避障向量。将避障向量与飞向目标点的向量叠加得到当前期望速度。这种算法不算精细但胜在计算量小、代码简单、行为可解释。仿真中遇到的大部分动态避障场景都能应付。等这个跑通了再去引入OctoMap建图、RRT全局规划会轻松很多。3.4 核心避障节点代码解析下面是我在项目里实际使用的避障节点核心代码。它订阅/scan激光话题计算避障向量并通过MAVROS发布速度设定点。#!/usr/bin/env python3 import rospy import math import numpy as np from sensor_msgs.msg import LaserScan from geometry_msgs.msg import TwistStamped class ObstacleAvoidNode: def __init__(self): rospy.init_node(obstacle_avoid_node) self.scan_sub rospy.Subscriber(/scan, LaserScan, self.scan_callback) self.vel_pub rospy.Publisher(/mavros/setpoint_velocity/cmd_vel, TwistStamped, queue_size10) self.rate rospy.Rate(30) # 安全距离低于该距离认为有障碍威胁 self.safe_dist 1.8 # 最大飞行速度 self.max_speed 2.0 # 避障增益 self.avoid_gain 1.5 # 目标点世界坐标系下的期望位置 self.target np.array([10.0, 0.0, 3.0]) self.latest_scan None def scan_callback(self, msg): self.latest_scan msg def compute_avoid_vector(self): if self.latest_scan is None: return np.zeros(3) ranges np.array(self.latest_scan.ranges) angle_min self.latest_scan.angle_min angle_inc self.latest_scan.angle_increment # 障碍方向向量累加距离越近权重越大 avoid_vec np.zeros(2) for i, r in enumerate(ranges): if r self.safe_dist: angle angle_min i * angle_inc # 只处理机头前方180度范围内的障碍 if -math.pi/2 angle math.pi/2: weight 1.0 / max(r, 0.2) obs_dir np.array([math.cos(angle), math.sin(angle)]) avoid_vec - weight * obs_dir if np.linalg.norm(avoid_vec) 1e-6: return np.zeros(3) # 归一化并乘以避障增益 return np.append(avoid_vec / np.linalg.norm(avoid_vec) * self.avoid_gain, 0.0) def run(self): while not rospy.is_shutdown(): avoid self.compute_avoid_vector() # 目标方向向量 to_target self.target - np.array([0.0, 0.0, 0.0]) # 实际项目中应从 /mavros/local_position/pose 获取当前坐标此处简略 to_target_norm to_target / np.linalg.norm(to_target) desired_vel to_target_norm * self.max_speed avoid # 限制速度大小 if np.linalg.norm(desired_vel) self.max_speed: desired_vel desired_vel / np.linalg.norm(desired_vel) * self.max_speed twist TwistStamped() twist.twist.linear.x desired_vel[0] twist.twist.linear.y desired_vel[1] twist.twist.linear.z desired_vel[2] self.vel_pub.publish(twist) self.rate.sleep() if __name__ __main__: node ObstacleAvoidNode() node.run()这段代码把核心逻辑压缩到了最小可运行状态但实际使用有几处一定要补目标方向的计算必须读取当前坐标不能像我简化到全0。建议订阅/mavros/local_position/pose用当前位置减去目标点位置得到真实方向。避障向量在角度大于90度时会发生向量方向翻转的问题严谨做法是用向量加权平均代表障碍方向而不是用角度平均。我这里用的是向量加权能避开大部分问题。代码里没有做机身坐标系到世界坐标系的转换。.scan得到的角度在机体系但速度设定点如果发给PX4是在机体系或世界系取决于MAVROS参数和PX4配置。如果发现避障方向不对大概率是坐标系转换没处理。4. 仿真联调从起飞到自主绕障环境、传感器、算法都到位后开始联调。联调阶段最能看出问题也是我耗时最多的部分。4.1 启动顺序与Rviz可视化联调时建议开多个终端按顺序执行# 终端1启动PX4 SITL Gazebo cd ~/PX4-Autopilot source Tools/setup_gazebo.bash make px4_sitl gazebo_iris_lidar# 终端2启动MAVROS roslaunch mavros px4.launch fcu_url:udp://:14540localhost:14557# 终端3启动避障节点 python3 obstacle_avoid_node.py# 终端4启动Rviz可视化 rviz在Rviz中添加LaserScan显示主题选择/scan就能实时看到激光雷达扫描到的障碍物轮廓。我建议再把MAVROS发布的/mavros/local_position/pose在Rviz中用Path显示出来这样能清楚看到无人机的完整飞行轨迹方便定位问题。启动避障节点后无人机不会自动起飞。你需要先通过MAVROS设置进入OFFBOARD模式并解锁再发送起飞指令。最简单的方式是使用QGroundControl在地面站里手动切模式和起飞也可以写一个控制节点调用MAVROS的服务。4.2 让四旋翼真正“看到”障碍再绕行如果只想快速验证避障逻辑可以用MAVROS的命令行工具直接发指令# 设置OFFBOARD模式 rosservice call /mavros/set_mode custom_mode: OFFBOARD # 解锁电机 rosservice call /mavros/cmd/arming value: true解锁后避障节点已经在以30Hz的频率发送速度设定点。PX4在OFFBOARD模式下会跟踪这些设定点如果一切正常无人机会朝着目标点方向飞行。当无人机飞近Gazebo里放置的立方体障碍物时激光雷达会检测到前方物体。避障算法计算出的避障向量会使无人机逐渐减速并向侧面平移。绕过障碍物后避障向量变小无人机重新朝目标方向加速飞行。这里有个值得注意的细节避障节点必须在OFFBOARD模式下持续不断发送速度指令哪怕指令全是0也不能停。如果停止发送超过几百毫秒PX4会认为控制链路丢失自动切换出OFFBOARD模式。我当时就把避障算法的输出放在定时器里调试时暂停了节点结果无人机直接悬在空中不动排查了好一会儿才意识到是控制超时问题。4.3 从撞上到绕开一次失败案例复盘我的第一次测试其实失败了。场景是在起飞点正前方6米处放了一个立方体障碍物目标点设在障碍物后方。参数设置安全距离1.5米飞行速度2m/s检测扇区正负90度。实际飞行时无人机确实检测到了障碍物也产生了避障向量但绕行时离障碍物太近机翼还是撞上了障碍物的棱角。复盘原因有两个安全距离太小。1.5米对于2m/s速度来说根本不够避障反应时间不到1秒飞机整体来不及横移。检测扇区太宽。正负90度意味着侧后方也有障碍参与计算容易产生横向震荡。第二次调整参数安全距离增加到2.2米速度降到1.2m/s检测扇区改成正负60度。结果无人机在距离障碍物约1.8米处就开始向右平移绕障过程非常顺滑整个绕障半径大概3米回到原航线后继续飞向目标点。5. 高频问题排查与实战经验仿真环境看着简单实际跑起来问题五花八门。我把自己踩过的坑和同行的经验汇总成了一张速查表每个问题几乎都是真实发生的。5.1 环境与编译问题速查表问题现象可能原因解决方案Gazebo界面一直闪烁显卡驱动/渲染后端问题安装显卡驱动或设置export LIBGL_ALWAYS_SOFTWARE1强制软件渲染PX4编译时提示缺少头文件子模块未拉全执行git submodule update --init --recursive再重新编译Gazebo加载模型缓慢或卡死模型文件未下载完整检查~/.gazebo/models目录预先下载好模型再启动MAVROS连接不上PX4端口错误或启动顺序错误先启动PX4再启动MAVROS核对fcu_url端口无人机在OFFBOARD模式下乱飞速度指令发布频率低于20Hz把避障节点发布频率提高到30Hz确保指令不断流避障时无人机左右震荡检测扇区太宽或增益过大收窄扇区到正负60度降低避障增益Rviz收不到/scanGazebo模型不是iris_lidar确认使用make px4_sitl gazebo_iris_lidar启动避障后回不到原航线目标方向计算没有加上当前位置偏移订阅/mavros/local_position/pose真正计算目标方向向量Gazebo界面闪烁问题特别常见尤其是刚装好ROS环境后。这个问题大部分时候不是代码问题仅仅是没有安装独立显卡驱动或者Gazebo渲染时和窗口管理器冲突。先试试强制软件渲染虽然画质差一点但不影响功能仿真。5.2 避障算法的调参心得避障算法有三个参数最影响效果安全距离、飞行速度和避障增益。它们之间是联动关系不能单独调。安全距离与飞行速度的关系是安全距离至少要大于速度乘以反应时间加制动距离。我的经验是速度1.2m/s时安全距离2.2米比较合适速度2m/s时安全距离要拉大到3米以上。安全距离太小避障来不及安全距离太大无人机在障碍稀疏的区域会特别绕远路效率下降。避障增益决定无人机避障时的反应强度。增益太小避障向量被目标向量淹没还是可能撞上增益太大飞机会猛地横向横移姿态震荡严重。我一般在1.0到2.0之间微调效果比较理想。还有一点容易被忽视激光雷达在Gazebo里虽然理想但还是会带一些噪声。真机雷达噪声更大所以在仿真阶段就加入传感器噪声提前积累滤波经验能让算法在迁移到真机时少踩很多坑。6. 把仿真当跳板走向真机前要补的三件事仿真跑通避障以后不要急着以为就大功告成。从仿真到真机还有一道坎我见过好几个项目都栽在迁移环节。第一件事是理解仿真和真机的传感器差异。Gazebo里的激光雷达数据是理想化的没有灰尘、反光、室外阳光干扰。真机激光雷达在室外强光下可能失效在玻璃幕墙旁边会打穿或折射。所以仿真中避障算法运行得再好也要在算法层面增加数据有效性校验比如连续多帧无效点超过阈值时就切换为安全悬停。第二件事是先做HITL硬件在环测试。把PX4固件烧到真实飞控板上但无人机仍然留在Gazebo里用真飞控去控制虚拟飞机。这一步能验证飞控驱动、MAVLink通信、传感器接线但还不需要实际起飞安全性比直接上真机高得多。很多在SITL下隐藏的时序问题在HITL阶段都会暴露出来。第三件事是加失效保护。真机飞行中如果避障节点崩溃、MAVROS断连、传感器掉线无人机不能直接失控。我的习惯是在PX4参数里设置OFFBOARD模式的断连保护时间并预设失去控制后的行为为悬停或返航。这些在仿真阶段就可以通过人为kill掉避障节点来测试千万不要等到真机试飞时才发现失效保护没配。现在回头看仿真环境最大的价值不是让项目看起来更“高科技”而是给了你一个可以无限重来的试错空间。我在仿真里跑了几十种不同障碍物位置、不同参数组合每次崩溃都变成一次经验积累。做飞控和避障开发稳比快重要一个稳定跑通的仿真闭环是走向真机的最稳路线。
