从仿真真值到传感器里程计：XTDrone上打通FAST-LIO2与ego-planner的完整部署记录

相信不少朋友和我一样，第一次在XTDrone里跑通ego-planner官方demo时，心里是既兴奋又心虚。兴奋的是B样条轨迹那个丝滑程度确实赏心悦目，心虚的是——它用的定位信息是Gazebo仿真里的真值(ground truth)，而不是传感器里程计。说白了，这种"开了天眼"的规划闭环，拿到真机上根本没有参考价值。所以我一直在琢磨一件事：如果往ego-planner前面接一个FAST-LIO2，用纯激光惯性里程计算法来喂状态估计，整个路径规划系统会变成什么样？会出现哪些幺蛾子？

这篇博文就是这次折腾的完整记录。我把XTDrone、FAST-LIO2、ego-planner三套东西从环境搭建、话题对接、坐标系对齐到动态避障实测的每一步都捋了一遍，包括中间踩过的坑和排查思路。文章面向的是有一定ROS基础、想在仿真环境里做"感知-定位-规划"完整闭环的开发者，如果你正好卡在不知道如何把FAST-LIO2的里程计接进ego-planner，或者想知道这套组合在动态障碍物场景下到底行不行，那这篇内容应该能帮你省下一整周的查资料时间。

1. 先搞清楚三套东西各自干什么，以及为什么要组合

在动手部署之前，我觉得有必要先把这三样东西的边界说清楚。很多人一上来就急着装环境，结果装到一半就崩了，根本原因不是操作问题，而是没有理解每个组件在整个链路里的位置和依赖关系。

1.1 FAST-LIO2、ego-planner和XTDrone各自的角色

XTDrone是一个基于PX4和Gazebo的开源无人机仿真平台，它最大的价值在于提供了一个接近实机环境的"数字靶场"：有动力学模型、有传感器仿真、有PX4飞控的软件在环接口。你可以把地图、机型和任务场景都在里面铺开，算法验证的成本比真机低得多。

FAST-LIO2是港大MaRS实验室开源的一个激光惯性里程计算法，核心思想是把IMU预积分和激光点云配准紧耦合在一起，通过ikd-Tree这种增量式数据结构来管理地图点，从而做到较低的计算开销和较高的精度。它输出的东西是机器人当前的位姿估计——在仿真里你可以理解成用"肉眼"（激光雷达）去推算自己在哪，而不是直接问仿真器要答案。

ego-planner则是同一实验室出品的无人机局部路径规划器，特点是不依赖ESDF建图，直接用B样条曲线在感知空间里做轨迹优化，并且能在规划过程中持续检测动态障碍物并做重规划。它需要输入当前位姿和目标点，输出一条安全、平滑、动力学可行的轨迹。

这三者的关系可以这么看：XTDrone是舞台，FAST-LIO2是"我从哪里来"的答案来源，ego-planner是"我要往哪里去"的决策引擎。单独用后两者都没有问题，但把它们串起来，才是一个真正可以从仿真过渡到实机的最小闭环。

1.2 为什么非要用里程计替代真值

我确实见过一些人，在XTDrone里做ego-planner验证时直接用仿真真值，理由是"反正算法验证只关注规划效果"。这个理由在纯规划算法层面说得通，但一旦你的目标是把这套系统搬到实机上，问题就来了。

真值在仿真里是零延迟、零噪声、零漂移的。而FAST-LIO2这类传感器里程计，天然存在延迟、有累计漂移、偶尔还会因为点云退化（比如无人机飞过一面白墙）而突然跳变。ego-planner本身并不关心定位信息是怎么来的，它只关心你给它的odometry话题是否稳定、是否有足够的更新频率。但规划效果对定位噪声非常敏感：位姿抖动会导致轨迹反复重规划，位姿跳变可能导致规划器以为自己撞上了障碍物，直接触发急停甚至失控。

所以，在仿真里把FAST-LIO2接进ego-planner，本质上是在提前暴露"定位不确定性对规划系统的影响"这类实机才会遇到的问题。你可以在仿真里把这些问题调明白了，再上真机，成本低太多。

