干机器人SLAM这一行建图和定位从来都是两件事。很多团队用Cartographer把地图建出来了结果到了正式上线的环节反而不会用了要么让机器人在已知环境里重新跑一遍建图要么干脆每次开机都重新建一次浪费时间还容易在重复区域里漂。实际上Cartographer早就内置了纯定位模式尤其对基于3D地图的纯定位它能直接加载之前保存的.pbstream地图在冻结地图的前提下只做实时点云到地图的匹配稳定输出机器人当前位姿。这篇内容就围绕cartographer基于3d地图的纯定位模式从原理、地图准备、配置、实操到排查按我做项目时的思路完整讲一遍。适合正在搞自主导航、巡检机器人、AGV和无人车的朋友特别是准备从2D定位往3D方向升级的工程师。1. 项目核心拆解纯定位模式到底在做什么1.1 建图与定位是两码事别再混着干我见过不少项目组算法工程师张口就是“我们用的Cartographer”但细问之下要么只跑了建图demo要么在建图模式下硬跑定位。这两种状态的本质区别很多人没想清楚建图是边探索边构建全局一致的submap集合需要持续维护图优化、回环闭合、累积误差修正计算量大、实时性压力也大而纯定位模式是“地图已经存在且默认为真”系统不再新建submap也不做回环只做一件事——把当前传感器采集到的3D点云和已有地图做匹配迭代求解出机器人相对地图坐标系的位姿。纯定位模式下Cartographer的Local SLAM前端依然在跑包括点云预处理、自适应体素滤波、实时相关扫描匹配、Ceres位姿优化等但Global SLAM后端的图优化被大幅限制不再对新生成的节点做大规模全局调整。这意味着定位的计算开销远低于建图可以在车载主控甚至嵌入式计算平台上跑同时定位结果相对于已有地图是稳定的不会因为后端优化把地图坐标体系改掉。1.2 3D纯定位的完整数据链路我们看一个典型的3D纯定位环节传感器假设是Velodyne VLP-16这类3D激光雷达加上IMU和轮式里程计。链路大致是3D激光雷达原始点云进入节点先做时间同步把多帧点云累积成一次range data。这个累积由num_accumulated_range_data控制设成1意味着每帧原始点云单独处理设成更大的值则可以降噪、补密。对累积后的点云进行自适应体素滤波降低点云密度同时保留环境几何特征。这个过程很关键3D雷达一帧几十万甚至上百万点不降采样直接送入匹配器计算量会非常夸张。以IMU和里程计的输出作为预测初值先跑实时相关扫描匹配Real-Time Correlative Scan MatchingRTCSM在局部窗口内给出一个粗略但可靠的位姿估计。再把这个估计作为初值送入Ceres扫描匹配器对位姿做精修让当前点云与已有地图的吻合度最大化。最终输出的是一个从map坐标系到odom坐标系的变换等价于定位系统对里程计累积误差的修正量机器人自身在odom坐标系下的位姿由底盘里程计或IMU积分提供。这条链路和建图模式最大的差异在于第4步之后建图模式会把匹配好的位姿插入到PoseGraph中参与后端优化甚至触发回环检测纯定位模式则只更新当前节点在已加载submap中的匹配结果不再对已经存在的submap做增量修正。1.3 为什么非要基于3D地图2D地图不香吗在开阔平地或简单仓库场景2D激光定位确实够用计算量小、调试也简单。但一旦环境变成多层厂房、坡道、码头、矿井、半室外园区问题就来了2D SLAM把激光扫描压缩到一个水平平面上overlap不足时匹配退化严重。我举一个实际踩过的例子某园区巡检项目路面有坡度2D SLAM在建图时把坡道强行投影到平面结果坡顶和坡底在2D地图上几乎重叠机器人走到坡道中段时2D定位结果开始在前后两个位置之间来回跳。后来换成Cartographer的3D建图3D纯定位坡道在高度维上自然区分开点云匹配约束从2个自由度扩展到了6个自由度定位稳定性大幅提升。3D的另一个优势是特征丰富。3D点云里柱子、墙棱、地面、天花板、标识牌都成为约束哪怕在空旷大厅这种2D定位容易“迷路”的地方3D匹配也能借助地面、墙角等几何元素把位姿锁住。当然3D也不是没代价传感器更贵、数据量更大、配置更复杂、IMU几乎是必需品。所以选型时要看场景如果环境确实比较“平”2D是更务实的选择但只要环境在高度维上有信息就别吝啬上3D否则后面调定位会调到怀疑人生。1.4 纯定位模式适合哪些落地场景把概念理清楚之后落地场景其实很清晰仓储AGV高架库货架固定地图基本不变AGV开机后只需快速定位就能立刻进入导航调度。园区巡检机器人路线固定但环境范围大靠纯定位全局路径规划不需要每次重新建图。无人配送车固定园区、固定楼宇的配送路线车辆跨天运行纯定位模式保证每次开机都在同一坐标系下工作。多机协同定位一台机器建图多台机器加载同一张地图做纯定位避免每台机器都消耗建图算力。这些场景的共同点就是“地图长期有效、环境相对稳定”。如果环境一直在变、布局经常调整那就没有纯定位的生存空间得走动态SLAM或者定期重绘地图的路线了。2. 3D地图的准备没有一张好地图定位无从谈起2.1 用Cartographer先建一张高质量的3D地图纯定位模式的地图来源基本都来自Cartographer自身的3D建图流程。前期建图质量直接决定后期定位的成败这个环节值得花最多的时间。建图流程一般分四步配置传感器、标定外参、采集数据、离线或在线优化。传感器配置上3D建图一般需要3D激光雷达IMU。Cartographer 3D模式对IMU依赖度很高因为它需要借助IMU的重力向量来约束z轴的姿态没有可靠的IMU3D定位就是空中楼阁。外参标定方面雷达与IMU、雷达与底盘之间的tum齐坐标变换必须精确到厘米级和度级否则建出来的地图会有虚影后边定位也会出现系统性的偏移。采集数据时要注意轨迹设计尽量让机器人走“回字形”或“8字形”路线保证各个区域被多次覆盖并制造足够的回环。回环是消除累积误差的关键我见过太多建图翻车案例都是因为轨迹太直、回环太少导致地图首尾对不上。另外建图时要清空动态物体推车、人员、停放的车辆都会在地图里留下“鬼影”定位时这些鬼影是灾难级别的干扰。3D建图跑完后如果用的是在线模式可以先跑一次离线优化cartographer_offline_node对采集的数据反复优化得到更平滑的轨迹和更一致的地图。离线优化对算力要求更高但效果通常比在线建图好不少。2.2 pbstream文件才是真正的“地图”很多同学对3D地图的理解还停留在PNG栅格图或者PCD点云文件。但Cartographer的3D地图核心载体是.pbstream文件它保存的不仅仅是点云而是完整的SLAM状态包括所有submap的激光数据range data和位姿PoseGraph中所有节点pose node的位姿约束关系包括scan-to-submap约束和回环约束传感器元数据、轨迹信息等。也就是说pbstream文件里存的不只是“长得像地图的几何数据”而是“可以让Cartographer重新恢复SLAM完整上下文的数据结构”。这个特性对纯定位模式至关重要因为Cartographer在加载pbstream之后不需要重新构建submap而是直接把已有的submap加载进PoseGraph机器人在这个“已经固定好的地图”里做匹配。如果只是想要一个可视化的3D点云地图可以直接用Cartographer的Python脚本把pbstream导出成xyz点云文件或者用cartographer_ros的相关工具转成其他格式。但要注意定位用的地图始终应该是原始的pbstream而不是导出后的点云因为一旦导出submap之间的关联和位姿信息就丢了。2.3 地图质量检查点云密度、闭合差与鬼影建完图先别急着上定位先做三项检查第一点云密度是否覆盖充分。把地图导成xyz后用CloudCompare之类的工具打开观察点云在墙面、柱体、地面处是否连续。如果某些区域点云稀疏甚至空洞说明建图时该区域覆盖不足要么补采要么重新规划轨迹。第二闭合差是否在可接受范围。具体做法是看地图里同一面墙在不同轨迹下是否有双重墙、错位重影。3D建图常见的“口红效应”就是起点和终点没有完全对齐导致整个地图呈微弧状这在长走廊场景里特别明显。如果里面只有轻微重影可以通过离线优化修正如果严重错位只能回去补采。第三鬼影静态化。建图期间任何移动的东西都会在地图里留下痕迹。定位的时候这些鬼影会对扫描匹配造成错误约束。一个比较笨但很有效的办法是在地图里人工检查一遍明显动态区域比如门厅、过道如果发现鬼影太多重新建图比后期清洗更省事。另外对于可视化展示的需求我习惯把导出的xyz点云通过Potree转成Web端可浏览的3D点云页面。这样不仅自己调试方便给甲方做方案演示时也很直观。毕竟3D地图不像2D栅格图直接在浏览器里就能旋转查看这对“3d地区地图可视化”类的展示场景特别加分。3. 纯定位模式的关键配置与坐标体系3.1 坐标系与TF树最容易翻车的地方Cartographer纯定位模式对坐标系的要求是非常严格的很多定位问题排查到最后根因往往是TF树断链或者frame定义错误而不是算法本身的问题。标准的TF树应该是这样的map地图坐标系全局坐标系也就是pbstream中submap所在的坐标系固定不变。odom里程计坐标系由机器人底盘里程计或Cartographer自己发布的中间坐标作为前端预测和局部位姿的参考。base_link机器人本体坐标系机器人底盘的载体坐标系。imu_link、激光雷达坐标系通过URDF或TF static发布器固定在base_link下。在纯定位模式下map到odom的变换就是定位系统的最终输出它表示“里程计的估计”和“在地图中的真实位姿”之间的差异。odom到base_link由底盘/里程计发布。base_link到各传感器则由静态TF发布。有一个参数需要特别注意provide_odom_frame。如果设置为trueCartographer会自己发布odom坐标系如果外部里程计已经提供了odom建议设置为false避免TF树冲突。我在实际项目中一般让底盘里程计发布odom到base_linkCartographer只负责map到odom这样定位系统只是一个“修正器”不干扰底盘自身的运动控制。3.2 lua配置文件逐项解读纯定位模式需要一份独立的lua配置不能直接拿建图配置硬套。下面是一份基于3D传感器、适合VLP-16级别雷达的定位配置逐项注释一下关键参数的含义方便参考调参。这份配置对应localization_3d.lua文件include map_builder.lua include trajectory_builder.lua options { map_builder MAP_BUILDER, trajectory_builder TRAJECTORY_BUILDER, map_frame map, tracking_frame imu_link, published_frame base_link, odom_frame odom, provide_odom_frame false, publish_frame_projected_to_2d false, use_pose_extrapolator true, use_odometry true, num_point_clouds 1, use_laser_scan false, use_multi_echo_laser_scan false, num_subdivisions_per_laser_scan 1, } MAP_BUILDER.use_trajectory_builder_3d true MAP_BUILDER.num_background_threads 4 MAP_BUILDER.collate_landmarks false TRAJECTORY_BUILDER_3D.num_accumulated_range_data 1 TRAJECTORY_BUILDER_3D.min_range 0.3 TRAJECTORY_BUILDER_3D.max_range 50. TRAJECTORY_BUILDER_3D.voxel_filter_size 0.05 TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching true TRAJECTORY_BUILDER_3D.ceres_scan_matcher.translation_weight 0.1 TRAJECTORY_BUILDER_3D.ceres_scan_matcher.rotation_weight 1.0 TRAJECTORY_BUILDER_3D.imu_gravity_time_constant 10. TRAJECTORY_BUILDER_3D.adaptive_resolution 0.05挨个说几个重点map_frame、odom_frame、tracking_frame、published_frame这四个就是整个TF树的骨架。tracking_frame是传感器参考系Cartographer会在这个坐标系下做位姿估计一般用IMU所在的坐标系published_frame是我们要对外输出位姿的坐标系一般就是底盘坐标系。use_odometry建议在底盘可靠时打开。Cartographer会把里程计作为位姿预测的输入之一能明显提升动态场景下的鲁棒性如果底盘里程计质量很差比如轮胎打滑严重的场景反而关掉靠IMU和激光匹配。num_accumulated_range_data3D激光雷达一般设成1或2。设成更多的帧累积相当于做了一次“时间积分”可以把单帧点云稀疏的问题缓解但会增加延迟动态环境里请注意延迟过大带来的匹配滞后。use_online_correlative_scan_matching建议打开。它能在正式Ceres匹配之前快速给一个粗略初值避免Ceres直接陷入局部最优。ceres_scan_matcher.translation_weight和ceres_scan_matcher.rotation_weight这两个参数决定优化时平移和旋转的权重比例。3D定位里旋转漂移往往比平移更致命所以rotation_weight一般可以高于translation_weight我常用0.1和1.0的组合。如果实际环境里平移误差大再尝试把translation_weight调到0.2~0.3。3.3 launch文件与话题remap配置好lua之后launch文件负责把实际传感器话题和Cartographer节点接起来。下面是一个比较标准的3D纯定位launch写法launch param name/use_sim_time valuefalse / node namecartographer_node pkgcartographer_ros typecartographer_node outputscreen args-configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename localization_3d.lua -load_state_filename $(find your_package)/maps/your_map.pbstream remap frompoints2 to/velodyne_points / remap fromimu to/imu/data / /node node namecartographer_occupancy_grid_node pkgcartographer_ros typecartographer_occupancy_grid_node outputscreen remap frommap to/localization_map / /node /launch注意-load_state_filename参数它接收的就是我们提前写好的pbstream文件路径。每次更换地图只需要改这个路径不需要改动节点逻辑。points2和imu的remap要和机器人实际的话题名对应否则Cartographer收不到数据节点虽然起来但根本没有输入。如果还想在rviz里看到栅格化后的定位地图可以额外启动cartographer_occupancy_grid_node不过要注意这只是用于可视化纯定位的主力匹配数据始终是3D点云和pbstream中的submap。4. 实操实录从启动到验证纯定位4.1 先决条件保证底盘、雷达、IMU都在线启动纯定位之前我的习惯是先启动底盘驱动和传感器驱动然后检查三个东西能正常听到点云话题吗IMU话题有数据且重力向量方向合理吗静态时加速度计读数大概在z轴方向为9.8m/s²TF树发布正常吗这几个检查可以用简单的命令行完成roslaunch your_robot robot_base.launch roslaunch your_robot sensors.launch # 检查频率 rostopic hz /velodyne_points rostopic hz /imu/data # 检查TF树 rosrun tf view_frames4.2 定位前的“证据”必须给初始位姿这是3D纯定位最容易踩的坑也是和建图模式最不一样的地方。建图模式下Cartographer可以从原点开始逐渐构建地图但纯定位模式下系统加载地图后它并不知道机器人此刻在地图的哪个位置。如果初始位姿给错了后面所有匹配都是在错误区域找相似的几何结构轻则定位收敛慢重则直接发散。所以启动定位节点的第一件事是在rviz里用2D Pose Estimate按钮在地图对应位置给一个大概的初始位姿。不用非常精确方向别差太多、位置别偏出几米就基本能收敛。给完初始位姿后观察点云和地图的对齐程度如果基本重合说明初始估计是靠谱的如果差得很远需要重新给。4.3 启动定位节点并观察收敛初始位姿发布后再启动定位节点或者调换顺序也行——先启动节点再立刻发布初始位姿。实际操作中我一般先启动节点、确认话题有数据再在rviz里给位姿这样能避免节点一启动就带着错误初值乱匹配。在rviz里添加map和map/PointCloud2显示观察两个关键信号map到odom的TF变换是否在持续平滑变化。刚开始可能会有一小段跳变那是系统在从初始位姿往精确位姿收敛如果几秒后还在持续大幅跳动说明匹配不稳定。实时点云和地图点云是否重叠。重叠越高定位越准。经验上收敛后两者偏差一般能在10cm以内取决于雷达精度和环境特征。如果想量化定位结果可以看Cartographer节点输出的位姿话题比如/tf里的map-base_link变换把它录成bag后处理的时候和真值对比评估误差。4.4 长时间运行与重定位机器人长时间运行定位误差可能存在缓慢漂移尤其是环境发生微小变化比如货架挪了、门开了。纯定位模式下Cartographer会持续做scan-to-map匹配正常情况下能保持定位稳定。如果出现漂移增大的趋势可以考虑定期用后端优化或人工重定位修正但这是更进阶的话题。遇到定位彻底丢失的情况最快的恢复手段不是重启节点而是在rviz里重新发布一个接近真实位置的初始位姿。Cartographer在收到新的初始位姿后会重新初始化局部SLAM不需要重启进程。5. 常见问题与排查经验一次把坑踩平5.1 定位结果跳变、漂移严重跳变一般有三个来源初始位姿给得太差、传感器时间没有对齐、环境特征退化。初始位姿的问题前面说过在rviz里校正到合理范围即可。时间对齐问题比较隐蔽雷达点云和IMU数据如果时间戳不一致扫描匹配时会引入系统误差表现为定位结果周期性抖动。排查方法是录制bag回放时检查点云话题和IMU话题的时间戳偏差如果偏差超过几十毫秒就需要在launch里做时间同步或者检查驱动的时间戳发布逻辑。环境特征退化指机器人走到长走廊、大面积平地这类自重复区域匹配约束不足定位误差会沿着某个方向膨胀。解决办法是尽可能让扫描匹配利用更多维度的信息比如3D模式下保留地面的约束如果依然退化只能考虑在地图中增加特征锚点或者调整运行路线避免长距离直线穿越。5.2 点云匹配完全不收敛如果定位节点启动后实时点云始终无法与地图对齐多半是pbstream加载的地图坐标系和当前传感器坐标系不匹配。最常见的原因有建图时的传感器外参和当前运行时的外参不一致。比如换过雷达支架、重新标定过IMU但launch里的TF没有同步更新。tracking_frame设置错误。如果设的是base_link导致没有跟随传感器运动匹配结果会一塌糊涂。建图使用的传感器配置和定位时的配置不同。比如建图时用了32线雷达定位时换成了16线雷达点云密度差异太大匹配器需要重新调参。遇到匹配不收敛先别急着调算法参数把外参、frame之类的基础问题查清楚90%的“不收敛”其实是配置错误。5.3 TF树错乱导致地图错位TF问题太常见了。定位节点刚启动时如果TF树不完整rviz里会直接报找不到坐标变换或者地图显示得歪七扭八。排查步骤很固定先用rosrun tf view_frames生成TF树图确认map、odom、base_link、tracking_frame在一棵树上再检查各frame之间的变换是否持续更新最后检查是否有两个节点同时发布同一个frame的TF这会导致跳变和错乱。一个小技巧关闭除底盘驱动之外所有可能发布TF的节点只留Cartographer和底盘驱动然后逐步加节点定位问题基本能锁定。5.4 动态环境误匹配仓库里有人推车经过定位结果瞬间被“拉走”过一会儿才恢复。这是动态环境下的经典问题。Cartographer的纯定位模式本质上假设环境是静止的动态物体会给扫描匹配引入异常约束。实际项目里我常用的减轻措施包括提高min_range、调小max_range避开雷达近处行人和远处大型车辆开启并使用use_online_correlative_scan_matching增加匹配初值的鲁棒性如果动态物体会长期存在比如某个区域总有叉车作业最好的办法是重新建图把该区域在低频时段采集或者后期在点云里手动剔除。5.5 常见问题速查表整理一个速查表方便现场排查问题现象可能原因快速处理定位完全不收敛pbstream未加载、初始位姿错误、TF断链先确认-load_state_filename再检查TF树重新发布初始位姿定位跳变时间同步差、外参错、特征退化检查时间戳偏差复核外参地图错位TF重复发布、frame不一致view_frames检查TF清理重复发布漂移增大环境变化、动态物体干扰提高匹配权重、重映射必要时重定位点云与地图分层雷达安装高度/角度变化重新标定传感器外参6. 一点私人心得关于调试习惯最后再多说一句个人习惯。我在做Cartographer纯定位项目时一定会保存两个版本的配置文件一个是“保守版”参数尽量稳定适合长时间运行另一个是“激进版”匹配权重更激进、滤波更小适合短时间快速收敛验证。调试的时候先用激进版把初始位姿调准等定位稳定了再切回保守版这样既能保证调试效率又不影响正式运行稳定性。另外别把纯定位模式当作“建图模式的简单衍生品”。它和建图是完全不同的工程思路建图允许犯错反正后端能修定位则必须战战兢兢地图一错后边全是错的。所以每次换场地、换传感器、换地图我都会强制自己重新走一遍从地图质检到初始位姿验证的完整流程宁可多花半小时也不愿意在真机上碰运气。这套方法实测下来很稳希望对你有用。