用Gazebo+ROS Melodic搭建你的第一个无人机自主导航仿真环境（FastPlanner规划+VINS定位）

从零构建Gazebo无人机仿真环境：FastPlanner与VINS的实战融合

当第一次看到无人机在仿真环境中自主避障飞行时，那种程序具象化的震撼至今难忘。作为机器人领域最激动人心的应用之一，自主导航系统正从实验室走向工业现场，而仿真环境就是最好的练兵场。本文将带你用ROS Melodic+Gazebo搭建完整的无人机自主导航系统，集成FastPlanner运动规划和VINS视觉惯性定位两大核心模块，最终实现通过Rviz点击目标点就能让无人机自主规划路径并飞行的完整Demo。

1. 基础环境准备：ROS工作空间与PX4仿真

在开始前，我们需要一个干净的ROS Melodic环境。建议使用Ubuntu 18.04系统，这是与ROS Melodic最兼容的版本。打开终端，按顺序执行以下命令初始化工作空间：

mkdir -p ~/uav_ws/src cd ~/uav_ws/src catkin_init_workspace cd .. catkin_make

接下来安装PX4仿真环境，这是整个系统的飞行控制核心：

git clone https://github.com/PX4/PX4-Autopilot.git --recursive bash ./PX4-Autopilot/Tools/setup/ubuntu.sh make px4_sitl_default gazebo

注意：PX4编译需要较长时间，建议在性能较好的机器上操作。如果遇到依赖问题，可以尝试sudo apt-get install ninja-build exiftool。

安装完成后，测试Gazebo中的无人机模型是否能正常启动：

source ~/uav_ws/devel/setup.bash roslaunch px4 posix_sitl.launch

正常情况下，你应该能看到Gazebo界面中加载出一个带有摄像头的无人机模型。此时在另一个终端运行rostopic list，应该能看到/mavros/imu/data等话题，说明PX4与ROS的通信已建立。

2. FastPlanner运动规划系统部署

FastPlanner是港科大团队开发的高效运动规划算法，特别适合无人机在复杂环境中的实时路径规划。我们先安装必要的依赖：

sudo apt-get install libarmadillo-dev ros-melodic-nlopt

然后克隆FastPlanner代码到工作空间：

cd ~/uav_ws/src git clone https://github.com/HKUST-Aerial-Robotics/Fast-Planner.git cd .. catkin_make

编译成功后，启动可视化界面测试：

source devel/setup.bash roslaunch plan_manage rviz.launch

在新终端中运行规划节点：

roslaunch plan_manage kino_replan.launch

此时在Rviz中使用2D Nav Goal工具点击地图任意位置，无人机应该能立即生成并执行一条避障路径。如果遇到规划失败的情况，可能是地图分辨率设置不当，可以调整kino_replan.launch文件中的grid_map/resolution参数。

3. VINS视觉惯性定位系统集成

VINS-Fusion是一个基于视觉和IMU的紧耦合状态估计器，能为无人机提供高精度的位姿信息。安装前需要先配置RealSense相机驱动：

sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u sudo apt-get install librealsense2-dkms librealsense2-utils

然后安装ROS封装包：

cd ~/uav_ws/src git clone https://github.com/IntelRealSense/realsense-ros.git cd .. catkin_make

测试相机是否能正常工作：

roslaunch realsense2_camera rs_camera.launch rviz

在Rviz中添加PointCloud2显示，话题选择/camera/depth/color/points，应该能看到实时点云。

接下来安装VINS-Fusion核心算法：

cd ~/uav_ws/src git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git cd .. catkin_make

提示：编译过程可能需要较长时间，如果遇到ceres-solver报错，需要手动安装最新版本的ceres。

4. 系统联调与实战演示

现在我们将所有模块串联起来。首先修改PX4启动文件，添加相机和IMU的Gazebo插件：

