企业官网建设流程全解析

很多人第一次接触 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, theta；theta 为车头朝向 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(f"step {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 做局部和全局 BA，Cartographer 用 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(f"X({i}): x={pose.x():.3f}, y={pose.y():.3f}, theta={pose.theta():.3f}")

这个示例的关键点是：回环之前，轨迹由里程计逐步推算，误差会累积；加入回环因子后，优化器会把第 5 个位姿拉回起点附近，同时整体调整中间所有位姿。实际工程中，回环因子来自激光点云匹配或视觉词袋检测，噪声模型需要根据匹配置信度设置。

4.4 滤波 vs 图优化对比

理解这张表的结论：滤波没有过时，它更适合在线局部估计；但大场景、多传感器融合、回环修正，几乎都走图优化。

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. 常见问题与排查思路

排查时遵循一个原则：先看输入，再看输出。数据有没有时间戳错位、外参有没有加载错文件、坐标系有没有统一，这些问题不解决，调优化参数只是浪费时间。

8. 最佳实践与工程建议

结合多个项目的经验，整理几条真正值得贯彻的实践建议。

第一，统一坐标系规范，并在代码里强制约定。所有传感器数据进算法前必须转换到同一个坐标系，禁止在模块内部各自定义局部坐标系。使用命名规范如lidar_link、camera_link、imu_link、base_link、map，提交代码评审时也好辨认。

第二，时间同步在采集端解决，不要留给算法端补。算法端的插值只能作为降级方案。如果有条件，在硬件层接 PPS 秒脉冲同步所有传感器，尤其对多雷达、多相机系统，收益非常明显。

第三，标定结果纳入版本管理。每次传感器结构变更后，标定文件路径要跟代码版本一起上线。建议把标定数据采集时间、车辆信息、工具版本记录下来，形成可追溯的标定记录。

第四，保留数据回放和回归测试能力。SLAM 系统改动后，用同一套 bag 数据跑回归，对比轨迹误差和地图质量，避免“改好一个模块，弄坏另一个模块”。

第五，从简单场景开始验证，再逐步增加复杂度