尧图网页设计 
很多人第一次接触 SLAM是从一个最朴素的问题开始的无人车到底怎么实时知道自己在哪GNSS 看起来已经解决了几十年的定位问题但真正上过车的人会告诉你隧道、高架桥下、城市峡谷里 GNSS 会瞬间失效激光雷达在动态车流里有大量运动畸变摄像头在强光和黑夜中频繁丢特征。真正撑起“高精度定位建图”这四个字的恰恰是那个看起来最“基础”的 SLAM 系统。本文打算把无人驾驶 SLAM 这条线完整串一遍坐标系、滤波、图优化、因子图、激光视觉融合以及一条真正能落地的工程流程。适合正在入门 SLAM、准备做定位建图或者在实车项目里踩过融合坑的开发者。先给出一个明确判断SLAM 不是某一个算法而是一套系统工程。坐标系定义、时间同步、外参标定这三件事占掉了实际项目中一半以上的工作量。算法只是最后那一步——把良好的输入变成稳定的输出。所以这篇文章不会只讲数学公式而是把工程视角放到和算法视角同等重要的位置。1. 无人驾驶 SLAM 为什么难难在系统而不是单个算法很多初学者下载 ORB-SLAM 跑通一个数据集或者用 LOAM 跑通一帧点云配准就觉得自己已经掌握 SLAM 了。但把这些算法放进无人驾驶系统时所有人都会经历一轮“认知重建”。第一层困难在于传感器种类多。无人车通常同时携带激光雷达、摄像头、IMU、GNSS/RTK、轮速计。每种传感器都有独立坐标系、独立采样频率、独立误差特性。IMU 高频但漂移激光雷达精度高但稀疏点云退化视觉信息丰富但受光照影响大。任何一个环节没有处理好融合系统的精度就会被最差的传感器拖下水。第二层困难在于环境苛刻。无人驾驶场景不像室内小场景道路上的车辆、行人、树木都在动路面纹理弱高架桥、隧道、长直走廊又充满退化结构。SLAM 最怕的三种场景——低纹理、几何退化、动态干扰——在自动驾驶场景里几乎全部存在。第三层困难在于工程链路长。从数据采集、传感器标定、时间同步、前端里程计、后端优化、回环检测到最终建图是一条完整链路。很多论文里的算法只覆盖一两个环节而系统落地需要你把这整条链路修通。所以当你带着“学习路径上应该先学什么”的心态来看这篇教程时我的建议是先建立全局坐标系意识再理解两类核心算法——滤波和图优化最后研究如何把激光和视觉融合起来。下面按这个顺序展开。2. 坐标系SLAM 入门的第一个大坑2.1 SLAM 中有哪些坐标系刚接触 SLAM 时最容易让人懵掉的就是坐标系。车里几乎每台传感器都有自己的一套坐标激光雷达坐标系、相机坐标系、IMU 坐标系、车体坐标系、世界坐标系还有 GNSS 用的 WGS84 经纬度坐标。从材料里也能看到很多人搜索的是“相机坐标系的右手系怎么判断 xy 轴方向”“wgs84 坐标系 zone 1-18 是啥意思”这类问题。这说明坐标系问题是真实且高频的入门障碍。先给一个通用约定。ROS 的 REP 103 建议机器人本体坐标系按右手系、x 轴朝前、y 轴朝左、z 轴朝上定义。激光雷达和 IMU 的坐标系一般也遵循类似习惯。世界坐标系通常取地图原点如果接 GNSS则用 WGS84 提供经纬度再投影到当地切平面 ENU 坐标系x 朝东、y 朝北、z 朝上使用。相机坐标系是两个最特殊的。OpenCV 习惯的针孔模型中相机坐标系是x 向右、y 向下、z 向前。注意这个组合按右手定则验证其实是左手系当 x 向右、y 向下时x 叉乘 y 指向相机后方所以 z 若取向前这套坐标系不是右手系。另一种常见定义是 x 向右、y 向上、z 向后这才是标准右手系。很多代码里的符号错误、旋转矩阵转置错误根子就出在这两套约定混用上。2.2 坐标变换与刚体变换矩阵SLAM 中的坐标系关系用刚体变换描述[ p R p t ]其中 R 是旋转矩阵t 是平移向量。为了把多次变换串起来工程上通常写成 4x4 齐次变换矩阵 T[ T \begin{bmatrix} R t \ 0 1 \end{bmatrix} ]点 p 从传感器坐标系变换到车体坐标系就是一次矩阵乘法。外参标定得到的本质就是这些 T 矩阵。需要注意的是传感器之间变换是单向的由 A 到 B 的 T 与由 B 到 A 的 T 互为逆矩阵不要想当然地“把向量取反”。2.3 代码示例用 numpy 做坐标变换下面这个最小示例演示如何构造变换矩阵并进行坐标变换这是所有 SLAM 工程里最常用的操作。# 文件路径transforms.py import numpy as np def rodrigues(rvec): 旋转向量转旋转矩阵罗德里格斯公式 theta np.linalg.norm(rvec) if theta 1e-8: return np.eye(3) kx, ky, kz rvec / theta K np.array([ [0.0, -kz, ky], [kz, 0.0, -kx], [-ky, kx, 0.0] ]) R np.eye(3) np.sin(theta) * K (1 - np.cos(theta)) * K K return R def make_tf(R, t): 由旋转矩阵和平移向量构造 4x4 齐次变换矩阵 T np.eye(4) T[:3, :3] R T[:3, 3] t return T # 激光雷达相对车体的外参来自外参标定结果 R_lidar_to_body rodrigues(np.array([0.0, 0.0, 0.0])) t_lidar_to_body np.array([0.3, 0.0, 0.5]) T_lidar_to_body make_tf(R_lidar_to_body, t_lidar_to_body) # 激光雷达坐标系下的一个点齐次坐标 p_lidar np.array([1.0, 2.0, 0.8, 1.0]) p_body T_lidar_to_body p_lidar print(车体坐标系下的坐标:, p_body[:3])这段代码的真正价值不是那几行矩阵运算而是让你养成一个习惯任何传感器数据进入算法之前先确认它所在坐标系再确认目标坐标系否则后续所有优化都建立在错误输入上。2.4 新手最容易踩的坐标系问题坐标系问题在代码里通常表现为“轨迹整体倾斜”“融合图形错位”“旋转方向反了”等。三个高频原因第一是相机与激光的模式混用。相机输出像素坐标要先通过内参转到相机坐标系激光输出的是传感器坐标系下的三维点。两者不能直接相加。第二是旋转矩阵与欧拉角的万向锁。欧拉角在表达车体姿态时直观但不适合做连续插值更不适合写进优化问题。工程上建议内部统一用旋转矩阵或四元数只在可视化时转成欧拉角。第三是左手系与右手系的符号问题。很多“看起来没毛病”的代码最后发现只是 z 轴符号取反了。遇到这类问题建议先打一个已知坐标系的点云再与 CAD 模型或可视化工具中的坐标轴对照检查。3. 滤波类 SLAM从 KF 到 EKF 再到粒子滤波3.1 滤波的本质是状态估计“滤波”这个词在 SLAM 语境下和“低通滤波”“高斯滤波”完全不同。搜索热词里出现了很多信号处理方向的滤波词例如 RC 滤波电路、LC 滤波、中值滤波这些是处理信号波形的而 SLAM 中的滤波指的是用贝叶斯推断做状态估计根据历史观测和当前观测估计机器人位姿和地图。两者名字相同但数学工具完全不同。如果你带着“低通滤波”的直觉去理解 EKF会一直找不到感觉。滤波类 SLAM 的核心思想是把状态位姿、路标点看成随机变量通过“预测更新”两步递归估计预测根据运动模型用上一时刻状态和当前控制输入推算当前状态更新获得传感器观测后用观测模型修正预测结果。3.2 卡尔曼滤波与扩展卡尔曼滤波卡尔曼滤波Kalman Filter, KF适用于线性高斯系统在自动驾驶中直接应用的场景不多因为车辆运动和传感器模型几乎都是非线性的。扩展卡尔曼滤波Extended Kalman Filter, EKF在 KF 基础上对非线性模型做一阶泰勒展开是早期视觉 SLAM 和组合导航中最常见的方案MSCKF 就是 EKF 框架的一个代表。EKF 的优势是计算量小、适合嵌入式实时场景缺点是线性化误差会累积而且一旦状态维度变大比如同时估计数百个路标点协方差矩阵规模会膨胀计算量快速上升。所以在现代 SLAM 中EKF 更多出现在组合导航、里程计输出这些局部估计环节而不是大规模建图环节。3.3 粒子滤波与全局定位粒子滤波用一组带权重的随机粒子近似状态分布不要求高斯分布也不用做线性化。二维室内 SLAM 里的 Gmapping、AMCL 就是粒子滤波的典型应用。AMCL 能够从未知初始位置开始做全局定位这在重定位和机器人绑架问题中很有价值。但粒子滤波在高维空间中需要指数级数量的粒子所以它很难扩展到三维大场景。无人驾驶里粒子滤波常用于定位阶段的重定位而不是实时建图的全局后端。3.4 一个简单的 EKF 示例下面用 Python 写一个二维机器人 EKF 的预测与更新演示 KF 家族的基本骨架。# 文件路径ekf_demo.py import numpy as np def predict(mu, sigma, u, dt): 运动模型为 v/w 的差速模型输入控制 u[v, w] v, w u theta mu[2, 0] if abs(w) 1e-6: mu_new mu np.array([[v * np.cos(theta) * dt], [v * np.sin(theta) * dt], [0.0]]) else: mu_new mu np.array([[v / w * (np.sin(theta w*dt) - np.sin(theta))], [v / w * (-np.cos(theta w*dt) np.cos(theta))], [w * dt]]) # 运动模型对状态的雅可比矩阵 G np.array([[1, 0, -v * np.sin(theta) * dt], [0, 1, v * np.cos(theta) * dt], [0, 0, 1]]) R np.diag([0.05, 0.05, 0.02]) # 过程噪声 sigma_new G sigma G.T R return mu_new, sigma_new def update(mu, sigma, z, Q): 观测为 GNSS 位置观测模型为线性 H[I 0] C np.array([[1, 0, 0], [0, 1, 0]]) K sigma C.T np.linalg.inv(C sigma C.T Q) mu_new mu K (z - C mu) sigma_new (np.eye(3) - K C) sigma return mu_new, sigma_new # 初始状态x, y, thetatheta 为车头朝向 mu np.array([[0.0], [0.0], [0.0]]) sigma np.eye(3) dt 0.1 Q np.diag([0.1, 0.1]) # 位置观测噪声 for step in range(10): mu, sigma predict(mu, sigma, [1.0, 0.0], dt) if step % 3 0: z np.array([[mu[0, 0] np.random.randn() * 0.1], [mu[1, 0] np.random.randn() * 0.1]]) mu, sigma update(mu, sigma, z, Q) print(fstep {step}: x{mu[0,0]:.3f}, y{mu[1,0]:.3f}, theta{mu[2,0]:.3f})运行后能看到位置逐渐修正、协方差收敛的过程。实际工程中这里的运动模型可能是 IMU 预积分观测可能是 GNSS 或激光匹配结果但“预测-更新-修正协方差”的基本循环不变。4. 图优化与因子图现代 SLAM 的核心引擎4.1 从滤波到图优化为什么要变滤波维护的只是“当前状态”过去的信息只能通过协方差间接保留回环检测一旦发生需要把误差传回历史所有位姿时滤波就非常吃力。图优化的思路完全不同它把历史上所有待估计的位姿和路标作为变量节点把传感器观测作为约束边构建一个图然后最小化所有约束的误差。这就是为什么今天主流 SLAM 系统几乎都走图优化路线。ORB-SLAM 用 g2o 做局部和全局 BACartographer 用 Ceres 做约束优化LIO-SAM、LVI-SAM 等激光惯性系统也在后端使用因子图优化。4.2 因子图用节点和边描述 SLAM因子图是图优化的一种表达方式。图中的节点分成两类变量节点待估计的量如某个时刻的位姿、某个路标点的坐标因子节点连接若干变量的约束如里程计因子、IMU 预积分因子、激光匹配因子、回环因子、先验因子。每一类因子对应一个误差项图优化就是求解一组变量让所有因子的加权残差平方和最小。与传统“位姿图路标点一起优化”的 BA 相比因子图更强调因子可插拔、可增量非常契合多传感器融合的场景——新增一种传感器就是新增一类因子。“滑动窗口优化”可以看作是滤波和图优化之间的桥梁。VINS-Mono 等 VIO 系统并不优化所有历史状态而是维护最近 N 帧位姿把窗口外的状态通过边缘化技术转成先验约束。这样既保留了图优化的精度又控制了计算量。在无人驾驶在线定位中这种模式很常见。4.3 代码示例GTSAM 构建位姿图优化GTSAM 是佐治亚理工学院开源的因子图库Python 接口非常友好。下面用 Pose2 位姿图演示一个带回环的优化过程。# 文件路径pose_graph_demo.py # 安装依赖pip install gtsam import gtsam from gtsam.symbol_shorthand import X graph gtsam.NonlinearFactorGraph() initial gtsam.Values() # 先验因子固定第一个位姿 prior_model gtsam.noiseModel.Diagonal.Sigmas([0.3, 0.3, 0.1]) graph.add(gtsam.PriorFactorPose2(X(1), gtsam.Pose2(0, 0, 0), prior_model)) initial.insert(X(1), gtsam.Pose2(0, 0, 0)) # 里程计因子相邻位姿的相对运动 odom_model gtsam.noiseModel.Diagonal.Sigmas([0.2, 0.2, 0.1]) odometry [ (1, 2, gtsam.Pose2(2, 0, 0)), (2, 3, gtsam.Pose2(0, 2, 0)), (3, 4, gtsam.Pose2(-2, 0, 0)), (4, 5, gtsam.Pose2(0, -2, 0)), ] for i, j, delta in odometry: graph.add(gtsam.BetweenFactorPose2(X(i), X(j), delta, odom_model)) if j not in initial: initial.insert(X(j), gtsam.Pose2(j * 2 1, 0, 0)) # 回环因子第 5 个位姿应回到第 1 个位姿附近 loop_model gtsam.noiseModel.Diagonal.Sigmas([0.2, 0.2, 0.1]) graph.add(gtsam.BetweenFactorPose2(X(5), X(1), gtsam.Pose2(0, 0, 0), loop_model)) # 构建并运行优化器 optimizer gtsam.LevenbergMarquardtOptimizer(graph, initial) result optimizer.optimize() # 打印结果 print(优化后的轨迹) for i in range(1, 6): pose result.atPose2(X(i)) print(fX({i}): x{pose.x():.3f}, y{pose.y():.3f}, theta{pose.theta():.3f})这个示例的关键点是回环之前轨迹由里程计逐步推算误差会累积加入回环因子后优化器会把第 5 个位姿拉回起点附近同时整体调整中间所有位姿。实际工程中回环因子来自激光点云匹配或视觉词袋检测噪声模型需要根据匹配置信度设置。4.4 滤波 vs 图优化对比维度滤波EKF 类图优化因子图核心思想只维护当前状态递归预测更新维护所有历史位姿统一最小化约束误差计算复杂度低适合轻量实时系统高但现代稀疏求解器已很高效历史信息利用通过协方差间接保留直接保留所有历史约束回环处理困难需要额外维护过去状态天然适合加一条边即可典型应用组合导航、轻量里程计VIO、LIO、激光视觉融合、大场景建图典型开源实现robot_localization、MSCKFGTSAM、g2o、Ceres、Cartographer理解这张表的结论滤波没有过时它更适合在线局部估计但大场景、多传感器融合、回环修正几乎都走图优化。5. 激光视觉融合高精度定位建图的工程解5.1 为什么需要激光视觉融合激光雷达精度高能直接测距在弱纹理场景下依然可靠但点云稀疏难以提取语义信息且成本高。摄像头纹理丰富、成本低、有语义信息但深度不可靠、受光照影响大。两者结合本质上是“几何精度 视觉丰富度”的互补。从工程效果看融合带来的提升主要有三点激光提供几何约束保证轨迹在弱纹理场景中不飘视觉提供特征关联在建图、回环检测、语义标注上弥补激光的不足IMU 作为高频惯性约束填补两帧激光之间和两帧图像之间的运动估计空白。这套“激光 相机 IMU”的组合正是目前高精度定位建图的主流传感器配置。5.2 松耦合与紧耦合融合方式分两类松耦合是“各算各的最后结果融合”。典型做法是激光里程计给出位姿视觉里程计给出位姿再用一个滤波器或简单的加权融合得到最终位姿。优点是模块解耦、工程简单、便于排查缺点是没有充分利用底层观测的信息精度上限有限。紧耦合是在一个优化框架里同时处理多传感器约束。LIO-SAM、LVI-SAM、R3LIVE 等都是这个思路帧间里程计、雷达匹配因子、视觉特征重投影因子、IMU 预积分因子、回环因子一起进入因子图。紧耦合精度更高但对时间同步、外参标定、噪声模型的要求也更高。如果刚起步建议先做松耦合把单传感器里程计调稳定再逐步收紧到底层约束。5.3 外参标定与时间同步激光视觉融合最容易翻车的两个环节不是算法而是标定和同步。外参标定的本质是求传感器之间的 T 矩阵。相机-激光标定常用标定板或特定场景自动标定工具有很多相机-IMU 标定常用 kalibr激光-IMU 标定也有专门的工具。这里特别强调标定结果要存档、可复现并且每次拆卸安装传感器后必须重新标定。外参误差 1 厘米在 50 米外就会造成明显的重投影错位。时间同步也有两种层次。硬件同步是通过 PPS 秒脉冲和 GPRMC 时间戳把各传感器对齐到同一时间基准精度高适合量产和实车软件同步是为每帧数据打上统一时间戳利用姿态插值补偿运动适合算法验证和中低速场景。激光雷达一帧点云在扫描期间车辆已经运动了一段距离如果不对点云做运动畸变补偿点云会“拖影”这在高速场景下尤其致命。6. 高精度定位建图的完整工程流程6.1 第一步数据采集与传感器配置先做好数据采集。采集时要注意传感器时间戳是否统一、激光点云是否有畸变、车辆是否覆盖了闭环线路、GNSS 信号好的区域和差的区域都要采。数据是算法验证的基础数据质量不够后续所有环节都被拖累。# 录制多传感器数据包示意 rosbag record -O campus.bag \ /imu/data \ /lidar/points \ /camera/image \ /gnss/fix# 播放数据包并发布仿真时钟保证多传感器时间戳对齐 rosbag play campus.bag --clock --rate 1.0第一段命令把多传感器数据录成一个 bag第二段命令按原始时间戳播放。时间戳对齐在多传感器融合中永远是第一优先级。6.2 第二步内外参标定相机要标内参焦距、主点、畸变系数激光-相机、激光-IMU、相机-IMU 要标外参。用一个示意配置文件记录标定结果# 文件路径config/sensors.yaml sensors: lidar: frame_id: lidar_link publish_rate_hz: 10 camera: frame_id: camera_link publish_rate_hz: 20 imu: frame_id: imu_link publish_rate_hz: 200 extrinsics: # 顺序按实际标定工具导出的格式填写此处仅示意 T_camera_lidar: [0.0, 0.03, -0.25, 0.0, -0.005, 0.0] T_lidar_imu: [0.05, 0.0, 0.2, 0.0, 0.0, 0.0]这份配置的要点是每个传感器有独立 frame_id外参统一放在一处管理而不是散落在代码里。工程上所有模块读取同一个配置来源可以避免“一个模块一套外参”的灾难。6.3 第三步前端里程计前端负责帧间位姿估计。激光里程计常用 ICP、NDT 做帧间配准视觉里程计提取特征点、光流跟踪、对极几何或 PnP 估计位姿。前端最重要的输出是不错的初值和差观测。特别注意退化场景检测长直走廊、空旷广场、隧道内激光前端的约束矩阵会出现退化方向此时只靠激光里程计必然漂移。检测方法通常是对约束矩阵做特征值分解当最小特征值远小于最大特征值时预警退化并降低该方向权重让 IMU 或视觉约束补位。6.4 第四步后端优化与回环检测后端把前端的帧间约束、IMU 预积分约束、回环约束全部送进因子图优化。场景规模大时用滑动窗口限制计算量需要全局一致性时做全局优化。回环检测对建图精度至关重要。激光回环常用点云匹配ScanContext、ICP 描述子视觉回环常用词袋模型DBoW2。回环检测一定要加验证步骤否则错误回环比没有回环更致命一旦把两条本来不相干的轨迹“焊”在一起整张地图就废了。6.5 第五步建图与定位建图通常分为离线建图和在线定位。离线建图用全量数据做全局优化生成高精度点云地图或占据栅格地图在线定位则是用预建地图做实时匹配常用 NDT 或 ICP 在局部地图中匹配当前帧输出高精度位姿。这里要区分两个概念定位是“在地图中找自己”建图是“把环境画出来”。很多项目失败是因为把两者混在一个线程里处理丢失了全局一致性。7. 常见问题与排查思路问题现象可能原因排查方式解决方案融合轨迹发散时间同步不齐多传感器时间戳错位打印各传感器最新时间戳比较差值硬件同步或软件插值对齐时间戳相机与激光投影错位外参标定不准可视化投影点云到图像上检查边缘对齐重新标定或增加标定板采集数据长直走廊定位偏移退化场景约束不足对约束矩阵做特征值分析加入 IMU 因子降低退化方向权重建图重影、轨迹不闭合回环检测未触发或错误回环可视化回环边检查闭环位置是否合理增加回环验证调节匹配阈值视觉特征跟丢运动模糊、光照突变、快速旋转查看特征点数量随时间变化曲线提高相机帧率减少曝光时间优化不收敛初值误差过大噪声模型不合理打印初始误差与迭代后误差改善前端初值重新调节噪声协方差点云出现拖影激光扫描期间车辆运动检查运动畸变补偿是否开启使用 IMU/里程计做逐点补偿排查时遵循一个原则先看输入再看输出。数据有没有时间戳错位、外参有没有加载错文件、坐标系有没有统一这些问题不解决调优化参数只是浪费时间。8. 最佳实践与工程建议结合多个项目的经验整理几条真正值得贯彻的实践建议。第一统一坐标系规范并在代码里强制约定。所有传感器数据进算法前必须转换到同一个坐标系禁止在模块内部各自定义局部坐标系。使用命名规范如lidar_link、camera_link、imu_link、base_link、map提交代码评审时也好辨认。第二时间同步在采集端解决不要留给算法端补。算法端的插值只能作为降级方案。如果有条件在硬件层接 PPS 秒脉冲同步所有传感器尤其对多雷达、多相机系统收益非常明显。第三标定结果纳入版本管理。每次传感器结构变更后标定文件路径要跟代码版本一起上线。建议把标定数据采集时间、车辆信息、工具版本记录下来形成可追溯的标定记录。第四保留数据回放和回归测试能力。SLAM 系统改动后用同一套 bag 数据跑回归对比轨迹误差和地图质量避免“改好一个模块弄坏另一个模块”。第五从简单场景开始验证再逐步增加复杂度 无人驾驶SLAM系统工程实践：从坐标系到激光视觉融合
很多人第一次接触 SLAM是从一个最朴素的问题开始的无人车到底怎么实时知道自己在哪GNSS 看起来已经解决了几十年的定位问题但真正上过车的人会告诉你隧道、高架桥下、城市峡谷里 GNSS 会瞬间失效激光雷达在动态车流里有大量运动畸变摄像头在强光和黑夜中频繁丢特征。真正撑起“高精度定位建图”这四个字的恰恰是那个看起来最“基础”的 SLAM 系统。本文打算把无人驾驶 SLAM 这条线完整串一遍坐标系、滤波、图优化、因子图、激光视觉融合以及一条真正能落地的工程流程。适合正在入门 SLAM、准备做定位建图或者在实车项目里踩过融合坑的开发者。先给出一个明确判断SLAM 不是某一个算法而是一套系统工程。坐标系定义、时间同步、外参标定这三件事占掉了实际项目中一半以上的工作量。算法只是最后那一步——把良好的输入变成稳定的输出。所以这篇文章不会只讲数学公式而是把工程视角放到和算法视角同等重要的位置。1. 无人驾驶 SLAM 为什么难难在系统而不是单个算法很多初学者下载 ORB-SLAM 跑通一个数据集或者用 LOAM 跑通一帧点云配准就觉得自己已经掌握 SLAM 了。但把这些算法放进无人驾驶系统时所有人都会经历一轮“认知重建”。第一层困难在于传感器种类多。无人车通常同时携带激光雷达、摄像头、IMU、GNSS/RTK、轮速计。每种传感器都有独立坐标系、独立采样频率、独立误差特性。IMU 高频但漂移激光雷达精度高但稀疏点云退化视觉信息丰富但受光照影响大。任何一个环节没有处理好融合系统的精度就会被最差的传感器拖下水。第二层困难在于环境苛刻。无人驾驶场景不像室内小场景道路上的车辆、行人、树木都在动路面纹理弱高架桥、隧道、长直走廊又充满退化结构。SLAM 最怕的三种场景——低纹理、几何退化、动态干扰——在自动驾驶场景里几乎全部存在。第三层困难在于工程链路长。从数据采集、传感器标定、时间同步、前端里程计、后端优化、回环检测到最终建图是一条完整链路。很多论文里的算法只覆盖一两个环节而系统落地需要你把这整条链路修通。所以当你带着“学习路径上应该先学什么”的心态来看这篇教程时我的建议是先建立全局坐标系意识再理解两类核心算法——滤波和图优化最后研究如何把激光和视觉融合起来。下面按这个顺序展开。2. 坐标系SLAM 入门的第一个大坑2.1 SLAM 中有哪些坐标系刚接触 SLAM 时最容易让人懵掉的就是坐标系。车里几乎每台传感器都有自己的一套坐标激光雷达坐标系、相机坐标系、IMU 坐标系、车体坐标系、世界坐标系还有 GNSS 用的 WGS84 经纬度坐标。从材料里也能看到很多人搜索的是“相机坐标系的右手系怎么判断 xy 轴方向”“wgs84 坐标系 zone 1-18 是啥意思”这类问题。这说明坐标系问题是真实且高频的入门障碍。先给一个通用约定。ROS 的 REP 103 建议机器人本体坐标系按右手系、x 轴朝前、y 轴朝左、z 轴朝上定义。激光雷达和 IMU 的坐标系一般也遵循类似习惯。世界坐标系通常取地图原点如果接 GNSS则用 WGS84 提供经纬度再投影到当地切平面 ENU 坐标系x 朝东、y 朝北、z 朝上使用。相机坐标系是两个最特殊的。OpenCV 习惯的针孔模型中相机坐标系是x 向右、y 向下、z 向前。注意这个组合按右手定则验证其实是左手系当 x 向右、y 向下时x 叉乘 y 指向相机后方所以 z 若取向前这套坐标系不是右手系。另一种常见定义是 x 向右、y 向上、z 向后这才是标准右手系。很多代码里的符号错误、旋转矩阵转置错误根子就出在这两套约定混用上。2.2 坐标变换与刚体变换矩阵SLAM 中的坐标系关系用刚体变换描述[ p R p t ]其中 R 是旋转矩阵t 是平移向量。为了把多次变换串起来工程上通常写成 4x4 齐次变换矩阵 T[ T \begin{bmatrix} R t \ 0 1 \end{bmatrix} ]点 p 从传感器坐标系变换到车体坐标系就是一次矩阵乘法。外参标定得到的本质就是这些 T 矩阵。需要注意的是传感器之间变换是单向的由 A 到 B 的 T 与由 B 到 A 的 T 互为逆矩阵不要想当然地“把向量取反”。2.3 代码示例用 numpy 做坐标变换下面这个最小示例演示如何构造变换矩阵并进行坐标变换这是所有 SLAM 工程里最常用的操作。# 文件路径transforms.py import numpy as np def rodrigues(rvec): 旋转向量转旋转矩阵罗德里格斯公式 theta np.linalg.norm(rvec) if theta 1e-8: return np.eye(3) kx, ky, kz rvec / theta K np.array([ [0.0, -kz, ky], [kz, 0.0, -kx], [-ky, kx, 0.0] ]) R np.eye(3) np.sin(theta) * K (1 - np.cos(theta)) * K K return R def make_tf(R, t): 由旋转矩阵和平移向量构造 4x4 齐次变换矩阵 T np.eye(4) T[:3, :3] R T[:3, 3] t return T # 激光雷达相对车体的外参来自外参标定结果 R_lidar_to_body rodrigues(np.array([0.0, 0.0, 0.0])) t_lidar_to_body np.array([0.3, 0.0, 0.5]) T_lidar_to_body make_tf(R_lidar_to_body, t_lidar_to_body) # 激光雷达坐标系下的一个点齐次坐标 p_lidar np.array([1.0, 2.0, 0.8, 1.0]) p_body T_lidar_to_body p_lidar print(车体坐标系下的坐标:, p_body[:3])这段代码的真正价值不是那几行矩阵运算而是让你养成一个习惯任何传感器数据进入算法之前先确认它所在坐标系再确认目标坐标系否则后续所有优化都建立在错误输入上。2.4 新手最容易踩的坐标系问题坐标系问题在代码里通常表现为“轨迹整体倾斜”“融合图形错位”“旋转方向反了”等。三个高频原因第一是相机与激光的模式混用。相机输出像素坐标要先通过内参转到相机坐标系激光输出的是传感器坐标系下的三维点。两者不能直接相加。第二是旋转矩阵与欧拉角的万向锁。欧拉角在表达车体姿态时直观但不适合做连续插值更不适合写进优化问题。工程上建议内部统一用旋转矩阵或四元数只在可视化时转成欧拉角。第三是左手系与右手系的符号问题。很多“看起来没毛病”的代码最后发现只是 z 轴符号取反了。遇到这类问题建议先打一个已知坐标系的点云再与 CAD 模型或可视化工具中的坐标轴对照检查。3. 滤波类 SLAM从 KF 到 EKF 再到粒子滤波3.1 滤波的本质是状态估计“滤波”这个词在 SLAM 语境下和“低通滤波”“高斯滤波”完全不同。搜索热词里出现了很多信号处理方向的滤波词例如 RC 滤波电路、LC 滤波、中值滤波这些是处理信号波形的而 SLAM 中的滤波指的是用贝叶斯推断做状态估计根据历史观测和当前观测估计机器人位姿和地图。两者名字相同但数学工具完全不同。如果你带着“低通滤波”的直觉去理解 EKF会一直找不到感觉。滤波类 SLAM 的核心思想是把状态位姿、路标点看成随机变量通过“预测更新”两步递归估计预测根据运动模型用上一时刻状态和当前控制输入推算当前状态更新获得传感器观测后用观测模型修正预测结果。3.2 卡尔曼滤波与扩展卡尔曼滤波卡尔曼滤波Kalman Filter, KF适用于线性高斯系统在自动驾驶中直接应用的场景不多因为车辆运动和传感器模型几乎都是非线性的。扩展卡尔曼滤波Extended Kalman Filter, EKF在 KF 基础上对非线性模型做一阶泰勒展开是早期视觉 SLAM 和组合导航中最常见的方案MSCKF 就是 EKF 框架的一个代表。EKF 的优势是计算量小、适合嵌入式实时场景缺点是线性化误差会累积而且一旦状态维度变大比如同时估计数百个路标点协方差矩阵规模会膨胀计算量快速上升。所以在现代 SLAM 中EKF 更多出现在组合导航、里程计输出这些局部估计环节而不是大规模建图环节。3.3 粒子滤波与全局定位粒子滤波用一组带权重的随机粒子近似状态分布不要求高斯分布也不用做线性化。二维室内 SLAM 里的 Gmapping、AMCL 就是粒子滤波的典型应用。AMCL 能够从未知初始位置开始做全局定位这在重定位和机器人绑架问题中很有价值。但粒子滤波在高维空间中需要指数级数量的粒子所以它很难扩展到三维大场景。无人驾驶里粒子滤波常用于定位阶段的重定位而不是实时建图的全局后端。3.4 一个简单的 EKF 示例下面用 Python 写一个二维机器人 EKF 的预测与更新演示 KF 家族的基本骨架。# 文件路径ekf_demo.py import numpy as np def predict(mu, sigma, u, dt): 运动模型为 v/w 的差速模型输入控制 u[v, w] v, w u theta mu[2, 0] if abs(w) 1e-6: mu_new mu np.array([[v * np.cos(theta) * dt], [v * np.sin(theta) * dt], [0.0]]) else: mu_new mu np.array([[v / w * (np.sin(theta w*dt) - np.sin(theta))], [v / w * (-np.cos(theta w*dt) np.cos(theta))], [w * dt]]) # 运动模型对状态的雅可比矩阵 G np.array([[1, 0, -v * np.sin(theta) * dt], [0, 1, v * np.cos(theta) * dt], [0, 0, 1]]) R np.diag([0.05, 0.05, 0.02]) # 过程噪声 sigma_new G sigma G.T R return mu_new, sigma_new def update(mu, sigma, z, Q): 观测为 GNSS 位置观测模型为线性 H[I 0] C np.array([[1, 0, 0], [0, 1, 0]]) K sigma C.T np.linalg.inv(C sigma C.T Q) mu_new mu K (z - C mu) sigma_new (np.eye(3) - K C) sigma return mu_new, sigma_new # 初始状态x, y, thetatheta 为车头朝向 mu np.array([[0.0], [0.0], [0.0]]) sigma np.eye(3) dt 0.1 Q np.diag([0.1, 0.1]) # 位置观测噪声 for step in range(10): mu, sigma predict(mu, sigma, [1.0, 0.0], dt) if step % 3 0: z np.array([[mu[0, 0] np.random.randn() * 0.1], [mu[1, 0] np.random.randn() * 0.1]]) mu, sigma update(mu, sigma, z, Q) print(fstep {step}: x{mu[0,0]:.3f}, y{mu[1,0]:.3f}, theta{mu[2,0]:.3f})运行后能看到位置逐渐修正、协方差收敛的过程。实际工程中这里的运动模型可能是 IMU 预积分观测可能是 GNSS 或激光匹配结果但“预测-更新-修正协方差”的基本循环不变。4. 图优化与因子图现代 SLAM 的核心引擎4.1 从滤波到图优化为什么要变滤波维护的只是“当前状态”过去的信息只能通过协方差间接保留回环检测一旦发生需要把误差传回历史所有位姿时滤波就非常吃力。图优化的思路完全不同它把历史上所有待估计的位姿和路标作为变量节点把传感器观测作为约束边构建一个图然后最小化所有约束的误差。这就是为什么今天主流 SLAM 系统几乎都走图优化路线。ORB-SLAM 用 g2o 做局部和全局 BACartographer 用 Ceres 做约束优化LIO-SAM、LVI-SAM 等激光惯性系统也在后端使用因子图优化。4.2 因子图用节点和边描述 SLAM因子图是图优化的一种表达方式。图中的节点分成两类变量节点待估计的量如某个时刻的位姿、某个路标点的坐标因子节点连接若干变量的约束如里程计因子、IMU 预积分因子、激光匹配因子、回环因子、先验因子。每一类因子对应一个误差项图优化就是求解一组变量让所有因子的加权残差平方和最小。与传统“位姿图路标点一起优化”的 BA 相比因子图更强调因子可插拔、可增量非常契合多传感器融合的场景——新增一种传感器就是新增一类因子。“滑动窗口优化”可以看作是滤波和图优化之间的桥梁。VINS-Mono 等 VIO 系统并不优化所有历史状态而是维护最近 N 帧位姿把窗口外的状态通过边缘化技术转成先验约束。这样既保留了图优化的精度又控制了计算量。在无人驾驶在线定位中这种模式很常见。4.3 代码示例GTSAM 构建位姿图优化GTSAM 是佐治亚理工学院开源的因子图库Python 接口非常友好。下面用 Pose2 位姿图演示一个带回环的优化过程。# 文件路径pose_graph_demo.py # 安装依赖pip install gtsam import gtsam from gtsam.symbol_shorthand import X graph gtsam.NonlinearFactorGraph() initial gtsam.Values() # 先验因子固定第一个位姿 prior_model gtsam.noiseModel.Diagonal.Sigmas([0.3, 0.3, 0.1]) graph.add(gtsam.PriorFactorPose2(X(1), gtsam.Pose2(0, 0, 0), prior_model)) initial.insert(X(1), gtsam.Pose2(0, 0, 0)) # 里程计因子相邻位姿的相对运动 odom_model gtsam.noiseModel.Diagonal.Sigmas([0.2, 0.2, 0.1]) odometry [ (1, 2, gtsam.Pose2(2, 0, 0)), (2, 3, gtsam.Pose2(0, 2, 0)), (3, 4, gtsam.Pose2(-2, 0, 0)), (4, 5, gtsam.Pose2(0, -2, 0)), ] for i, j, delta in odometry: graph.add(gtsam.BetweenFactorPose2(X(i), X(j), delta, odom_model)) if j not in initial: initial.insert(X(j), gtsam.Pose2(j * 2 1, 0, 0)) # 回环因子第 5 个位姿应回到第 1 个位姿附近 loop_model gtsam.noiseModel.Diagonal.Sigmas([0.2, 0.2, 0.1]) graph.add(gtsam.BetweenFactorPose2(X(5), X(1), gtsam.Pose2(0, 0, 0), loop_model)) # 构建并运行优化器 optimizer gtsam.LevenbergMarquardtOptimizer(graph, initial) result optimizer.optimize() # 打印结果 print(优化后的轨迹) for i in range(1, 6): pose result.atPose2(X(i)) print(fX({i}): x{pose.x():.3f}, y{pose.y():.3f}, theta{pose.theta():.3f})这个示例的关键点是回环之前轨迹由里程计逐步推算误差会累积加入回环因子后优化器会把第 5 个位姿拉回起点附近同时整体调整中间所有位姿。实际工程中回环因子来自激光点云匹配或视觉词袋检测噪声模型需要根据匹配置信度设置。4.4 滤波 vs 图优化对比维度滤波EKF 类图优化因子图核心思想只维护当前状态递归预测更新维护所有历史位姿统一最小化约束误差计算复杂度低适合轻量实时系统高但现代稀疏求解器已很高效历史信息利用通过协方差间接保留直接保留所有历史约束回环处理困难需要额外维护过去状态天然适合加一条边即可典型应用组合导航、轻量里程计VIO、LIO、激光视觉融合、大场景建图典型开源实现robot_localization、MSCKFGTSAM、g2o、Ceres、Cartographer理解这张表的结论滤波没有过时它更适合在线局部估计但大场景、多传感器融合、回环修正几乎都走图优化。5. 激光视觉融合高精度定位建图的工程解5.1 为什么需要激光视觉融合激光雷达精度高能直接测距在弱纹理场景下依然可靠但点云稀疏难以提取语义信息且成本高。摄像头纹理丰富、成本低、有语义信息但深度不可靠、受光照影响大。两者结合本质上是“几何精度 视觉丰富度”的互补。从工程效果看融合带来的提升主要有三点激光提供几何约束保证轨迹在弱纹理场景中不飘视觉提供特征关联在建图、回环检测、语义标注上弥补激光的不足IMU 作为高频惯性约束填补两帧激光之间和两帧图像之间的运动估计空白。这套“激光 相机 IMU”的组合正是目前高精度定位建图的主流传感器配置。5.2 松耦合与紧耦合融合方式分两类松耦合是“各算各的最后结果融合”。典型做法是激光里程计给出位姿视觉里程计给出位姿再用一个滤波器或简单的加权融合得到最终位姿。优点是模块解耦、工程简单、便于排查缺点是没有充分利用底层观测的信息精度上限有限。紧耦合是在一个优化框架里同时处理多传感器约束。LIO-SAM、LVI-SAM、R3LIVE 等都是这个思路帧间里程计、雷达匹配因子、视觉特征重投影因子、IMU 预积分因子、回环因子一起进入因子图。紧耦合精度更高但对时间同步、外参标定、噪声模型的要求也更高。如果刚起步建议先做松耦合把单传感器里程计调稳定再逐步收紧到底层约束。5.3 外参标定与时间同步激光视觉融合最容易翻车的两个环节不是算法而是标定和同步。外参标定的本质是求传感器之间的 T 矩阵。相机-激光标定常用标定板或特定场景自动标定工具有很多相机-IMU 标定常用 kalibr激光-IMU 标定也有专门的工具。这里特别强调标定结果要存档、可复现并且每次拆卸安装传感器后必须重新标定。外参误差 1 厘米在 50 米外就会造成明显的重投影错位。时间同步也有两种层次。硬件同步是通过 PPS 秒脉冲和 GPRMC 时间戳把各传感器对齐到同一时间基准精度高适合量产和实车软件同步是为每帧数据打上统一时间戳利用姿态插值补偿运动适合算法验证和中低速场景。激光雷达一帧点云在扫描期间车辆已经运动了一段距离如果不对点云做运动畸变补偿点云会“拖影”这在高速场景下尤其致命。6. 高精度定位建图的完整工程流程6.1 第一步数据采集与传感器配置先做好数据采集。采集时要注意传感器时间戳是否统一、激光点云是否有畸变、车辆是否覆盖了闭环线路、GNSS 信号好的区域和差的区域都要采。数据是算法验证的基础数据质量不够后续所有环节都被拖累。# 录制多传感器数据包示意 rosbag record -O campus.bag \ /imu/data \ /lidar/points \ /camera/image \ /gnss/fix# 播放数据包并发布仿真时钟保证多传感器时间戳对齐 rosbag play campus.bag --clock --rate 1.0第一段命令把多传感器数据录成一个 bag第二段命令按原始时间戳播放。时间戳对齐在多传感器融合中永远是第一优先级。6.2 第二步内外参标定相机要标内参焦距、主点、畸变系数激光-相机、激光-IMU、相机-IMU 要标外参。用一个示意配置文件记录标定结果# 文件路径config/sensors.yaml sensors: lidar: frame_id: lidar_link publish_rate_hz: 10 camera: frame_id: camera_link publish_rate_hz: 20 imu: frame_id: imu_link publish_rate_hz: 200 extrinsics: # 顺序按实际标定工具导出的格式填写此处仅示意 T_camera_lidar: [0.0, 0.03, -0.25, 0.0, -0.005, 0.0] T_lidar_imu: [0.05, 0.0, 0.2, 0.0, 0.0, 0.0]这份配置的要点是每个传感器有独立 frame_id外参统一放在一处管理而不是散落在代码里。工程上所有模块读取同一个配置来源可以避免“一个模块一套外参”的灾难。6.3 第三步前端里程计前端负责帧间位姿估计。激光里程计常用 ICP、NDT 做帧间配准视觉里程计提取特征点、光流跟踪、对极几何或 PnP 估计位姿。前端最重要的输出是不错的初值和差观测。特别注意退化场景检测长直走廊、空旷广场、隧道内激光前端的约束矩阵会出现退化方向此时只靠激光里程计必然漂移。检测方法通常是对约束矩阵做特征值分解当最小特征值远小于最大特征值时预警退化并降低该方向权重让 IMU 或视觉约束补位。6.4 第四步后端优化与回环检测后端把前端的帧间约束、IMU 预积分约束、回环约束全部送进因子图优化。场景规模大时用滑动窗口限制计算量需要全局一致性时做全局优化。回环检测对建图精度至关重要。激光回环常用点云匹配ScanContext、ICP 描述子视觉回环常用词袋模型DBoW2。回环检测一定要加验证步骤否则错误回环比没有回环更致命一旦把两条本来不相干的轨迹“焊”在一起整张地图就废了。6.5 第五步建图与定位建图通常分为离线建图和在线定位。离线建图用全量数据做全局优化生成高精度点云地图或占据栅格地图在线定位则是用预建地图做实时匹配常用 NDT 或 ICP 在局部地图中匹配当前帧输出高精度位姿。这里要区分两个概念定位是“在地图中找自己”建图是“把环境画出来”。很多项目失败是因为把两者混在一个线程里处理丢失了全局一致性。7. 常见问题与排查思路问题现象可能原因排查方式解决方案融合轨迹发散时间同步不齐多传感器时间戳错位打印各传感器最新时间戳比较差值硬件同步或软件插值对齐时间戳相机与激光投影错位外参标定不准可视化投影点云到图像上检查边缘对齐重新标定或增加标定板采集数据长直走廊定位偏移退化场景约束不足对约束矩阵做特征值分析加入 IMU 因子降低退化方向权重建图重影、轨迹不闭合回环检测未触发或错误回环可视化回环边检查闭环位置是否合理增加回环验证调节匹配阈值视觉特征跟丢运动模糊、光照突变、快速旋转查看特征点数量随时间变化曲线提高相机帧率减少曝光时间优化不收敛初值误差过大噪声模型不合理打印初始误差与迭代后误差改善前端初值重新调节噪声协方差点云出现拖影激光扫描期间车辆运动检查运动畸变补偿是否开启使用 IMU/里程计做逐点补偿排查时遵循一个原则先看输入再看输出。数据有没有时间戳错位、外参有没有加载错文件、坐标系有没有统一这些问题不解决调优化参数只是浪费时间。8. 最佳实践与工程建议结合多个项目的经验整理几条真正值得贯彻的实践建议。第一统一坐标系规范并在代码里强制约定。所有传感器数据进算法前必须转换到同一个坐标系禁止在模块内部各自定义局部坐标系。使用命名规范如lidar_link、camera_link、imu_link、base_link、map提交代码评审时也好辨认。第二时间同步在采集端解决不要留给算法端补。算法端的插值只能作为降级方案。如果有条件在硬件层接 PPS 秒脉冲同步所有传感器尤其对多雷达、多相机系统收益非常明显。第三标定结果纳入版本管理。每次传感器结构变更后标定文件路径要跟代码版本一起上线。建议把标定数据采集时间、车辆信息、工具版本记录下来形成可追溯的标定记录。第四保留数据回放和回归测试能力。SLAM 系统改动后用同一套 bag 数据跑回归对比轨迹误差和地图质量避免“改好一个模块弄坏另一个模块”。第五从简单场景开始验证再逐步增加复杂度