1. 先有地图还是先有位置拆开这个互为前提的死结1.1 为什么单独的定位和建图都做不了我第一次接触SLAM的时候憋在心里的第一个问题就是到底先建图还是先定位这不是杠精提问。你仔细想如果机器人不知道自己在哪它凭什么把传感器看到的墙、桌子、柱子“拼”成一幅完整的地图反过来如果它手上没有地图它又凭什么判断自己“在哪”定位需要地图建图需要位置两者互为前提这就是SLAM号称“难”的最底层原因——它是一个先有鸡还是先有蛋的闭环。我打个比方。你被蒙上眼睛带进一间完全陌生的房间手里只有一根盲杖任务是在脑子里画出一张房间的平面图。你敲一敲发现前方三米有一堵墙。问题是这堵墙“在世界坐标系里的位置”是多少你不知道自己站在哪个点朝哪个方向那墙的绝对位置就是个未知数。如果你走一步记一步脚底下还有误差走十几步方向就歪了。你既没有起点锚定也没有地图可以参考这个活根本没法靠“逐步叠加”干出来。传统工业上的AGV自动引导车为什么看着没那么难因为它们提前在厂房里布好了二维码、反射板或磁条。这本质上等于“先有人工地图”机器人带着地图再去定位等于鸡和蛋里你先拿到了一个。SLAM要解决的场景要苛刻得多没有人工信标、没有GPS、没有任何环境的先验信息机器人要从一片空白里同时猜出自己的轨迹和环境结构。所以你觉得这个问题像死结不是你的错觉它在数学上确实是一个互为耦合的联合估计问题。1.2 把死结翻译成数学语言运动方程与观测方程用更严谨一点的话来说SLAM要同时估计两类未知量机器人每一时刻的位姿位置加姿态记作x以及环境里的路标或地图记作m。我们能拿到的输入也有两类控制量u比如轮速计读数、IMU测量和观测量z比如激光雷达打到墙上的距离、相机拍到的一帧图像。整个问题可以写成两个方程运动方程x_k f(x_{k-1}, u_k) w_k 观测方程z_k h(x_k, m) v_kw_k和v_k是噪声代表我们的传感器和模型永远不完美。现在问题来了你只知道z和u想反推出x和m。观测方程h(x_k, m)里位姿和地图搅在一起。一旦想同时求解这就变成了一个高维非凸优化问题但如果已知其中一个求解另一个就简单得多——已知地图求位姿是定位问题已知位姿求地图是建图问题。单独看都相对成熟难就难在两个一起解。这是这篇文章想先立住的框架SLAM 不是“定位”加“建图”两个模块简单拼起来而是要把“位姿—地图”这个互相锁死的联合状态从观测数据中一起解锁开。明白了这个后面看什么EKF、图优化、回环检测你就知道它们都是在用不同的策略解这个死结。2. 死结难在“人的直觉失效”噪声、累积误差与数据关联有人可能会说我边走边修正不行吗每走一步把新看到的东西拼到地图上地图越来越完整位置不就能越来越准吗这个想法就是典型的“直觉陷阱”。如果现实中每个测量都精确无比这么做确实可以。问题是没有一条路是按照“理想传感器”走通的。2.1 传感器噪声与误差累积走100米差1米的由来先说噪声。激光雷达看起来很准但遇到黑色吸光物体、透明玻璃、雨天扬尘测量点就可能变成飞点或直接丢帧。相机呢弱纹理的白墙拍下来没有任何特征点强逆光下曝光一片白运动快了还会产生模糊。IMU更不用说陀螺仪存在零偏零偏还会随温度缓慢漂移。这些单次观测的小误差并不可怕可怕的是它们会累积。举一个轮式里程计的例子一般机器人轮式里程计的误差在0.5%到2%左右。听起来不高但直行100米误差就可能到0.5到2米。对一张要求厘米级精度的室内地图来说这直接把地图画废了。一张 20 米见方的房间如果机器人在里面绕了一圈最后回到起点时累积出的位置误差可能让“终点”和“起点”差出一米多地图上会出现一堵墙的错位。视觉里程计也是类似。假设每帧运动估计的误差是0.1%连续处理几千帧后轨迹上的漂移量会越来越大。你跑得越远地图越“飘”这就是误差累积效应。2.2 数据关联同样一面墙你怎么知道它就是那面墙比噪声更隐蔽的问题是数据关联。什么叫数据关联就是你得判断两次观测是不是同一个东西。想象你在房间里绕了一圈又走回走廊现在传感器看到前方三米有一面白墙。问题是这面墙和你五分钟前路过的那面白墙是同一面吗如果你的算法认定错了把当前看的位置强行和旧地图里的某段对齐就会产生“地图折叠”——两堵本来不同的墙被叠到一起地图结构直接被扯坏。这个问题在视觉SLAM里尤其突出。视觉特征点虽然丰富但匹配不是百分百可靠。相似纹理、重复结构、光线变化都可能让特征匹配把“错的人”当成“对的人”。激光雷达相对好一些但也在长走廊、对称环境里吃过亏——一个旋转90度看过去几乎一样的通道很容易匹配错方向。这种“不同位置看起来一样”的现象专业上叫感知别名Perceptual Aliasing。它直接打击了“先建图再定位”这种朴素方案建图的时候你根本不知道机器人走的是哪里如果过程中出现了感知别名地图从一开始就建错了后面再怎么定位都白搭。2.3 为什么“找GPS先定位”这条路也被堵死了再说一种常见的思路那我先用GPS把位置确定了再去建图死结不就解开了吗这个想法本身没错但SLAM的用武之地恰恰就是GPS失效的场景。室内商场、地下车库、矿井隧道、茂密树林、高楼峡谷里GPS信号要么没有要么跳得离谱。就算有信号普通GPS的精度也在米级根本满足不了生成厘米级地图的需求更不用说获取姿态信息了。RTK在开阔地用确实能达到厘米级但需要基站覆盖室内根本无法建设。所以你会看到SLAM核心场景基本都具备两个特征一是不存在可靠的全局定位信号二是不允许预铺人工信标。在这种情况下机器人只能“就地取材”从传感器数据里自举出位置和地图。认清这三座大山——噪声、累积误差、数据关联你才算真正理解了死结为什么是死结。3. 滤波思路把位姿和地图塞进同一个状态变量里那最初的研究者是怎么解这个结的早期的主流思路是一类叫“滤波”的方法代表是扩展卡尔曼滤波EKFSLAM。它的核心思想说起来很简单不区分“先有地图还是先有位置”而是把位姿和所有路标全部放进同一个状态变量里然后一边走一边“猜”。3.1 贝叶斯视角下的“同时估计”把问题放到概率框架里看。我们要算的是这个后验概率P(x_0:T, m | z_1:T, u_1:T)意思是给定所有观测z和所有控制输入u机器人位姿轨迹x和环境地图m的联合分布是什么。利用贝叶斯公式和“观测之间相互独立”的假设这个联合后验可以分解成许多因子项的乘积每个因子对应一个运动约束或一个观测约束。从结构上看这就变成了一张因子图机器人的位姿是节点地图路标是节点观测是连接它们的边。所谓“同时估计”就是在这张图上做推理。这个方法第一个好处就是名正言顺地绕开了“先有谁”的问题。状态量里既有位姿又有地图它们一起被最优估计。相当于把鸡和蛋同时放进了一个篮子里然后让它们互相校核位置不对地图也不对地图不对位置也跟着错。这个互相约束的关系被显式建模了出来——这正是SLAM解结的本质。3.2 EKF SLAM是怎么工作的又为什么扛不住大场景EKF SLAM的做法是用一个高维高斯分布来表示机器人的联合状态。均值向量里包含了当前位姿加所有已经观测到的路标位置协方差矩阵则记录了两两之间的相关性。每一步它干两件事。一是预测根据运动方程用控制量预测机器人位姿怎么变同时扩大不确定性。二是更新拿到新的观测后用卡尔曼增益做修正让整个均值向量和协方差矩阵根据“观测到的路标位置”和“预测的位置”之间的差异去调整。这里有个细节很有意思由于协方差矩阵存储了不同路标之间的相关性新的观测不仅能修正当前路标和机器人位姿还能间接修正以前所有路标。这就是EKF SLAM理论上能“同时解结”的原因——信息在协方差矩阵里流动起来了。但它有两个硬伤。第一是计算量协方差矩阵的维度随路标数量增长设机器人位姿维度为3路标有N个协方差矩阵就是(3N3)乘(3N3)。每次更新要做一次矩阵求逆路标一多计算量就变成 O(N^2) 甚至更高在线运行越来越吃力。第二是线性化误差EKF依赖对非线性方程做一阶线性化问题是SLAM里的观测模型非线性很强遇到大场景或者急转弯局部线性化近似会失真滤波器可能直接发散。3.3 粒子滤波的尝试与维度爆炸后来研究者又试了粒子滤波。它不假设高斯分布用一堆带权重的粒子去逼近任意分布每个粒子代表一条可能的轨迹和地图假设。这个思路理论上更“暴力”但你得想想状态维度是多少。一个3D SLAM问题光位姿就是6维粒子采样覆盖6维空间已经不容易再加上地图状态空间的维度随路标数量爆炸。粒子滤波最大的问题就是为了保持估计精度需要的粒子数量随维度指数增长。二维小场景还行三维大场景直接废掉。Rao-Blackwellized 粒子滤波也就是FastSLAM做了一个聪明变通每个粒子只采样轨迹路标部分用解析的高斯分布来处理。这大大降低了采样维度但也仍难逃“时间越长粒子越耗”的宿命。反正到今天纯粒子滤波路线的系统在工业界已经很少见了大家普遍转向了优化路线——滤波快成了历史课内容。但我觉得它的思想还是值得学因为“把未知量塞进一个状态空间里互相约束”这个提法是理解一切SLAM问题的基础。4. 优化思路非线性最小二乘如何把死结“摊平”滤波方法的问题在于“在线地、逐帧地”维护后验。每一帧数据来了就要更新一遍状态而过去的信息只能通过“当前状态”间接参与修正信息一直在被压缩。优化思路换了个角度把过去一段时间甚至全部的关键帧捞出来放在一起让它们共同参与一次大规模的最优估计。这就是现代SLAM普遍采用的图优化/非线性最小二乘路线。4.1 从滤波到图为什么优化能行图优化把问题变成一张图。图的顶点是待估计的变量——位姿节点、路标节点图的边是约束——两个位姿之间的相对运动约束、位姿观测到路标的约束。优化目标就是调整所有顶点的取值使得所有边的“误差”整体最小。打个比方你有一批照片每张照片里拍到了几个共同的人。你想根据这些照片确定每张照片的拍摄角度和每个人站的位置。单独看一张照片你只能得到一些模糊的相对关系把所有照片放在一起让每个“人—照片”的对应关系都参与计算整体上就能解出一个让所有人都自洽的几何布局。SLAM的图优化就是这么干的——用大量约束把“位姿地图”这个联合状态一次性调整到最一致的状态而不是递推着临时凑。这也是它解开死结的关键不再区分先算谁而是把位姿和地图当作一个整体里的两组未知数用最小二乘一起求。4.2 Bundle Adjustment 与重投影误差图优化的核心模块叫光束法平差Bundle AdjustmentBA。在视觉SLAM里BA优化的对象是相机位姿和3D路标点。误差项通常用重投影误差定义——把一个3D路标点按照当前估计的相机位姿投影到图像平面得到“预测像素位置”和相机实际观测到的像素位置相减差值就是重投影误差。minimize Σ ‖ z_ij - π( T_i, P_j ) ‖^2其中z_ij是第i个位姿观测到的第j个路标的像素坐标π是相机投影函数T_i是位姿的变换矩阵P_j是路标在三维空间中的坐标。这个误差函数是非线性的所以用高斯牛顿法或列文伯格-马夸尔特法迭代求解。迭代需要初值初值越好收敛越快、越不容易掉进局部极小。而初值从哪来这就是前端视觉里程计的活——先用帧间匹配给一个“大致不差”的轨迹和地图后端BA再在这个基础上做精细化打磨。前端给后端“靠谱的初值”后端给前端“精确的修正”这本身就是一种解开死结的分工设计。4.3 稀疏性才是真正救命的稻草光有BA还不行。如果每一帧都优化所有路标计算量照样爆炸。真正让SLAM实时化成为可能的是这个问题天然的稀疏结构。一个误差项只和“某个位姿”以及“该位姿观测到的少数几个路标”相关所以整个系统的雅可比矩阵或者说近似的Hessian矩阵是稀疏块状的。大部分块都是零。这个稀疏性意味着我们可以用Schur补也叫边缘化把路标部分先行消去只对相机位姿做求逆再回代求路标。这样计算复杂度大幅下降才有了实时SLAM的可能。我第一次看到别人讲“稀疏性让SLAM实时化”时没太在意后来自己在代码里试着实现一遍才意识到这是分水岭一样的东西。没有稀疏性BA就是一个大规模稠密非线性优化问题跑一帧可能要好几分钟有了稀疏性现在手机上的AR都能实时跑视觉SLAM。死结不是被蛮力解开的是被巧劲解开的。4.4 回环检测全局一致性的最后一块拼图光靠局部优化误差还是会慢慢累积轨迹照样会漂。这时候需要回环检测Loop Closure出场。回环检测要做的事就一句话让机器人认出“我以前来过这里”。一旦认出某个历史位置就在当前位姿和历史位姿之间加一条“回环边”。这条边的本质是把当前位置和历史位置约束在一起。它像一个钉子把不断漂移的轨迹硬生生钉回原来的位置。一个真正的回环约束能同时修正整条轨迹上所有位姿漂移被“拉紧”回来地图的全局一致性才有保障。视觉SLAM里常用的回环检测方案是词袋模型Bag of Words。做法是把图像提取出局部特征比如ORB特征把所有特征“量化”成一个个视觉单词每一帧图像本身变成一组单词的集合。比较两帧图像的相似度只需要比较它们的单词向量。但词袋模型也不是万无一失碰上感知别名严重的场景也可能误报。所以工程上一般还要做几何一致性校验先对回环候选做特征匹配再用对极几何或PnP去验证几何关系是否成立通过了才真正接受这个回环。5. 现代SLAM的标准分工前端、后端、回环与建图从“死结”这个抽象问题走到可以落地的代码现代SLAM系统普遍按一套标准架构来分工。5.1 五模块架构是怎么解结的一套典型的SLAM系统至少包含下面五个模块模块职责解什么结传感器数据预处理去畸变、时间戳同步、IMU预积分保证数据干净减小噪声前端里程计视觉里程计/VO在相邻帧/相邻扫描之间估计运动给出短时可靠的位姿初值后端优化对位姿和路标做BA或位姿图优化利用历史信息削减累积误差回环检测识别历史位置提供全局约束消除长期漂移建图根据最终轨迹生成栅格/点云/八叉树地图输出可用的环境模型看到没死结不是被某一个模块单独解开的而是被整套架构分工解开的。前端负责短期的“还不错的初值”后端负责长期“全体的精修”回环负责“掐头去尾的全局封装”。每个模块都有短板但组合起来才有今天SLAM在机器人、自动驾驶、无人机、AR/VR里遍地开花的结果。5.2 视觉SLAM与激光SLAM的路线选择做SLAM经常会面对一个选型问题用激光雷达还是相机激光SLAM的核心优势是直接测量距离几何精度高受光照影响小点云配准用ICP或NDT这类算法短时间内匹配可靠性很高构建的二维栅格地图直接可以拿去做导航避障。缺的是成本高而且雷达点云缺乏语义信息——它不知道那是一面白墙还是一张海报。视觉SLAM的优势是相机便宜、信息丰富能提供纹理、颜色、语义等丰富线索缺点是对光照和纹理敏感弱纹理环境容易跟丢单目还存在尺度不确定性问题——你根本不知道地图的单位是“米”还是“某个任意单位”。因此实际项目里很多会用“视觉惯性”VIO或“激光视觉惯性”的组合取长补短。如果环境是规则的室内仓库导航需求明确预算充足用2D激光SLAM是最省心的选择如果是探索未知环境、要做语义理解或AR应用视觉SLAM的价值更大。5.3 视觉惯性SLAM为什么越来越主流近几年几乎没有新出的主流SLAM系统不融合IMU。原因很具体纯视觉SLAM在快速运动、剧烈旋转、短时遮挡的时候特征点很容易丢失前端一旦跟丢整个系统就崩了而IMU虽然长期漂移但短时间内的相对测量非常准角速度和加速度积分给视觉提供了“即使短暂看不到特征也能大概猜出运动”的能力。视觉和IMU的互补关系正好又呼应死结的解法视觉提供“环境的几何信息”IMU提供“运动的惯性信息”两边一融合估计的鲁棒性大幅提升。代价是系统复杂度上升——除了相机内参还要精确标定相机和IMU之间的相对姿态外参并且要在后端处理IMU的预积分、零偏估计等一堆事。由此也带火了一个工具kalibr专门用来做这类标定。我到现在还记得第一次用kalibr标定相机-IMU外参时因为运动太温柔导致IMU激励不足标定结果发散后来才知道这个工具必须要大幅旋转各轴、充分激励六自由度才行。这类细节后面第6章我会再展开聊。6. 上手实践从ORB-SLAM2到evo、kalibr的完整链路理论讲再多不动手永远隔着一层。这一节我把“从零跑通一个SLAM”完整链路讲清楚包括工具链、命令、坑都是我实际踩过的。6.1 建议的学习路线先跑通再读懂最后改代码很多新手很容易犯一个错一上来就抱着《视觉SLAM十四讲》啃公式啃到崩溃代码一行没跑。我的建议是反着来先跑通一个现成系统比如ORB-SLAM2用公开数据集看到相机在真实场景里建图然后用evo工具去评估这个系统的误差直观感受“不准”到底是什么样回头再读《视觉SLAM十四讲》这时候你对“前端”“后端”“回环检测”这些词已经不再陌生公式自然有了落脚点最后再尝试改代码、换数据集、甚至自己标定传感器。这套路线的最大好处是每一步都有正反馈不容易被理论劝退。6.2 Ubuntu 20.04搭建ORB-SLAM2单目实例的经验ORB-SLAM2可以说是视觉SLAM最经典的入门代码库支持单目、双目、RGB-D。虽然现在ORB-SLAM3已经出来了但ORB-SLAM2代码结构更简单适合入门。我以在Ubuntu 20.04下编译运行单目TUM数据集为例把步骤串一遍。先装依赖。ORB-SLAM2需要Pangolin用于显示GUI、OpenCV、Eigen3、DBoW2和g2o源码里自带。sudo apt update sudo apt install libglew-dev libpython2.7-dev build-essential cmake git clone https://github.com/stevenlovegrove/Pangolin.git cd Pangolin mkdir build cd build cmake .. make -j4OpenCV用apt装就行sudo apt install libopencv-dev但注意如果你装的是OpenCV 4.xORB-SLAM2的源码会有一些兼容问题。比较典型的坑有两个一是DBow2和g2o是早期版本编译时可能因为C标准报错需要把ORB-SLAM2的CMakeLists.txt里加上-stdc11二是OpenCV 4里有些头文件路径变了比如#include opencv2/ximgproc.hpp这类需要改成实际的路径或者直接搜报错去源码里调整#include。改完以后正常编译cd ORB_SLAM2 ./build.sh编译完成就可以跑TUM数据集。先去TUM官网下载rgbd_dataset_freiburg1_desk约2GB解压后数据集里应该有rgb/、depth/文件夹以及rgb.txt、depth.txt、groundtruth.txt这些文件。单目运行命令./Examples/Monocular/mono_tum \ Vocabulary/ORBvoc.txt \ Examples/Monocular/TUM1.yaml \ /path/to/rgbd_dataset_freiburg1_desk运行完终端会显示跟踪的帧率程序结束后会在当前目录生成KeyFrameTrajectory.txt这就是SLAM估计出的关键帧轨迹。我建议你做的第一件事不是看那个炫酷的GUI窗口而是把视线盯住这个txt文件——它是欣不了因为它记录了每帧的timestamp tx ty tz qx qy qz qw。6.3 用evo量化SLAM精度不仅要“看得像”还要“测得准”“跑出来的轨迹对不对”盯屏幕看觉得像还不够要用工具量化。可视化后你会发现轨迹和真实轨迹差得很直观特别是单目SLAM还存在尺度未知的问题——地图被缩放了轨迹形状像但是整体比例和真实世界差了一个倍数。这个场景下最实用的工具是evo。它支持TUM、KITTI、EuRoC等主流格式的轨迹评估。估计轨迹是KeyFrameTrajectory.txt真值是groundtruth.txt对应的命令是evo_ape tum groundtruth.txt KeyFrameTrajectory.txt -a -s这里参数-a表示做SE(3) Umeyama对齐处理原点坐标不同的问题-s表示同时估计尺度并校正单目SLAM尤其需要这个因为单目地图天生没有绝对尺度。输出结果里你会看到RMSE、mean、median、max等一行行数字。最常看的指标是RMSE和max。如果RMSE在几厘米到十几厘米之间说明系统工作正常如果差出几十厘米甚至几米先别急着怀疑算法大概率是你把时间戳对齐搞错了或者对齐参数没加对。画轨迹对比evo_traj tum KeyFrameTrajectory.txt --refgroundtruth.txt -a -s -p它会把估计轨迹和真实轨迹画在一起折叠处、漂移处一眼就能看出来。我强烈建议新手养成“每次跑完算法都用evo记录一下误差”的习惯这是你做任何SLAM改进时判断“到底改好没改好”的唯一标准。6.4 kalibr标定为什么是VIO的前提如果你想玩相机IMU融合VIO那就绕不开外参标定。kalibr是一个常用的视觉-惯性标定工具可以标定相机内参、相机-IMU外参、以及IMU自身的零偏。用kalibr标定相机-IMU外参一般流程是打印一个高对比度的Apriltag或棋盘格标定板固定好录制一个rosbag相机对着标定板同时让设备做大幅度的旋转运动尤其是让每个轴都转起来标定前先用kalibr_calibrate_cameras标定相机内参再用kalibr_calibrate_imu_camera联合标定外参。标定过程中最容易犯的错误是运动不够“凶猛”。IMU需要充分的加速度激励才能把尺度、姿态可观地估计出来。我第一次用的时候怕把图像拍糊拿着设备慢慢晃结果标定结果完全发散。后来才知道kalibr推荐的动作应该是大幅旋转、快速启停、各轴轮流转但过程中又要保证标定板始终在视野内不能出现太严重的运动模糊。这个平衡本身就是一门经验活。标定结束后会得到camchain.yaml和imu.yaml里面有相机内参矩阵、畸变系数、以及T_cam_imu变换矩阵。这个外参矩阵的精度直接决定VIO系统能否正确融合视觉和惯导数据。很多跑VIO跑出来轨迹发飘的情况仔细排查后都发现是外参标定不准。7. 分享几个我踩过的坑以及后面准备写什么从“死结”这个概念走到能跑通ORB-SLAM2并量化评估中间我踩过太多次坑。说几个对新手最有参考价值的。第一个感受最深的坑不要一上来就自己写SLAM。我见过太多人把大量时间耗在“想自己搓一个SLAM”上结果光重投影误差公式就折腾了两周最后依然跑不过开源方案。正确的做法是先跑通ORB-SLAM2或者ORB-SLAM3甚至先用别人的内存、数据集和评估工具把“SLAM系统的数据流”彻底摸清楚再考虑写自己的前端或后端。造轮子的前提是你知道轮子是怎么转的而不是凭空造。第二个坑单目尺度别忘对齐。我刚开始用evo评估单目轨迹时没加-s参数结果RMSE报出好几米我差点以为是ORB-SLAM2的问题。后来才明白单目SLAM本身的尺度就是任意值必须让评估工具先估计一个尺度再比对。这个“先对齐再比较”的思路几乎适用于所有SLAM评估。第三个坑数据集的时序同步。ORB-SLAM2跑TUM数据集时如果rgb.txt和depth.txt里的时间戳没有对齐或者你用了和当前数据集型号不匹配的配置文件比如把TUM1的配置拿去跑TUM3跟踪质量会肉眼可见地变差。这类问题看起来是算法问题实际上一查全是数据问题。最后说下这个系列接下来的打算。这篇文章只是把最底层的“死结”讲清楚后面我会按模块拆下去第二篇想聊前端里程计到底是怎么从两帧图像里“猜”出运动的也就是视觉特征匹配与本质矩阵那套东西第三篇会写后端优化和BA的代码实现用一个小例子手把手带你建一个最简图优化问题第四篇大概率会聊工程落地比如怎么把SLAM接到ROS导航栈上怎么做地图转换和定位复用。如果你也有卡了很久的SLAM问题欢迎在评论区聊聊说不定下一篇就写你这个点。