图 2-1 机器人姿态和观测结果图

图形 SLAM 将 SLAM 问题视为图形结构的非线性优化，其中节点表示机器人的姿态和不同时间的观测结果，边表示姿态之间的约束条件。图 2-1 展示了一个玩具示例，说明了如何借助机器人移动时的姿态和观测结果来构建图形。x_i、x_j 和 x_k 是机器人的姿态，P₀ 和 P₁ 是摄像头观测到的地标、物体或点。此处假设通过 x_i 和 x_j 观测 P₀，通过 x_j 和 x_k 观测 P₁。p_nt（其中 n = 0、1，t = i、j、k）定义为通过 x_t 观测的 P_n 的位置。图形 SLAM 的目标是确定机器人姿态之间的变换，即 z’_ij 和 z’_jk，使由不同姿态产生的相同观测结果尽可能地重叠。因此，需要确定 x_t（其中 t = i、j、k），使 (p_0i – p_0j)² + (p_1j – p_1k)² 最小。

图 2-2 图形 SLAM 流程

图 2-2 显示了图形 SLAM 流程，主要由两部分组成，即前端和后端。前端处理输入传感器数据，以估计移动机器人的姿态以及机器人周围的明显特征。前端处理包括以下步骤：

• 数据采集：从摄像头和激光雷达等传感器连续捕获数据。捕获的传感器数据在此处进行预处理。例如，在数据采集期间执行 ISP、镜头失真校正、立体图像校正和激光雷达点云运动补偿等操作。在固定时间间隔内捕获的数据构成一帧。
• 特征提取：从每一帧中提取独特的描述性特征。这些特征可以是图像中的关键点（视觉 SLAM），也可以是几何特征（例如边缘、平面以及扫描的 2D 或 3D 点本身）（激光雷达 SLAM）。
• 特征关联：移动机器人将从不同帧中提取的特征进行关联，以确定哪些特征与环境中的相同特征相对应。
• 姿态估计：根据相关特征，移动机器人估计其位置和方向在两帧之间的变化，从而估计移动机器人的新姿态。估计完姿态，就可以使用相关特征来更新地图。

在前端估计的移动机器人姿态是有误差的，并且误差会随着移动机器人的移动而累积，从而导致巨大的漂移误差。后端负责细化估计的姿态并更新地图。此细化包括以下步骤：

• 闭环检测：此过程可确定以前是否访问过当前位置。要通过图形优化减少漂移误差，确定访问过的位置至关重要。
• 图形优化：图形保留在图形 SLAM 中，如图 2-1 所示。目标是更新先前估计的姿态，使从一个姿态观测到的结果（如特征）与从其他姿态观测到的相同结果尽可能重叠。牛顿法、高斯牛顿法和列文伯格-马夸尔特 (LM) 优化等非线性优化技术均用于此用途。

一般来说，在视觉 SLAM 中，创建的地图是一组提取的特征及其位置和描述符；在激光雷达 SLAM 中，创建的地图是一组几何特征或点本身。在许多用例中，地图在经过后处理后还会以占据栅格地图的形式保存，以便于障碍物检测和路径规划。