简介这份资源面向ROS机器人开发者与学习SLAM定位的进阶读者聚焦雷达定位EKF与环境建图这一典型自主导航场景帮助理解扩展卡尔曼滤波如何融合激光雷达与IMU数据并配合gmapping、amcl完成小车室内建图与定位。压缩包共107个文件约21.01MB以cpp源码与h头文件为核心实现辅以test测试用例、sample示例、yaml配置、launch启动文件、srv服务定义及bag数据包另有rst文档与pdf说明目录结构接近标准ROS功能包便于按模块阅读与二次开发。目前已有4358人学习下载。读者可从中获得EKF状态模型与传感器融合的实现思路、雷达数据预处理与坐标转换的代码参考、建图与定位节点的配置方式以及测试用例和示例数据带来的排错与验证线索适合作为ROS小车建图定位项目的实践素材。1. 从一堆飘移的里程计说起这套 robot_localization 资源到底能救什么场如果你玩过 ROS 小车大概率经历过这种崩溃明明车走的是直线rviz 里的轨迹却像喝多了画出来的弧线轮式里程计跑个三五米还行一转弯、一打滑位姿直接飘到姥姥家。这时候光靠odom发 TF建出来的地图墙是歪的导航更是无从谈起。这套资源围绕的就是这个痛点——用robot_localization这个 ROS 里最经典的传感器融合包把轮式里程计、IMU、激光雷达的定位信息揉到一起通过扩展卡尔曼滤波EKF输出一条稳定、可用的位姿估计再配合雷达做环境建图。它适合正在折腾 ROS 小车建图、被里程计漂移折磨、想搞明白 EKF 到底怎么配的从业者和学生。资源本身是一套可复现的配置与流程不是纯理论照着调能跑通。2. robot_localization 与 EKF 融合先搞懂它在算什么再动手配2.1 为什么单靠轮式里程计一定会飘轮式里程计的本质是积分从编码器读出左右轮转了多少圈乘以轮径得到位移再按两轮差速算出航向变化。问题在于这个积分过程没有任何外部绝对参考。轮子打滑、地面不平、轮胎磨损导致轮径变化、两轮间距标定不准误差会一点点累积而且航向角的误差是“不可逆”的——一旦转错了几度后面所有位移方向全跟着错。IMU 的角速度积分短期准、长期飘激光雷达的 scan matching 能提供绝对约束但计算量大、在长廊等退化场景容易失效。所以工程上的常见做法是用 EKF 把这几路各有所长的数据按协方差加权融合谁在当下更可信就多信谁。robot_localization提供的ekf_localization_node就是干这个的。它内部维护一个 15 维状态向量位置 xyz、姿态 rpy、线速度 xyz、角速度 rpy、线加速度 xyz你可以按需只启用其中一部分。每个传感器输入都带一个协方差矩阵EKF 根据协方差决定融合权重。协方差设得越小代表你越信任这路数据。2.2 两套 EKF 的分工local 与 global 别混用这是新手最容易翻车的地方。robot_localization通常要起两个 EKF 实例ekf_local_node输入轮式里程计 IMU输出连续的odom - base_linkTF。它负责短时间内的平滑和连续性不处理跳变。ekf_global_node输入 GPS室外或激光定位室内如 AMCL 的位姿输出map - odomTF负责把局部里程计拉回全局坐标系消除累积漂移。如果你只有一个 EKF把激光定位和轮式里程计一起塞进去会出现位姿跳变——激光定位偶尔跳一下EKF 跟着跳小车导航直接抽搐。分开之后local 保证平滑global 负责纠偏各司其职。2.3 配置文件逐参数拆解下面是一份我常用的ekf_local.yaml针对室内差速小车、带 IMU 的场景。先看完整配置再逐段说。# ekf_local.yaml ekf_local_node: ros__parameters: frequency: 30.0 # 滤波输出频率跟传感器最高频率对齐 sensor_timeout: 0.1 # 某路传感器超时多久后不再使用 two_d_mode: true # 小车在平面运动锁死 z/roll/pitch publish_tf: true # 发布 odom - base_link map_frame: map odom_frame: odom base_link_frame: base_link world_frame: odom # local EKF 的 world 就是 odom # 轮式里程计只信 x 方向速度和 yaw 角速度 odom0: /wheel_odom odom0_config: [false, false, false, false, false, false, true, false, false, # vx false, false, true, # vyaw false, false, false] odom0_differential: false odom0_relative: false odom0_queue_size: 10 # IMU信 yaw 角速度和 x 方向线加速度 imu0: /imu/data imu0_config: [false, false, false, false, false, false, false, false, false, false, false, true, # vyaw true, false, false] # ax imu0_differential: false imu0_relative: true imu0_remove_gravitational_acceleration: true imu0_queue_size: 10two_d_mode: true是关键。小车在地面跑z、roll、pitch 理论上不变锁死这三个自由度能大幅减少 EKF 被噪声带偏的概率。如果你做的是无人机或机械臂这行必须关掉。odom0_config这个 15 位布尔数组的顺序是固定的x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az。轮式里程计我一般只开vx和vyaw因为轮式里程计算出的绝对 x、y 位置本身就是积分来的直接喂给 EKF 等于把漂移也喂进去了。让它只提供速度位置由 EKF 自己积分反而更稳。imu0_relative: true表示 IMU 数据相对于第一帧做归一化避免上电时 IMU 的绝对朝向和机器人坐标系不一致。imu0_remove_gravitational_acceleration: true必须开否则重力分量会被当成运动加速度z 轴直接爆掉。2.4 启动文件与 TF 树验证配置写完用 launch 文件把节点拉起来# ekf_local.launch.py from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node( packagerobot_localization, executableekf_node, nameekf_local_node, parameters[config/ekf_local.yaml], remappings[(odometry/filtered, odom)] ) ])启动后第一件事不是看 rviz而是查 TF 树ros2 run tf2_tools view_frames生成的frames.pdf里odom - base_link这条边必须由ekf_local_node发布而不是原来的轮式里程计节点。如果发现有两个节点都在发odom - base_linkTF 会报“multiple parents”错误rviz 里模型疯狂闪烁。解决办法是把轮式里程计节点的publish_tf关掉只让它发/wheel_odom话题。提示view_frames生成的 pdf 里每个节点旁边会标注发布频率和最近一次更新时间。如果某个 TF 超过 1 秒没更新说明对应节点挂了或话题名对不上。3. 雷达定位与环境建图从单帧点云到可用栅格地图3.1 雷达选型与数据预处理环境建图对雷达的要求比导航低一些但也不是随便一个都行。常见的是 2D 单线雷达如 RPLIDAR A1/A2、EAI X2和 3D 雷达转 2D 用。2D 雷达输出sensor_msgs/LaserScan数据量小、建图快适合室内平坦环境。3D 雷达如 Velodyne、Livox输出PointCloud2需要先降维成 2D 切片再喂给建图算法。数据预处理主要做两件事过滤无效点和限制距离范围。无效点通常是inf或nan直接进建图算法会导致栅格地图出现放射状黑线。距离限制是因为太远的点噪声大、太近的点可能是车体自身遮挡。# scan_filter.py import rclpy from rclpy.node import Node from sensor_msgs.msg import LaserScan import math class ScanFilter(Node): def __init__(self): super().__init__(scan_filter) self.sub self.create_subscription(LaserScan, /scan, self.cb, 10) self.pub self.create_publisher(LaserScan, /scan_filtered, 10) self.min_range 0.15 # 小于此值视为车体遮挡 self.max_range 8.0 # 大于此值噪声大截断 def cb(self, msg): out LaserScan() out.header msg.header out.angle_min msg.angle_min out.angle_max msg.angle_max out.angle_increment msg.angle_increment out.time_increment msg.time_increment out.scan_time msg.scan_time out.range_min self.min_range out.range_max self.max_range ranges [] for r in msg.ranges: if math.isinf(r) or math.isnan(r) or r self.min_range or r self.max_range: ranges.append(float(inf)) else: ranges.append(r) out.ranges ranges self.pub.publish(out) def main(): rclpy.init() rclpy.spin(ScanFilter()) rclpy.shutdown()这段代码把超出[0.15, 8.0]范围的点全部置为inf建图算法会自动忽略。range_min和range_max要同步改否则 rviz 里显示范围不对。3.2 用 slam_toolbox 建图并与 EKF 位姿配合建图算法我一般用slam_toolbox它对 2D 激光的 scan matching 做得比较成熟而且支持在线和离线两种模式。关键配置是让它使用 EKF 输出的odom作为里程计先验而不是原始轮式里程计。# mapper_params_online.yaml slam_toolbox: ros__parameters: odom_frame: odom map_frame: map base_frame: base_link scan_topic: /scan_filtered mode: mapping resolution: 0.05 # 栅格分辨率 5cm max_laser_range: 8.0 minimum_time_interval: 0.5 transform_timeout: 0.2 use_scan_matching: true use_scan_barycenter: true minimum_travel_distance: 0.3 # 走 30cm 才插入新帧 minimum_travel_heading: 0.3 # 转 0.3rad 才插入新帧 loop_search_maximum_distance: 3.0 do_loop_closing: trueminimum_travel_distance和minimum_travel_heading这两个参数直接决定地图质量和计算量。设太小静止时雷达噪声也会触发插入地图糊成一团设太大转弯时细节丢失墙角变圆角。室内小车 0.3m / 0.3rad 是个比较稳的起点。启动顺序很重要先起 EKF确认odom - base_link正常再起雷达驱动和过滤节点最后起 slam_toolbox。如果顺序反了slam_toolbox 启动时拿不到 TF会直接报错退出。3.3 建图过程中的 TF 与话题检查清单建图跑起来后用下面几条命令快速排查# 查看 TF 树是否完整 ros2 run tf2_tools view_frames # 查看关键话题频率 ros2 topic hz /scan_filtered ros2 topic hz /odom # 查看 EKF 输出位姿 ros2 topic echo /odom --once/scan_filtered的频率应该和雷达标称频率一致通常 5-10Hz。/odom应该稳定在 30Hz 左右。如果/odom频率远低于配置的 30Hz检查 EKF 的frequency参数和传感器输入频率——EKF 输出频率不会超过输入频率。注意建图时尽量让小车慢速、匀速移动避免急转弯和原地打转。EKF 在角速度突变时会有短暂滞后slam_toolbox 如果在这时候插入帧地图会出现重影。4. 避坑与排查那些让我熬夜的 EKF 和建图问题4.1 现象rviz 里小车模型疯狂闪烁TF 报 multiple parents原因轮式里程计节点和 EKF 节点同时发布odom - base_link。ROS 的 TF 树要求每个 child frame 只能有一个 parent两个节点抢着发rviz 就在两个位姿之间来回跳。解决找到轮式里程计节点的配置把publish_tf设为false或者把它的 TF 话题重映射到一个不用的名字。确认ros2 run tf2_tools view_frames生成的 pdf 里odom - base_link只连到ekf_local_node。4.2 现象EKF 输出位姿在静止时缓慢漂移原因IMU 的角速度零偏没有校准或者imu0_differential和imu0_relative设置不当。IMU 静止时输出的角速度不是绝对零EKF 把这个小值积分进去yaw 就慢慢转。解决先做 IMU 静态校准采集几分钟静止数据算零偏。然后在 EKF 配置里把imu0_differential设为true让 EKF 使用角速度的变化量而不是绝对值。如果 IMU 驱动支持开启内部的零偏估计功能。4.3 现象建图时地图出现放射状黑线原因雷达数据里有inf或nan值直接进了 slam_toolbox。这些无效点被当成极远距离的有效测量在栅格地图上从机器人位置向外辐射。解决在雷达驱动和 slam_toolbox 之间加一层过滤节点如 3.1 节的scan_filter.py把所有无效值和超范围值置为inf。同时检查slam_toolbox的max_laser_range是否和过滤后的范围一致。4.4 现象EKF 启动后位姿直接跳到很远的地方原因odom0_relative或imu0_relative设为falseEKF 把传感器第一帧的绝对值当成了初始位姿。如果轮式里程计上电时不是从零开始或者 IMU 的 yaw 不是从零开始EKF 就会从一个错误的起点开始积分。解决对于轮式里程计odom0_relative一般设为false因为轮式里程计本身就从零开始。对于 IMUimu0_relative必须设为true让 EKF 以第一帧为基准做归一化。如果问题依旧检查 IMU 驱动是否在启动时发布了绝对朝向如磁力计融合的 yaw这种数据不能直接喂给 EKF。4.5 现象slam_toolbox 建图时 CPU 占用率飙升地图更新卡顿原因minimum_travel_distance和minimum_travel_heading设得太小小车静止时雷达噪声不断触发新帧插入scan matching 频繁计算。或者resolution设得太小如 0.01栅格数量爆炸。解决把minimum_travel_distance调到 0.3 以上minimum_travel_heading调到 0.3 以上。resolution保持 0.05 即可除非做精细建图否则没必要更小。另外检查loop_search_maximum_distance设太大如 10.0会导致回环检测搜索范围过大计算量成倍增加。5. 进阶技巧用 rosbag 回放做可复现的 EKF 调参调 EKF 参数最痛苦的是每次都要推着小车跑一遍跑完发现参数不对再跑一遍。血泪经验是先用 rosbag 把一轮完整数据录下来之后所有调参都在回放里做效率提升十倍不止。录包时把 EKF 需要的所有原始话题都录上ros2 bag record /wheel_odom /imu/data /scan /tf /tf_static -o ekf_tuning_bag回放时用--clock让节点使用 bag 里的时间戳同时启动 EKF 和 rvizros2 bag play ekf_tuning_bag --clock然后改一次ekf_local.yaml重启 EKF 节点观察 rviz 里/odom的轨迹。判断标准很简单让小车在 bag 里走一段直线和一段 90 度转弯看 EKF 输出的轨迹是否和实际物理轨迹吻合。如果直线段轨迹弯曲说明vyaw的协方差设得太小或 IMU 零偏没校准如果转弯段轨迹滞后说明vyaw协方差设得太大EKF 不够信任 IMU。下面这张表是我调参时常用的协方差起点针对普通消费级 IMU 和轮式里程计传感器参数建议初值调整方向轮式里程计vx 协方差0.01打滑严重调大轮式里程计vyaw 协方差0.05转弯飘调大IMUvyaw 协方差0.005静止漂移调大IMUax 协方差0.1振动大调大调参时每次只改一个参数改完回放同一段 bag对比轨迹。不要一次改五个参数否则你永远不知道是哪个起了作用。还有一个验证 EKF 是否收敛的技巧在 rviz 里同时显示/odomEKF 输出和/wheel_odom原始里程计让小车走一个闭合矩形。如果 EKF 输出的终点和起点重合度明显好于原始里程计说明融合有效如果两者差不多说明 EKF 没起到纠偏作用检查协方差是否设得过于保守。从那以后我每次调 EKF 都强制先录一段包含直线、转弯、静止三种状态的 bag所有参数改动都在回放里验证确认没问题再上实车。这样省下的不只是时间还有被小车撞坏的桌角和摔碎的杯子。希望帮到你。本文还有配套的精品资源点击获取