2. 环境准备里的版本陷阱：别在最开始就埋雷

这套组合对环境版本极其挑剔。我在第一次部署时，就因为在Ubuntu版本和ROS版本上做了一个自认为"稳妥"的选择，结果在编译FAST-LIO2时折腾了两天。这里把版本选型和初始化要点一次性说清楚。

2.1 版本矩阵选择和我的推荐组合

XTDrone官方虽然支持不同环境，但在实际使用中，ROS Melodic + Ubuntu 18.04和ROS Noetic + Ubuntu 20.04是两条主流分支。FAST-LIO2官方仓库对ROS1支持比较成熟，ego-planner源码对Noetic的兼容性也不错，所以我的推荐组合是Ubuntu 20.04 + ROS Noetic + PX4 1.11.x。

如果你的机器上已经装了Melodic的XTDrone，我的建议是重新配一台干净的Noetic环境，而不是尝试共存。因为XTDrone本身依赖的PX4和Gazebo版本跟ROS版本是绑定的，混用容易在rosdep环节出现依赖冲突，而且这种问题排查起来非常恶心，往往是你改了A的依赖，B又开始报错。

# 推荐环境配置 sudo apt install ros-noetic-desktop-full # XTDrone依赖的gazebo和px4，按官方脚本走即可 # ROS Noetic 默认Python3，注意不要用Python2的ros包

2.2 编译顺序：从下往上，逐层验证

环境装好后，我的习惯是严格按照"仿真平台 -> 里程计 -> 规划器"的顺序编译，每装一个就必须跑通一个，绝不往后推进。具体顺序如下：

1. 先装XTDrone并跑通自带的通信示例，确保Gazebo里的无人机能起飞、能接收指令。
2. 再编译FAST-LIO2及其依赖（livox_ros_driver或velodyne驱动），用XTDrone自带的激光雷达话题做输入，确认里程计输出频率正常。
3. 最后编译ego-planner，先不改任何代码，直接跑它自带的demo确认规划器在原生仿真里能工作。

这个顺序的逻辑在于：每一步的输入都是上一步的输出，如果顺序颠倒或者跳步，出了问题你根本不知道是上一步的接口没打通，还是这一步的代码有bug。我见过太多人一次性把三个workspace编译完，然后启动时全部GDB都上阵也找不到问题，最后发现是FAST-LIO2根本没订阅到点云话题——这种错误本该在第二步就暴露的。

2.3 编译时常见的坑和我的处理习惯

编译FAST-LIO2时，有几个坑是高频出现的：一是PCL版本不匹配导致pcl::PointCloud<PointType>的编译报错，二是Eigen版本太老或太新导致模板推导失败。我的建议是，直接用sudo apt install libeigen3-dev装系统版本，不要自己下载源码编译高版本Eigen，否则容易和ROS自带的版本冲突。

另外，google的glog和gflags在Ubuntu 20.04上如果没装，FAST-LIO2编译时会在链接阶段报一堆undefined reference错误。这个错误比较隐蔽，因为编译前的CMake检查不会提醒你。

sudo apt install libgoogle-glog-dev libgflags-dev

ego-planner的编译相对温和，但它依赖plan_env和path_searching两个子模块，如果没有用--recursive克隆，编译时会直接报找不到头文件。如果你看到fatal error: plan_env/... no such file or directory，不用怀疑，就是子模块没拉全。重新执行一次完整克隆：

git clone --recursive https://github.com/ZJU-FAST-Lab/ego-planner.git

3. 在XTDrone里让FAST-LIO2先"睁开眼睛"

在ego-planner介入之前，我们要确保FAST-LIO2能在XTDrone的仿真环境里稳定跑起来，输出可信的里程计。这个阶段是最枯燥但其实最有意思的部分——因为你实际上是在和仿真器里的传感器数据较劲。

3.1 激光雷达和IMU话题怎么找、怎么确认

