LeGO-LOAM地面分离与聚类优化：从BFS图搜索到两步优化的工程实践

1. LeGO-LOAM地面分离算法详解

在室外复杂环境中，地面点的准确分离是点云预处理的关键步骤。LeGO-LOAM采用基于角度阈值的地面点提取算法，其核心思想是通过分析相邻激光束点之间的几何关系来识别地面。想象一下用扫把扫地时的场景——扫把与地面的夹角越小，扫过的区域就越平坦。激光雷达的扫描原理与此类似，只是把扫把换成了激光束。

具体实现时，算法会计算同一列相邻扫描点之间的垂直高度差（dZ）和水平距离差（dX、dY）。通过反正切函数得到两点连线与水平面的夹角θ。在理想平坦地面上，这个角度应该接近0度。但实际场景中需要考虑两个因素：一是激光雷达安装不可能绝对水平，二是自然地面存在微小起伏。因此LeGO-LOAM设置10度作为判断阈值，这个值是通过大量草坪、土路等场景实测得出的经验值。

代码实现中有几个关键细节需要注意：

cpp复制// 关键参数设置示例
const float DEG_TO_RAD = M_PI / 180.0;
const float GROUND_ANGLE_THRESHOLD = 10 * DEG_TO_RAD; 
const int GROUND_SCAN_INDEX = 7; // 16线雷达的地面点上限

// 核心计算逻辑
float dX = upperPoint.x - lowerPoint.x;
float dY = upperPoint.y - lowerPoint.y; 
float dZ = upperPoint.z - lowerPoint.z;
float vertical_angle = std::atan2(dZ, sqrt(dX*dX + dY*dY));
if ((vertical_angle - sensor_mount_angle) <= GROUND_ANGLE_THRESHOLD) {
    // 标记为地面点
}

实际工程中我遇到过几个典型问题：在斜坡场景下，固定角度阈值会导致地面点误判。解决方法是通过IMU数据动态调整sensor_mount_angle参数。另一个常见问题是低矮障碍物（如路缘石）被误判为地面，这时可以结合高度阈值进行二次过滤。

2. 基于BFS的点云聚类优化

地面点分离后，剩余点云需要通过聚类算法进行组织。LeGO-LOAM采用广度优先搜索(BFS)算法，这就像用油漆桶工具给图像上色——从一个种子点开始，逐步"染色"所有符合条件的相邻点。与传统DFS相比，BFS更适合处理激光雷达点云这种具有明确空间层次结构的数据。

算法实现时，首先将点云投影到16×1800的矩阵中（对应16线雷达的扫描结构）。每个点的邻域定义为上下左右四个方向，注意水平方向需要循环处理（第1800列与第0列相邻）。判断邻接点是否属于同一聚类的标准是两点间的夹角是否小于segmentTheta（默认60度）。

cpp复制// BFS聚类核心代码结构
void labelComponents(int row, int col) {
    queue.push(startPoint);
    while(!queue.empty()) {
        current = queue.pop();
        for(neighbor in [上,下,左,右]) {
            if(满足边界条件 && 未被访问 && 角度差<阈值) {
                queue.push(neighbor);
                labelMat[neighbor] = currentLabel;
            }
        }
    }
    // 后处理：过滤小聚类
}

在树林场景实测时，我发现三个关键调参经验：

1. segmentTheta角度越小，聚类分割越精细，但容易过度分割
2. 有效聚类的最小点数（默认30）需要根据场景动态调整
3. 对于垂直结构（如树干），需要额外检查跨扫描线的连续性

一个工程优化技巧是使用双指针数组代替std::queue，这在嵌入式设备上能提升约15%的处理速度。实测数据显示，优化后的BFS算法在Jetson Xavier上单帧处理时间可控制在8ms以内。

3. 两步优化里程计的技术实现

LeGO-LOAM最核心的创新在于将六自由度的位姿估计分解为两个三步优化问题。这就像先调整相机的水平仪（第一步优化），再对准拍摄主体（第二步优化），比直接六参数调整更高效稳定。

3.1 地面点优化阶段

地面点主要提供对pitch、roll和z方向的约束。算法构建点到面的距离残差，使用LM优化器求解。这里有个重要发现：地面点对x、y和yaw的约束很弱，强行优化会导致矩阵病态。在实际代码中，这部分对应findCorrespondingSurfFeatures函数。

cpp复制// 平面特征残差计算示例
float pa = (p2.y-p1.y)*(p3.z-p1.z) - (p3.y-p1.y)*(p2.z-p1.z);
float pb = (p2.z-p1.z)*(p3.x-p1.x) - (p3.z-p1.z)*(p2.x-p1.x); 
float pc = (p2.x-p1.x)*(p3.y-p1.y) - (p3.x-p1.x)*(p2.y-p1.y);
float pd = -(pa*p1.x + pb*p1.y + pc*p1.z);
float ps = sqrt(pa*pa + pb*pb + pc*pc);
float residual = (pa*point.x + pb*point.y + pc*point.z + pd)/ps;

3.2 角点优化阶段

边缘特征主要提供x、y和yaw的约束。代码中通过findCorrespondingCornerFeatures函数构建点到线的距离残差。特别注意，此时会将第一阶段优化的z、pitch、roll作为固定值，避免引入耦合误差。

在工程实践中，我总结出两个关键点：

1. 地面点与角点的优化顺序不能颠倒，否则容易陷入局部最优
2. 两次优化的权重需要平衡，通常地面点权重设为角点的1.5-2倍

4. 复杂场景下的工程调优

在真实场景部署时，单纯依靠算法默认参数往往难以取得最佳效果。基于多个项目经验，我总结出一套参数调优方法论：

4.1 地面分离参数调整

针对不同地面类型建议的阈值：

• 平坦硬化路面：8-12度
• 草地/土路：12-15度
• 崎岖地形：15-20度+高度滤波

4.2 聚类优化参数配置

典型场景的参数组合：

markdown复制| 场景类型       | segmentTheta | 最小点数 | 有效扫描线数 |
|----------------|--------------|----------|--------------|
| 稀疏树林       | 50度         | 20       | 3            |
| 城市建筑       | 60度         | 30       | 4            |  
| 密集灌木丛     | 70度         | 40       | 5            |

4.3 里程计优化技巧

1. 动态权重调整：根据点云密度自动调整两次优化的权重比
2. 运动一致性检查：通过IMU数据验证优化结果的合理性
3. 退化场景处理：当特征点不足时切换为匀速运动模型

在最近的一个AGV项目中，通过这套方法将定位精度从初始的0.3m提升到了0.08m（RMS），特别是在长走廊等特征匮乏场景表现突出。关键是在地面分离阶段增加了高度方差检测，有效避免了光滑地面的误判问题。