2. SLAM基础回顾

在深入激光SLAM的回环检测与后端优化之前，我们必须先夯实底层基础。回环检测的本质是“识别曾经到过的地方”，而后端优化的本质是“修正历史轨迹与地图的累积误差”。如果对激光雷达的物理特性、点云数据的组织方式以及前端里程计的配准原理缺乏系统认知，后续的图优化、位姿图构建和回环约束添加都将成为空中楼阁。本章将围绕四大核心模块展开：激光雷达的测距原理与误差来源、点云的数据表示与预处理、前端里程计的两大主流配准算法（ICP与NDT），以及它们与后端优化之间的衔接关系。

2.1 激光雷达工作原理与误差模型

2.1.1 核心测距原理

激光雷达（LiDAR）通过发射激光束并接收目标反射信号来测量距离。目前主流方案分为两类：飞行时间法（TOF）和相位式测距。TOF通过直接测量激光脉冲的往返时间计算距离，公式为 \( d = c \cdot t / 2 \)，其中 \( c \) 为光速。TOF方案响应速度快、作用距离远，是车载和机器人领域的主流选择。相位式测距则通过调制连续波的相位差来推算距离，精度更高但量程较短，多用于室内低速场景。

从扫描机构来看，激光雷达可分为机械旋转式（如Velodyne VLP-16）、固态式（如Livox Horizon）和混合固态式（如RoboSense RS-LiDAR-M1）。机械式通过电机旋转实现360°扫描，点云密度均匀但存在机械磨损；固态式通过光学相控阵或微振镜实现扫描，可靠性高但视场角受限。理解这些物理差异对后续点云畸变校正和配准策略选择至关重要。

2.1.2 误差来源与建模

激光雷达的测量误差主要来自三个方面：测距噪声（高斯分布，标准差通常为1-3cm）、角度分辨率限制（导致远距离点云稀疏）以及运动畸变（扫描过程中雷达自身运动导致点云扭曲）。运动畸变在机械式雷达中尤为显著——一帧点云中不同点对应不同时刻的雷达位姿，若直接用于配准会引入系统性偏差。

2.2 点云数据表示与预处理

2.2.1 点云的组织形式

激光雷达输出的原始数据通常以无序点集形式存储，每个点包含三维坐标（x, y, z）和反射强度（intensity）。部分雷达还提供时间戳（timestamp）和通道号（ring），形成有序点云。有序点云保留了扫描线的拓扑关系，便于快速邻域搜索和地面分割。在PCL（Point Cloud Library）中，pcl::PointCloud<pcl::PointXYZI>是最常用的数据结构，而pcl::PointCloud<pcl::PointXYZRGB>则用于融合视觉信息的场景。

点云数据具有三个显著特点：稀疏性（远距离点间距增大）、非均匀密度（近处密集、远处稀疏）和无拓扑结构（无序点集需要建立索引）。这些特点直接影响了配准算法的设计——例如NDT算法通过体素化将无序点云转换为概率分布模型，正是为了应对稀疏性问题。

2.2.2 关键预处理步骤

在将点云送入配准算法之前，必须进行以下预处理：

1. 体素滤波（Voxel Grid Filter）：将空间划分为固定尺寸的立方体（如0.1m），每个体素内保留重心点。这能显著降低点云密度，同时保持几何特征。体素尺寸的选择需要权衡——过大丢失细节，过小计算量激增。
2. 离群点去除（Statistical Outlier Removal）：计算每个点与其k近邻的平均距离，若超过全局均值加n倍标准差则视为离群点剔除。这能有效去除飞点（如玻璃反射产生的虚假点）。
3. 地面分割（Ground Segmentation）：在室外场景中，地面点约占点云总量的30%-50%，且对配准贡献有限。通过RANSAC平面拟合或射线法去除地面点，可提升配准效率和精度。

2.3 前端里程计：ICP与NDT配准

2.3.1 ICP算法原理与变体

迭代最近点（Iterative Closest Point, ICP）是最经典的点云配准算法。其核心思想是：给定源点云 \( P \) 和目标点云 \( Q \)，通过迭代寻找刚体变换 \( T \)（旋转矩阵 \( R \) 和平移向量 \( t \)），使得 \( P \) 经过变换后与 \( Q \) 的对应点距离最小化。目标函数为：

E(R,t) = Σ || R·p_i + t - q_j ||²

其中 \( q_j \) 是 \( p_i \) 在 \( Q \) 中的最近邻点。算法迭代步骤为：① 寻找对应点对；② 计算最优变换（SVD分解或四元数法）；③ 应用变换并重新寻找对应点；④ 直到收敛（均方误差变化小于阈值）。