XTDrone的机型配置不同，激光雷达话题名也可能不一样。我这次使用的是带Velodyne仿真插件的机型，点云话题是/velodyne_points，IMU话题是/imu/imu。但有些版本用的是Livox模型的gazebo插件，点云话题就是/livox/lidar。先不要盲信任何教程，直接用命令确认：

rostopic list | grep -E "points|imu|odom" rostopic hz /velodyne_points rostopic hz /imu/imu

rostopic hz的输出非常关键。点云频率至少要保证在10Hz以上，IMU频率在100Hz以上，FAST-LIO2的位姿估计才会稳定。如果仿真里点云只有5Hz，后面ego-planner重规划时会明显感觉轨迹卡顿，因为里程计的更新率不够，规划器在两次更新之间只能在"盲飞"。

另外一个容易忽略的点是点云的类型。Velodyne仿真一般发布的是PointCloud2，而FAST-LIO2里默认的FeatureExtraction节点订阅的也是PointCloud2，这一步通常没问题。但如果你用的是Livox驱动，它发布的可能是自定义的livox_ros_driver/CustomMsg类型，这种情况就必须改FAST-LIO2的launch文件，换成对应的点云类型解析。很多人在这一步卡住，本质上就是"话题名对了，消息类型没对上"。

3.2 FAST-LIO2的launch参数怎么改

FAST-LIO2的launch文件里，我要改的主要是以下几个方面：

1. 点云话题名，从默认的/livox/lidar改成/velodyne_points。
2. IMU话题名，改成/imu/imu。
3. 外参文件路径，指向你当前机型对应的外参yaml。
4. 雷达类型和畸变补偿开关，Velodyne仿真一般不开畸变补偿，但实机建议打开。

<param name="pointcloud_topic" value="/velodyne_points"/> <param name="imu_topic" value="/imu/imu"/> <param name="config_file" value="$(find fast_lio)/config/velodyne.yaml"/>

关于外参文件，这里有个很重要的逻辑要理解：FAST-LIO2里IMU到激光雷达的外参，描述的是IMU坐标系和雷达坐标系之间的刚体变换。在XTDrone的仿真中，这两个传感器在模型里的相对位置是固定的，通常可以从urdf或sdf文件里查到。如果外参填错，最典型的表现是里程计轨迹发飘，地图点云出现重影或错位。

我在第一次配置时，外参文件里用的是默认值（单位米），但XTDrone模型里雷达和IMU的安装位置是用厘米表述的，一换算就错了。导致里程计输出的轨迹虽然形状对，但有明显的尺度漂移，而且越飞越偏。这个坑如果你没遇到过一定不会往外参方向想，但遇到过的朋友应该都懂——一个数值差了100倍，整个系统就废了。

3.3 验证里程计是否可信的土办法

里程计跑起来后，不要急着接ego-planner，先在Rviz里观察轨迹和后端地图。一个简单有效的验证方法是：手动给无人机一个悬停指令，然后观察FAST-LIO2输出的odom话题在x、y、z三个轴上的值是不是稳定在一个很小的范围内。

如果悬停时位置漂移超过0.1米，那说明点云配准有问题或者IMU数据质量不行，继续往下接ego-planner只会让问题更严重。如果漂移在几厘米以内，恭喜你，传感器里程计的精度够用了。

这里我强烈建议把里程计数据保存一份录包（rosbag），后面调ego-planner参数时可以反复回放，不用每次都重新跑仿真。我调系统中遇到规划异常时，就是靠回放之前的bag，先在离线状态下定位是定位问题还是规划问题，效率高很多。

4. 把FAST-LIO2的里程计接进ego-planner：坐标系与话题重映射

现在到了整个部署过程的核心环节——让ego-planner用上FAST-LIO2的里程计。这一步表面上只是改一个订阅话题名，实际上背后牵扯到坐标系对齐和TF树重构，不少人的项目就是砸在了这里。

4.1 ego-planner原生demo里用的是哪一路odom

ego-planner的demo里，规划器的odometry话题来自仿真真值（通常是/vins_fusion/odometry或者/Odometry这种由gazebo插件直接发布的位姿）。这个真值的话题频率高、无噪声，ego-planner跑起来当然顺畅。

