近两年无人机自主导航方向的项目绕不开两个港大的开源作品FAST-LIO2 和 ego-planner。前者是做激光惯性里程计和建图的后者是做局部路径规划的都是各自领域里被引用到手软、被移植到各种平台上的硬通货。但很多初学者甚至一些有经验的开发者在把这两个东西真正接到同一个系统里跑通时还是会卡在环境配置、坐标对齐、话题对接这些“看不见的坑”里。我前段时间刚好在 XTDrone 仿真平台上把这套链路完整部署了一遍从编译到跑出平滑避障轨迹前后折腾了小两周这篇就把整个部署思路、关键配置和踩过的坑一次说清楚。先说结论这套组合在 XTDrone 里跑通之后效果非常接近真机表现。FAST-LIO2 实时输出里程计和局部点云地图ego-planner 基于这些信息规划出带动力学约束的避障轨迹整个闭环在 Gazebo 仿真里可以稳定运行后续迁到真机上只需要换传感器驱动和改少量参数。整个过程适合正在学习无人机自主导航、想在自己环境里复现完整感知-规划闭环的朋友参考。1. 项目整体设计与思路拆解1.1 FAST-LIO2 在整个链路里扮演什么角色先把这个系统的“分工逻辑”捋清楚。无人机想要自主飞行第一件事是知道自己在哪里、周围长什么样这属于状态估计和建图的范畴。FAST-LIO2 干的就是这个活。它的核心思想可以概括为两句话紧耦合的迭代卡尔曼滤波做状态估计ikd-Tree 维护的增量地图做配准。相比早期的一些激光里程计算法FAST-LIO2 直接对原始点云做配准省去了特征提取这一步。这意味着它在退化环境比如长长的走廊、空旷场地里不太容易丢因为它用的是全量点云的几何信息而不是依赖墙角、平面这些特征。实际跑下来它的鲁棒性确实比传统 ICP 类算法好很多。在 XTDrone 里部署时FAST-LIO2 直接订阅 Gazebo 仿真出来的激光雷达点云话题再接收 IMU 数据经过滤波融合之后输出两个关键东西一个是高频里程计/Odometry另一个是不断更新的局部地图点云/cloud_registered。后者是给 ego-planner 做代价地图用的核心输入。1.2 ego-planner 解决的又是什么问题有了定位和地图接下来就是“往哪飞、怎么飞”。ego-planner 是一个基于优化的局部规划器它的特色是直接在欧氏距离场ESDF里做无约束优化轨迹用均匀 B 样条表示。这里有个很多人第一次接触时不理解的点为什么不用传统的栅格地图加 A* 全局规划原因在于 ego-planner 的定位是“局部避障平滑跟踪”。它不关心从起点到终点的全局最优路径它关心的是在传感器能感知到的范围内如何生成一条平滑、安全、符合动力学约束的轨迹让无人机既能快速飞行又能躲开突然出现的障碍物。全局路径规划可以交给更高层的节点比如 RRT* 或者 A*ego-planner 负责的是把全局路径变成无人机真正能飞出来的轨迹。它的轨迹优化考虑了三方面的代价碰撞代价离障碍物越近代价越大、平滑代价加速度和 jerk 要小、可行性代价速度、加速度要在无人机物理极限内。这三项加权求和后做梯度下降优化最终输出的 B 样条控制点就是一条可执行轨迹。1.3 为什么选择在 XTDrone 平台上做结合XTDrone 这个平台在无人机仿真圈子里口碑一直不错因为它把 PX4 固件、Gazebo 物理仿真、MAVROS 通信、以及一堆常用的感知算法都集成好了。它的最大优势是“开箱即用度”高你不需要自己从零搭一套 ROSGazeboPX4 的环境省掉了大量环境配置的时间。选择在 XTDrone 上做 FAST-LIO2 和 ego-planner 的结合还有一层考虑是它内置了多种传感器模型包括 Livox 系列激光雷达、普通三维激光雷达、IMU、相机等。FAST-LIO2 在真机上最常见的搭档就是 Livox 雷达XTDrone 恰好提供了对应模型这就让整个仿真环境非常接近真实部署场景。后面从仿真迁到真机时改动量可以做到很小。系统整体结构是这样的PX4 飞控通过 MAVROS 提供无人机状态和控制接口Gazebo 里模拟的激光雷达和 IMU 发布原始数据给 FAST-LIO2FAST-LIO2 输出里程计和点云地图给 ego-plannerego-planner 规划出轨迹后通过控制接口发送速度指令给 PX4形成完整闭环。2. 环境准备与平台部署细节2.1 XTDrone 环境安装的几个关键检查点XTDrone 的安装官方文档写得很详细照着走基本能通但有几个点容易出错我单独提一下。第一是系统版本。XTDrone 目前对 Ubuntu 18.04 ROS Melodic 和 Ubuntu 20.04 ROS Noetic 支持得比较好。我用的是 Ubuntu 20.04 Noetic整体兼容性最稳。如果你用的是 Ubuntu 22.04需要自己处理很多依赖兼容问题不建议初学者碰。第二是 Gazebo 版本。Ubuntu 20.04 对应的是 Gazebo 11XTDrone 完全兼容。安装完以后一定要先跑一次gazebo --version确认版本后面很多奇怪的问题都出在 Gazebo 版本不对上。第三是 PX4 固件的编译。XTDrone 会拉取对应版本的 PX4 固件源码编译时间比较长我建议用make px4_sitl gazebo之前先确认依赖装全了否则编译到一半报错很浪费时间。具体来说要确保装了python3-jinja2、python3-numpy、python3-tox、ninja-build等工具这些都是编译 PX4 时常用的。安装完成后建议执行一遍 XTDrone 自带的check_env.sh脚本它会帮你检查 ROS 环境、PX4 环境、Gazebo 环境是否都配置正确。这一步很多人会跳过但我强烈建议跑一下它能提前暴露 80% 的环境问题。2.2 FAST-LIO2 编译时需要留意的依赖问题FAST-LIO2 的编译本身不复杂一个 ROS 功能包几个依赖库但坑就坑在依赖库版本上。首先需要确认已经安装了 PCL点云库和 Eigen3。Ubuntu 20.04 系统自带的版本分别是 PCL 1.10 和 Eigen 3.3.7这个组合编译 FAST-LIO2 没问题。但如果你之前因为其他项目升级过 Eigen 或者编译过更高版本的 PCL就很容易出现头文件路径冲突或者 ABI 不兼容的问题。另外一个比较隐蔽的坑是OpenCV。FAST-LIO2 本身不依赖 OpenCV但如果你在同一个 ROS 工作空间里编译其他功能包时需要 OpenCV版本冲突就会冒出来。我的建议是给这个项目单独建一个工作空间比如catkin_ws_fastliox避免和现有的 ROS 功能包混在一起。这个习惯帮我避免了很多玄学问题。编译的时候直接用catkin_make或者catkin build都可以我个人推荐catkin build它在增量编译和错误提示方面比catkin_make友好一些。编译完记得source devel/setup.bash否则终端找不到节点。2.3 ego-planner 的编译与仿真环境搭建ego-planner 的源码在 GitHub 上有两个版本一个是可以直接编译运行的 ROS 功能包另一个是用于仿真测试的完整环境。后者里面已经包含了一个简单的四旋翼仿真器但我们这里不需要它因为我们要用 XTDrone 的 Gazebo 环境来替代。编译 ego-planner 时需要注意的是它依赖plan_env、path_searching、traj_opt、bspline这几个子模块如果用git clone拉代码一定要加--recursive参数否则子模块是空的编译直接报错找不到头文件。这是我见过最多的低级错误。ego-planner 还依赖nlopt库这个不能通过apt-get直接装到正确位置需要源码编译安装。编译 nlopt 时注意要启用 C 接口支持否则后面编译 traj_opt 时会报找不到nlopt.hpp头文件。具体操作是在 cmake 时加-DNLOPT_CXXAPION参数。还有一点ego-planner 的原始代码是基于 ROS Melodic 写的如果用的是 Noetic编译时可能会遇到一些boost::shared_ptr或者 C 标准版本相关的报错。解决办法是在 CMakeLists.txt 里加上set(CMAKE_CXX_STANDARD 14)这个改动能让绝大多数 Noetic 下的编译问题迎刃而解。3. 核心配置与话题对接方案3.1 坐标系关系必须先从 TF 树理清楚在 ROS 系统里做多传感器融合和多算法级联坐标系问题永远是最容易翻车的地方。FAST-LIO2 和 ego-planner 对接时核心涉及三个坐标系雷达坐标系livox_frame、机体坐标系base_link、世界坐标系map 或者 odom。FAST-LIO2 在启动时会自己广播一个从livox_frame到camera_init也就是它定义的世界系的 TF。这里有个关键点FAST-LIO2 假定雷达坐标系和机体坐标系是重合的或者至少在初始化时是同一个位置。XTDrone 的车辆模型里激光雷达一般安装在机体中心附近所以这个假设基本成立不需要额外做外参标定。但 ego-planner 订阅里程计时它默认里程计提供的位姿是从机体坐标系到世界坐标系的变换。这里如果直接把 FAST-LIO2 输出的/Odometry喂给 ego-planner理论上没问题因为 FAST-LIO2 的里程计本身就是机体在世界系下的位姿。实际操作中我习惯增加一个tf中转节点显式地把camera_init帧映射到map帧同时把机体坐标系映射到base_link。这样做的目的是避免后续加其他算法模块时不同节点对坐标系命名不一致导致 TF 树断裂。如果你发现 Rviz 里点云和无人机模型“错位”了第一反应不要怀疑算法先检查 TF 树。用rosrun tf view_frames生成 TF 树图一眼就能看出哪条边断了、哪个坐标系被重复广播了。这个问题排查工具比看日志高效得多。3.2 话题链路点云、IMU、里程计、轨迹四条主线整个系统跑起来之后话题流主要分四条线把这四条线理清楚系统结构就清晰了。第一条是激光雷达线Gazebo 里的雷达插件发布/livox/lidar原始点云话题FAST-LIO2 的livox_repub节点会把它转换成 FAST-LIO2 需要的话题格式。如果雷达模型是 Velodyne 类型需要改 FAST-LIO2 配置里pointcloud_topic参数为对应的 Velodyne 话题名。第二条是 IMU 线Gazebo 发布/imu数据FAST-LIO2 的imu_topic参数直接指向它。这里要注意 IMU 的频率和噪声参数需要和真实传感器匹配否则滤波估计出的姿态会漂。XTDrone 自带的 IMU 仿真参数已经比较合理可以直接用。第三条线是状态估计线FAST-LIO2 经过迭代卡尔曼滤波后输出/Odometrynav_msgs/Odometry 类型和/cloud_registeredsensor_msgs/PointCloud2 类型。前者给 ego-planner 的odom话题后者给 ego-planner 的地图输入话题。第四条线是控制输出线ego-planner 规划出的轨迹经过位置控制转换发布速度或者位置指令给 PX4 飞控。这里我采用的是 XTDrone 自带的px4_pos_controller节点它把轨迹指令转换成 PX4 的offboard模式控制指令实现自主飞行。一个实用的排查技巧是用rqt_graph看节点和话题的连线图一旦某个话题没有连接上图里会非常直观地显示出来。这比盯着终端日志猜问题效率高很多。3.3 参数配置里最影响效果的几个关键项FAST-LIO2 和 ego-planner 的参数都不算多但有几个是真正影响效果的我在调参时花了不少时间总结如下。FAST-LIO2 侧最重要的是filter_size_surf体素滤波大小和max_iteration卡尔曼更新最大迭代次数。体素滤波大小直接影响建图精度和计算量XTDrone 仿真的点云密度比真机低一些我把它设为 0.5 米左右效果较好。太大了地图会变得粗糙影响后续避障精度太小了计算负载高实时性受影响。max_iteration默认是 3对于室内慢速飞行场景够用室外高速飞行可以适度增加到 5。ego-planner 侧影响最大的是这几个参数max_vel最大速度、max_acc最大加速度、feasibility_threshold可行性阈值、map_size局部地图尺寸。max_vel和max_acc直接决定了无人机飞行的激进程度。XTDrone 仿真的 PX4 四旋翼模型默认动力学参数比较保守我把max_vel设为 3 m/smax_acc设为 2 m/s²这个组合在仿真里又稳又快避障响应也来得及。如果你发现轨迹规划出来飞不动大概率是这两个参数超过了无人机模型的实际能力PX4 的控制器会强行限制输出。map_size决定了 ego-planner 用它感知范围内的多少点云来构建 ESDF。太大计算慢太小避障不及时。我试了几组值局部地图尺寸设为 10m×10m×3m 比较合适既能覆盖雷达的有效感知范围又能保证每轮规划的计算耗时控制在 30ms 以内。另外不要忽视odom_topic的时间戳同步问题。FAST-LIO2 输出的里程计的时基和 Gazebo 仿真时间如果不一致ego-planner 计算轨迹时会出现时间跳变表现为无人机飞行时一卡一卡甚至失控。我的解决办法是在启动 ego-planner 前先确认 Gazebo 的/clock话题有没有发布以及use_sim_time参数是否被正确设置为true。这个细节看起来不起眼但 80% 的仿真时序问题都跟它有关。4. 实操过程与效果调优4.1 完整启动流程记录整个系统的启动有一套固定的先后顺序顺序一旦乱了就会出现各种奇奇怪怪的问题。我整理了一份我自己用下来最稳定的启动流程。第一步启动 Gazebo 仿真环境和 PX4 固件。XTDrone 提供了一键启动脚本执行之后会加载无人机模型和周围环境同时启动 PX4 的 SITL 仿真。确认 Gazebo 加载完成、无人机模型出现在正确位置后再进入下一步。这一步有个判断标准Gazebo 窗口右下角的“实时因子”显示为 1.0说明物理仿真已经稳定。第二步启动 MAVROS 和通信节点。这一步的作用是让 ROS 世界和 PX4 世界建立联系。XTDrone 里有对应的 launch 文件直接执行即可。启动完后用rostopic echo /mavros/state确认连接已建立能看到connected: True的输出。第三步启动 FAST-LIO2。执行它的 launch 文件之前先确保已经把配置文件里的点云话题、IMU 话题、以及雷达类型改成了和 XTDrone 对应的话题名称。启动后观察终端输出如果数字能正常递增说明里程计解算已经在运行。这时候打开 Rviz添加 PointCloud2 显示选择/cloud_registered话题应该能看到激光雷达扫描周围环境形成的点云地图在逐渐累积。第四步启动 ego-planner 的规划节点。启动后再开一个新终端运行 Rviz 的可视化配置能实时看到规划出的 B 样条轨迹以及对应的速度、加速度曲线。第五步也是最后一步给飞控发送起飞和自主飞行指令。XTDrone 自带了一个键盘控制脚本可以切换到 offboard 模式并发送起飞命令。在 ego-planner 正常运行的前提下起飞后给它一个目标点它就会自动规划并执行飞行。这套流程我跑了几十次稳定性非常高。如果你在某个环节卡住了建议回到对应步骤检查不要往后续步骤推进因为后续的现象会把真实问题掩盖掉。4.2 避障效果验证与关键性能指标系统跑起来之后怎么量化验证效果我一般关注三个核心指标规划耗时、轨迹跟踪误差、避障成功率。规划耗时直接看 ego-planner 节点终端的输出正常情况下单次规划应该在 10-50ms 之间。如果经常超过 100ms说明局部地图太大或者点云体素太密需要调参数。轨迹跟踪误差可以通过对比 ego-planner 发布的目标轨迹和 PX4 反馈的实际位置来评估一般建议在 0.3 米以内超过了就说明速度参数设置过于激进或者控制环跟不上。避障成功率需要设计测试场景。XTDrone 里有现成的树林环境和城市环境你可以把起点和终点放在障碍物的两侧看无人机能不能在没有人工干预的情况下飞过去。我测试时增加了几个自定义障碍物用 Gazebo 的模型编辑器往环境里放几个柱子测试避障的灵敏程度。我发现当飞行速度在 2 m/s 左右时ego-planner 对 1 米以外的障碍物基本都能提前规划出绕行轨迹响应非常及时。还有一个值得看的指标是 FAST-LIO2 的里程计漂移。在仿真里没有真值也可以做参考让无人机悬停 60 秒观察里程计位置输出是否稳定在一个小范围内。正常情况下漂移应该在 10 厘米以内超过这个值说明雷达点云配准有问题或者 IMU 参数设置不合理。4.3 从仿真到真机迁移时需要注意的差异点XTDrone 里跑通这套系统之后下一步自然是想往真机上迁。仿真和真机的差异不容小觑提前了解这几个差异能少走很多弯路。传感器噪声是第一大差异。仿真里的 IMU 数据是理想化的高斯噪声真机上还有温漂、振动、非线性误差。FAST-LIO2 对 IMU 噪声参数非常敏感迁到真机时imu_noise和imu_gyro_noise这两个参数必须根据真实传感器手册重新设置否则滤波发散的概率极大。雷达点云质量是第二大差异。仿真里激光雷达测距无误差真机上会有玻璃、黑暗表面、金属反光等导致测距噪点。这些噪点会让 FAST-LIO2 的配准产生误差也会让 ego-planner 错识别出虚假障碍物。迁真机时建议在 FAST-LIO2 的配置里开启异常点滤除filter_off这项要设置合理在 ego-planner 侧也可以考虑增加一个点云缓存过滤节点把孤立噪点去掉。坐标系对齐是第三大差异。真机安装雷达和 IMU 时会有安装偏差必须做外参标定。FAST-LIO2 的配置文件里提供了extrinsic_T和extrinsic_R参数需要用标定工具测量后填入。仿真里默认外参是单位阵这一步在真机上不能省。任务规划和全局路径也是需要补的一环。ego-planner 是局部规划器它不知道目标点在哪、怎么绕过大范围障碍区。仿真测试时我们是用键盘给定目标点真机运行时需要一个全局规划节点比如 RRT* 或者 A*先规划全局路径再让 ego-planner 沿全局路径做局部避障跟踪。XTDrone 里已经有path_searching模块可以复用其中的全局规划能力。5. 常见问题与排查技巧实录5.1 启动阶段的典型问题速查部署过程中遇到的启动类问题我整理了一张表基本覆盖了最常出问题的几个地方。问题现象可能原因解决办法launch 文件启动后节点立即退出配置文件里话题名称写错rostopic list查看实际话题名并修改配置Rviz 收不到点云话题FAST-LIO2 未正确启动检查终端日志是否有“Segmentation fault”字样终端报“找不到坐标系 livox_frame”TF 广播未启动确认 FAST-LIO2 节点是否正常运行并发布 TFPX4 无法切换到 offboard 模式MAVROS 连接断开重启 MAVROS检查飞控仿真状态Gazebo 加载模型后无人机下沉PX4 与 Gazebo 仿真时序不同步重启整个环境等待实时因子稳定到 1.0 再操作这里面最阴间的是第一种情况launch 文件启动后节点立刻退出但终端还不报错给人感觉一切正常。排查方法就是看rostopic list里有没有对应节点的输出话题没有就说明节点没起成功。再去看节点的 log 文件一般在~/.ros/log目录下找到对应节点的日志尾部能看到具体的退出原因。5.2 运行过程中遇到的算法层问题启动成功但运行不正常的问题比启动问题更难排查我在测试时遇到过坐标漂移、避障失败、规划超时三个典型问题分别说下我的排查思路。坐标漂移的表现是无人机明明悬停着但 Rviz 里点云地图在缓慢移动或者旋转。第一反应查 IMU 话题的数据频率和数值范围。XTDrone 里 IMU 发布频率通常在 200Hz 左右数值范围在 ±4g 和 ±2000°/s 之内。如果发现 IMU 数据异常大概率是话题订阅错了比如订阅了另一个无人机的 IMU。另一个原因是 FAST-LIO2 的初始姿态没收敛需要让无人机在地面静止 10 秒左右再启动 FAST-LIO2让它完成初始化。避障失败的表现是无人机朝着障碍物直飞过去完全不管规划轨迹。优先检查 ego-planner 是否真的收到了局部地图点云。我在调试时见过一个情况ego-planner 内部订阅的是经过自定义过滤的/map_generator/global_cloud话题而 FAST-LIO2 发布的是/cloud_registered两者没接上规划器一直在“空地图”模式下运行自然看不到障碍物。后来加了一个pointcloud_to_map中转节点才搞定。规划超时的表现是无人机悬停在原地不动终端高频打印“规划失败”或者“不满足动力学约束”。这种情况八成是目标点设置超出了无人机的可达范围或者max_vel设置过小导致轨迹无法在给定时间内完成。把目标点拉近一点、max_vel调大一点问题往往就能解决。5.3 独家避坑经验与调参心得最后分享几个我认为最值得记下来的实战经验这些是常规文档里不会写的东西。第一个经验给 FAST-LIO2 单独建一个 ROS 工作空间不要和其他功能包混在一起。这个在前面提过但值得再强调一次。因为这个项目涉及的依赖库多且版本敏感混在一起编译时一个包的 CMake 配置改变就可能影响另一个包的编译结果。分开放在两个工作空间里互不干扰出问题了重编也快。第二个经验在 XTDrone 里调试规划效果时先把速度调慢。把max_vel设为 1.0 m/smax_acc设为 1.0 m/s²先把整条链路跑通确认避障逻辑正确后再逐步把速度往上加。直接上高速容易让问题变得难以定位到底是规划器有问题还是控制环跟不上低速跑通能保证每一层逻辑都是正确的后面提速只是参数的线性调整。第三个经验善用 Rviz 的测量工具。ego-planner 生成的轨迹有时候看起来离障碍物很近这个时候用 Rviz 顶部的“Measure”工具量一下轨迹点到点云障碍物的实际距离往往能发现距离比视觉上感觉的要大得多。因为点云是离散的轨道和点云最近点之间可能刚好没有点视觉上会造成“轨迹穿墙”的错觉。这个工具能帮你判断到底是真碰撞还是视觉误差。第四个经验修改任何一个参数之后重启对应节点不要用动态调参工具热更新。ego-planner 的很多参数在初始化时就预分配了内存热更新虽然能改数值但内部缓存结构不会重建表现为参数改了但行为没变甚至变奇怪。重启节点成本很低别省这一步。这套系统跑通之后我对“感知-规划-控制”闭环的理解比看书深刻得多。以前看论文里那些符号公式总觉得隔着一层纱真正把 FAST-LIO2 的输出接进 ego-planner 的输入看到 Rviz 里那条 B 样条轨迹自己绕开障碍物、无人机跟着轨迹稳稳飞过去的时候那些公式里的每一个变量都有了具体的物理意义。如果你也在折腾类似的部署希望这篇能帮你少走几个弯路有问题欢迎交流。