3. 前端里程计

前端里程计是激光SLAM系统的“第一道定位防线”。它直接消费激光雷达的原始点云，在相邻帧之间估计相对位姿，为后端优化提供初始约束和局部轨迹。量产系统中，前端里程计不仅要准，还要稳、快、可解释：在隧道、长走廊、动态车辆遮挡、地面退化等场景下，必须能识别退化并给出可信的协方差或失败标志，否则后端会被错误约束拖偏。

核心原理：从点到帧的位姿估计

前端里程计的本质是求解相邻两帧点云之间的刚体变换 T ∈ SE(3)。根据是否提取特征，可分为两大类：特征法（提取角点、平面点、线段等，匹配后求解）和直接法（ICP、NDT、GICP等，直接最小化点云间距离或概率分布差异）。特征法速度快、对初值不敏感，但特征稀疏时容易退化；直接法精度高、对结构利用充分，但依赖初值和体素参数。

运动估计通常采用“预测—匹配—求解—校正”的闭环。IMU或轮速计提供先验位姿，将当前帧点云变换到上一帧坐标系附近，再建立对应关系，最后用高斯-牛顿或LM迭代求解。量产中还会加入鲁棒核函数（Huber、Cauchy）抑制动态物体和错误匹配。

方法流程：特征提取与匹配

以LOAM系列为例，前端流程可拆为五步：

1. 点云预处理：去畸变、降采样、去除NaN和过近点。
2. 特征提取：按曲率区分角点（sharp）和平面点（flat），每帧保留固定数量特征。
3. 特征匹配：角点匹配到上一帧的角点线，平面点匹配到上一帧的平面面片。
4. 运动估计：构建点到线、点到面的残差，迭代求解位姿。
5. 畸变校正：利用估计的帧内运动，将每个点投影到帧首或帧尾时刻。

特征提取的关键参数是曲率阈值和邻域点数。曲率计算通常取同一扫描线前后各5个点，计算局部平滑度。角点取曲率最大的若干点，平面点取曲率最小的若干点。匹配时使用KD-Tree或ikd-Tree加速最近邻搜索。

// 曲率计算示例（简化）
for (int i = 5; i < cloud->size() - 5; ++i) {
  float diffX = -10 * cloud->points[i].x
              + cloud->points[i-5].x + cloud->points[i-4].x
              + cloud->points[i-3].x + cloud->points[i-2].x
              + cloud->points[i-1].x + cloud->points[i+1].x
              + cloud->points[i+2].x + cloud->points[i+3].x
              + cloud->points[i+4].x + cloud->points[i+5].x;
  float curvature = diffX * diffX + diffY * diffY + diffZ * diffZ;
  // 按曲率排序，选取角点与平面点
}

ICP、NDT与LOAM系列对比

三类方法在量产中的定位不同。ICP直接最小化点到点距离，精度高但收敛域小，适合初值较好的场景；NDT将空间划分为体素，用高斯分布表示局部点云，对初值更鲁棒，适合城市道路；LOAM系列提取特征后匹配，计算量可控，适合机械式激光雷达和固态雷达的混合部署。

运动估计与畸变校正

机械式激光雷达旋转一周约100ms，车辆在此期间可能移动数米。若不做畸变校正，点云会“拖尾”，导致墙面变厚、角点模糊。校正方法有两种：一是利用IMU积分得到帧内每个点的时间戳位姿，逐点变换；二是用前端里程计估计的帧间运动做线性插值。量产中常用IMU+轮速计融合，保证帧内运动估计的连续性。

案例与场景：城市道路与隧道

在城市道路场景中，NDT或LIO-SAM通常表现稳定，因为建筑立面、路沿、车辆提供丰富约束。进入隧道后，两侧墙面平行且长直，点到面残差在纵向和偏航方向约束不足，前端容易发生“纵向漂移”和“偏航漂移”。此时需要引入退化检测：计算残差雅可比矩阵的条件数或特征值，若最小特征值低于阈值，则判定该方向退化，并降低对应位姿分量的权重。

另一个典型场景是动态车辆遮挡。前方大车遮挡导致点云中大量动态点被误匹配，前端会突然跳变。量产中常用“动态点剔除+鲁棒核+运动一致性检查”三重防护：先根据上一帧位姿和当前帧预测剔除异常点，再用Huber核降低大残差影响，最后检查估计速度是否超过车辆物理极限。

误区与复盘

• 误区一：只调特征数量，不看特征分布。角点集中在近处会导致远处约束不足，应保证特征在距离和方位上均匀分布。
• 误区二：忽略时间同步。激光雷达与IMU时间戳偏差10ms，在高速下可造成米级误差。
• 误区三：用固定体素尺寸应对所有场景。NDT体素在开阔区域可大，在狭窄区域应小，量产中可自适应调整。
• 误区四：不做退化检测。前端在退化场景下仍输出高置信位姿，后端会被错误约束污染。

总结来看，前端里程计的设计要在精度、鲁棒性和计算量之间取得平衡。特征法适合结构化场景，NDT适合城市道路，紧耦合LIO适合高频输出和退化场景。无论选择哪种方法，都必须把畸变校正、时间同步和退化检测作为量产必选项，而不是可选项。