现在我们要做的是把/Odometry真值话题替换成FAST-LIO2输出的/Odometry（注意fast_lio默认发布的话题名为/Odometry，两个话题同名，容易混淆）。不改代码、只改launch文件就行。在ego-planner的launch文件里找到planning相关的节点，把odometry话题改成FAST-LIO2发布的话题。

<remap from="/odom" to="/Odometry"/> <!-- 或者直接把ego_planner_node里订阅odom的参数改成fast_lio的话题 --> <param name="odometry_topic" value="/Odometry"/>

这里最容易犯的错误是：忘了把FAST-LIO2发布的话题名和ego-planner订阅的话题名做映射，结果Rviz里明明能看到里程计数据，规划器却一动不动，日志里全是"wait for odometry"的提示。

4.2 坐标系对齐：map、odom、body、camera_init一锅粥怎么理

坐标系是这套系统最大的隐形杀手。在ego-planner的规划模块中，它默认无人机当前位置在map坐标系原点附近，所有规划和避障都在map坐标系下进行。而FAST-LIO2的输出，默认在它自己定义的camera_init坐标系下，这个坐标系通常取的是第一帧IMU的位置和朝向，相当于里程计世界系。

如果你把这些坐标系直接混用，最直观的表现是：ego-planner认为自己在地图原点，但FAST-LIO2告诉你无人机在(10, 20, 3)的位置，规划器会疯狂地尝试"回到原点"，无人机飞出一个诡异的弧线。

解决办法有两条路：

• 第一条路：在启动ego-planner之前，让FAST-LIO2先把第一帧位姿初始化在地图原点，然后发送一个静态TF变换，把camera_init对齐到map。
• 第二条路（我用的方案）：写一个极简的TF中继节点，把FAST-LIO2发布的camera_init到body的TF重新广播为map到base_link的TF，然后在ego-planner的launch里把规划器的坐标系参数设置为map。

代码如下，不到三十行，但解决了我一整个下午的坐标系问题：

#!/usr/bin/env python3 import rospy import tf2_ros from geometry_msgs.msg import TransformStamped class TfRelay: def __init__(self): self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.tf_broadcaster = tf2_ros.TransformBroadcaster() rospy.Timer(rospy.Duration(0.05), self.timer_callback) def timer_callback(self, event): try: trans = self.tf_buffer.lookup_transform('camera_init', 'body', rospy.Time(0), rospy.Duration(0.1)) t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = 'map' t.child_frame_id = 'base_link' t.transform = trans.transform self.tf_broadcaster.sendTransform(t) except Exception: pass if __name__ == '__main__': rospy.init_node('tf_relay') TfRelay() rospy.spin()

这个节点的逻辑很简单：订阅TF树里camera_init到body的变换，然后重新以map为父坐标系广播出去。因为FAST-LIO2初始化时camera_init就约等于规划器的初始位置，所以这个"欺骗"在起始点附近是完全等价的，而且保证了ego-planner不用改任何内部代码。

如果你用的是Rviz，还有一个细节：Rviz里的Fixed Frame要设置成map，否则你看到的轨迹和障碍物点云会全部分离，那种画面很像灵异事件，但其实就是坐标系没对上。

4.3 话题频率和带宽的匹配逻辑

FAST-LIO2的输出频率通常可以到10-20Hz，ego-planner对odometry的订阅频率要求不高，一般8Hz以上就能稳定运行。但我发现，在配置较低的电脑上跑XTDrone + FAST-LIO2 + ego-planner时，CPU占用往往接近满载，里程计频率会掉到5Hz以下，这时候ego-planner的重规划会有明显延迟，动态避障基本成了摆设。

如果你是这种情况，我建议先砍Rviz的显示负担（比如关闭PointCloud2显示，只显示路径和位置），然后看FAST-LIO2的后端频率是否恢复正常。仿真环境的性能瓶颈通常不是算法本身，而是可视化渲染吃掉了太多资源。

