企业官网建设流程全解析

前言

LeGO-LOAM 的算法核心在于地面分割和两步 Levenberg-Marquardt 优化。地面分割将 3D 激光SLAM 问题解耦为地面面特征约束（z/roll/pitch）和非地面特征约束（x/y/yaw），两步优化利用这种解耦实现快速收敛。回环检测则通过 ICP 验证 + GTSAM 位姿图优化消除长期漂移。

1. 算法全景图

LeGO-LOAM 算法体系 │ ┌─────────────────────┼─────────────────────┐ │ │ │ 地面分割 两步LM优化 回环检测 (数据预处理) (位姿估计) (全局校正) │ │ │ ├─ 列式角度法 ├─ Step1: 地面约束 ├─ 欧氏距离筛选 │ (atan2(dz,dr)) │ (tz/roll/pitch) │ │ │ │ ├─ 地面/非地面分离 ├─ Step2: 全局约束 ├─ 时间间隔过滤 │ │ (完整6DoF) │ │ │ │ ├─ 地面→面特征 ├─ 梯度下降比例 ├─ ICP 验证 │ (体素均匀采样) │ (λ阻尼因子) │ │ │ │ └─ 非地面→角/面特征 └─ 雅可比数值计算 └─ GTSAM因子 (曲率特征) (手动求导) (位姿图)

2. 地面分割算法 —— 详细推导

2.1 列式角度法

将 3D 激光扫描按水平角度分为NcolN_{col}Ncol​列，每列包含垂直方向上的NscanN_{scan}Nscan​个点。

对于同一列中相邻的两个点plowerp_{lower}plower​和pupperp_{upper}pupper​：

α=arctan⁡(zupper−zlower(xupper−xlower)2+(yupper−ylower)2)\alpha = \arctan\left(\frac{z_{upper} - z_{lower}}{\sqrt{(x_{upper} - x_{lower})^2 + (y_{upper} - y_{lower})^2}}\right)α=arctan((xupper​−xlower​)2+(yupper​−ylower​)2​zupper​−zlower​​)

• α<10°\alpha < 10°α<10°→ 地面点
• α≥10°\alpha \geq 10°α≥10°→ 非地面点

2.2 直观理解

雷达 │ │ / │/ α （俯仰角） ───────●────────── 地面 /│ / │ / │ ● │ p1 p2 当雷达扫描地面时： 相邻两个地面点的俯仰角 α ≈ 0° 在起伏地形上，α 会有变化，但通常 < 10° 当扫描到垂直物体（如墙壁）时： α 可能接近 90°

2.3 代码细节

// imageProjection.cpp 核心算法voidgroundRemoval(){// groundScanInd: 参与地面分割的扫描线数// VLP-16: groundScanInd = 7 (下7条线)// HDL-64: groundScanInd = 20for(size_t j=0;j<Horizon_SCAN;++j){// 水平方向for(size_t i=0;i<groundScanInd;++i){// 垂直方向（下方）size_t lowerInd=j+i*Horizon_SCAN;size_t upperInd=j+(i+1)*Horizon_SCAN;// 跳过无效点if(fullCloud->points[lowerInd].intensity==-1||fullCloud->points[upperInd].intensity==-1)continue;// 计算俯仰角floatdX=fullCloud->points[upperInd].x-fullCloud->points[lowerInd].x;floatdY=fullCloud->points[upperInd].y-fullCloud->points[lowerInd].y;floatdZ=fullCloud->points[upperInd].z-fullCloud->points[lowerInd].z;floatangle=atan2(dZ,sqrt(dX*dX+dY*dY));// 地面判断constfloatGROUND_ANGLE_THRESHOLD=10.0*M_PI/180.0;if(fabs(angle)<GROUND_ANGLE_THRESHOLD){groundMat.at<int8_t>(i,j)=1;groundMat.at<int8_t>(i+1,j)=1;}}}}

3. 两步 LM 优化

3.1 Levenberg-Marquardt 算法

LM 算法是 Gauss-Newton 和梯度下降的插值：

(JTJ+λI)⋅Δx=−JTr(J^T J + \lambda I) \cdot \Delta x = -J^T r(JTJ+λI)⋅Δx=−JTr

• λ→0\lambda \to 0λ→0：退化为 Gauss-Newton（二阶收敛，步长大）
• λ→∞\lambda \to \inftyλ→∞：退化为梯度下降（一阶收敛，步长小但稳定）

LeGO-LOAM 通过两阶段 LM 来利用地面分割的优势。

3.2 第一步 LM：地面面特征约束

// 优化变量: [tz, roll, pitch]// 固定变量: [tx, ty, yaw] （保持上一步的值不变）voidoptimizeGroundPlane(){// 残差：点到平面距离（地面特征）// 平面 = 地面（法向量 ≈ [0, 0, 1]）for(intiter=0;iter<25;iter++){// 1. 用当前位姿变换点// 2. 在 KD-Tree 中找对应// 3. 计算 Jacobian（仅对 tz, roll, pitch）// 对于地面面特征：// ∂d/∂tz ≈ 1 (高度方向的位移直接影响点到面距离)// ∂d/∂roll ≈ x (绕x轴旋转影响高度)// ∂d/∂pitch ≈ y (绕y轴旋转影响高度)// ∂d/∂tx ≈ 0 (水平位移不影响地面约束)// 4. 求解 3×3 正规方程// 5. 更新 tz, roll, pitch}}

