SLAM入门实战：从里程计到目标检测的机器人定位与感知

SLAM 在移动机器人和具身智能里解决的是一个绕不开的问题机器人怎么知道自己在哪怎么知道周围有什么。它不是一个单一算法而是一整套“定位 建图 感知”的配合方式。这篇文章适合两类人看一是准备做移动机器人、无人机、AGV 相关项目想系统理解 SLAM 定位与感知的入门者二是已经跑过开源 SLAM但对里程计、目标检测、刚体变换、射影变换、李群与李代数这些概念还停留在名词层面的人。我会按“问题—几何—感知—选型—实操—排坑”的顺序展开少堆公式多给判断标准和可复现步骤。1. SLAM 解决的不只是“建图”而是“我在哪、周围有什么”1.1 定位与建图为什么是一对共生问题SLAM 全称是 Simultaneous Localization and Mapping同时定位与建图。它处理的核心矛盾是机器人要建立环境地图就需要知道自己在地图中的位置要知道自己的位置又需要一张可靠的地图。这个互相依赖的关系是所有 SLAM 系统都要面对的“先有鸡还是先有蛋”问题。工程上的通用做法是机器人一边移动一边用传感器观测环境先做一次带误差的位姿猜测然后用路标、回环等约束把位姿和地图一起修正。整个过程不断重复形成“预测—观测—修正—再预测”的闭环。所以 SLAM 本质上不是一个离线处理工具而是一个实时状态估计过程。这里要先建立一个判断SLAM 不是某个开源仓库里的单文件而是多模块组合。前端负责从传感器数据里估计帧间运动后端负责对历史位姿和路标做优化回环检测负责认出“我来过这里”建图模块负责把估计结果组织成可用的地图。你读开源项目时不要只盯着某一个文件先问自己这段代码属于前端、后端、回环还是建图这个问题答不上来说明还没建立起系统视图。1.2 定位、感知、规划在机器人里怎么分工具身智能强调机器人与环境实时交互机器人需要连续回答三件事我在哪周围是什么下一步怎么动。目标检测和分割解决“周围是什么”里程计和全局定位解决“我在哪”路径规划和控制解决“怎么动”。这三种能力不能独立成立。举个例子你训练了一个目标检测模型输出“障碍物 0.85”的框但如果检测结果没有从像素坐标系转换到相机坐标系再转换到世界坐标系机器人根本不知道障碍物离它多远、在什么方向。很多入门项目表面上是“检测 SLAM”实际大量时间花在坐标转换、时间戳同步和消息类型对接上这不是浪费时间恰恰是必经之路。所以我的建议是目标可以定位与感知一起学但切入顺序先从定位下手。定位模块的输入输出更明确有 ground truth 可以验证结果好不好一眼能看出来目标检测模型单独跑通很简单但要真正让机器人用上反而要处理一堆定位侧的问题。1.3 AGV、无人机和水下机器人场景不同底座相同从室内 AGV 到无人机再到水下机器人应用场景差别很大但底座问题一致都是“无 GPS 或 GPS 不可靠环境下如何持续估计自身状态并构建环境表示”。室内机器人常用 2D 激光雷达和轮式里程计地面可近似平面无人机有六个自由度运动快、视角变化剧烈更需要视觉惯性融合水下环境光线衰减严重视觉特征少声纳成为主要传感器。看到“水下目标检测”“无人机目标检测”这类需求时不要误以为它们是完全不同的技术分支。它们的差异主要在传感器、数据形态和约束条件上但感知模块最终都要输出“目标在哪里、是什么、属于静态还是动态”定位模块都要输出“机器人当前位姿”。先掌握共同底座再针对场景换传感器和数据格式是更高效的学习方式。2. 从里程计到李群李代数先分清位姿、变换和优化对象2.1 里程计的本质与漂移来源里程计是 SLAM 前端里最直白的位姿估计方式。轮式机器人用编码器数轮子转了多少圈再根据轮距和转向模型推算位姿变化视觉里程计通过相邻两帧图像特征匹配估计相机相对运动激光里程计则用点云配准常见方法包括 ICP、NDT 等。它们的共同点是“增量式”只估计当前时刻相对于上一时刻的变化量然后不断累加。增量式方案的优点是实现简单、速度快、不依赖外部全局信息缺点是误差会累积。旋转估计哪怕只有一点偏差走完一段长路径后位置误差可能扩大到几米。所以里程计通常作为前端初始值或融合源之一不能单独作为长期定位答案。如果你在 SLAM 代码里看到 twist 这个词它通常被翻译为“速度旋量”或“空间速度”本质是线速度与角速度的组合常出现在匀速模型、IMU 预积分和位姿插值代码里。先把它理解成“一个带方向的增量运动”就够了不必在术语翻译上卡住。2.2 刚体变换、仿射变换、射影变换怎么区分这三类变换是机器人视觉里最容易混淆的概念我建议按“保什么不变”来记。刚体变换对应机器人或相机在真实三维空间中的运动保持物体形状、长度、角度不变。它由一个旋转矩阵加一个平移向量组成是 SE(3) 群里的元素。传感器外参标定、机器人位姿表示、坐标框架转换用的都是刚体变换。仿射变换允许平移、旋转、缩放和错切不再保角度和长度但仍然保持平行性。它在二维图像处理中出现最多比如目标检测数据增强里的随机旋转、缩放、错切以及图像配准里的仿射匹配。射影变换也叫单应变换是针孔相机成像时发生的变换。一个三维平面上的点在图像平面上的映射就是射影变换它连平行性都不保证。全景拼接、平面标记识别、视觉 SLAM 里的单应矩阵分解都会用到它。区分它们的实际价值在于看到一段矩阵运算代码先判断它描述的是真实空间运动还是图像坐标变化。如果是真实空间运动旋转矩阵必须是正交矩阵逆矩阵等于转置如果是图像坐标变换自由度更高不能机械套用刚体变换的求逆方式。2.3 李群与李代数优化位姿时为什么绕不开李群与李代数在 SLAM 里不是干巴巴的数学而是为了解决一件事位姿求导。视觉 SLAM 的核心优化目标是让重投影误差最小优化时要对旋转矩阵或位姿求导。旋转矩阵本身带正交约束直接对矩阵元素求导很难保持约束欧拉角有万向锁问题四元数适合表示和插值但导数处理依然不直观。李代数提供了一种旋转和位姿的“局部向量表达”把带约束的矩阵优化问题转化为无约束的向量优化问题。具体来说SO(3) 是三维旋转群对应李代数 so(3) 是三维向量SE(3) 是三维刚体运动群对应李代数 se(3) 是六维向量。二者通过指数映射和对数映射互相转换。实际工程里你不需要手推李代数公式但必须掌握三点一是 Sophus、Eigen 这类库里指数映射和对数映射怎么调用二是 g2o、Ceres 等优化库里位姿顶点和边用的扰动模型是什么三是为什么代码里常用Sophus::SE3d而不是直接用 4x4 矩阵去做优化。刚接触时不要一开始就卡在对数映射的公式推导上。先跑一个最小例程看看SO3、SE3、exp、log的输入输出长什么样形成直觉之后再回头补推导会高效很多。3. 目标检测与分割不是独立模块要和定位共享坐标系3.1 2D 目标检测的评价与落地局限目标检测的任务是“框出来 认出来”。YOLO 系列、DETR 系列这类模型输出目标类别、置信度和二维边界框。在机器人场景里二维框本身不够因为它缺少深度信息只能告诉你图像上哪里有目标不能直接告诉你目标的世界坐标。想要拿到世界坐标需要把二维框和深度图、点云、双目视差或激光数据融合。训练目标检测模型时评价标准不能只看“准确率”。标准做法是先按类别计算 Precision 和 Recall再通过不同置信度阈值画出 PR 曲线计算每个类别的 AP最后对所有类别取平均得到 mAP。还要注意 IoU 阈值mAP0.5 和 mAP0.75 代表不同严格程度不能随便拿两个模型在不同阈值下的结果对比。机器人任务里尤其要多看召回率漏检一个障碍物比误检更危险。小目标检测是个典型难点。无人机视角下的地面行人、远处车辆、仓库高货架上的障碍物目标像素少、特征弱。YOLOv8 这类工具可以通过配置文件调整检测头或锚框尺寸但最终效果仍然取决于训练数据中小目标的比例。先检查数据分布再调网络结构才是正确顺序。3.2 三维目标检测与语义、实例分割三维目标检测输出的是三维包围框中心坐标、长宽高、朝向角通常表达在相机坐标系或激光雷达坐标系下。它可以基于激光点云实现也可以基于图像做单目或双目三维检测还可以做图像和点云融合。三维框比二维框多一个朝向角而这个朝向角恰恰是机器人避障和路径规划最需要的信息。分割任务则更细语义分割给每个像素或每个点一个类别标签实例分割还会区分同一类别里的不同个体。对 SLAM 来说分割最有价值的应用是识别动态物体。如果场景里有人走动把落在人身上的特征点剔除就能避免把移动目标当成静止路标减少定位漂移。这也是“动态 SLAM”最常见的工程实现思路。3.3 动态目标过滤与语义地图把检测和分割接入 SLAM目前常见做法有几类动态特征过滤检测到动态目标后剔除目标框内的特征点或用分割掩码屏蔽对应区域。语义地图把检测和分割结果与地图点绑定建出来的地图带语义标签机器人可以搜索“椅子在哪”而不只是面对一堆无标签点。重定位辅助检测门牌、二维码、固定标志物用已知位置的目标帮助恢复全局位姿。感知驱动的建图改进纹理缺失或小物体区域用深度学习方法补充约束。这些做法的共同前提是检测模块和定位模块必须共享坐标系和时间戳。我见过很多项目模型精度不低最后却失败在检测框没有正确投影到相机坐标系或帧时间戳差了几十毫秒。先把坐标转换链路跑通再来调置信度阈值顺序不能反。4. 视觉 SLAM 和激光 SLAM 怎么选关键看环境和成本4.1 视觉方案单目、双目、RGB-D 各自边界视觉 SLAM 是入门性价比最高的方向因为相机便宜、信息丰富。但不同相机形态的边界完全不同。单目相机只有一个摄像头成本最低但存在尺度不确定问题。单目 SLAM 可以估计轨迹和地图的相对结构无法直接得出“这个物体距离我 3 米”这样的绝对尺度因此不能直接用于高精度导航。双目相机通过左右视差估计深度能解决尺度问题代价是标定复杂、计算量增加。RGB-D 相机直接输出像素对齐的深度图室内近距离效果好但室外强光下深度传感器容易失效。特征点方案的代表是 ORB-SLAM 系列稳健、文档多适合入门直接法和半直接法适合纹理少但亮度稳定的场景光照变化大时鲁棒性较差。判断视觉方案适不适合你的场景就看三个问题光照是否稳定纹理是否足够运动是否太快。三个条件都不满足视觉 SLAM 会很难受。4.2 激光方案2D 建图导航与 3D 激光里程计激光 SLAM 用激光雷达直接测距不依赖光照点云精度高。室内移动机器人常用 2D 激光雷达配合 gmapping、Cartographer 做建图这类方案在地面近似平面的场景里非常成熟扫地机器人、仓储 AGV 大量采用。3D 激光雷达适合户外复杂环境常用 LOAM 系列、LIO-SAM、FAST-LIO 等方案配合 IMU 提高快速运动下的稳定性。激光方案的缺点是成本高2D 激光只能看到某一高度平面在坡道、分层货架等场景会漏掉关键几何信息3D 激光价格更贵点云处理也更重。如果预算有限又是在室内光照稳定环境视觉方案更容易起步如果要在夜间或强光照变化环境长期运行激光方案更可靠。4.3 标定与多源融合别让时间戳和坐标系拖后腿无论选哪种方案传感器标定都要提前做。相机内参标定影响图像去畸变和投影精度常用工具是 Kalibr 或 OpenCV 标定板相机与激光雷达、相机与 IMU 的外参标定决定多传感器数据能否对齐到同一坐标系。Kalibr 是基于 ROS 的工具一般用 Aprilgrid 标定板录制 rosbag再离线标定。很多项目定位效果差不是 SLAM 算法不行而是标定参数不对或时间戳没对齐。IMU 和相机之间存在延迟快速运动时几十毫秒偏差就会造成明显漂移。建议先把传感器时间同步和坐标系关系整理清晰再谈算法调参。下面给一个选型参考维度视觉 SLAM激光 SLAM主要传感器单目/双目/RGB-D 相机2D/3D 激光雷达成本低中到高光照敏感度高低尺度问题单目有尺度不确定可直接测距典型地图稀疏特征点地图/稠密点云2D 栅格地图/3D 点云适用场景室内、光照稳定、纹理丰富室内外、光照变化大、几何结构清晰入门难度中数学要求稍高中重投影较直观但工具链复杂5. 从开源库到轨迹评估把一条 SLAM 流程完整跑通5.1 环境准备先确认依赖版本再编译我一般建议在 Linux 环境里跑 SLAMUbuntu 20.04 或 22.04 都比较常见。使用 ROS 可以省去很多消息和时间同步的重复工作但如果不打算用 ROS也可以直接编译 ORB-SLAM 这类纯 C 项目。依赖通常包括Eigen3矩阵运算、OpenCV图像处理、Sophus李代数、Pangolin可视化界面、g2o 或 Ceres图优化。目标检测模型如果要在 SLAM 流程里实时运行大概率需要 GPU如果只是验证思路纯 CPU 也能跑但要降低输入分辨率和 batch 大小。# 示意命令具体版本以你的系统为准 sudo apt update sudo apt install build-essential cmake git libeigen3-dev # 轨迹评估工具通常用 Python 安装 pip install evo第一次跑 SLAM 项目最容易翻车的地方是依赖版本不匹配。OpenCV 3 和 OpenCV 4 的接口有差异Pangolin 对 OpenGL 版本有要求Sophus 有非模板版和模板版之分。先读项目的 README 和 CMakeLists确认依赖版本范围再逐项安装。不要一上来就全量 apt install缺少哪个就补哪个报错日志里通常已经写了线索。5.2 跑通 ORB-SLAM 的最小流程ORB-SLAM 系列是入门视觉 SLAM 最常见的开源项目。以单目为例最小流程是编译项目下载 TUM 或 EuRoC 数据集运行单目示例。# 示意命令路径和参数以你的环境为准 cd ORB_SLAM2 ./Examples/Monocular/mono_tum \ Vocabulary/ORBvoc.txt \ Examples/Monocular/TUM1.yaml \ /path/to/rgbd_dataset_freiburg1_desk这里最容易忽略的几点路径不能包含中文和空格否则加载词典或数据集时会出错。运行前确认数据集序列和时间戳文件存在ORB-SLAM 依赖关联文件来找图像。单目方案初始化需要缓慢平移摄像头如果一开始对着白墙或纯纹理区域初始化会失败。跑通之后先别急着换数据集。打开可视化界面观察相机视角、关键帧和地图点的生成情况确认建图不是一团乱麻之后再换更复杂的数据序列。5.3 用 evo 做轨迹评估指标怎么选跑通只是第一步评估精度才是证明方案可行的关键。evo 是 SLAM 轨迹评估最常用的开源工具支持 TUM、KITTI、EuRoC 等数据集格式核心指标有两个绝对位姿误差 APE 和相对位姿误差 RPE。# 将估计轨迹与真值对比-a 表示先做坐标对齐 evo_ape tum groundtruth.txt keyframe_trajectory.txt -a # 相对误差更适合评估里程计的局部漂移 evo_rpe tum groundtruth.txt keyframe_trajectory.txt -a看评估结果时不要只盯着 RMSE 一个数。还要看最大值、中位数和误差曲线了解误差集中出现在哪个时间段。如果误差在某个转弯处突然增大很可能是跟踪丢失后重定位造成的和普通漂移是两类问题。evo 也可以读 rosbag 里的轨迹 topic方便实机测试时直接评估。5.4 把检测模型接进来先离线、再同步、再实时如果你想把目标检测和 SLAM 结合我的建议是分三步做。第一步离线验证用数据集图像跑检测模型把检测结果保存成文件不接入 SLAM。先确认模型本身的效果、类别的准确性、小目标召回的短板。第二步同步对接把检测结果按时间戳与图像帧对齐再把检测框或分割掩码投影到相机坐标系验证坐标转换正确。这一步通常在独立的 Python 脚本里完成不要直接改 SLAM 主循环。第三步实时集成检测线程和 SLAM 线程分别运行用队列或消息缓存做同步。先在低分辨率、低帧率下验证稳定性再逐步提高输入大小。不要一上来就开最大并发GPU 占用瞬间拉满SLAM 线程反而被拖慢。我见过很多项目在这一步栽跟头模型精度很高但每次接进 SLAM 后地图就花。原因往往是检测线程把 SLAM 线程的锁抢了或检测结果直接写入了共享地图点数组。先保证两个线程的边界清晰再谈精度。6. 常见坑、边界认识与一条现实的上手路线6.1 最容易踩的五个坑第一时间戳不对齐。多传感器系统里不同传感器触发时间不同直接按消息到达顺序处理会产生错位。先用工具画出时间戳分布再决定是否用插值或硬件同步。第二坐标系没转换。检测框是从像素坐标系输出的SLAM 优化的是相机坐标系或世界坐标系中间隔着内参、外参和畸变模型。少一个环节结果就偏了。第三标定参数不准。相机内参、相机与 IMU 外参、激光与相机外参任何一项不准都会让多传感器融合效果大打折扣。大多数情况下先重新标定比换算法更有效。第四复制数据集参数到自己的环境。每个系统都有焦距、分辨率、失真系数、话题名差异不按自己的传感器重新配置跑出来一片乱是正常现象。第五用评估工具时不做对齐就报误差。evo 的轨迹评估默认可以先做 Sim(3) 或 SE(3) 对齐不对齐就统计误差误差里混合了坐标框架偏差和真实算法误差结论没有意义。6.2 SLAM、三维重建、检测与分割的边界很多初学者把 SLAM 和三维重建混为一谈。SLAM 的核心目标是实时定位和一致性地构建可用于导航的地图地图可以是稀疏特征点不一定要漂亮三维重建的目标是把场景还原成高精度的稠密模型和纹理计算量大实时性要求低。侧重点完全不同。检测和分割也一样。目标检测告诉你“这是什么、在哪里”但如果不结合深度和坐标转换它只是图像层的感知不是空间层的感知。支持某个功能不等于它在所有场景都稳定。RGB-D 相机在室内好用拿到阳光下可能直接丢深度YOLO 在公开数据集上精度高换到你的机器人视角和光照条件下可能需要重新采集数据微调。对工具能力保持边界感是工程判断力的一部分。6.3 一条现实的学习路线如果想系统入门 SLAM 与机器人感知我建议按下面顺序走。第一步读完《视觉SLAM十四讲》的核心章节重点是三维空间刚体运动、李群与李代数、相机模型、状态估计和非线性优化。配合书中代码在 Ubuntu 里亲手编译一遍熟悉 Eigen、Sophus、OpenCV 的用法。第二步跑通 ORB-SLAM 或同类开源项目先用公开数据集验证再用自己的相机录制数据。自己录一次数据就会理解标定、时间戳、图像质量对结果的影响这些是公开数据集给不了的体验。第三步在定位基础上接入检测或分割模块做一个动态目标过滤的小实验。哪怕只用最简单的方式把移动目标区域的点剔除也能让你真正理解感知和定位怎么协同。第四步建立评估闭环。用 evo 或 ROS 工具记录并评估轨迹把每次实验的配置、参数、结果和日志整理清楚。后续调优时这套记录就是你排查问题的最好依据。最后提醒一句学习 SLAM 最容易犯的错误是花大量时间追最新论文却不关心数据、标定和评估是否可靠。先把一条小流程跑稳再扩大场景比什么技术都重要。真正决定项目成败的往往不是模型选得有多新而是前置环境和输入材料有没有处理干净。