Cartographer这个名字做机器人和无人车定位的人基本都绕不开。我最早用它跑3D建图的时候对“纯定位”的理解还很天真以为把建图程序跑起来地图保存下来下次加载进去机器人就能自动知道自己在哪里。结果第一次在已有3D地图上做纯定位就栽了个大跟头地图加载成功机器人在原地转了两圈Rviz里的位姿却一路乱飘最后整个轨迹干脆飞出地图外。后来翻了源码和配置我才意识到Cartographer压根没有一个独立的“纯定位模式”开关它靠的是一套“冻结旧地图轨迹 新建定位轨迹”的机制来实现只定位、不建图。这篇文章就把这个机制彻底拆开内容包括纯定位的底层逻辑是什么、建图阶段要怎么做才能方便后续定位、定位模式的配置和launch文件怎么改、初始位姿怎么给、跑起来之后漂移了怎么排查以及怎么把定位结果接到Unity 3D楼层小地图、3D地区可视化大屏这类应用里。无论你是刚接触Cartographer的新手还是已经建好图但卡在定位上的开发者这篇都能给你一个能直接上手的参考。1. 建图与定位是两回事Cartographer的纯定位到底在干什么1.1 为什么不能让机器人“边跑边建图”很多人在第一次做定位时脑子里默认的流程是“把建图程序继续跑着地图已经有了机器人应该能定位吧”。但实际做项目时这种思路会带来一堆问题。最典型的情况是环境里有人走动、有临时堆放的货物、有停停走走的其他车辆如果定位时还在不停更新地图这些临时物体会被当成永久物体写进地图地图就“脏”了。你今天在通道左边放了个箱子地图上记下一笔明天箱子搬走了地图上会多出一块不存在的障碍机器人后续匹配时就会被这块“幽灵障碍”干扰。另外还有一个很多人没意识到的坑子图一直在累积地图一直在膨胀。Cartographer的核心是子图加回环检测长时间运行会产生大量子图后端优化压力越来越大最终整个状态估计会被带偏。我见过有人让机器人在仓库里连续跑了一下午期间没有重启定位程序结果傍晚的时候位姿在坐标原点附近“鬼打墙”原地打转地图却越画越厚。纯定位之所以叫“纯定位”就是要把“更新地图”这条路径从算法链路里彻底切断机器人只负责回答“我在哪”不负责回答“周围长什么样”。从业务角度也一样仓库、园区巡检、室内配送这些场景里地图往往是一个月甚至一个季度才更新一次日常任务只需要机器人带着一张固定的3D地图反复跑。每一台车都重新建图既浪费时间又浪费存储而且多台车同时建图还会互相干扰。基于3D地图的纯定位模式本质上是“一次建图多次复用多车复用”。1.2 “纯定位模式”不是一个开关而是冻结地图的机制我第一次找定位开关的时候在Cartographer的配置里翻了半天结果发现它根本没有类似AMCL里那种“定位模式”的布尔参数。Cartographer的做法要底层得多加载一份之前保存的.pbstream状态文件里面包含了整个PoseGraph位姿图、所有子图、所有约束关系加载之后这些历史轨迹会被标记为“冻结轨迹”也就是frozen trajectory。冻结的意思很简单这个轨迹上的所有节点和子图在后续优化中都不会再变动了。然后我们再新建一条轨迹这条轨迹专门负责接收实时的激光点云、IMU、里程计数据通过前端scan-to-submap匹配把机器人的位姿不断对齐到已经冻结的旧地图上。因为旧地图是冻住的所以匹配过程只会调整新轨迹的位姿不会反过来修改地图。你甚至可以把旧地图理解成一块“巨大的、静止的拼图模板”新轨迹负责把实时点云一块一块拼到模板里去但模板本身永远不会变。这个机制也解释了为什么纯定位模式下地图加载后会有一段“收敛过程”而不是一上来就完全精准。机器人的初始位姿只是个大约值Cartographer要通过连续几帧点云和旧子图进行匹配不断修正位姿估计直到误差收敛到一定范围内。老手判断定位是否成功一般不是看第一帧而是看机器人动起来后一两秒内位姿是否稳定下来。1.3 2D地图和3D地图定位的本质区别同样叫定位2D和3D的差别非常大。2D定位通常用单线激光地图是平面栅格机器人只需要关心x、y和yaw三个自由度。3D定位则面对的是x、y、z、roll、pitch、yaw六个自由度的状态空间地图是体素化的三维子图集合。状态空间翻倍意味着匹配算法的搜索空间大得多对传感器和计算资源的要求自然也高得多。3D定位带来的好处也很明显能利用高度信息。在室外坡道、停车场上下层、楼宇大厅这类场景里2D定位很容易把不同楼层的相似结构搞混而3D定位可以通过Z轴和点云的整体形状区分出不同高度层。我做过一个地下车库的项目坡道很长单线雷达建图时上下层只靠坡道连接2D定位经常在坡道顶部丢跟踪换成多线雷达配3D地图后车辆在上坡过程中的姿态变化被完整保留下来定位稳定很多。代价就是3D定位对激光雷达的线束数量、IMU质量、外参标定精度都更敏感。2D定位就算IMU数据差一点靠激光匹配还能勉强撑住3D定位如果IMU歪了或者安装角标定错姿态估计很快发散点云匹配会完全错乱。所以做3D纯定位之前一定要先做好“地图质量”和“传感器质量”这两门功课。2. 地图是定位的上限建图阶段就得为定位留好余地2.1 传感器时间同步与外参标定定位命脉在源头有个观点我特别认同定位算法再强也是在给定数据上做文章数据源有问题后面调参都是白费功夫。尤其做3D定位第一个要查的就是外参标定。激光雷达装在机器人上相对于IMU或底盘中心的安装位置和角度有没有标准确直接决定了点云在空间中被投影到哪里。外参有偏差建图阶段可能还看不出来因为建图时前端匹配也在同步优化会把一部分外参误差“吸收”掉。但是到了纯定位阶段加载的是已经固定的地图实时点云再带着同样的外参误差去匹配误差就没有地方可吸收了于是表现为局部匹配总是差那么一点走直线还好一转弯就开始漂。时间戳同步同样关键。Cartographer在定位时拿到一帧点云会根据点云的时间戳去查询对应的TF变换然后把点云变换到tracking frame下。如果激光雷达的驱动节点和IMU节点之间时间戳偏差很大比如差了几十毫秒机器人一转起来点云就会带上一层“运动畸变”匹配误差直线上升。我建议在跑定位前先录一段静止和低速运动的bag离线回放看看Rviz里实时点云和地图的重合程度。如果静止时点云像“雪花”一样抖动大概率是IMU数据或时间同步有问题先把这个修好再谈定位。2.2 建图参数要为定位留余地别把地图建“虚胖”不少人的做法是先拿一套默认配置建图等要定位了再来调参数。实际上定位效果的上限在建图那一刻就决定了。建图时如果点云太稀疏、闭环太少、地图里有大量错位重影后面定位调参怎么调都救不回来。建图阶段有几个参数应该用心盯一下。第一个是num_accumulated_range_data它表示累积多少帧点云再算一次扫描匹配。这个值建图时通常会设得高一些比如官方3D demo里是10因为累积的帧数越多点云越密匹配质量越高。但如果你建图时把这个值压得很低地图里的体素网格会非常稀疏定位时实时点云扫过去很多位置找不到对应障碍物匹配自然不稳。第二个是min_range和max_range这两个值决定了点云的有效范围。建图和定位必须保持一致否则实时点云里去掉了近处和远处的点但地图里保留了这些区域的特征匹配时会出现“地图有、点云没有”的尴尬局面。还有一点很容易被忽视采集路径的设计。Cartographer的3D建图比较依赖闭环没有闭环的区域误差会不断累积。建图时别追求快尽量让机器人低速、匀速走遇到死路掉头也要预留足够的转弯空间保证激光能扫到足够多的环境特征。我见过有人开着机器人以1.5m/s的速度扫一个走廊结果走廊深处的点云稀稀拉拉到了窄通道位置匹配经常卡死。建图速度和定位稳定性是直接相关的这话我说过很多次真有项目卡住了回头重扫一遍往往比调半天参数更有效。2.3 保存地图文件pbstream才是真正的地图容器Cartographer保存的地图不是一张PNG或PGM图片而是一个.pbstream文件。这个文件里不仅有体素网格还包含PoseGraph里的所有节点、子图、约束和传感器位姿相当于把建图时的“记忆”完整存了下来。所以保存时一定要操作规范两条service调用顺序不能乱rosservice call /finish_trajectory 0 rosservice call /write_state {filename: /path/to/map.pbstream, include_unfinished_submaps: true}先调finish_trajectory告诉Cartographer当前轨迹已经结束它会做一次后端优化把该关的闭环全部关上再调write_state把最终状态写盘。我见过有人只调write_state不调finish_trajectory虽然也能保存但PoseGraph里残留了一些未闭合的约束后面定位时可能在一些区域出现小的跳变。include_unfinished_submaps这个参数很多人不理解简单说就是“没完全建完的子图要不要一起存”。我建议一般情况下设为true不然地图边缘的区域可能就没了。保存完之后务必备份好这个文件同时在项目里建立一个“配置版本”列表记录这份地图是哪个版本的外参、哪个版本的Lua配置建出来的。以后如果改了外参或配置旧地图可能就得作废重建这个坑越早踩到越省心。3. 从建图配置切到定位配置关键修改点逐个说3.1 启动时加载pbstreamlaunch文件怎么改纯定位的启动方式和建图相比最直观的区别就是在cartographer_node的启动参数里多了一个-load_state_filename。先看一个简化版的launch文件launch arg nameload_state_filename default/path/to/map.pbstream / arg nameconfiguration_directory default$(find my_robot)/config / arg nameconfiguration_basename defaultmy_robot_3d.lua / node namecartographer_node pkgcartographer_ros typecartographer_node args-configuration_directory $(arg configuration_directory) -configuration_basename $(arg configuration_basename) -load_state_filename $(arg load_state_filename) outputscreen /node /launch启动之后Cartographer会按两条路径并行处理一条是加载进来被冻结的旧轨迹另一条是当前实时传感器数据对应的新轨迹。新轨迹会不停和旧子图做匹配返回机器人在map坐标系下的位姿。这里有个容易踩的雷load_state_filename给的是.pbstream路径不是点云PCD文件也不是图片栅格地图。Cartographer的定位和基于map_server的传统导航定位完全不一样它不需要先加载一张栅格图片再启动AMCL而是把整个PoseGraph直接恢复进内存。如果加载失败最典型的原因是当前Lua配置里MAP_BUILDER.use_trajectory_builder_3d的状态和保存地图时不一致。比如地图是用3D模式建的但定位配置里写的却是use_trajectory_builder_2d或相反Cartographer会直接报状态不兼容。所以定位配置和建图配置最好基于同一份基础Lua文件修改保持use_trajectory_builder_3d、tracking_frame、map_frame这些核心字段完全一致。3.2 定位轨迹的传感器与匹配参数怎么调纯定位模式下旧地图不能被修改但新轨迹的前端匹配参数是可以单独调节的。下面这几个参数是最值得关注的use_imu_data3D定位几乎必须置为true。Cartographer的前端位姿外推器很依赖IMU提供的角速度信息没有IMU姿态的滚转和俯仰角只能靠激光匹配去猜收敛慢且容易发散。如果IMU不好用宁可先修IMU也不要在定位时关掉它。num_accumulated_range_data建图时是10定位时可以试试保持10也可以降到5到8。降低这个值能减少定位延迟因为不需要凑齐那么多帧才开始匹配但降得太低会让单次匹配用的点云太稀疏反而容易抖动。我的习惯是先保持建图时的值确认定位稳定后再尝试降低。min_range和max_range这两个值必须和建图时保持一致。很多人定位不稳定查来查去最后发现是雷达量程参数被改过导致实时点云的有效范围和地图里的特征范围对不上。ceres_scan_matcher里的translation_weight和rotation_weight这两个权重控制匹配时对平移和旋转误差的敏感程度。如果发现定位后位姿缓慢漂移可以尝试适当提高旋转权重如果机器人在原地抖动则提高平移权重。但每次改权重都要跑一遍实车验证别指望一次调好。use_odometry如果有轮式里程计、IMU预积分或者任何形式的航迹推算数据建议打开。虽然Cartographer本身能靠点云匹配做位姿估计但外部里程计可以提供高频的运动预测让前端在两次匹配之间有更好的初值定位会平滑不少。3.3 初始位姿纯定位里最容易被低估的一步加载了地图之后Cartographer并不知道机器人在地图上的哪个位置它需要一个初始值然后通过扫描匹配把位姿收敛到正确位置。这个初始值给得越准收敛越快给得太离谱比如距离真实位置好几米或者朝向差了几十度匹配很容易掉进局部最优解位姿直接飞走。设置初始位姿其实不复杂。在Rviz里用“2D Pose Estimate”工具点击地图上机器人所在的位置拖出朝向就会往/initialpose话题发一条geometry_msgs/PoseWithCovarianceStamped消息。也可以用命令行直接发rostopic pub /initialpose geometry_msgs/PoseWithCovarianceStamped \ {header: {frame_id: map}, pose: {pose: {position: {x: 1.0, y: 2.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, covariance: [0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01]}}这里有个3D定位特有的细节/initialpose通常只会设置位置和偏航角Z轴高度、俯仰和横滚角会沿用当前外推器的估计。所以如果机器人是放在水平地面上启动那问题不大如果是在坡道或者小台阶上启动最好先在静止状态下等一两秒让Cartographer通过IMU把姿态初始化好再发初始位姿这样成功率会高很多。4. 定位跑起来之后平滑性、漂移排查与调参顺序4.1 先从数据源头查再动算法参数定位跑起来之后如果发现效果不好我的第一反应绝对不是说“去调Ceres权重”而是先把传感器数据从头到尾验一遍。调参数是最后的手段不是一开始的救命稻草。首先要看TF树和时间戳。Cartographer对时间戳极度敏感如果雷达点云、IMU、里程计的消息时间戳不连续或者TF有跳变定位位姿会跟着抽搐。在终端里用rosrun tf2_ros tf2_echo map base_link之类命令盯一会儿看看map到base_link的变换是不是平滑变化。其次看Rviz里的实时点云和地图叠加情况如果点云整体偏离地图、局部扭曲或者出现“重影”那大概率不是参数问题而是外参没标好或时间同步有问题。还有一个很容易忽略的数据源问题是雷达本身。多线激光雷达的某几条扫描线如果时好时坏或者有返回距离异常的点建图时可能被滤波器处理掉定位时却会以随机噪声的形式混进点云导致匹配结果来回跳。我建议在定位前单独跑一下雷达驱动把点云在Rviz里显示出来看看有没有明显的噪点、飞点、周期性丢线。先用voxel_filter和min_range/max_range把明显离群点过滤掉再谈下一步。4.2 常见问题速查表现象、原因和排查方向现象可能原因排查方向启动后位姿直接甩出去初始位姿离真实位置太远匹配掉进局部最优重新发布更准确的/initialpose保持机器人静止2秒原地转圈或小范围抖动点云太稀疏、Ceres平移权重过低、IMU数据噪声大检查点云质量适当提高translation_weight检查IMU原始数据走直线还可以转弯后漂移点云运动畸变、时间戳不同步、转弯速度过快降速检查时间戳和TF必要时降低num_accumulated_range_dataZ轴高度缓慢漂移IMU重力方向估计不准、雷达安装角偏差检查IMU标定和安装角外参静止时观察姿态输出是否稳定特定区域必漂建图时该区域闭环少、地图质量差回到建图阶段补充路线重新出图地图加载后Rviz里看不到子图加载方式不对或submap_publish_period_sec太大检查-load_state_filename参数调小submap_publish_period_secCPU占用过高、位姿更新延迟num_accumulated_range_data过高、点云频率太高适当降低累积帧数降低雷达发布频率或滤波这张表我建议截图存下来。遇到问题先对号入座不要上来就改一堆参数。很多时候多改一个参数反而会把原本还能用的状态搞得更糟。4.3 3D定位特有的“杀手”Z轴漂移和动态障碍物3D定位相比2D定位多出来的自由度正是它更容易“翻车”的地方。最常见的现象是Z轴漂移机器人在平地上跑高度应该保持不变但Rviz里的位姿高度却慢慢往下沉或者往上飘。这个通常和IMU的重力方向估计有关。Cartographer会利用IMU的加速度计数据来确定机器人的姿态参考如果IMU安装不是绝对水平或者外参里的小角度偏差没标准重力方向就会带一个固定的倾斜误差体现在定位结果里就是整个地图被“压斜”了Z轴自然不稳。解决办法是在静止状态下检查IMU输出算出安装误差角并对外参做一次补偿标定。动态障碍物也是3D定位很头疼的问题。2D定位被一个人路过扫到影响范围通常就一条线3D定位扫到的是一整片三维点云如果一个人站在机器人旁边那他身上的点云会形成一个“柱状障碍物”和地图里的墙面、货架产生大量错误匹配点。对策有两种一是尽量在环境中人少的时候任务二是在前端做动态物体过滤比如把距离地面一定高度的点云去掉或者用射线法检测“地图里不该有东西”的点并剔除。Cartographer本身没有内置动态物体检测模块但可以在话题源头上用PCL处理完再灌给定位节点。5. 从定位到可视化楼层小地图与大屏背后的坐标故事5.1 把Cartographer的位姿接出去TF和话题的配合既然定位已经稳定运行下一步就是把位姿接给业务使用了。Cartographer输出的核心坐标关系是map - odom - base_link。map是固定地图坐标系可以理解为“绝对世界坐标”odom是里程计坐标系表示机器人运动累计的中间层base_link是机器人本体坐标系。定位节点会发布TF树同时每个位姿更新周期也会发PoseStamped话题。实际项目中通常只需要监听map到base_link的TF就能拿到当前机器人在3D地图中的位置和姿态。如果你要把位姿存起来或转发给其他程序直接在代码里做一个TF监听即可。简单的参考逻辑是用tf2_ros的TransformListener等待map和base_link之间的变换然后转成nav_msgs/Odometry或自定义消息推出去。需要注意的是Cartographer输出的位姿频率和地图精度有关默认情况下几十赫兹是没问题的但下游如果要做运动控制建议再加一个插值缓存的环节避免位姿话题有偶发抖动。5.2 Unity 3D楼层小地图和3D地区可视化大屏里怎么画很多人做定位项目最后的展示层都喜欢做得炫酷一点。这两年比较火的是Unity 3D楼层小地图和3D地区可视化大屏。这两个方向核心都是“坐标对齐”你要把Cartographer输出的map坐标系下的坐标变换到前端场景里的Unity世界坐标或大屏的三维坐标。做法不复杂关键是确定一个“标定变换”。比如你有一个楼层的三维模型模型里某个墙角在Unity世界坐标里的位置是(1, 2, 0)而Cartographer地图里同一个墙角在(10, 20, 0)那这就存在一组固定的平移和旋转关系。把这组变换算好Cartographer每输出一帧位姿你就在Unity里对应位置放一个小光标或机器人模型楼层小地图就活了。3D地区可视化大屏也是一样的道理只是地图范围更大可能需要先把Cartographer的局部坐标和城市或园区级的坐标系统做一次配准再把轨迹、状态、告警等信息叠加到三维地图上。很多团队在这一步容易忽视一个细节Cartographer的map坐标通常以雷达初始位置为原点方向也和正北没有必然关系。如果大屏上要叠加真实的东南西北朝向你需要在启动定位前用一个已知朝向的标定动作确定map坐标和地理坐标之间的旋转偏移量。这个偏移量一旦算好就写进配置不要每次启动都重新手工输入否则大屏上的朝向会一次对一次不对。5.3 想改代码的先看这两个文件如果你不满足于纯调参想深入了解Cartographer定位链路到底是怎么跑的我建议从两个文件入手。第一个是cartographer_ros/map_builder_bridge.cc它是ROS和Cartographer核心库之间的桥梁负责接收雷达、IMU、里程计数据转成Cartographer内部的SensorData然后把定位结果翻译成TF和位姿话题。想搞明白“一帧点云进来后系统怎么和旧地图发生关系”看这个文件准没错。第二个是cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc这是3D定位前端匹配的核心实现。点云累积、体素滤波、scan-to-submap匹配、Ceres优化都在这里面。你可以看到AddAccumulatedRangeData函数的完整流程先对点云做运动畸变矫正再把点云变换到tracking frame然后用Ceres将点云配准到最近的活动子图最后更新位姿估计。理解了这个节奏你就知道num_accumulated_range_data、voxel_filter_size这些参数到底在链路哪个环节起作用调参时心里就有谱了。最后分享一点个人体会。我在做某个园区巡检项目时被“定位漂移但地图不更新”这个问题折磨了整整一周。后来发现不是算法问题而是我拿着建图时的大转弯速度去跑定位点云畸变超过了匹配的容忍范围。把采集速度压到0.5m/s以下之后定位立刻就稳了。如果你第一次做3D纯定位建议先离线把bag完整回放一遍重点验证三件事初始位姿给得准不准、TF时间戳连续不连续、实时点云范围和建图时是否一致。这三件事没问题Cartographer的3D纯定位基本不会让你失望。