5. 动态避障实测：从"能飞"到"会躲"的调参过程

系统能飞起来之后，真正的考验才开始——动态避障。XTDrone里放几个静态障碍物，ego-planner轻松绕过，这不算什么本事。只有当障碍物开始移动，或者突然出现在路径前方时，规划器的实时反应能力和参数鲁棒性才会真正暴露。

5.1 在Gazebo中布置动态障碍物

在XTDrone里加动态障碍物，最省事的方法是直接在Gazebo里导入一个带运动插件的小车模型，或者用脚本控制它的位置。我用的办法是写一个普通Python节点，周期性地发布/dynamic_obs小车模型的model_state，让它以1m/s的速度横穿无人机的预定航线。

from gazebo_msgs.srv import SetModelState from gazebo_msgs.msg import ModelState from geometry_msgs.msg import Pose, Twist import rospy def move_obstacle(): rospy.wait_for_service('/gazebo/set_model_state') set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) rate = rospy.Rate(50) state = ModelState() state.model_name = 'obstacle_car' state.pose.position.x = 5.0 state.pose.position.y = -2.0 state.pose.position.z = 0.0 state.twist.linear.x = 1.0 # 让它沿x方向移动 while not rospy.is_shutdown(): set_state(state) rate.sleep() if __name__ == '__main__': rospy.init_node('dynamic_obstacle_node') move_obstacle()

Gazebo里模拟动态障碍物的好处是，障碍物模型会被仿真激光雷达真实感知到，点云会实时变化，这对FAST-LIO2和ego-planner的整套感知链路都是真实压力测试，不是那种直接在代价地图里塞一个假障碍的"作弊式避障"。

5.2 关键参数调整：感知范围、最大速度、离障碍距离

在动态避障实测中，有几个参数对效果影响最大，我一个个说自己的实测感受：

perception_range（感知范围）。ego-planner需要在一定范围内接收点云或代价地图信息。如果这个值设得太小，障碍物进入规划器视野时已经太近了，重规划来不及；如果设得太大，计算量又上去了，而且远处点云的噪声会导致误判。我实测下来，在XTDrone的标准地图里，感知范围设置在8-10米比较合理，既保证了对动态障碍物的提前感知，又不会让CPU爆掉。

max_velocity（最大速度）。这个参数值决定了B样条轨迹优化时的动力学约束上限。如果设成仿真无人机的极限速度，那么一旦前方突然出现障碍物，规划器能做出的最大反应是刹不住车的——因为轨迹优化器在保证安全距离和满足速度约束之间必须找平衡，速度越高，安全距离就需要越大。我把最大速度从默认的4m/s降到2.5m/s后，动态避障的成功率显著提升。

obstacle_distance（期望离障碍安全距离）。作为规划器的优化目标之一，它控制生成轨迹时对障碍物保持的期望距离。这个值设得越大，轨迹越保守，绕飞幅度越大；太小则会出现贴脸飞过的惊险画面。我建议针对动态障碍物，把安全距离设到0.8米以上，给定位噪声和规划延迟留出余量。

5.3 我实测最典型的一个失败场景

说了这么多参数，还是讲一个具体的失败案例比较直观。我测试的场景是：无人机以2m/s飞向目标点，一个障碍物以1m/s从侧面横向穿过航线。一开始我不改任何参数直接跑，结果无人机在障碍物距离还有3米时猛地刹车，悬停了一秒，然后又加速——两次重规划之间有明显停顿，最终虽然没有撞上，但轨迹非常难看，而且无人机有明显的前后震荡。

这个问题的根源在于：FAST-LIO2的里程计有轻微延迟，ego-planner接收到障碍物点云时，障碍物其实已经比点云显示的位置更往前移动了一段距离。规划器按"看到的"障碍物位置规划了一条躲闪轨迹，但等无人机飞过去时，障碍物已经在另一个位置了，于是又要重新规划。两个环节的延迟叠加，就造成了震荡和急停。

