做无人机自主导航的人应该都被同一件事折磨过SLAM 跑得好好的规划器也调得挺顺但俩东西一接就是各种崩。定位漂了规划器乱飞规划器避障太激进直接把定位带崩最后三分钟炸机五分钟查日志半小时怀疑人生。我这次在 XTDrone 平台上把 FAST-LIO2 和 ego-planner 串起来做路径规划部署踩过的坑比想象中多但最后跑通的那一刻那种满足感也是真上头。这篇博文就把完整过程拆开揉碎讲清楚从环境准备到传感器配置从算法原理到代码级联调再到排错手册全部基于我的实际经历不掺水。1. XTDrone 环境准备与平台验证1.1 为什么选 XTDrone 作为仿真底座XTDrone 是目前国内开源无人机仿真平台里生态比较完整的一个基于 Gazebo 和 PX4 搭建支持多旋翼、固定翼、无人车等多种载体。最核心的价值在于它把 PX4 固件、Gazebo 模型、MAVROS 通信、SLAM 算法全都集成到了一套体系里。相比自己从头搭 Gazebo 环境XTDrone 帮你解决了模型导入、传感器仿真、话题映射这些脏活累活。用 XTDrone 做 FAST-LIO2 和 ego-planner 的联合验证是社区里比较省力的一条路。原因有三点XTDrone 提供了带 IMU 和激光雷达的无人机模型而 FAST-LIO2 恰好是一个紧耦合 LiDAR-惯性里程计这两个传感器缺一不可。默认的 2D 激光不够用后面我会详细说怎么改装 3D 雷达。XTDrone 的场景丰富有室内走廊、户外树林、城市建筑等。ego-planner 做局部规划需要障碍物环境来验证避障效果这些场景拿来就能用。XTDrone 的接口和 PX4 官方仿真保持一致意味着后期把算法部署到真机时MAVROS 那条链路几乎不用改动。1.2 版本选型与依赖安装避坑XTDrone 官方推荐的是 Ubuntu 18.04 ROS Melodic Gazebo 9但如果你像我一样喜欢用新系统Ubuntu 20.04 ROS Noetic 也不是不行只是需要手动处理一些依赖问题。实测下来Ubuntu 20.04 方案更省心因为 Gazebo 11 对激光雷达仿真支持更好。依赖安装有一个大坑值得专门提醒不要用apt直接装ros-noetic-desktop-full之后就去跑 XTDrone 脚本。里面有一堆 Python 包和 Gazebo 模型没装齐跑起来会报各种奇怪的错。XTDrone 的sitl_config.sh脚本会自动下载模型但这要求你的网络能访问 GitHub 和模型仓库否则卡在下载阶段非常痛苦。我的建议是分三步走先装好 ROS 基础再装 XTDrone 依赖最后单独验证 PX4 固件能否正常启动仿真。顺序不能乱不然排查问题的时候你会疯掉。# 安装 ROS Noetic 基础Ubuntu 20.04 sudo sh -c echo deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main /etc/apt/sources.list.d/ros-latest.list sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E9E45B3EDB8EFD3EAF9E0F8C7E4D1C0 sudo apt update sudo apt install ros-noetic-desktop-full装完 ROS 之后把 XTDrone 源码克隆到~/xtdrone然后运行官方给的安装脚本。这里我个人的习惯是手动装依赖不用一键脚本因为一键脚本总是默认你用的是 Ubuntu 18.04在 20.04 上会有偏差。1.3 在 Gazebo 里唤醒无人机验证传感器链路环境装好之后先不要急着上 SLAM把最基础的仿真飞起来再说。XTDrone 提供的xtdrone_run.launch会自动加载无人机模型、启动 PX4 SITL、拉起 MAVROS。整个链路如果通了你会在终端里看到一堆日志其中最关键的是 MAVROS 的connected字样。验证传感器是否正常可以用 rostopic 查看rostopic echo /mavros/imu/data -n 5 rostopic echo /laser/scan -n 5前者是 IMU 数据后者是 2D 激光数据。XTDrone 默认会给无人机挂一个 2D 激光雷达但 FAST-LIO2 需要的是 3D 激光雷达点云。所以你要么换一个带 3D LiDAR 的模型要么自己往 URDF 里加一个 Velodyne 16 的传感器模型。这一步非常关键我刚开始就是因为用了默认模型点云话题一直是空的排查了大半天才发现是模型问题。2. FAST-LIO2 激光惯性里程计部署2.1 FAST-LIO2 的核心原理先搞明白FAST-LIO2 是香港大学火星实验室开源的 LiDAR-Inertial 里程计核心思想是直接把原始点云配准到增量更新的地图上不需要提取特征点这是它和 FAST-LIO1 最大的区别。它维护一个全局体素地图ikd-Tree 数据结构每次新来一帧点云就通过迭代卡尔曼滤波把 IMU 预测的位姿和激光点云配准的观测融合在一起输出高频、低漂移的里程计。为什么要强调这个原理因为部署的时候你会发现FAST-LIO2 有两个关键话题一个是/Odometry里程计输出一个是/cloud_registered配准后的点云。ego-planner 需要的是全局一致的地图点云和稳定的里程计位姿这俩话题缺一不可。/Odometry是 nav_msgs/Odometry 类型ego-planner 订阅它作为无人机的状态估计输入。/cloud_registered是 sensor_msgs/PointCloud2ego-planner 用它来构建局部代价地图。搞清楚数据流之后你就明白为什么 FAST-LIO2 的参数配置里雷达外参和 IMU 内参必须精确因为任何一个偏差都会导致点云畸变进而影响里程计精度和地图质量。2.2 给 XTDrone 无人机装一个 3D 激光雷达XTDrone 默认的无人机模型不带 3D 激光雷达这需要用 Velodyne 插件来扩展。我在~/.gazebo/models里新建了一个uav_with_lidar模型在一个基础四旋翼上挂载了 Velodyne VLP-16 的 Gazebo 插件。下面是模型文件里核心的传感器部分直接在无人机 link 下加入就行gazebo referencebase_link sensor namevelodyne typegpu_ray always_ontrue/always_on update_rate10/update_rate visualizefalse/visualize plugin namevelodyne_plugin filenamelibgazebo_ros_velodyne_laser.so frame_namevelodyne/frame_name topic_name/velodyne_points/topic_name point_cloudtrue/point_cloud /plugin ray scan horizontal samples1800/samples resolution1/resolution min_angle-3.14159/min_angle max_angle3.14159/max_angle /horizontal vertical samples16/samples resolution1/resolution min_angle-0.2618/min_angle max_angle0.2618/max_angle /vertical /scan range min0.3/min max100.0/max resolution0.01/resolution /range /ray /sensor /gazebo这段配置里我把水平采样数设成了 1800也就是 0.2 度一条射线保证近距离障碍物也能被捕捉到。垂直方向 16 线覆盖正负 15 度这是 VLP-16 的真实参数。发布频率设成 10Hz和 FAST-LIO2 的默认配置匹配。完成之后要做一次source和catkin_make重新启动仿真然后验证话题。这一步很多人漏掉修改 URDF 之后没有重新构建导致新模型根本没加载进去。rostopic hz /velodyne_points如果频率稳定在 10Hz 左右说明雷达已经工作了。这时候你再跑 FAST-LIO2才会有数据可吃。2.3 编译 FAST-LIO2 遇到的依赖问题FAST-LIO2 依赖 livox_ros_driver但那是给 Livox 固态雷达用的我们在 Gazebo 里用的是 Velodyne 仿真不需要也不能把 livox_ros_driver 作为强制依赖编进去。官方仓库的CMakeLists.txt里默认会找 livox_ros_driver这在纯仿真环境下会报找不到包的错。我的做法是直接把 FAST-LIO2 源码里的CMakeLists.txt中关于 livox_ros_driver 的find_package和头文件包含注释掉同时把LIVOX_AVIA相关的编译宏去掉。只有需要编译fastlio2_mapping这个主节点时保留 livox_ros_driver 的接口才是有意义的。还需要注意 Eigen 版本。Ubuntu 20.04 默认的 Eigen 是 3.3.7FAST-LIO2 用了不少 Eigen 的高级特性实测 3.3.7 是能编译过且运行正常的但如果你手动升级到了 Eigen 3.4有概率出现内存对齐问题。建议保持系统默认版本不要手贱去升级。2.4 launch 文件配置与雷达外参校准FAST-LIO2 的配置文件主要改两个config/avia.yaml和launch/mapping_avia.launch。在仿真环境里外参矩阵是明确的因为雷达是我自己挂上去的。我把激光雷达装在无人机正上方 0.2 米处水平朝向与机体 x 轴一致那么外参就是body_T_lidar: - [1.0, 0.0, 0.0, 0.0] - [0.0, 1.0, 0.0, 0.0] - [0.0, 0.0, 1.0, 0.2] - [0.0, 0.0, 0.0, 1.0]外参搞错的话点云会分层地图会糊掉里程计位姿会慢慢漂。仿真环境下因为外参是已知的反而比真机好排查。如果你以后上真机外参标定这一步是逃不掉的。launch 文件里最需要注意的是设置雷达点云话题。默认的 FAST-LIO2 话题是/livox/lidar我们要改掉remap from/livox/lidar to/velodyne_points / remap from/livox/imu to/mavros/imu/data /IMU 话题直接重映射到 PX4 的 MAVROS 数据流这样整个系统的传感器输入就是雷达点云 MAVROS 的 IMU。跑起来之后如果你在 RViz 里能看到绿色的地图点云在慢慢扩展同时无人机位姿跟着动就说明 FAST-LIO2 已经跑通了。3. ego-planner 局部规划器配置与接入3.1 ego-planner 在整套链路里的角色ego-planner全称是 EGO-Planner: An Efficient, Generalized, and Optimizable Goal-Driven Planner是一个基于优化的局部规划器特别适合无人机这种动力学约束强的载体。它不依赖事先建好的全局地图而是通过传感器实时感知周围环境生成一条满足动力学可行性的平滑轨迹。它在整个导航系统里的位置是这样的FAST-LIO2 负责回答“我在哪里”ego-planner 负责回答“我下一步往哪飞”。一个典型的数据流是FAST-LIO2 输出里程计/Odometry给 ego-planner。ego-planner 接收点云/cloud_registered或者直接接收传感器点云构建局部障碍物地图。用户通过 rviz 的 2D Nav Goal 发布目标点ego-planner 在局部地图里搜索并优化出一条无碰撞轨迹。轨迹以/planning/pos_cmd话题发给 PX4 位置控制器。这套链路的好处是FAST-LIO2 的输出频率高10Hz 以上ego-planner 每次重新规划都能基于最新的位姿所以即使无人机飞得比较快也不容易出现“规划器还在用旧位姿规划”的尴尬情况。3.2 编译 ego-planner 前的必要修改ego-planner 官方仓库是基于 ROS Melodic 开发的在 Noetic 上编译要注意几个点。首先它依赖plan_env、path_searching、bspline、bspline_opt、traj_utils这几个子模块需要 uav_simulator 里的so3_control和Utils包。直接catkin_make会出现找不到Utils的情况记得把uav_simulator/Utils也加进 catkin 工作区。其次ego-planner 默认订阅/map_generator/global_cloud和/map_generator/cloud这是它自带栅格地图生成器发的话题。如果我们想用 FAST-LIO2 的真实点云替代仿真地图生成器就必须把 launch 里的相关参数改掉remap from/map_generator/global_cloud to/cloud_registered / remap from/map_generator/cloud to/cloud_registered /这样 ego-planner 吃到的就是 FAST-LIO2 配准后的点云而不是模拟器生成的假障碍物点云。3.3 ego-planner 关键参数调整ego-planner 的调参是重头戏参数直接决定轨迹质量。我先把实际跑通的一组参数放在这里后面解释为什么这么调ego_planner_node: ros__parameters: map_size: 20.0 local_map_size: 10.0 resolution: 0.1 drone_id: 0 max_vel: 3.0 max_acc: 2.0 max_jerk: 3.0 obstacle_inflation: 0.3 use_ellipsoid: false sdf_map: resolution: 0.1 local_map_margin: 2.0 obstacle_inflation: 0.3max_vel设成 3.0 米每秒这个速度在室内场景已经很快了再快 FAST-LIO2 的点云配准容易出问题。obstacle_inflation是障碍物膨胀半径0.3 米是为了给机体留安全余量因为无人机本身有一定尺寸不能贴着障碍物边缘飞。local_map_size设成 10 米是这个系统的核心它决定了 ego-planner 每次重新规划时考虑的范围。太大计算量上升规划变慢太小无人机很容易飞出规划范围导致规划器跟不上。10 米在室内走廊和树林场景里都够用而且时间开销控制在 20ms 以内完全满足 10Hz 的重规划频率。3.4 把 FAST-LIO2 的输出接进 ego-planner到了这一步两台算法都各自能跑了接下来就是缝合。launch 文件里我建了一个新的combined.launch把 FAST-LIO2 和 ego-planner 一起拉起来。注意启动顺序先启动 FAST-LIO2等它输出稳定的里程计和点云再启动 ego-planner因为 ego-planner 在启动瞬间如果收不到点云会在局部地图里以全空为初始状态一旦后面点云进来会产生错误的膨胀地图。一个更稳妥的做法是加一个延时启动或者在 ego-planner 节点里加一个等待点云话题数据到达的逻辑。我用的是 launch 文件的launch-prefixbash -c sleep 5; $0 $这种粗暴方式实测有效。另外还要处理 TF 树。FAST-LIO2 输出的是camera_init到body的 TFego-planner 默认期待world到base_link的 TF。我在 combined.launch 里加了一个static_transform_publisher把camera_init框架映射到worldnode pkgtf2_ros typestatic_transform_publisher nameworld_to_camera_init args0 0 0 0 0 0 world camera_init /如果你不做这一步RViz 里会看到机器人模型不知道飘到哪里去规划出来的轨迹位置也对不上。4. 实飞测试与参数联动调优4.1 起飞前必做的三项检查实飞测试前我给自己定了一条铁律不检查三件事绝对不推油门。第一检查 FAST-LIO2 的地图质量。RViz 里如果点云地图是清晰的双层墙面而不是一团模糊的雾状点说明外参和配准正常。如果地图分层大概率是外参旋转矩阵错了或者 IMU 数据有延时。第二检查 TF 树是否连续。运行rqt_tf_tree确认从world到map到camera_init到body到velodyne的链路无断点。第三检查里程计漂移。让无人机悬停 30 秒观察 RViz 里的位置是否稳定如果位置慢慢漂移说明 IMU 偏置没有收敛或者激光配准有问题。仿真环境下一般不会漂会漂就要反思是不是点云话题接错了。4.2 室内走廊场景的避障实测我用 XTDrone 自带的室内走廊场景做的第一次联合测试。目标点设在走廊尽头左侧。起飞后无人机初始阶段比较稳但进入走廊狭窄段时ego-planner 规划出的轨迹明显往右侧偏然后在一个拐角处悬停了一下重新规划。我看日志发现是因为 FAST-LIO2 在走廊转角处点云特征不足导致里程计位姿跳变ego-planner 检测到位姿跳变后触发了重新规划。这个问题的根因是FAST-LIO2 在没有足够几何特征的走廊里退化场景比较明显。解决办法有两个方向。第一个是修改 FAST-LIO2 的filter_size_surf和filter_size_map把地图分辨率调细让更多点云参与配准但这会增加计算量。第二个是给无人机加一个 downward 朝下的激光雷达或者相机补充地面特征。仿真环境里加一个向下的测距传感器成本很低我实测效果立竿见影。我最后采用的是调参数方案把filter_size_surf从 0.5 降到 0.3定位稳定性明显改善。缺点是 CPU 占用上涨了大概 15%但还在可接受范围。4.3 动态障碍物加入后的避障测试XTDrone 支持直接在场景里放置动态障碍物模型。我在走廊里加了一个来回移动的小车速度 0.5 米每秒。第一次测试时无人机差点撞上因为我没把点云的动态障碍物信息及时更新到 ego-planner 的局部地图里。排查发现是 FAST-LIO2 的cloud_registered里动态障碍物产生的点云被当成静态地图的一部分了。实际上 FAST-LIO2 会把这些点残差较大的点剔除掉于是 ego-planner 根本看不到这个动态障碍物。这在 SLAM 算法层面是合理的因为剔除动态点能提高定位稳定性但在规划层面就变成了隐患。解决方案是让 ego-planner 直接订阅原始点云/velodyne_points而不是只依赖 FAST-LIO2 配准后的点云。这样动态障碍物点云能实时进入局部地图。同时打开 ego-planner 的use_raw_pointcloud参数让它把原始点云和配准点云做融合。做完这个改动之后动态避障测试就顺利通过了。4.4 速度参数与定位稳定性之间的平衡另一个很有意思的调参问题发生在速度参数上。把max_vel从 3.0 提到 4.5 后无人机飞行变激进但 FAST-LIO2 的里程计开始出现跳变。原因是速度过快导致同一帧点云内部的畸变增大FAST-LIO2 的配准精度下降。在仿真环境下雷达是理想传感器不会有真实的运动畸变但这种跳变还是出现了。我怀疑是 ego-planner 在高速下生成的轨迹曲率变化太快导致仿真中无人机模型的实际运动跟上了指令但惯性导航解算出现数值问题。后来我把max_vel调到 3.5max_acc调到 2.5max_jerk保持 3.0这组参数下定位稳定性和飞行速度取得了不错的平衡。这套参数在走廊场景里飞了十几圈没有出现位姿跳变规划成功率也保持在 95% 以上。5. 常见问题速查与排错思路5.1 FAST-LIO2 侧高频问题运行过程中我遇到了几个高频问题多数集中在 FAST-LIO2 这一侧。第一个是 IMU 数据频率不匹配。MAVROS 默认 IMU 发布频率是 100Hz 左右FAST-LIO2 默认也接受 100Hz但如果你的无人机模型配置把 IMU 频率改成了 200HzFAST-LIO2 里卡尔曼滤波的预测步长会出错表现为位姿抖动。在 launch 里确认/mavros/imu/data的实际频率用rostopic hz一看便知。第二个是点云密度不够导致建图稀碎。Velodyne 仿真里水平采样数太小时远处墙面点云会稀疏FAST-LIO2 的 ikd-Tree 里特征不足建出来的地图有空洞。把水平采样数从 900 提到 1800情况明显好转。第三个是时间戳不同步。FAST-LIO2 要求点云和 IMU 的时间戳在同一个时钟域Gazebo 仿真里默认没问题但如果你手动改了传感器插件里的frame_name或者用了rosbag回放时间戳可能会错位。用rostopic echo /velodyne_points/header/stamp和/mavros/imu/data/header/stamp对比一下偏差超过 10ms 就该查时钟了。5.2 ego-planner 侧高频问题ego-planner 侧的问题也有几个典型场景。第一个是规划轨迹一直在原地打转。这通常是因为局部地图里没有障碍物信息ego-planner 感知到的环境是空的但又不知道为什么飞不过去于是反复在原点附近生成抖动轨迹。我遇到这个问题的原因是 FAST-LIO2 的点云话题和 ego-planner 的局部地图话题没有接上ego-planner 一直在拿旧地图规划。第二个是轨迹穿墙而过。这基本是障碍物膨胀半径设置太小obstacle_inflation只有 0.1 米时无人机模型的机臂已经超出膨胀区域看起来轨迹没碰墙实际上桨叶已经擦墙。至少设到 0.3 米才安全。第三个是规划不及时导致急停。ego-planner 默认的重规划频率是 10Hz如果单次规划耗时超过 100ms整条链路的实时性就崩了。在复杂环境里map_size太大或者点云量太大都会拖慢规划速度。用roslaunch里的timeout或者 top 命令观察 CPU 占用如果 ego-planner 节点单核占用超过 80%就该降低分辨率或者缩小规划范围。5.3 整套系统的排错顺序我总结了一套排错顺序遇到问题按这个顺序查能少走很多弯路。第一步查话题rostopic list看 FAST-LIO2、ego-planner 关键话题是否都有数据。第二步查 TFrqt_tf_tree看坐标变换有没有断裂和跳变。第三步查频率rostopic hz看点云和 IMU 频率是否正常。第四步查地图在 RViz 里看局部地图是否完整、障碍物是否膨胀。第五步查日志ego-planner 的 terminal 输出里有大量调试信息[planner]开头的行会告诉你当前规划状态。这套排查流程我至少用了十几次每次都能快速定位问题比东点一下西点一下高效得多。6. 仿真到真机的差距与经验总结6.1 仿真和真机最大的三个坑仿真跑得再顺真机该踩的坑一个都不会少。我列一下三个最经典的差距点给以后想迁移到真机的朋友做个心理建设。第一个是传感器噪声模型。Gazebo 里的 Velodyne 是理想传感器没有测距噪声、没有运动畸变、没有时间戳抖动。真机上的 VLP-16 数据噪声明显更大FAST-LIO2 需要重新调整filter_size_surf和filter_size_map。第二个是 IMU 噪声和偏置。MAVROS 从 PX4 拿到的 IMU 数据是经过滤波的而真机 IMU 原始数据里有大量噪声和温漂FAST-LIO2 的初始化阶段需要在地面静止足够久让 IMU 偏置收敛。第三个是规划器输出的控制指令与飞控的匹配。ego-planner 输出的是位置、速度、加速度期望值PX4 的 offboard 模式可以通过 MAVROS 接收但 PX4 对不同控制模式的支持程度不一样。仿真里直接发位置指令没问题真机上需要切换到 offboard 模式并设置好参数。6.2 这套方案后续还可以怎么扩展如果你不想只在仿真里跑想把这套方案用到实际项目里我建议从这三个方向扩展。用真实 VLP-16 或 Livox 替换仿真雷达重新跑一遍外参标定把 FAST-LIO2 的配置文件改成真实传感器参数。引入全局规划器比如把 FAST-LIO2 建出来的全局地图喂给 A* 或者 RRT*先做全局路径搜索再用 ego-planner 做局部优化这样可以避免 ego-planner 陷入局部极小。结合动态目标检测在点云里做目标跟踪把动态障碍物的运动预测信息加进 ego-planner 的轨迹优化里实现真正的动态避障。我自己的下一步计划是把这套方案迁移到实际的无人机平台上先把外参标定做了然后在室外小范围场景试飞。说实话心里也没底但仿真里已经把主要逻辑和参数都验证过了真机上的问题应该是可以一个个解决的。最后分享一个实操心得整套部署中最容易让人崩溃的不是算法本身而是环境问题。Gazebo 模型加载失败、ROS 包版本冲突、launch 文件里一个空格错误都能让你折腾一下午。遇到这种问题别硬刚先 pkill 清掉所有残留进程再重新启动往往比反复改代码有效得多。我的习惯是维护一个环境检查脚本一键检查 ROS 环境、Gazebo 进程、话题频率、TF 树完整性每次开始调试前先跑一遍省下来的时间足够再调三组参数了。