1. 项目缘起当FAST-LIO遇上移动机器人底盘最近在折腾一个移动机器人项目核心需求是实现室内外融合场景下的高精度自主导航。项目初期我们直接使用了机器人底盘自带的轮式里程计配合激光雷达做2D SLAM在平整的室内环境里跑得还算顺畅。但一旦到了有斜坡、有打滑比如光滑地砖或草地或者需要跨越小障碍物的场景轮式里程计的误差就会急剧增大甚至出现“原地漂移”的情况。这直接导致建图失真和导航路径规划失败机器人动不动就“撞墙”或者“迷路”。相信很多做移动机器人无论是巡检、配送还是服务机器人的朋友都遇到过类似问题。轮式里程计本质是通过电机编码器积分来推算位移它对轮子与地面的接触状态假设是理想的一旦出现打滑、悬空或者地面不平这个假设就被打破了误差不可逆地累积。这时候一个不依赖于轮子接触的、更高频率和精度的里程计就成了提升机器人鲁棒性的关键。我首先想到的是激光里程计。在ROS 1时代LOAM系列和后来的LIO-SAM是绝对的主流它们通过匹配连续两帧或多帧激光点云来计算机器人的运动增量精度很高。但问题也很明显计算量大对CPU资源消耗高在算力有限的嵌入式工控机上很难稳定跑在高频比如10Hz以上。这对于需要快速响应控制的移动底盘来说是个硬伤。直到我注意到了FAST-LIO系列。FAST-LIO的核心创新在于它采用了紧耦合的迭代卡尔曼滤波IEKF框架并且巧妙地利用了激光雷达点云的固有特性如平面特征和ikd-Tree这种高效的数据结构实现了数百赫兹的里程计输出频率和极低的计算延迟。论文和开源代码都显示它在一些标准数据集上表现惊人。但FAST-LIO官方定位更多是一个“激光惯性里程计”算法包输出的是高频率的Odometry话题。而像Nav2这样的现代导航框架其整个定位、代价地图更新、路径规划环路严重依赖于机器人tf树中odom坐标系到base_link坐标系的变换。这就引出了核心矛盾如何将FAST-LIO输出的“算法里程计”变成一个Nav2能无缝使用、稳定可靠的“底盘里程计”这不仅仅是发布一个tf变换那么简单它涉及到坐标系对齐、数据同步、异常处理、与轮式里程计的融合可选等一系列工程化问题。网上很少有把这件事讲透的完整方案大多只是提一句“可以用FAST-LIO做里程计”。于是我决定自己动手把FAST-LIO改造并深度集成到Nav2导航栈中并把整个踩坑和实现的过程记录下来。2. FAST-LIO作为里程计的优势与局限性分析在动手改造之前我们必须先搞清楚FAST-LIO到底能给我们带来什么以及它的“原生形态”有哪些地方不适合直接用作底盘里程计。2.1 为什么选择FAST-LIO抛开那些复杂的数学公式从工程实用角度FAST-LIO我主要用的是FAST-LIO2有以下几个让我决定选它的硬核优点极高的频率与极低的延迟这是最吸引人的一点。在配备Livox Mid-70或Velodyne VLP-16这类雷达的机器上轻松跑出100Hz以上的里程计输出。这意味着odom-base_link的tf变换更新极快对于Nav2的控制器比如DWB或TEB局部规划器来说能获取到几乎实时的位姿反馈对于高速移动或紧急避障场景至关重要。延迟通常只在几毫秒到十几毫秒远低于LOAM系列动辄上百毫秒的处理时间。对运动畸变的鲁棒性FAST-LIO在IEKF的预测步骤中融合了IMU数据能够有效地对单帧激光点云由于机器人自身运动造成的畸变进行校正。这对于扫地机器人、配送机器人这种经常需要转弯、加减速的平台来说能显著提升点云匹配的准确性从而得到更稳定的里程计。资源消耗相对友好虽然紧耦合优化听起来很耗资源但得益于ikd-Tree的高效增量式更新和滤波框架的简洁FAST-LIO2的CPU占用率在我使用的Intel NUC工控机i7-8559U上可以稳定在30%-40%单核峰值内存占用也在可接受范围内。这比运行LIO-SAM需要运行一个完整的图优化节点要轻松得多。开源且活跃代码质量高社区活跃遇到问题比较容易找到讨论或解决方案。而且它原生支持ROS 2这与我们使用Nav2的方向一致。2.2 “拿来即用”的障碍FAST-LIO的原始输出与Nav2的期望输入直接运行FAST-LIO的run.launch.py它会发布两个对我们最关键的话题/Odometry(类型nav_msgs/msg/Odometry)包含位姿pose和速度twist的完整里程计信息。/tf发布从map或odom取决于配置到lidar_link雷达坐标系的变换。看起来好像直接就能用了但一旦你把它接入一个完整的机器人系统尤其是Nav2问题就来了坐标系错位FAST-LIO默认的里程计输出其frame_id通常是odom或map而child_frame_id是lidar_link或body。但在标准的ROS导航框架中tf树要求存在一条从odom到base_link机器人底盘中心的连续变换。我们的雷达可能安装在机器人顶部前方lidar_link它到base_link还有一个固定的静态变换static_transform。Nav2的robot_localization、amcl或nav2_amcl等模块监听的是odom-base_link。如果直接使用FAST-LIO的tf链式查找odom-lidar_link-base_link在某些情况下尤其是tf时间戳同步不好时会导致查找失败进而导致定位模块崩溃。缺少“速度”信息的直接可用性虽然Odometry消息里包含twist但FAST-LIO输出的速度是基于雷达坐标系lidar_link的。而底盘控制器以及Nav2的控制器通常需要基于base_link坐标系的速度指令线速度vx, vy角速度wz。这里涉及一个坐标系转换不能直接赋值。无异常状态处理FAST-LIO在初始化、剧烈运动导致特征丢失、或点云质量极差时可能会输出不可靠甚至错误的位姿。如果直接将这个位知发布给Nav2会导致机器人“跳变”或“发疯”。一个健壮的底盘里程计必须具备一定的故障检测和降级处理能力。与轮式里程计的关系未定义在很多应用中我们并不想完全抛弃轮式里程计。轮式里程计在低速、平面、无打滑的短时内是非常可靠的。理想的方案是能以某种方式融合两者或者在FAST-LIO失效时有一个可靠的备份。原生FAST-LIO不提供这个接口。所以我们的改造目标很明确开发一个“适配器”节点这个节点订阅FAST-LIO的原始输出经过坐标系转换、数据校验和可能的融合处理后发布出符合ROS导航标准REP-105的Odometry话题和odom-base_link的tf变换并妥善处理各种边界情况。3. 核心改造构建FAST-LIO到Nav2的桥梁节点这个“适配器”节点我称之为fast_lio_odom_bridge。它是一个用C或Python但出于性能考虑推荐C编写的ROS 2节点。下面我详细拆解它的核心逻辑和实现要点。3.1 节点架构与数据流整个节点的数据流如下图所示文字描述输入订阅FAST-LIO发布的/Odometry话题。订阅/imu/data话题可选用于增强或作为健康度判断参考。通过参数服务器读取静态变换参数如lidar_link到base_link的变换。处理核心坐标变换将FAST-LIO输出的位姿和速度从lidar_link坐标系转换到base_link坐标系。健康度检查对输入数据如协方差矩阵、时间戳进行校验判断FAST-LIO输出是否可信。融合策略可选设计逻辑在特定条件下融合轮式里程计信息。平滑与滤波可选对输出的位姿进行简单的低通滤波抑制高频噪声。输出发布新的/odom话题nav_msgs/msg/Odometry其frame_id为odomchild_frame_id为base_link。发布odom到base_link的tf变换。可选发布一个/odom_status话题指示当前里程计的健康状态。3.2 关键实现细节与代码片段这里我分享几个最关键的实现部分并解释为什么这么做。1. 坐标系转换这是最核心的一步。FAST-LIO输出的位姿Pose_lidar是相对于odom坐标系在lidar_link坐标系下的表达。我们需要得到Pose_base即base_link在odom下的位姿。假设我们通过static_transform_publisher或URDF已经定义了从lidar_link到base_link的静态变换T_lidar_base。这个变换在机器人组装后是固定不变的。那么转换公式为Pose_base T_odom_lidar * T_lidar_base但在tf和geometry_msgs中我们通常用变换的逆或乘法顺序来表示。更直观的做法是使用tf2库进行变换查找和计算。// 伪代码逻辑 geometry_msgs::msg::TransformStamped T_lidar_to_base; // 这是一个常量从参数或tf树中获取 geometry_msgs::msg::PoseStamped pose_lidar; // 从FAST-LIO的Odometry消息中提取 geometry_msgs::msg::PoseStamped pose_base; // 使用 tf2::doTransform 进行坐标变换 tf2::doTransform(pose_lidar, pose_base, T_lidar_to_base); // 注意这里需要理解T_lidar_to_base的方向。通常是查找从base_link到lidar_link的变换然后求逆或直接使用。 // 更安全的做法是在节点启动时通过tf_buffer.lookupTransform获取一次这个静态变换并缓存。对于速度的转换更需要注意。FAST-LIO输出的速度twist_lidar是lidar_link坐标系下的瞬时速度。我们需要将其转换到base_link坐标系下。twist_base R_lidar_base * twist_lidar其中R_lidar_base是T_lidar_base中的旋转矩阵部分。角速度在纯旋转变换下是不变的但线速度需要旋转。tf2::Vector3 lin_vel_lidar(twist_in.twist.linear.x, twist_in.twist.linear.y, twist_in.twist.linear.z); tf2::Vector3 ang_vel_lidar(twist_in.twist.angular.x, twist_in.twist.angular.y, twist_in.twist.angular.z); tf2::Transform T_lidar_base; // 从geometry_msgs::Transform转换而来 tf2::Matrix3x3 R_lidar_base T_lidar_base.getBasis(); tf2::Vector3 lin_vel_base R_lidar_base * lin_vel_lidar; // 角速度在刚体变换下不变除非考虑坐标系原点不同引起的线速度耦合但通常近似认为不变。 tf2::Vector3 ang_vel_base ang_vel_lidar; // 填充到输出的twist消息中 twist_out.twist.linear.x lin_vel_base.x(); // ... 其他分量2. 健康度检查与容错我们不能盲目相信FAST-LIO的每一个输出。以下是我实现的几个检查点协方差检查FAST-LIO输出的Odometry消息的pose.covariance和twist.covariance矩阵。如果某个方向特别是Z轴位置或旋转的方差协方差矩阵对角线元素突然变得极大例如超过一个阈值COVARIANCE_THRESHOLD说明这一帧的估计非常不确定可能源于特征缺失。此时可以选择丢弃这一帧或者使用上一帧的有效数据进行外推并发布一个警告。时间戳检查检查收到的Odometry消息的时间戳与当前时间的差值。如果延迟过大例如0.1秒说明FAST-LIO可能处理卡顿数据已经“过时”不适合用于实时控制。运动合理性检查根据机器人的物理极限最大速度、最大加速度进行判断。例如计算当前帧与上一帧的位移差除以时间差得到瞬时速度。如果这个速度超过了机器人电机能提供的最大速度例如2 m/s那么这很可能是错误估计。同样加速度也可以用来判断。IMU辅助判断如果订阅了IMU可以将FAST-LIO输出的角速度与IMU原始的陀螺仪数据进行短期对比例如计算滑动窗口内的相关性。如果差异持续过大可能表明FAST-LIO跟踪失败。当检测到异常时我的策略是发布一个状态标志如/odom_status让其他节点如导航恢复行为知道里程计可能不可靠。不更新tf变换。这是关键与其发布一个可能错误的位姿导致机器人“跳飞”不如让tf树保持上一次正确的变换。Nav2的robot_localization或控制器通常有处理tf超时的机制比如假设机器人停止。这比提供一个错误数据要安全得多。在节点日志中记录详细的错误信息方便离线分析。3. 与轮式里程计的简单融合可选策略如果你不想完全依赖激光里程计或者想在FAST-LIO初始化阶段前几秒通常不准提供位姿可以考虑一个简单的融合方案。这里我采用一个“选择器”模式而非复杂的滤波融合因为robot_localization包ekf_filter_node本身就能做得更好。我们的桥接节点可以作为一个数据源提供者。方案A输出选择。同时订阅FAST-LIO里程计和轮式里程计。默认输出FAST-LIO的结果。当健康度检查失败时自动切换到轮式里程计并给出状态提示。切换时要注意位姿的衔接避免跳变。方案B作为robot_localization的一个输入。这是更推荐的做法。将本桥接节点转换后的、基于base_link的Odometry消息作为一个输入源odometry0提供给ekf_filter_node。同时将轮式里程计作为另一个输入源odometry1。在ekf_filter_node的配置中你可以设置每个源的噪声参数。这样EKF会根据协方差自动信任更可靠的源。当激光雷达失效协方差变大时EKF会自然地更多依赖轮式里程计。我的实现里直接采用了方案B因为这样更符合ROS导航的模块化设计哲学把专业的事交给专业的包robot_localization去做。我们的桥接节点就专心做好“坐标转换和基本校验”这件事。4. 集成到Nav2配置与实战调试改造好桥接节点后下一步就是将其嵌入到完整的Nav2导航系统中。这里涉及到tf树的配置、nav2_bringup启动文件的修改以及关键的参数调试。4.1 重构TF树一个清晰正确的tf树是ROS导航的基石。集成后的tf树应该如下所示map - odom - base_link - lidar_link (由我们的bridge节点发布)map - odom由定位模块如amcl发布。当使用FAST-LIO本身进行建图即map系为FAST-LIO的世界系时FAST-LIO可能直接发布map-lidar_link。此时我们的桥接节点需要处理的是map-base_link。为了兼容Nav2我强烈建议在FAST-LIO配置中将其世界坐标系设置为odom。这样整个导航栈的坐标系语义map为全局地图odom为里程计局部世界更加清晰。odom - base_link这就是我们桥接节点的核心输出。它必须是高频、低延迟且连续的。base_link - lidar_link静态变换通过URDF或static_transform_publisher发布。在nav2_bringup的启动文件如tb3_simulation_launch.py中你需要确保不再启动原有的轮式里程计节点除非你要做融合。先启动static_transform_publisher发布base_link-lidar_link。启动FAST-LIO节点。启动我们的fast_lio_odom_bridge节点。启动robot_state_publisher如果使用URDF。最后启动Nav2的相关节点。4.2 Nav2控制器参数调优这是将理论变为实践的关键一步。Nav2的控制器Controller Server默认是为轮式里程计设计的其参数对激光里程计的响应特性需要调整。主要修改controller_server的YAML配置文件特别是DWBDynamic Window Approach或TEB控制器的部分odom_topic确保指向我们桥接节点发布的新话题例如/odom。base_frame_id设置为base_link。odom_frame_id设置为odom。use_odometry_twist通常设为true这样控制器会使用里程计提供的速度反馈进行控制。max_vel_xmax_vel_theta等这些极限参数需要根据你桥接后输出的速度值来重新设定。因为FAST-LIO估计的速度可能比轮式里程计更“激进”或更“平滑”需要实测调整避免控制器因速度指令超限而失效。sim_time和lookahead_dist由于激光里程计频率高、延迟低你可以尝试减小预测时间(sim_time)或前视距离(lookahead_dist)。这能让机器人的反应更敏捷路径跟踪更紧密。但太小会导致控制不稳定需要反复测试。path_distance_bias和goal_distance_biasDWB用于评价轨迹的权重。高精度的里程计意味着机器人对自己位置信心更足可以适当增加path_distance_bias更严格地跟踪全局路径同时可能减小goal_distance_bias对朝向目标的急切性可以降低一点让机器人行走更精确。调试过程实录 我第一次接入后机器人原地打转。用rqt_tf_tree检查发现odom-base_link的tf有但rviz里机器人模型不动。问题出在桥接节点发布的tf的frame_id和child_frame_id写反了。修正后机器人能动了但走向目标点时总是“画龙”左右摇摆。用rqt_plot绘制/odom话题的twist.angular.z角速度发现噪声比轮式里程计大。这是因为FAST-LIO对旋转的估计尤其是主要依靠激光匹配时在特征稀疏的环境长走廊下会有抖动。解决方案在桥接节点内对输出的角速度twist.angular.z加入一个简单的低通滤波器。我用了一个一阶IIR滤波器current_output alpha * current_input (1 - alpha) * previous_output。将alpha设为0.3左右平滑效果立刻显现机器人行走直线稳定多了。另一个问题是急转弯时机器人有时会“冲过头”。检查发现是DWB的max_vel_theta设置得太高而FAST-LIO提供的角速度估计值确实能瞬间达到很大。我根据滤波后的角速度数据重新校准了max_vel_theta和角加速度限制问题解决。4.3 实测效果与对比经过上述改造和调试在同样的室内外测试场景下平整室内与纯轮式里程计2D SLAM方案相比导航精度差异不大但得益于更高频率的里程计机器人在执行DWB的局部轨迹规划时转弯动作显得更丝滑少了一些“卡顿感”。光滑地面/轻微打滑轮式里程计方案会出现明显的位姿漂移导致机器人偏离路径甚至撞上障碍物。而FAST-LIO里程计方案几乎不受影响机器人稳稳地按规划路径行走。这是提升最明显的场景。室外有坡度的路面轮式里程计因为轮子直径等效变化会产生累积误差。FAST-LIO方案能很好地维持全局位置的一致性建图时闭环效果也更好。资源消耗桥接节点本身消耗可忽略不计。整个系统FAST-LIO Nav2的CPU占用比之前的gmapping/amcl方案略高但仍在工控机承受范围内。内存占用增加主要来自FAST-LIO本身。5. 进阶思考局限性与未来优化方向这个改造方案在实践中取得了成功但它并非银弹也有其局限性和可优化空间。当前方案的局限性纯激光依赖FAST-LIO的核心传感器是激光雷达。在极端环境如充满透明玻璃、镜面或者浓雾、粉尘的场景激光雷达会失效导致里程计完全丢失。我们的健康度检查可以检测到并告警但无法从根本上解决问题。无全局闭环我们只使用了FAST-LIO作为里程计没有启用其或搭配LIO-SAM等方案的后端闭环优化和建图功能。这意味着长时间、大范围运行后累积误差依然存在。对于真正的长期自主导航需要将FAST-LIO作为前端与一个全局优化后端如rtabmap结合。动态物体干扰虽然FAST-LIO对运动物体有一定鲁棒性但在人流极其密集的环境动态物体会污染点云特征影响匹配精度。初始化要求FAST-LIO需要几秒钟的初始化时间IMU静止在此期间输出不可用。我们的桥接节点需要妥善处理这段空窗期例如使用轮式里程计填充或发布零速度。可能的优化方向紧耦合视觉融合考虑集成一个轻量化的视觉里程计如VINS-Fusion或直接使用支持多传感器紧耦合的框架如FAST-LIVO 激光-惯性-视觉。通过视觉信息弥补激光在纹理丰富但几何特征单一场景如白墙长廊的不足提升整体鲁棒性。智能融合策略在桥接节点内实现更复杂的多源融合逻辑而不是简单交给robot_localization。例如基于场景识别通过点云密度、IMU运动状态等动态调整激光里程计和轮式里程计的融合权重。预测与平滑在健康度检查预测到FAST-LIO可能失效如协方差持续增大时可以提前切换到基于IMU和运动模型的短时预测为系统切换争取时间避免位姿跳变。与Nav2深度集成将里程计的健康状态/odom_status提供给Nav2的恢复行为Recovery Server。当检测到里程计长时间不可信时可以触发特定的恢复行为例如“清除代价地图并重新定位”而不是让机器人盲目乱撞。这次把FAST-LIO改造成机器人底盘里程计并集成进Nav2的过程让我深刻体会到将一个前沿的感知算法落地到实际的机器人系统中中间有大量的工程细节需要打磨。它不仅仅是跑通一个Demo更是要考虑到整个系统的可靠性、安全性和可维护性。希望我的这些经验和踩过的坑能给正在尝试类似方案的朋友们一些切实的帮助。至少在下次你的机器人在光滑地板上打滑时你知道除了换轮胎还可以从里程计这个根源上想想办法。