我把感知范围从8米调到了10米，同时把max_velocity从4m/s降到2.5m/s后，问题基本消失。背后的逻辑很简单：感知范围变大，让规划器更早"看见"障碍物；速度降低，让规划器有更多时间做优化和重规划。这套"看得更早、飞得稍慢"的组合拳，在仿真和实机上都是最有效的动态避障保险策略。

6. 高发问题排查链路：从现象倒推根因

部署这套系统，遇到问题不可怕，可怕的是没有排查思路。这里我把几个高频问题整理成一张排查表，每个问题都给出完整的排查链路，照着做可以省掉大量试错时间。

我重点展开讲一个最难排查的坑：FAST-LIO2初始化位置不在原点导致规划器路径偏移。这个问题的表象是无人机起飞后，ego-planner规划的目标点看起来是偏的，但TF树、话题、频率全都没问题。最后我在FAST-LIO2的日志里发现，它的camera_init（里程计原点）是在x=1.2, y=-0.8的位置初始化的，因为无人机在Gazebo里摆放的位置就不在(0,0,0)，FAST-LIO2默认把开机瞬间的位置作为原点。而ego-planner认为起点是(0,0,0)，两个坐标系之间就有一个固定的平移偏差。

解决办法是在FAST-LIO2启动前，先通过PX4的指令把无人机飞到(0,0,1)附近再启动fast_lio，确保里程计原点和规划器原点尽量一致。如果你希望更工程化的做法，应该写一个动态TF补偿节点，读取FAST-LIO2初始化的偏移量，叠加到发布给ego-planner的TF上。这个动态补偿方案在实机上更加通用，因为实机开机位置不可能每次都精确停在地图原点。

7. 从XTDrone仿真到真机和外设扩展的参考价值

最后我想聊一下这套系统的延展性。很多人做仿真部署时容易陷入一个误区：只为了跑通demo而跑通demo，仿真里的东西到了真机全作废。但XTDrone + FAST-LIO2 + ego-planner这套组合的价值，恰恰在于它的接口设计已经朝真机靠拢了——换掉仿真传感器输入，接上真实激光雷达和IMU，规划部分几乎可以原封不动地迁移。

如果你是做机器人路径规划方向，这个部署经验的复用性就更强了。把FAST-LIO2的里程计输出给任何移动机器人底盘（包括差速小车、阿克曼底盘），再搭配适合底盘的局部规划器，本质上就是同一套感知-规划闭环。我在测试完无人机动态避障后，试着把这套里程计接入一个仿真小车的move_base框架，除了控制指令话题不同，前面定位和坐标系处理的思路完全一致。

对于多机器人路径规划的场景，这套系统的模块化优势也非常明显：每个机器人可以运行自己的FAST-LIO2里程计和规划器，只需要在最上层加一个任务分配中心，告诉每个机器人各自的目标点，底层感知和避障逻辑完全自治。这也是我从这套部署经验里看到的更大想象空间——现在很多人在搜多机器人路径规划和机器狗路径规划，本质上的难点不在单个机器人跑通，而在于怎么让多个机器人在共享空间里互不碰撞地协同作业，而底层那套稳定的里程计和避障闭环，正是协同的基础。

我做仿真部署有个习惯：每完成一个阶段就整理一份自己的笔记，把当时的报错信息、解决办法和参数设置全部记录下来。这次部署XTDrone + FAST-LIO2 + ego-planner，我的笔记从环境搭建写到动态避障调参，前前后后累计了十几页。这些内容虽然不如论文里的公式那么严密，但都是实打实排查出来的工程经验，每次回看都能发现新的理解。

关于这套系统，如果你正在尝试同样的组合，我最想提醒的一点是：不要追求一步到位，先把FAST-LIO2的里程计跑稳了再谈规划，先把静态避障跑通了再上动态障碍物。每次只引入一个变量，出问题时才能快速定位是定位的问题还是规划的问题。仿真平台存在的意义，就是让你在低成本的环境里把系统磨得足够可靠。把这套组合调明白了，以后无论换什么样的传感器、换什么样的规划器，你都能快速搭起一套自己的感知规划闭环。