激光里程计+IMU融合：解决ROS小车定位漂移的实战方案

做ROS小车定位里程计不准这个问题我估计你们十有八九都撞上过。轮子空转、地面湿滑、轮胎气压变了编码器算出来的odom直接就飘了导航里小车画着圈往前跑地图也对不齐。后来我换了一套方案——用rf2o_laser_odometry做激光里程计再把它的输出和IMU一起丢给robot_localization做融合整套定位才算稳下来。这套组合尤其适合差速小车和麦克纳姆轮底盘在室内结构化环境里效果非常明显。这篇文章我就把整个落地过程、参数配置、还有我调试时掉进去的坑一次讲清楚前面是原理和选型思路中间是配置和实测后面是专门给你们的避坑指南。这套东西解决的核心问题就是当轮式里程计不可信的时候怎么用激光雷达本身去估算机器人走了多远再把IMU和雷达数据融成一份稳定、平滑、能直接给导航用的odom输出。如果你是正在做ROS导航、SLAM建图或者小车在瓷砖地板、草地上老跑偏的这篇文章应该能帮你省掉好几个星期的折腾时间。1. 为什么要抛弃轮式里程计改上激光里程计先说一个很多人不爱听的事实轮式里程计在理论模型里很好用但一到真实地面就露馅。想搞清楚rf2o_laser_odometry这套方案为什么香就得先知道轮式里程计的病根在哪。1.1 轮式里程计的“阿喀琉斯之踵”轮式里程计的核心逻辑很简单靠电机上的编码器数轮子转了多少圈然后乘上轮子周长算出位移再通过左右轮的差速算出转角。理想情况下这个推算非常精确所以它是绝大多数ROS小车默认的odom来源。但问题在于它有一个前提假设轮子和地面之间必须是纯滚动没有任何滑动。这个假设在实验室大理石地面上都很难成立更不用说走廊瓷砖缝、塑胶跑道、草地这种场合。我自己实测过一台10kg左右的差速小车在普通瓷砖地上急加速轮子空转一瞬编码器记录的位移能比实际多出十几厘米。这十几厘米在单圈里不算什么但不断累积几十秒后odom就比真实位置偏了半米以上AMCL粒子滤波分分钟被打散导航规划出来的路径全是斜的。除此之外轮胎胎压变化、负重不同导致轮子半径改变、地形起伏带来的上下颠簸都会让轮式里程计产生系统性误差。所以我的结论是如果你只在平整地面上跑几米轮式里程计够用但只要你稍微上点难度就必须引入外部感知来修正这就是激光里程计登场的原因。1.2 rf2o_laser_odometry的核心思路rf2o_laser_odometry是个非常轻量的2D激光里程计算法它不依赖轮子直接从2D激光雷达的连续帧数据里算机器人的运动。每次拿到一帧新的scan数据它就把当前帧和上一帧做扫描匹配找出一个最优的旋转和平移让两帧激光点云尽量重合。这个运动增量累积起来就是机器人相对起点走过的轨迹。说人话就是它把激光雷达当成一只“眼睛”靠着观察墙壁、柱子和障碍物的位置变化倒推出自己动了多少。这和人类闭着眼靠脚底感觉走不稳、睁开眼看周围参照物就能走直线是同一个道理。它的好处特别直接轮子打滑跟它无关轮胎磨损跟它无关地面摩擦系数跟它也无关。只要激光雷达能看到足够多的静态特征它就能给出比较准的位移估计。当然它也不是没有软肋。我专门在长走廊里跑过测试当机器人沿着走廊方向前进时激光点云在走廊轴向几乎没有变化扫描匹配在“前进/后退”这个自由度上会退化表现为odom前后漂移、速度估计抖动。这种情况单靠纯激光里程计救不回来所以才需要后面那个融合环节来兜底。1.3 为什么还要加robot_localization既然rf2o_laser_odometry已经很能打了为什么还要再引入robot_localization做融合原因有两个第一激光里程计的短时噪声和退化问题无法避免单用它做里程计长期精度可以短时平滑度却不如码盘稳。第二没有融合的话IMU这种高频传感器就白装了而IMU恰恰能在扫描退化、快速旋转时提供角速度参考修正激光里程计在转角上的漂移。robot_localization里最常用的是ekf_localization_node它基于扩展卡尔曼滤波把多个来源的位姿、速度、角速度观测合并成一个最优估计。放到这套方案里就是让rf2o输出odom作为位姿观测让IMU输出角速度和姿态作为另一路观测最后由EKF输出一份融合后的odom给move_base或者Nav2用。这样既保住了激光里程计抗打滑的优势又补上了它短时抖动的短板组合起来比任何单一里程计都稳。2. rf2o_laser_odometry安装、配置和参数调优工具选好了接下来就是动手装。这一节我把安装、launch文件、关键参数全部过一遍并附上我调过多次的一套配置你们可以直接拿去改。2.1 安装环境准备我这边主力环境是Ubuntu 20.04 ROS Noeticrf2o_laser_odometry目前对ROS1的支持最成熟ROS2版本也有但功能包名和参数会有些差异。如果你还没装ROS网上有那种一键安装脚本比如“鱼香ROS一键安装”能帮你把ROS环境快速搭好省得手动配源、配密钥那堆破事。装好ROS之后安装这个包最简单的方式就是直接clone源码编译cd ~/catkin_ws/src git clone https://github.com/MapIV/rf2o_laser_odometry.git cd ~/catkin_ws catkin_make # 或者如果你用的是catkin_tools catkin build编译过程中可能遇到依赖缺失一般就是roscpp、sensor_msgs、nav_msgs、tf2这些基础包缺什么装什么就行。装完之后用rospack find rf2o_laser_odometry验证一下能输出路径就说明装好了。我建议不要用apt直接装因为apt源的版本往往偏老某些参数名跟新版不一致你照着网上教程一写容易对不上号。源码编译虽然慢个一两分钟但至少你能确定自己手头是什么版本。2.2 launch文件与参数逐项拆解rf2o_laser_odometry的launch文件看起来简单但里面的参数直接影响精度我直接放一份我调试后觉得最稳的配置逐行讲一下launch node pkgrf2o_laser_odometry typerf2o_laser_odometry_node namerf2o_laser_odometry outputscreen param namelaser_scan_topic value/scan/ param nameodom_topic value/odom_laser/ param namebase_frame valuebase_footprint/ param nameodom_frame valueodom/ param namepublish_tf valuefalse/ param namemin_scan_points value180/ param namemax_laser_range value5.0/ param namemin_laser_range value0.1/ param namefreq value10.0/ /node /launch逐个说laser_scan_topic你的2D激光雷达话题一般是/scan如果用了gazebo仿真则是/scan或你自定义的名称。odom_topic这个包输出里程计的话题名。强烈建议改成/odom_laser这种非默认名称不要直接覆盖/odom留出空间给后面EKF融合用。我最初直接让它发/odom后面接入robot_localization时frame和topic搅在一起乱成一锅粥。base_frame机器人本体坐标系我统一用的是base_footprint。如果你机器人URDF里是base_link这里就必须跟着改否则tf树直接报错。odom_frame里程计父坐标系一般为odom。publish_tf这个非常关键先设为false。后面融合配置里我们只需要它发话题tf由robot_localization统一发避免两个节点抢odom→base_footprint这条tf的发布权。min_scan_points一帧scan里最少要有多少个点才参与匹配。点数太少的场景比如空旷场地匹配质量会差雷达会被环境“带偏”。180是个折中值如果你的雷达是360度、每帧几百个点这个值可以设高一点。max_laser_range和min_laser_range匹配时忽略掉太远和太近的点。太远的点噪声大、容易混入动态障碍物太近的点往往是机器人自身结构反射出来的不处理会严重干扰匹配。一般max设成雷达实际有效量程的80%左右min设0.1到0.2。freq里程计输出频率。我雷达10Hz就直接设10.0。如果你雷达是20Hz这里可以跟着提但后面EKF频率也要匹配。2.3 参数调的三个经验第一publish_tf千万想清楚再设。第一次用的时候我图省事设成true结果EKF也在发odom到base_footprint的tf两个tf在同一个坐标系关系上打架rviz里机器人模型不停抖动导航路径抖动得没人敢用。所以这个开关一旦决定走“EKF融合路线”就老老实实设false。第二laser range的上下限值得根据你的场地好好调。我一开始直接用的默认值10.0结果在8米宽的厂房里跑雷达扫到远处的墙角匹配时被一些噪声点带偏整个odom时不时跳一下。把max_laser_range降到5.0之后明显稳了一个档次。核心思想是用近处高置信度的点做匹配别让远处低质量点主导你的结果。第三如果发现里程计输出有高频抖动可以在launch里加一组低通滤波参数不同版本参数名不一样请以你当前版本文档为准。或者更省事的方式是调低EKF对odom速度项的信任权重让EKF自己平滑掉一些毛刺。这两个办法我后文还会讲到。3. robot_localization多传感器融合的真正主角rf2o_laser_odometry把激光里程计这个话题输出来了但它还只是个“单项选手”。真正把激光里程计和IMU捏在一起、输出干净平滑的odom是robot_localization的活。3.1 用EKF节点干什么robot_localization包里提供了ekf_localization_node和ukf_localization_node。差速小车这种平面运动场景用EKF就够了UKF是非线性更强时才会发挥明显优势而且计算量更大。这个EKF节点做的事可以理解为一个“数据中央厨房”你把各路传感器数据都丢给它它按各自的噪声特性加权融合输出一个带协方差的状态估计。比如激光里程计告诉你“我走了0.3米”IMU告诉你“我角速度是每秒5度”EKF结合这两条信息推算出最可能的位置和速度。它对每个传感器都有一组config开关控制着这个传感器的哪些量参与融合。很多人第一次被robot_localization整懵就是死在这串true/false上。我下面给出一份配置直接解释清楚每个开关。3.2 一个能直接跑的ekf配置文件frequency: 30.0 sensor_timeout: 0.1 two_d_mode: true transform_time_offset: 0.0 odom_frame: odom base_frame: base_footprint world_frame: odom odom0: /odom_laser odom0_config: [true, true, false, false, false, true, true, true, false, false, false, false, false, false, false] odom0_queue_size: 10 odom0_nodelay: true odom0_differential: false odom0_relative: false imu0: /imu/data imu0_config: [false, false, false, true, true, true, false, true, false, true, true, false, false, false, false] imu0_queue_size: 10 imu0_differential: true imu0_remove_gravitational_acceleration: true这条配置我跑了很久都没出大问题关键点逐个拆frequency: 30.0EKF输出频率。IMU一般100Hz激光雷达10Hz设30Hz是个折中既能让下游拿到比较高频的odom又不至于让EKF在没有新观测时空推太远。two_d_mode: true让EKF在平面内工作z轴、roll、pitch方向自动忽略。对普通地面小车来说这个开关一旦开了能省掉很多z轴和姿态上的怪问题。odom0_config这是odom的15位开关矩阵顺序是x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az。我这里的设置是位置x/y参与融合z不参与roll/pitch不参与yaw参与速度vx/vy参与vz不参与其余角速度、加速度都不参与。原因很简单2D小车在平面上运动z方向不可观roll/pitch靠IMU来管odom只提供平面位置和偏航。imu0_configIMU的配置。IMU负责提供roll/pitch/yaw的姿态信息还有角速度vyaw以及线速度或加速度中的一部分。我这个配置里位置xyz全false姿态roll/pitch/yaw全true线速度vy参与、其余不要角速度vyaw要加速度不参与EKF状态更新只是用来去重力。这里的核心理念是IMU姿态更新不需要位置它只修正“方向感”。odom0_differential: false和imu0_differential: trueodom我们不做差分因为rf2o输出本身就是增量式的里程计再差分会损失信息IMU反而要做差分处理用来消除IMU的固定零偏。这个细节很容易被人忽略但不处理的话长期运行IMU的偏置会一点点吃掉你的方向精度。imu0_remove_gravitational_acceleration: true利用IMU内置的重力加速度来估计姿态。这一步必须开否则IMU的加速度计读数会带一个重力分量姿态估计会歪。3.3 少走弯路的配置思路frame、tf与config开关配置完参数后首先检查三件事frame_id一致吗tf树完整吗有没有重复发布tfframe_id这条我踩过最深的坑。rf2o输出的是base_footprint坐标系下的odomIMU数据如果是在imu_link坐标系下发布的EKF拿过来用之前必须保证base_frame填的是base_footprint而IMU的坐标系和base_footprint之间的静态tf要存在。否则EKF在状态预测阶段把IMU数据当成base_link系来用出来的结果全是乱的。再说一遍tf树目前我的世界里odom到base_footprint的tf由EKF发布不再由rf2o发布base_footprint到base_link、base_link到laser_link、base_link到imu_link的tf由robot_state_publisher或static_transform_publisher负责。这是最清晰的职责划分路径规划器、AMCL、rviz都只认这一棵tf树不会再打架。最后config开关矩阵千万别随意抄。我见过有人把odom里vx、vy、vyaw全开把IMU的yaw也开最后EKF里对同一个量有两路传感器同时说“我知道”结果EKF不知道该信谁输出一路狂抖。先分析你的传感器各有什么强项再决定开关比盲目全开靠谱得多。4. 实测流程与避坑实录配置写完了下面说说怎么看它到底跑得好不好以及那些让无数人卡壳的坑。我尽量按时间顺序来你在自己的机器上照做就行。4.1 五分钟快速验证整套链路先起rf2o再起EKF然后rviz里加两个显示一个是/scan点云一个是/odometry/filtered路径。操作顺序roslaunch rf2o_laser_odometry rf2o_laser_odometry.launch roslaunch robot_localization ekf.launch rvizrviz里把Fiexed Frame设为odom加一个LaserScan显示topic选/scan加一个Path显示topic选/odometry/filtered。然后手动推着小车走一圈直线或者发出一个cmd_vel让它动。如果一切正常你会看到激光点云和路径贴合得很紧小车模型在rviz里稳定不抖。如果发现路径画出来的轨迹比实际路线宽或者小车在导航时明显偏向一侧先不要急着动EKF回头看一下rf2o输出的/odom_laser在同样的路径下表现如何。这一步能帮你确定偏差是激光里程计带来的还是EKF融合出来的。另外强烈推荐用rqt_plot看速度曲线选/odometry/filtered/twist/twist/linear/x和/odometry/filtered/twist/twist/angular/z正常情况下曲线应该平滑没有毛刺。出现高频抖动去查激光话题的时间戳八成是时间戳不对齐。4.2 避坑点一tf被双写这个坑我在前面提过这里再展开讲一遍。如果你同时开着了rf2o的publish_tftrue和EKF的tf输出那odom到base_footprint这条边就有两个发布者在抢。rviz里症状是机器人模型疯狂抖动tf_echo里看到的变换频率忽高忽低严重时/pose相关的话题会乱套。解决方案rf2o只负责发odom话题不负责发tfEKF负责发odom到base_footprint的tf。这样整个系统里只有EKF一个发布者干净利落。注意如果你只想单独用rf2o练手不接入EKF那可以开publish_tftrue。这里的分界线是“你到底要不要多传感器融合”。一旦打定主意用EKF就把tf的活全交给EKF。4.3 避坑点二时间戳与队列机器人上传感器的同步问题是另一个大坑。激光雷达10Hz、IMU 100Hz、EKF 30Hz三者时间戳差一点点看似无所谓但融合时一旦IMU时间戳比激光早太多EKF会认为这个观测已经过期直接丢弃或者激光数据时间戳晚太多EKF空推时间边长位置估计平滑但滞后。我用rostopic echo /odom_laser/header/stamp和rostopic echo /imu/data/header/stamp对比过发现我的激光驱动发布的时间戳比实际时间慢了几毫秒到几十毫秒不等。解决手段有两条一是尽量保证各传感器驱动使用同一台机器上的/use_sim_time机制或者在launch层面让传感器都从同一个时钟源读取时间二是在robot_localization里调大queue_size比如20让EKF有足够的缓冲窗口去对齐不同频率的观测。如果实在对齐不上可以试着把odom0_nodelay设为true让odom观测尽量早进入滤波器但这也意味着它会在更早的时间被处理下游导航如果对延迟敏感可能需要用transform_time_offset微调。4.4 避坑点三frame_id命名不统一很多小车的URDF里base_link、base_footprint混着用雷达驱动里frame_id写的又是laserIMU写imu_link结果EKF一启动报“frame_id [base_link] does not match expected [base_footprint]”之类的tf错误。我的建议是把所有传感器的frame_id统一到base_footprint下或者统一到base_link下二选一。我个人习惯用base_footprint因为它在平地运动时比base_link更不容易出现由于底盘中心高度变化导致的位置误差。所有link之间用robot_state_publisher加静态tf连接而不是靠传感器驱动自己发。rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 base_footprint base_link rosrun tf2_ros static_transform_publisher 0.1 0 0.2 0 0 0 base_link laser_link rosrun tf2_ros static_transform_publisher 0 0 0.1 0 0 0 base_link imu_link这里面的坐标偏移只是示例实际要根据你的小车结构改。我最常遇到的奇怪问题是车辆转个弯rviz里激光点云和地图错开半个车身一查全是IMU_link没有正确接到base_link导致IMU姿态被当成底座姿态用了。4.5 避坑点四IMU数据与协方差设置robot_localization里面每个传感器输入除了数据本身还自带协方差矩阵。很多IMU驱动发布的数据里协方差矩阵是0意味着“我完全相信这个观测”这会让EKF直接放弃其他传感器只信IMU。我曾经在IMU没校准的情况下跑融合结果车明明停着EKF输出的yaw还在缓慢漂移就是因为协方差为0导致IMU az/yz方向噪声被忽略。解决方法很简单在抓取IMU话题之后用rqt_plot看一下IMU静止时的数据涨落a角按实际噪声填一个合理的协方差初始值到驱动配置里。比如静止时yaw角噪声在±0.5度以内yaw协方差就填(0.5*pi/180)^2角速度噪声0.1rad/s就填0.01。这些数值不需要精确但不能是0。还有一个容易被忽略的细节imu0_remove_gravitational_acceleration必须为true。如果为false加速度计读数会一直包含一个1g的重力分量EKF会尝试用这个重力去解释机器人的运动导致接受融合的位置出现低频晃动。4.6 常见问题速查表症状可能原因解决手段小车静止odom还在缓慢漂移IMU未校准或协方差为0静止校准IMU给协方差填合理值rviz里模型抖动tf被多个节点重复发布检查publish_tf设置保留EKF一个发布者导航时小车画圈odom数据时间戳延迟或严重偏转调大queue_size检查激光驱动时间戳转弯后位置偏移大激光退化或scan匹配失败提升laser的min_scan_points检查雷达安装EKF输出有毛刺传感器的config开关冲突检查odom0_config/imu0_config是否同时开了yaw地图对不齐laser_frame和base_link静态tf错误用tf tree检查laser_link是否在正确位置5. 与导航栈对接以及我的最终建议机器人最终是要干活的融合后的odom最终要给导航用。这里再说说和move_base或Nav2对接时要注意的事情。5.1 把融合后的odom接到move_basemove_base和Nav2默认订阅的是/odom话题但我们的融合输出是/odometry/filtered所以要么你把话题重映射要么把CMakeLists或导航配置文件里的odom_topic参数改掉。我推荐话题重映射做法remap from/odom to/odometry/filtered/如果是Nav2在nav2_params.yaml的robot_base_model相关配置里确认base_frame_id: base_footprint、odom_frame_id: odom、scan_topic: /scan这三项和你实际发布的名称一致。我自己第一次对接时忘了改topic导航发了一堆指令但是定位一直“认为”自己在原地折腾了一圈才发现订阅的是旧odom。另外要注意AMCL如果用的话它输出的/amcl_pose和/map到/odom的tf会跟EKF的odom同时存在。这是正常的AMCL负责修正地图级别的漂移EKF负责短期里程计两者分工不要混。5.2 实测效果数据说下我实测的效果给你们一个参考量级。我的底盘是普通的差速小车电机带编码器RPLIDAR A1雷达JY61 IMU。之前在瓷砖地上急加速轮式odom累计30秒漂移接近0.4米单独用rf2o激光里程计急加速打滑时不会出现大跳变但在走廊里走了15秒后前进方向存在轻微漂移加EKF融合IMU后同样路径和工况下30秒终点误差压在0.08米以内而且速度曲线平滑给导航用起来舒服很多。当然这个数值跟雷达精度、IMU安装位置、地面特征丰富度都有关不要拿这个当绝对标准但可以作为判断你的组合是否正常工作的参考。如果跑下来误差比这个大很多建议回第4章慢慢排查。5.3 我的最终建议如果你也在做低成本ROS小车我的建议是轮式里程计留着别急着删。轮式odom在短距离、低速、平整地面的场景下依然很有价值你完全可以把三轮数据轮式odom、激光odom、IMU都送进EKF做三源融合。robot_localization对传感器数量没有限制多一个观测源系统的鲁棒性就高一截。我就是这样做的rf2o在当前帧退化时EKF会自动更信任轮式odom和IMU整体定位一整天都不带飘的。最后分享一个只有踩过坑才懂的小技巧调完EKF后在rviz里同时显示/odom_laser的路径和/odometry/filtered的路径然后手柄遥控车走一个8字形。如果两条路径都贴着实际轨迹说明激光odom本身健康如果/odom_laser有问题但/odometry/filtered正常说明融合起了作用问题在激光里程计参数上如果两条都不正常先去查tf树和frame_id。这招基本能帮你定位80%以上的里程计异常省下的排查时间够你再写好几篇博客了。