前言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)2zupper−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 j0;jHorizon_SCAN;j){// 水平方向for(size_t i0;igroundScanInd;i){// 垂直方向下方size_t lowerIndji*Horizon_SCAN;size_t upperIndj(i1)*Horizon_SCAN;// 跳过无效点if(fullCloud-points[lowerInd].intensity-1||fullCloud-points[upperInd].intensity-1)continue;// 计算俯仰角floatdXfullCloud-points[upperInd].x-fullCloud-points[lowerInd].x;floatdYfullCloud-points[upperInd].y-fullCloud-points[lowerInd].y;floatdZfullCloud-points[upperInd].z-fullCloud-points[lowerInd].z;floatangleatan2(dZ,sqrt(dX*dXdY*dY));// 地面判断constfloatGROUND_ANGLE_THRESHOLD10.0*M_PI/180.0;if(fabs(angle)GROUND_ANGLE_THRESHOLD){groundMat.atint8_t(i,j)1;groundMat.atint8_t(i1,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(intiter0;iter25;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×3Hground[∑∂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∂d2.........∑∂roll∂d2.........∑∂pitch∂d23.3 第二步 LM全局 6DoF 约束// 用第一步的结果作为初始值// 优化变量: 完整 [tx, ty, tz, roll, pitch, yaw]voidoptimizeFull6DoF(){for(intiter0;iter25;iter){// 1. 用当前位姿变换所有特征点// 2. 角点 → 找最近2个点 → 点到直线距离// 3. 面点含地面面点→ 找最近3个点 → 点到平面距离// 4. 计算 6×6 Jacobian// 5. 求解 6×6 正规方程 (LM 公式)// 6. 更新完整 6DoF}}3.4 两步 vs 单步对比特性两步 LM单步 LM (A-LOAM)第1步初始值上一帧位姿上一帧位姿第1步优化变量tz/roll/pitch (3DoF)全部 6DoF第2步初始值第1步结果地面已对齐N/A第2步优化变量全部 6DoFN/A收敛速度快初始值好慢初始值差退化鲁棒性高约束解耦低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::SelfAdjointEigenSolverEigen::Matrixdouble,6,6solver(H);Eigen::VectorXd eigenvaluessolver.eigenvalues();for(inti0;i6;i){if(eigenvalues(i)DEGENERACY_THRESHOLD){// 第 i 个方向退化 → 在该方向上减少更新量delta_x[i]*0.1;// 保守更新}}5. 回环检测算法5.1 ICP 精配准// mapOptmization.cppbooldetectLoopClosure(Pose6Dcurrent_pose,std::vectorPose6Dhistory_poses,PointCloudcurrent_cloud,std::vectorPointCloudhistory_clouds){for(inti0;ihistory_poses.size();i){// 1. 距离筛选floatdist(current_pose.position-history_poses[i].position).norm();if(dist7.0)continue;// 7m 不是回环// 2. 时间筛选floattime_diffcurrent_pose.time-history_poses[i].time;if(time_diff30.0)continue;// 30s 不可能是回环// 3. ICP 配准pcl::IterativeClosestPointPointType,PointTypeicp;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 Ticp.getFinalTransformation();// T 就是回环相对位姿addLoopFactor(current_pose_id,history_poses[i].id,T);returntrue;}}returnfalse;}5.2 GTSAM 因子图优化// 回环因子连接两个非相邻关键帧的位姿gtsam::BetweenFactorgtsam::Pose3loop_factor(key_current,// 当前关键帧IDkey_history,// 历史关键帧IDloop_relative_pose,// ICP 算出的相对位姿loop_noise// 回环约束的噪声模型);graph.add(loop_factor);// ISAM2 增量求解isam.update(graph,initial_estimates);6. 各算法复杂度算法复杂度每帧耗时地面分割O(N_scan × N_col)~2ms地面面特征提取O(N_ground)~1ms非地面特征提取O(N_non_ground)~2ms第1步 LMO(N_ground_feat × iter)~2ms第2步 LMO(N_features × iter)~5ms回环检测 (ICP)O(N_pts × iter)~20ms1Hz 触发位姿图优化O(VE)~10ms总耗时约 10-15ms/帧不含回环适合 Jetson TX2 等嵌入式平台。7. 总结LeGO-LOAM 算法体系的三大核心创新地面分割 约束解耦将 3D 激光SLAM 分解为 z/roll/pitch地面和 x/y/yaw非地面两个子问题 → 优化更快、更稳两步 LM 优化先地面约束后全局优化 → 退化的安全网 → 长走廊等几何退化场景鲁棒回环检测 GTSAM 优化轻量级回环 位姿图优化 → 长期运行无漂移LeGO-LOAM 的算法哲学是**“利用场景先验地面来简化问题”**。这种针对性优化使其在地面机器人场景中比通用的 A-LOAM 精度更高、速度更快但在无人机场景中几乎不可用没有地面可供分割。
LeGO-LOAM 核心算法详解：地面分割、两步LM、回环因子
前言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)2zupper−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 j0;jHorizon_SCAN;j){// 水平方向for(size_t i0;igroundScanInd;i){// 垂直方向下方size_t lowerIndji*Horizon_SCAN;size_t upperIndj(i1)*Horizon_SCAN;// 跳过无效点if(fullCloud-points[lowerInd].intensity-1||fullCloud-points[upperInd].intensity-1)continue;// 计算俯仰角floatdXfullCloud-points[upperInd].x-fullCloud-points[lowerInd].x;floatdYfullCloud-points[upperInd].y-fullCloud-points[lowerInd].y;floatdZfullCloud-points[upperInd].z-fullCloud-points[lowerInd].z;floatangleatan2(dZ,sqrt(dX*dXdY*dY));// 地面判断constfloatGROUND_ANGLE_THRESHOLD10.0*M_PI/180.0;if(fabs(angle)GROUND_ANGLE_THRESHOLD){groundMat.atint8_t(i,j)1;groundMat.atint8_t(i1,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(intiter0;iter25;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×3Hground[∑∂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∂d2.........∑∂roll∂d2.........∑∂pitch∂d23.3 第二步 LM全局 6DoF 约束// 用第一步的结果作为初始值// 优化变量: 完整 [tx, ty, tz, roll, pitch, yaw]voidoptimizeFull6DoF(){for(intiter0;iter25;iter){// 1. 用当前位姿变换所有特征点// 2. 角点 → 找最近2个点 → 点到直线距离// 3. 面点含地面面点→ 找最近3个点 → 点到平面距离// 4. 计算 6×6 Jacobian// 5. 求解 6×6 正规方程 (LM 公式)// 6. 更新完整 6DoF}}3.4 两步 vs 单步对比特性两步 LM单步 LM (A-LOAM)第1步初始值上一帧位姿上一帧位姿第1步优化变量tz/roll/pitch (3DoF)全部 6DoF第2步初始值第1步结果地面已对齐N/A第2步优化变量全部 6DoFN/A收敛速度快初始值好慢初始值差退化鲁棒性高约束解耦低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::SelfAdjointEigenSolverEigen::Matrixdouble,6,6solver(H);Eigen::VectorXd eigenvaluessolver.eigenvalues();for(inti0;i6;i){if(eigenvalues(i)DEGENERACY_THRESHOLD){// 第 i 个方向退化 → 在该方向上减少更新量delta_x[i]*0.1;// 保守更新}}5. 回环检测算法5.1 ICP 精配准// mapOptmization.cppbooldetectLoopClosure(Pose6Dcurrent_pose,std::vectorPose6Dhistory_poses,PointCloudcurrent_cloud,std::vectorPointCloudhistory_clouds){for(inti0;ihistory_poses.size();i){// 1. 距离筛选floatdist(current_pose.position-history_poses[i].position).norm();if(dist7.0)continue;// 7m 不是回环// 2. 时间筛选floattime_diffcurrent_pose.time-history_poses[i].time;if(time_diff30.0)continue;// 30s 不可能是回环// 3. ICP 配准pcl::IterativeClosestPointPointType,PointTypeicp;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 Ticp.getFinalTransformation();// T 就是回环相对位姿addLoopFactor(current_pose_id,history_poses[i].id,T);returntrue;}}returnfalse;}5.2 GTSAM 因子图优化// 回环因子连接两个非相邻关键帧的位姿gtsam::BetweenFactorgtsam::Pose3loop_factor(key_current,// 当前关键帧IDkey_history,// 历史关键帧IDloop_relative_pose,// ICP 算出的相对位姿loop_noise// 回环约束的噪声模型);graph.add(loop_factor);// ISAM2 增量求解isam.update(graph,initial_estimates);6. 各算法复杂度算法复杂度每帧耗时地面分割O(N_scan × N_col)~2ms地面面特征提取O(N_ground)~1ms非地面特征提取O(N_non_ground)~2ms第1步 LMO(N_ground_feat × iter)~2ms第2步 LMO(N_features × iter)~5ms回环检测 (ICP)O(N_pts × iter)~20ms1Hz 触发位姿图优化O(VE)~10ms总耗时约 10-15ms/帧不含回环适合 Jetson TX2 等嵌入式平台。7. 总结LeGO-LOAM 算法体系的三大核心创新地面分割 约束解耦将 3D 激光SLAM 分解为 z/roll/pitch地面和 x/y/yaw非地面两个子问题 → 优化更快、更稳两步 LM 优化先地面约束后全局优化 → 退化的安全网 → 长走廊等几何退化场景鲁棒回环检测 GTSAM 优化轻量级回环 位姿图优化 → 长期运行无漂移LeGO-LOAM 的算法哲学是**“利用场景先验地面来简化问题”**。这种针对性优化使其在地面机器人场景中比通用的 A-LOAM 精度更高、速度更快但在无人机场景中几乎不可用没有地面可供分割。
