SLAM 是什么一条链路讲透传感器 → 数据处理 → 前端匹配 → 子图与关键帧 → 闭环检测 → 图优化以 Cartographer 为主线把「同步定位与建图」拆成六个环节一次讲清楚。前言如果你刚接触机器人、自动驾驶或者激光雷达第一个绕不开的词大概率是SLAM。SLAM 是Simultaneous Localization and Mapping的缩写中文叫「同步定位与建图」。听起来很玄但它要回答的问题朴素得不行一台不知道自己在哪里的机器人如何一边走一边画出周围的地图本文不堆公式也不假设你有 SLAM 基础。我会沿着数据流动的方向把整条链路拆成六个环节讲一遍传感器——机器人的感官数据处理——从原始数据到可用信息前端匹配——当前帧对齐到局部地图子图与关键帧——把扫描贴成地图后端闭环——识别「我回来过」图优化——把整张图拉正文中涉及的具体实现以 Google 开源的 Cartographer 为例。它是目前工程上最值得读的 2D/3D SLAM 系统之一。一、SLAM 到底难在哪一个鸡生蛋问题先看机器人睁开眼面对的处境没有地图也不知道自己站在哪里。它想问两件事我在哪里—— 定位Localization周围是什么—— 建图Mapping麻烦的是这两件事互为前提定位需要一张地图作参照才能知道自己在图中的位置建图又需要知道每一帧扫描是在哪个位置拍下的才能把数据拼起来。定位依赖地图建图依赖定位谁也离不开谁——这就是经典的「鸡生蛋问题」。SLAM 给出的答案是不要拆开解同时求解。把「位姿」和「地图」当作同一个优化问题里的两组未知量一起估计、一起更新。想通这一点后面所有的模块设计就都顺理成章了。二、传感器机器人的感官SLAM 的第一步是让机器人拥有感官。传感器是它感知世界的唯一入口。工程上最常用的有四类。2.1 激光雷达 LiDAR发射激光、测量到障碍物的距离一圈扫下来就是环境的一张「剖面图」。优点测距直接、精度高可以直接用来建图几何信息是「量」出来的不依赖纹理。缺点成本高雨雾、镜面反射会干扰测距。产出一帧扫描scan本质是一堆(角度, 距离)或(x, y)点——也就是点云。2.2 惯性测量单元 IMU输出三轴加速度与角速度。优点频率极高100–1000 Hz短时间内非常准。缺点靠积分推算姿态会随时间慢慢漂移必须靠外部观测校正。2.3 轮式里程计 Odometry靠编码器数轮子转了多少圈推算位移。优点连续、便宜、短距离平滑可靠。缺点一旦打滑就失效误差会悄悄累积。2.4 相机视觉 SLAM信息丰富、成本低但受光照和纹理影响大。属于另一条技术路线本文不展开。2.5 为什么要融合每一种传感器都有短板传感器强项短板激光雷达几何精度高可直接建图贵怕雨雾/镜面IMU高频、短时精确积分漂移轮式里程计连续、便宜打滑即失效相机信息丰富、便宜怕光照、怕弱纹理所以真实系统从不单打独斗而是做多传感器融合——用 IMU 的高频补雷达的低频用雷达的绝对观测校正 IMU 的漂移用里程计提供平滑的短时约束。三、数据处理从原始数据到可用信息传感器吐出来的原始数据并不能直接用它带着噪声、冗余而且各传感器的时间戳并不对齐。在进入匹配之前通常要走完四步。3.1 时间同步雷达可能只有 10 HzIMU 却有 100 Hz。必须把不同频率、不同延迟的数据归算到同一时间基准否则「同一时刻」的姿态和扫描对不上匹配从一开始就错了。3.2 带通滤波量程过滤按最小 / 最大量程过滤丢掉太近和太远的点太近的点往往是机器人自身遮挡比如车体、支架太远的点已经超出可信量程信噪比极低。Cartographer 里对应min_range/max_range参数。3.3 体素降采样Voxel Filter一帧 3D 扫描动辄几万个点直接参与匹配计算量爆炸。做法是把空间切成体素格子每个格子里只保留一个点通常是质心。这一步几乎是无损的——因为它丢掉的只是「同一小块空间里的重复观测」而匹配关心的是几何结构不是点的绝对数量。3.4 运动畸变校正这是最容易被忽略、但必须做的一步。激光雷达不是瞬间拍完一帧的。一帧扫描的这段时间里比如 100 ms机器人本身在移动、在转弯。于是同一帧里的点实际上是在不同位姿下测到的——直接用会让墙壁「歪掉」。解决思路按每个点的时间戳结合这段时间内的运动估计把点统一扳回到同一个时刻通常是帧起始或帧结束时刻。Cartographer 里由MotionFilter与累积点云的体素滤波配合完成。本环节的输出干净、稀疏、时间对齐的一帧扫描——这才是一份可以直接参与匹配的数据。四、前端匹配Local SLAM把当前帧对齐到子图现在进入前端。它的任务很具体把当前这一帧扫描对齐到已有的局部地图上。4.1 位姿外推如果没有任何先验扫描匹配就得在一个巨大的搜索空间里盲目找位置和角度——既慢又容易掉进局部最优。所以第一步是位姿外推用 IMU 和里程计预测下一时刻的位姿作为搜索的初值。有了靠谱的初值匹配只需要在初值附近做小幅微调效率和鲁棒性都大幅提升。4.2 扫描匹配在初值附近搜索最优位姿使当前帧与子图的重合度最高。Cartographer 用了两个匹配器配合Ceres 扫描匹配器CeresScanMatcher把匹配写成连续优化问题用非线性最小二乘求最优解。目标大致是让扫描点落在占据概率高的地方arg ⁡ min ⁡ ξ ∑ k ( 1 − M smooth ( S k ( ξ ) ) ) 2 \arg\min_{\xi}\sum_k \Big(1 - M_{\text{smooth}}\big(S_k(\xi)\big)\Big)^2argξmin​k∑​(1−Msmooth​(Sk​(ξ)))2其中ξ \xiξ是待求位姿S k ( ξ ) S_k(\xi)Sk​(ξ)是第k kk个点按位姿ξ \xiξ变换后的坐标M smooth M_{\text{smooth}}Msmooth​是平滑后的占据概率栅格。点落在墙上的概率越高代价越小。实时相关匹配器RealTimeCorrelativeScanMatcher在离散的位姿网格上暴力搜索配合分支定界加速详见第六节。它的作用是给 Ceres 提供一个足够好的初值避免连续优化掉进局部极小。工程实现上Ceres 匹配通常分三段递进先只优化平移 → 再只优化旋转 → 最后联合优化逐步放开自由度。4.3 前端的边界匹配的输出就是机器人当前这一刻的位姿( x , y , θ ) (x, y, \theta)(x,y,θ)。前端的特点是快能实时跑。代价是只看局部——每一帧都只和附近的地图对齐误差会一点点累积最终表现为漂移。这正是后端要解决的问题。五、子图与关键帧把扫描贴成地图匹配得到位姿之后要把这一帧扫描「贴」到地图上。Cartographer 的做法是累积成一张局部地图——子图。5.1 子图 Submap子图是一张概率栅格每个格子记录「被占据的概率」。多次观测取平均噪声自然被抹平。这里有个很漂亮的工程思想不追求每一帧都精确而靠多次观测的平均来消除噪声。单次测距可能偏差 3 cm但几十次观测叠加后墙的位置会收敛到很准的地方。5.2 关键帧 Keyframe但并不是每一帧都要记下来。当机器人位移超过一定距离d dd、或转角超过一定角度θ \thetaθ才插入一个关键帧。关键帧是位姿图里的锚点它记住「我在这个位置、看到了什么」。两个关键帧之间用里程计约束相连。这样既保留了必要的信息又把数据量压到了可控范围。5.3 混合栅格 Hybrid Grid子图规模变大后内存会成为问题。Cartographer 的折中方案是混合栅格近处用概率栅格保证精度远处用TSDF截断符号距离场压缩存储节省内存。本质是「精度与内存的折中」——离得近的地方机器人还要反复用离得远的地方只需要保留大致轮廓。六、后端闭环Global SLAM识别「我回来过」现在进入后端。它要处理前端留下的那个问题漂移。6.1 位姿图 Pose Graph前端走过的每一步都会被记成位姿图里的一条边节点 关键帧与子图边 它们之间的相对位姿约束。前端的里程计只连相邻节点。这意味着一圈走下来图是一个「环」但首尾两个节点之间没有边——所以首尾对不上。6.2 漂移 Drift当机器人绕一圈回到起点时两条路线对不上明明走过同一个地方位置却偏了。举个具体的量级走了 100 m回来可能偏了 2 m。误差就是这么一点点攒出来的。6.3 闭环检测于是后端做闭环检测把当前扫描拿去和历史子图逐一比对寻找「我曾经来过这里」的证据。一旦找到可靠的匹配就在位姿图里新增一条闭环约束边——告诉优化器「这两个节点其实是同一个地方。」6.4 分支定界加速逐一比对很慢因为位姿搜索空间很大。Cartographer 用分支定界Branch and Bound来加速粗分辨率扫描先在低分辨率栅格上快速算一个上界剪掉大量不可能的候选逐层细化只在还有希望的候选上逐层提高分辨率最终精确求解。这是一棵「搜索树」的剪枝过程——由粗到细把指数级的搜索压成可实时运行的计算量。Cartographer 在FastCorrelativeScanMatcher中实现了它。七、图优化把整张图拉正闭环约束加进来了但新的矛盾也随之出现这条新边和原有的里程计边并不完全自洽。每条边都希望两端的相对位姿符合它的观测可由于漂移它们彼此冲突。7.1 目标图优化的任务是找一组节点位置让所有约束的总误差最小min ⁡ ξ ∑ i j ρ ⁣ ( e i j ⊤ Ω i j e i j ) \min_{\xi}\ \sum_{ij}\ \rho\!\left(e_{ij}^{\top}\,\Omega_{ij}\,e_{ij}\right)ξmin​ij∑​ρ(eij⊤​Ωij​eij​)其中ξ \xiξ是所有节点位姿e i j e_{ij}eij​是约束( i , j ) (i,j)(i,j)的残差观测相对位姿与当前估计相对位姿之差Ω i j \Omega_{ij}Ωij​是信息矩阵表示这条约束有多「可信」ρ ( ⋅ ) \rho(\cdot)ρ(⋅)是鲁棒核函数用来抑制离群约束的影响。约束越多、闭环越多结果越稳。7.2 方法稀疏位姿调整Cartographer 用的是Sparse Pose AdjustmentSPA——把位姿图当成一个大型稀疏最小二乘问题求解。因为每条边只连接两个节点雅可比矩阵极其稀疏可以用稀疏 Cholesky 分解高效求解。7.3 关键直觉这是整个后端最值得记住的一点一处的闭环大误差会「均匀分摊」到整张图上而不是只挪动闭环的两个端点。如果只修正闭环两端的节点地图会在局部被硬掰出一个畸形。而全局优化的结果是每个节点都稍微动一点总误差最小地图整体自洽。7.4 结果当残差被压到最小地图被整体「拉正」走廊不再错位回到起点时严丝合缝得到一张全局一致的地图。八、把整条链路串起来环节输入输出核心手段特点传感器物理世界原始点云 / 加速度 / 编码器多传感器融合互补短板数据处理原始数据干净、稀疏、时间对齐的一帧时间同步、带通滤波、体素降采样、畸变校正决定上限前端匹配当前帧 子图位姿( x , y , θ ) (x,y,\theta)(x,y,θ)位姿外推 Ceres / 相关匹配快局部准确子图与关键帧位姿 扫描概率栅格子图 / 关键帧概率栅格、混合栅格把扫描贴成地图后端闭环当前帧 历史子图闭环约束分支定界搜索找回「我来过」图优化位姿图 闭环约束全局一致的地图稀疏位姿调整 SPA慢全局一致一句话概括两条主线的分工前端Local SLAM负责局部准确——快但会漂移后端Global SLAM负责全局一致——慢但能纠正累积误差。两者配合才构成 Cartographer 这类现代 SLAM 系统的骨架。九、一句话记住 SLAM如果这篇长文你只记住一句话我希望是这句SLAM就是一边走一边画一边纠正。「一边走」——传感器在持续观测「一边画」——前端把扫描贴成子图「一边纠正」——后端闭环 图优化把漂移拉正。三者缺一不可也正好对应了本文的六个环节。视频讲解如果你更喜欢视频形式可以看看这个视频用更直观的方式把 SLAM 的整条链路串一遍点击观看视频讲解B 站