多传感器融合的移动机器人SLAM：激光雷达+IMU+里程计实战
简介：面向ROS开发者和SLAM入门学习者，围绕Autolabor Pro1小车、小觅双目相机及Delta-1A 2D激光雷达搭建的完整建图定位与路径规划工程。项目基于Ubuntu 16.04和ROS Kinetic，整合gmapping、cartographer_ros建图算法、VINS-Fusion融合定位及ROS navigation导航框架，并给出map→odom→base_link→传感器坐标系的TF变换配置思路，适合需要对照真实硬件学习多传感器融合与自主导航的读者。资源共109个文件，以C++源码、launch启动文件、yaml参数配置、urdf模型、rviz视图及pgm栅格地图为主，另含说明文档与驱动配置，压缩包仅6MB，便于快速下载和部署。目前已有613人学习下载。内容覆盖底盘驱动、雷达数据解析、静态坐标变换、建图与导航参数调优等关键环节，可直接参考或移植到相似车型与传感器组合，有效缩短环境搭建和功能验证时间。
1. 为什么一台小车要把激光雷达、IMU 和里程计绑在一起做 SLAM
从仓库里拉一个带 C++ 源码的 ROS 机器人项目不难，难的是把它跑稳。很多入门者第一步只接激光雷达，发现建图时走廊尽头墙是歪的，转角处地图还会叠成重影。原因不是雷达坏了，而是激光在长直走廊里缺少纵向约束，位姿估计只靠帧间匹配，一点角速度误差都会被轨迹放大。这时候给小车加上 IMU 和轮式里程计，等于在视觉之外补了高帧率的转动观测和局部位移约束，三者互相兜底，这就是这个标题背后真正想解决的事。
这个组合解决的是移动机器人最核心的三个问题：建图时轨迹不发散，定位时在地图中找回自己的位置，导航时能规划出一条不撞墙的路径。适合正在做室内移动机器人、AGV 或者准备 SLAM 相关岗位面试的人，也适合那些手上已经有一套 ROS 小车但一直在调不通整套流程的工程师。下面按我从零搭这套系统的顺序，把选型、驱动、标定、建图定位和路径规划拆开讲。
2. 先立框架：从 ROS 版本到 TF 树，激光雷达加 IMU 的融合方案怎么选
2.1 建图算法选型：gmapping 和 cartographer 到底怎么挑
SLAM 建图这一步，开源方案里最常碰到的就是 gmapping 和 cartographer。两者思路完全不同：gmapping 基于粒子滤波，每一帧激光都去修正粒子的权重，遇到闭环场景能靠粒子多样性把轨迹拉回来；cartographer 走的是图优化路线，用子图（submap）累积扫描匹配，再用后端做回环约束。选哪个不取决于名气，取决于你的激光雷达是什么样的数据。
先看一张对比表，是日常选型时我会做的判断：
| 对比项 | gmapping | cartographer |
|---|---|---|
| 激光输入 | 只支持 2D LaserScan，要求单线雷达 | 2D LaserScan 或 3D PointCloud，支持多线雷达 |
| IMU 融合 | 基本不用 IMU，主要靠里程计 | 原生支持 IMU，imu 数据直接参与扫描匹配 |
| 回环检测 | 无显式回环，靠粒子收敛 | 有子图回环，能修正大轨迹漂移 |
| 地图形式 | 2D 栅格地图 | 2D 栅格地图，也能输出 3D 地图 |
| 对算力要求 | 低，树莓派也能跑 | 中高，推荐 x86 工控机或带 CUDA 的板子 |
| 典型代步小车 | 是，轮式里程计质量好时效果不错 | 是，但安装和参数配置成本更高 |
如果你的小车是在走廊、办公室这种规则环境里走，轮式里程计标定得又准，gmapping 完全够用，配置量小，调起来快。如果环境里有回环，比如绕一圈回到起点，或者经常走同一条路，cartographer 的优势就很明显，它能在一圈结束后修正整个轨迹。标题里同时出现 IMU 和激光雷达，我一般会优先选 cartographer，因为 IMU 数据在 cartographer 里是被认真使用的，而不是挂在那里不参与计算。
2.2 TF 树设计：base_link、laser_link、imu_link 的父子关系必须提前定死
建图定位的代码逻辑可以后调，TF 树必须在写 launch 文件前就定好。常见的小车坐标系结构是： map → odom → base_footprint → base_link ，然后 laser_link 和 imu_link 都挂在 base_link 下面，激光雷达和 IMU 相对车体是固定不变的，所以它们的坐标变换用静态变换发布就行。
下面这段 URDF 描述了我常用的挂载方式：
<link name="base_link"/>
<joint name="base_link_to_laser" type="fixed">
<parent link="base_link"/>
<child link="laser_link"/>
<origin xyz="0.2 0 0.15" rpy="0 0 0"/>
</joint>
<joint name="base_link_to_imu" type="fixed">
<parent link="base_link"/>
<child link="imu_link"/>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
</joint>
这里的 xyz 是激光雷达和 IMU 相对车体中心的位置， rpy 是安装姿态角，三个值必须和你小车上的实际装配尺寸一致。 base_link 前面还有 odom 和 base_footprint ，那部分是由底盘驱动节点发布的， odom 到 base_footprint 的变换来自轮式里程计， base_footprint 到 base_link 只是一个带高度的静态偏移。
如果 TF 树里少了某个关节，cartographer 启动时会直接报“Could not find transform source”之类的错误，所以先把这段 URDF 跑通，用 rviz 的 TF 插件查看所有坐标系的位置关系，确认雷达和 IMU 没有装反，再往下做。
2.3 数据质量检查：发布频率和消息时间戳决定融合效果
SLAM 融合算法最怕的不是噪声大，而是丢帧和乱序。激光雷达、IMU、轮式里程计三路数据在 ROS 里是独立话题，融合节点必须能拿到稳定、最新的消息。开始建图前我会用三个命令把数据质量过一遍。
第一个是查发布频率：
rostopic hz /scan
rostopic hz /imu/data
rostopic hz /odom
激光雷达的 /scan 一般在 10Hz 到 20Hz，IMU 在 100Hz 到 200Hz，轮式里程计 /odom 在 30Hz 到 50Hz。如果 IMU 只有 20Hz，说明驱动配置有问题，融合时角速度分辨率不够，转动估计会迟钝。第二个是查消息时间戳：
rostopic echo /imu/data/header/stamp --noarr
持续看几秒钟，确认时间戳是递增的，没有突然回退到 0。第三个是可视化链路：
rqt_graph
看 /scan 、 /imu/data 、 /odom 三个话题是否都连接到了建图节点或 EKF 节点。这一步能筛掉至少一半的“算法调不好”问题，因为很多所谓的地图漂移，其实是某一路传感器根本没在发布数据。
3. 从装环境到驱动跑通：鱼香ROS、激光雷达、IMU 和差速底盘
3.1 用一键脚本搭 ROS 环境，省出时间写代码
ROS 安装本身不复杂，但对刚接触的人而言，源、依赖、环境变量三步里任何一步出错都会卡住。常见做法是用鱼香ROS 的一键安装脚本，在 Ubuntu 终端里执行：
wget http://fishros.com/install -O fishros && bash fishros
执行后按提示选择对应的 ROS 版本，比如 ROS1 Noetic 或 ROS2 Humble。这个脚本会把软件源、rosdep、依赖一次配好，装完再执行：
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
如果你是 Ubuntu 20.04，用 ROS1 Noetic 搭配 cartographer 最顺手；如果是 Ubuntu 22.04，直接上 ROS2 Humble。这个标题里既然带 C++ 源码和项目说明，大概率是按 ROS1 写的，注意编译时用 catkin_make ，在 ROS2 里则对应 colcon build 。两个版本的消息接口有差异，不要混着用。
3.2 激光雷达驱动：串口权限和 topic 规约
激光雷达品牌很多，镭神、思岚、速腾、大疆，驱动包不一样，但发布的消息类型基本统一为 sensor_msgs/LaserScan 。以常见的 RPLIDAR 为例，先给串口权限：
sudo usermod -a -G dialout $USER
然后重新登录，否则当前用户访问不了 /dev/ttyUSB0 。启动驱动：
roslaunch rplidar_ros rplidar.launch
常见的驱动 launch 里会设置 serial_port 、 frame_id 、 angle_min 和 angle_max 这几个参数，分别对应串口设备名、坐标系名、扫描起始角和终止角。启动后先执行一次 rostopic echo /scan ，确认角度范围和距离值正常。激光雷达原理上靠 TOF 或三角测距，驱动包要做的就是把原始数据转成标准 LaserScan，如果角度范围只覆盖 180 度，检查 angle_min 和 angle_max 是否写成了 0 到 6.28。
3.3 IMU 驱动和差速底盘：让 /imu/data 和 /odom 两个话题稳定输出
IMU 驱动的差异比雷达大。便宜的模块一般是串口输出，驱动节点把串口数据解析成 sensor_msgs/Imu ，这个消息里包含角速度、线加速度和姿态四元数。通常我会先跑驱动，然后用 rostopic echo /imu/data 检查静止时四元数是否接近 0,0,0,1，也就是重力对齐基础是否符合预期。如果 IMU 模块不带内部姿态解算，驱动节点可以把 angular_velocity 和 linear_acceleration 直接发出来，姿态由后面的融合节点去算。
底盘驱动的核心是两件事：读轮式编码器发布 /odom ，订阅 /cmd_vel 控制电机。下面这段 C++ 代码是驱动节点里常见的里程计发布部分：
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "base_odom_publisher");
ros::NodeHandle nh;
ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("/odom", 10);
tf::TransformBroadcaster odom_broadcaster;
double x = 0.0, y = 0.0, th = 0.0;
ros::Rate rate(50);
while (ros::ok())
{
// 这里的 dx, dth 应该来自编码器读数，示例用固定增量代替
double dx = 0.005;
double dth = 0.001;
x += dx * cos(th);
y += dx * sin(th);
th += dth;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.orientation = odom_quat;
odom.twist.twist.linear.x = dx * 50;
odom.twist.twist.angular.z = dth * 50;
odom_pub.publish(odom);
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.rotation = odom_quat;
odom_broadcaster.sendTransform(odom_trans);
rate.sleep();
}
return 0;
}
odom 话题带两个 frame_id： odom 是父坐标系， base_footprint 是子坐标系。里程计的 pose 表示车体在世界中的位姿， twist 表示当前线速度和角速度，这两个都要发，因为 AMCL 定位时主要依赖 twist 做运动预测，只发位姿不发速度会直接影响后续导航。
这类源码里，常见问题就出在 odom 帧上：驱动把 frame_id 写成了 base_link ，TF 树里 odom 、 base_footprint 、 base_link 三级链路断裂，建图节点找不到传感器位置。遇到这问题别急着调算法，先看 rqt_tf_tree 里有没有完整的三级变换。
4. IMU 标定、数据融合，然后把 SLAM 建图和定位跑起来
4.1 先做 IMU 内参标定，再手解 lidar-IMU 外参
不标定的 IMU 直接凑合能用，但长时间建图会出现明显的角度漂移。IMU 内参标定施工时我会先把小车固定不动，录两小时的静止数据，再用 imu_utils 工具包处理，生成包含零偏和噪声密度的 yaml 文件。标定完成后把 yaml 里的参数填进 imu 驱动节点的参数区，或者直接在融合节点读取，作为固定值参与滤波。
IMU 内参标定能改善多少？一个典型的低端 IMU 静止时零偏可能达到 0.02 rad/s，换算下来一分钟就有 1.2 度的纯角速度误差，对一个要走十分钟的建图任务来说，这个数值足以让地图变形。标定后零偏会明显下降，剩下的主要误差来源就轮到外参。
lidar-IMU 外参标定的常规做法是用开源标定工具自动求解，典型流程是在场景里放置多个平面板，让小车原地转动，记录激光点云和 IMU 数据，算法会估计出雷达坐标系相对 IMU 坐标系的旋转和平移。如果条件简陋，退而求其次的办法是直接量安装尺寸，lsd 量好距离写在 URDF 里，角度误差控制在 2 度以内时，cartographer 的扫描匹配一般能容忍。外参错误的表现是地图在车辆转弯时出现锯齿形错位，这是判断标定好坏的最直观信号。
4.2 用 robot_pose_ekf 融合 IMU 和里程计
cartographer 自己会用 IMU 数据，但如果你想把方案做成模块化，或者打算先用 gmapping 跑一遍，建图前最好先把 robot_pose_ekf 融合节点配置好。这个节点的作用是接收 /odom 和 /imu/data ，输出一个融合后的位姿，提高频率低、误差大的一路数据。
在 launch 文件里这样配置：
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
<param name="output_frame" value="odom_combined"/>
<param name="base_footprint_frame" value="base_footprint"/>
<param name="freq" value="50.0"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="true"/>
<param name="gps_used" value="false"/>
<remap from="odom" to="/odom"/>
<remap from="imu_data" to="/imu/data"/>
</node>
freq 是融合节点的输出频率，设成 50Hz 比较合理，太高会放大 IMU 噪声，太低会让控制节点觉得位姿更新慢。 sensor_timeout 是传感器超时时间，当一路数据中断超过 0.1 秒时，EKF 会暂时只用其他路数据继续推。如果你小车上的 IMU 安装方向和车体坐标轴方向不一致，需要在 URDF 里把 imu_link 的 rpy 改写，让 IMU 的 z 轴竖直向上，x 轴朝前。融合后把 /odom_combined 作为建图或导航节点的里程计输入。
4.3 用 cartographer 跑通建图三条命令
cartographer 在 ROS1 里的启动方式是先加载自身的 lua 配置，再给雷达和 IMU 话题做 remap。下面是一个典型的建图 launch 文件：
<launch>
<node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" output="screen">
<param name="configuration_directory" value="$(find cartographer_ros)/configuration_files"/>
<param name="configuration_basename" value="my_robot.lua"/>
<remap from="scan" to="/scan" />
<remap from="imu" to="/imu/data" />
<remap from="odom" to="/odom" />
</node>
</launch>
建图开始后，手动遥控小车以 0.2 到 0.4 m/s 的速度走。走的过程中时刻盯着 rviz 里的地图，墙线是否平直、转角是否清晰，都是在验证传感器质量。建图结束后用 map_server 保存地图：
rosrun map_server map_saver -f ~/map/my_map
这会生成 my_map.pgm 和 my_map.yaml 两个文件， .pgm 是图像， .yaml 里记录分辨率、原点坐标和占据阈值。保存时注意地图名称不要带中文和空格，否则后续 AMCL 加载时会有编码问题。另外，如果你选择 gmapping，把 cartographer 的 launch 换掉，只用 scan 和 odom 两个话题，保存地图的方式相同。
4.4 从建图切换到定位：AMCL 加载地图，建图节点退场
建完图后有一个高频错误：把建图节点关了，但直接把原来的 TF 树留给 AMCL。AMCL 定位和建图不一样，它需要事先加载地图文件。我用 AMCL 的固定配置是：
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<param name="map_file" value="~/map/my_map.yaml"/>
<param name="use_map_topic" value="false"/>
<param name="odom_model_type" value="diff"/>
<param name="transform_tolerance" value="0.2"/>
<param name="base_frame_id" value="base_footprint"/>
<param name="odom_frame_id" value="odom"/>
</node>
odom_model_type 用的是 diff ，表示差速模型，如果你的底盘是全向轮，要改成 omni 。 transform_tolerance 是 TF 变换允许的延迟，过小会频繁报无法得到变换。
定位启动后，在 rviz 里用“2D Pose Estimate”给小车一个初始位姿。不要把地图切回建图模式，也不要同时启动 cartographer，不然两个节点同时发布 map 到 odom 的变换会打架。AMCL 持续工作时，激光雷达点云应该贴合地图边缘，位姿粒子逐渐收敛成一小团。
5. move_base 路径规划必调参数与整机验证技巧
5.1 代价地图和局部规划器最该动的几个参数
路径规划相关的问题，九成出在 move_base 的代价地图参数上。 inflation_radius 是障碍物膨胀半径，设大了小车离墙远、容易绕路，设小了会有剐蹭风险。对普通室内小车，我会先给 0.2 到 0.35 米，再根据车身宽度微调。 robot_radius 是机器人本体半径，实测比底盘宽度一半稍大一点即可。
局部规划器的关键参数在 DWA 里，我常用这组：
| 参数 | 推荐值 | 作用 |
|---|---|---|
| max_vel_x | 0.5 m/s | 限制线速度上限，建图时降到 0.3 |
| min_vel_x | 0.0 m/s | 允许原地转向 |
| max_vel_trans | 0.5 m/s | 平移速度上限 |
| max_vel_theta | 1.2 rad/s | 转动速度上限，防止转弯过度 |
| min_in_place_vel_theta | 0.4 rad/s | 原地旋转的最小速度，过低会转不动 |
| path_distance_bias | 32.0 | 对全局路径的跟随权重 |
| goal_distance_bias | 24.0 | 对目标点的趋近权重 |
调这三组参数的核心是平衡，路径跟随权重调太高，小车会执着于贴着全局路径走，在窄道里反复调整；调太低，它又会抄近路穿障碍。每改一次参数，用遥控器把车移到起点，继续用导航命令观察两到三轮行为再下结论。
5.2 用 rosbag 记录建图和导航过程，把偶发问题固定下来
偶发漂移和偶发定位失败是最难现场的，最好的方法是拖动 rosbag 记录完整数据。建图过程中执行：
rosbag record -O mapping_data /scan /imu/data /odom /tf /tf_static
记录完成后停止小车，之后可以反复回放这段数据，调整标定参数或 cartographer 配置，每调一次重放一次，比较地图质量。这个流程比每次重新实车测试省时间得多。回放时注意把当前正在运行的传感器驱动节点停掉，避免真实数据和 bag 数据混在一起。还有一个技巧，把 --clock 参数加上，回放时 ROS 会以 bag 里的时间戳运行，也更容易复现时序问题。
rosbag play mapping_data.bag --clock
如果你怀疑是 IMU 数据本身的问题，可以用 rqt_plot 单独画出 /imu/data/angular_velocity.z 和 /odom/twist/twist.angular.z ，看同一次左转时两路数据是否方向一致、比例是否接近。方向反了，说明 IMU 安装时 yaw 偏了 180 度，只需要在 urdf 里给 imu_link 的 rpy 加一个绕 z 轴的 180 度旋转，不用重新标定。
5.3 验证 IMU 重力对齐的一个手推小技巧
整机跑通之后，我建议动手推一次小车：先关掉所有建图定位节点，只启动底盘驱动、IMU 驱动和一个 rviz，把小车平放在桌面上，观察 /imu/data 的四元数。如果小车水平且 IMU 的 z 轴向上，四元数应该接近 0, 0, 0, 1。然后用手缓慢推小车直线前进，看 rviz 里 odom 到 base_footprint 的箭头是否笔直向前；如果箭头画出一条弧线，说明左右轮编码器标定偏差明显，需要检查轮距和编码器分辨率参数。
接着握住小车原地左转 90 度，停下后观察地图中这个转角是否成直角。这一步把 IMU 角速度和里程计角速度放一起对比，如果两者差值超过 10%，优先调里程计，不要碰 IMU 参数。把小车先后退、再前进、再绕一个直径一米的圈，观察最终位姿能不能回到起点附近。这个手推测试虽然简单，但能同时验证掉码、重力对齐和 odom 正反向这三个最容易出问题的点，全部通过后再跑自动导航，会少掉很多灵异漂移。
更多推荐
0
0- 0
已为社区贡献5条内容
所有评论(0)