第一步 LM 的约束矩阵（3×3）：

Hground=[∑∂d∂tz2.........∑∂d∂roll2.........∑∂d∂pitch2]H_{ground} = \begin{bmatrix} \sum \frac{\partial d}{\partial t_z}^2 & ... & ... \\ ... & \sum \frac{\partial d}{\partial roll}^2 & ... \\ ... & ... & \sum \frac{\partial d}{\partial pitch}^2 \end{bmatrix}Hground​=​∑∂tz​∂d​2......​...∑∂roll∂d​2...​......∑∂pitch∂d​2​​

3.3 第二步 LM：全局 6DoF 约束

// 用第一步的结果作为初始值// 优化变量: 完整 [tx, ty, tz, roll, pitch, yaw]voidoptimizeFull6DoF(){for(intiter=0;iter<25;iter++){// 1. 用当前位姿变换所有特征点// 2. 角点 → 找最近2个点 → 点到直线距离// 3. 面点（含地面面点）→ 找最近3个点 → 点到平面距离// 4. 计算 6×6 Jacobian// 5. 求解 6×6 正规方程 (LM 公式)// 6. 更新完整 6DoF}}

3.4 两步 vs 单步对比

4. 特征的约束分析

4.1 不同类型特征的贡献

特征类型 约束的方向 ───────────────────────────────── 地面面特征 → tz (高度) ★★★★★ roll (翻滚) ★★★★★ pitch (俯仰) ★★★★★ tx, ty (水平) ☆ (几乎无约束) yaw (偏航) ☆ 非地面角点 → tx, ty (水平) ★★★★★ yaw (偏航) ★★★★★ tz (高度) ★★★ roll, pitch ★★ 非地面面点 → tx, ty (水平) ★★★★ tz (高度) ★★★ roll, pitch ★★★ yaw ★★

约束的互补性正是 LeGO-LOAM 鲁棒性的来源。

4.2 退化检测

当某个方向的约束不足时（如长走廊中 tx 方向缺少角点），LM 的 Hessian 矩阵对应特征值会接近零：

// 退化检测Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6>>solver(H);Eigen::VectorXd eigenvalues=solver.eigenvalues();for(inti=0;i<6;i++){if(eigenvalues(i)<DEGENERACY_THRESHOLD){// 第 i 个方向退化 → 在该方向上减少更新量delta_x[i]*=0.1;// 保守更新}}

5. 回环检测算法

5.1 ICP 精配准

// mapOptmization.cppbooldetectLoopClosure(Pose6D&current_pose,std::vector<Pose6D>&history_poses,PointCloud&current_cloud,std::vector<PointCloud>&history_clouds){for(inti=0;i<history_poses.size();i++){// 1. 距离筛选floatdist=(current_pose.position-history_poses[i].position).norm();if(dist>7.0)continue;// >7m 不是回环// 2. 时间筛选floattime_diff=current_pose.time-history_poses[i].time;if(time_diff<30.0)continue;// <30s 不可能是回环// 3. ICP 配准pcl::IterativeClosestPoint<PointType,PointType>icp;icp.setMaxCorrespondenceDistance(0.3);icp.setMaximumIterations(100);icp.setInputSource(current_cloud);icp.setInputTarget(history_clouds[i]);PointCloud aligned;icp.align(aligned);// 4. 验证if(icp.hasConverged()&&icp.getFitnessScore()<0.05){Eigen::Matrix4f T=icp.getFinalTransformation();// T 就是回环相对位姿addLoopFactor(current_pose_id,history_poses[i].id,T);returntrue;}}returnfalse;}

5.2 GTSAM 因子图优化

// 回环因子：连接两个非相邻关键帧的位姿gtsam::BetweenFactor<gtsam::Pose3>loop_factor(key_current,// 当前关键帧IDkey_history,// 历史关键帧IDloop_relative_pose,// ICP 算出的相对位姿loop_noise// 回环约束的噪声模型);graph.add(loop_factor);// ISAM2 增量求解isam.update(graph,initial_estimates);

6. 各算法复杂度

总耗时约 10-15ms/帧（不含回环），适合 Jetson TX2 等嵌入式平台。

7. 总结

LeGO-LOAM 算法体系的三大核心创新：

1. 地面分割 + 约束解耦：将 3D 激光SLAM 分解为 z/roll/pitch（地面）和 x/y/yaw（非地面）两个子问题 → 优化更快、更稳
2. 两步 LM 优化：先地面约束后全局优化 → 退化的"安全网" → 长走廊等几何退化场景鲁棒
3. 回环检测 + GTSAM 优化：轻量级回环 + 位姿图优化 → 长期运行无漂移

LeGO-LOAM 的算法哲学是**“利用场景先验（地面）来简化问题”**。这种针对性优化使其在地面机器人场景中比通用的 A-LOAM 精度更高、速度更快，但在无人机场景中几乎不可用（没有地面可供分割）。