从仿真真值到传感器里程计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 编译顺序从下往上逐层验证环境装好后我的习惯是严格按照仿真平台 - 里程计 - 规划器的顺序编译每装一个就必须跑通一个绝不往后推进。具体顺序如下先装XTDrone并跑通自带的通信示例确保Gazebo里的无人机能起飞、能接收指令。再编译FAST-LIO2及其依赖livox_ros_driver或velodyne驱动用XTDrone自带的激光雷达话题做输入确认里程计输出频率正常。最后编译ego-planner先不改任何代码直接跑它自带的demo确认规划器在原生仿真里能工作。这个顺序的逻辑在于每一步的输入都是上一步的输出如果顺序颠倒或者跳步出了问题你根本不知道是上一步的接口没打通还是这一步的代码有bug。我见过太多人一次性把三个workspace编译完然后启动时全部GDB都上阵也找不到问题最后发现是FAST-LIO2根本没订阅到点云话题——这种错误本该在第二步就暴露的。2.3 编译时常见的坑和我的处理习惯编译FAST-LIO2时有几个坑是高频出现的一是PCL版本不匹配导致pcl::PointCloudPointType的编译报错二是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-devego-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.git3. 在XTDrone里让FAST-LIO2先睁开眼睛在ego-planner介入之前我们要确保FAST-LIO2能在XTDrone的仿真环境里稳定跑起来输出可信的里程计。这个阶段是最枯燥但其实最有意思的部分——因为你实际上是在和仿真器里的传感器数据较劲。3.1 激光雷达和IMU话题怎么找、怎么确认XTDrone的机型配置不同激光雷达话题名也可能不一样。我这次使用的是带Velodyne仿真插件的机型点云话题是/velodyne_pointsIMU话题是/imu/imu。但有些版本用的是Livox模型的gazebo插件点云话题就是/livox/lidar。先不要盲信任何教程直接用命令确认rostopic list | grep -E points|imu|odom rostopic hz /velodyne_points rostopic hz /imu/imurostopic 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文件里我要改的主要是以下几个方面点云话题名从默认的/livox/lidar改成/velodyne_points。IMU话题名改成/imu/imu。外参文件路径指向你当前机型对应的外参yaml。雷达类型和畸变补偿开关Velodyne仿真一般不开畸变补偿但实机建议打开。param namepointcloud_topic value/velodyne_points/ param nameimu_topic value/imu/imu/ param nameconfig_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里用的是哪一路odomego-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 nameodometry_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-20Hzego-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. 高发问题排查链路从现象倒推根因部署这套系统遇到问题不可怕可怕的是没有排查思路。这里我把几个高频问题整理成一张排查表每个问题都给出完整的排查链路照着做可以省掉大量试错时间。现象可能原因排查步骤解决手段ego-planner一直等待odom话题名不匹配用rostopic list对比发布和订阅话题remap或改launch中的odometry_topicRviz里轨迹和点云分离坐标系不对齐查看TF树确认map和body的父子链写TF中继节点或让camera_init对齐map无人机悬停时位置漂移大FAST-LIO2外参错误或IMU噪声大悬停测试观察odom的XYZ方差校正外参检查IMU话题频率动态避障反应迟缓感知范围过小或里程计频率过低用rostopic hz确认odom频率调大感知范围降低规划器CPU占用规划轨迹震荡严重最大速度过高观察速度曲线和重规划次数降低max_velocity增加安全距离编译时报子模块缺失ego-planner未递归克隆检查plan_env目录是否为空重新克隆仓库或手动补子模块我重点展开讲一个最难排查的坑FAST-LIO2初始化位置不在原点导致规划器路径偏移。这个问题的表象是无人机起飞后ego-planner规划的目标点看起来是偏的但TF树、话题、频率全都没问题。最后我在FAST-LIO2的日志里发现它的camera_init里程计原点是在x1.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的里程计跑稳了再谈规划先把静态避障跑通了再上动态障碍物。每次只引入一个变量出问题时才能快速定位是定位的问题还是规划的问题。仿真平台存在的意义就是让你在低成本的环境里把系统磨得足够可靠。把这套组合调明白了以后无论换什么样的传感器、换什么样的规划器你都能快速搭起一套自己的感知规划闭环。