ICP的改进变体包括：点对面ICP（Point-to-Plane ICP），利用目标点云的法向量信息，允许点沿切平面滑动，收敛速度更快且对初始误差更鲁棒；GICP（Generalized ICP），结合了点对点和点对面的协方差信息，在结构化环境中表现更优。实际工程中，纯点对点ICP在走廊、隧道等退化场景中容易陷入局部最优，因此推荐使用点对面或GICP。

2.3.2 NDT算法原理

正态分布变换（Normal Distributions Transform, NDT）将点云空间划分为固定尺寸的体素（如1m），每个体素内的点云被建模为高斯分布，用均值 \( \mu \) 和协方差矩阵 \( \Sigma \) 表示。配准时，将源点云中的每个点通过变换 \( T \) 映射到目标点云的体素空间中，计算该点落在对应体素高斯分布下的概率密度，最大化所有点的概率密度之和：

score(T) = Σ exp( -0.5 · (T·p_i - μ_j)ᵀ · Σ_j⁻¹ · (T·p_i - μ_j) )

NDT的优势在于：① 不需要显式寻找对应点对，计算效率高；② 对初始位姿误差的容忍度优于ICP；③ 体素化天然平滑了噪声影响。但NDT对体素尺寸敏感——体素过小则统计信息不足，过大则丢失局部几何特征。

2.3.3 配准实战案例：走廊退化场景

场景背景：某仓储AGV在一条长30m、宽2m的走廊中运行，激光雷达为16线机械式，前端使用PCL库的ICP配准。运行初期定位正常，但行驶10m后航向角开始漂移，最终定位失败。

现象分析：走廊两侧墙壁平行，沿走廊方向（x轴）的几何约束极弱——点云在x方向上的分布几乎不变，导致ICP在x方向上的位移估计退化。此时ICP的Hessian矩阵接近奇异，微小噪声被放大为显著的位姿误差。

解决过程：

1. 将配准算法从点对点ICP切换为NDT，体素尺寸设为0.5m。NDT通过体素内的高斯分布建模，在退化方向上提供了更平滑的代价函数，缓解了局部最优问题。
2. 在配准前增加IMU预积分信息，为ICP/NDT提供初始位姿估计，将初始误差从0.5m缩小到0.1m以内。
3. 在走廊场景中降低对x方向位移的信任权重，通过信息矩阵对配准结果进行加权——在退化方向上降低权重，避免错误约束传递到后端。

结果对比：切换NDT后，航向角漂移从0.5°/m降至0.1°/m；加入IMU预积分后进一步降至0.03°/m。但NDT在走廊中的x方向位移仍有约0.2m的累积误差，最终通过后端回环检测（在走廊尽头转弯处识别到回环）彻底消除。

复盘经验：退化场景是前端配准的“阿喀琉斯之踵”，单靠改进配准算法无法根治。工程上应结合多传感器融合（IMU/轮式里程计）和退化检测机制（分析Hessian矩阵特征值），并在后端通过回环约束修正累积漂移。

2.4 前端与后端的衔接：关键帧与位姿图

前端里程计输出的是帧间相对位姿，若直接串联所有帧的位姿，误差会随距离线性累积。因此，SLAM系统通常引入关键帧（Keyframe）机制——只在位姿变化超过阈值（如平移0.5m或旋转10°）时创建新关键帧，并丢弃中间帧。关键帧之间的相对位姿作为里程计约束（Odometry Edge）加入位姿图，而回环检测发现的匹配对则形成回环约束（Loop Closure Edge）。

位姿图优化的目标是最小化所有约束的残差平方和：

min Σ || e_ij(T_i, T_j) ||²_Σij

其中 \( e_ij \) 是位姿 \( T_i \) 和 \( T_j \) 之间的相对位姿残差，\( \Sigma_{ij} \) 是信息矩阵（协方差矩阵的逆）。前端配准的精度直接决定了信息矩阵的设定——配准得分高、退化程度低时，信息矩阵的权重应增大；反之则减小。这也是为什么理解前端配准的误差特性对后端优化至关重要。

2.5 常见误区与总结复盘

误区一：认为ICP是万能的。ICP在结构化良好、初始位姿接近的场景中表现优异，但在稀疏、重复纹理或退化环境中极易失败。NDT虽然鲁棒性更好，但体素尺寸的选择需要根据场景调整——室内小场景用0.5m，室外大场景用1-2m。

误区二：忽略运动畸变校正。许多初学者直接使用原始点云进行配准，在快速旋转或高速行驶时，运动畸变会导致配准误差增大数倍。正确的做法是：在配准前利用IMU或上一帧位姿对点云进行去畸变处理，将每个点的时间戳映射到统一坐标系。

误区三：前端与后端割裂看待。前端配准的精度直接影响后端优化的收敛速度和最终地图质量。如果前端频繁输出错误的相对位姿，即使后端有回环约束，也无法完全修正。因此，工程上应建立前端配准质量的在线评估机制，并将评估结果反馈到后端的鲁棒核函数（如Huber核）中，降低异常约束的影响。