<include file="$(find gazebo_ros)/launch/empty_world.launch"> <arg name="world_name" value="$(find mavlink_sitl_gazebo)/worlds/empty.world"/> <arg name="paused" value="false"/> <arg name="gui" value="true"/> <arg name="verbose" value="true"/> </include> <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-sdf -file $(find mavlink_sitl_gazebo)/models/iris_depth_camera/iris_depth_camera.sdf -model iris -x 0 -y 0 -z 0.5" output="screen"/>

然后创建系统启动的launch文件uav_nav.launch：

<launch> <!-- PX4 SITL --> <include file="$(find px4)/launch/posix_sitl.launch"/> <!-- VINS-Fusion --> <node pkg="vins" type="vins_node" name="vins_estimator" output="screen" args="$(find vins)/../config/px4/vins_rviz_config.yaml"/> <!-- FastPlanner --> <node pkg="plan_manage" type="kino_replan_node" name="kino_replan" output="screen"/> <!-- Rviz --> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find plan_manage)/launch/default.rviz"/> </launch>

启动完整系统：

roslaunch uav_nav.launch

在Rviz中执行以下操作：

1. 添加/vins_estimator/camera_pose话题显示无人机估计位姿
2. 添加/planning_vis/trajectory话题显示规划路径
3. 使用2D Nav Goal工具指定目标位置

如果一切正常，你应该能看到无人机根据VINS提供的定位信息，通过FastPlanner规划出最优路径并自主飞行到目标点。初次运行时可能会遇到以下典型问题：

• 定位漂移：检查IMU数据是否正常，尝试重新初始化VINS
• 规划失败：调整FastPlanner参数中的max_vel和max_acc降低动态约束
• 通信延迟：使用rostopic hz检查各话题频率，优化系统资源分配

5. 进阶优化与调试技巧

当基础功能跑通后，我们可以进一步优化系统性能。首先是VINS的参数调优，打开vins_rviz_config.yaml重点调整：

# 视觉特征点参数 max_cnt: 150 # 特征点最大数量 min_dist: 30 # 特征点最小间距 freq: 10 # 处理频率 # IMU噪声参数 acc_n: 0.08 # 加速度计噪声 gyr_n: 0.004 # 陀螺仪噪声

对于FastPlanner，修改kino_replan.yaml中的动力学参数：

max_vel: 2.0 # 最大速度(m/s) max_acc: 1.5 # 最大加速度(m/s²)

实际调试时，建议使用rqt工具实时监控系统状态：

rqt_graph # 查看节点通信 rqt_plot /mavros/imu/data/linear_acceleration/x # 绘制IMU数据 rqt_reconfigure # 动态调整参数

为了评估系统性能，可以记录并回放ROS bag：

# 记录 rosbag record -O test.bag /vins_estimator/odometry /mavros/local_position/pose # 回放 rosbag play test.bag --clock

6. 场景扩展与实战应用

掌握了基础仿真后，我们可以创建更复杂的测试环境。在Gazebo中构建带障碍物的世界：

<!-- 添加墙壁障碍物 --> <model name='wall1'> <pose>2 0 1.5 0 0 0</pose> <static>true</static> <link name='link'> <collision name='collision'> <geometry> <box> <size>0.1 4 3</size> </box> </geometry> </collision> <visual name='visual'> <geometry> <box> <size>0.1 4 3</size> </box> </geometry> </visual> </link> </model>

对于需要更高精度的场景，可以导入真实建筑的CAD模型到Gazebo。也可以尝试以下进阶功能：

• 多机协同：在Gazebo中生成多个无人机模型
• 动态避障：添加移动障碍物测试实时避障能力
• 室外场景：使用Gazebo的GPS插件模拟室外飞行

我在实际测试中发现，当无人机速度超过1.5m/s时，VINS的定位精度会明显下降。这时可以采用以下策略：

1. 降低最大飞行速度
2. 增加VINS处理频率
3. 融合PX4的本地位置估计