针对地面机器人在卫星拒止的非结构化环境中，激光SLAM易产生垂直漂移、精度下降甚至失效的问题，提出一种面向地面机器人的可用于多种雷达的鲁棒激光SLAM算法。 首先，使用提出的非超参数地面初始化方法自动完成地面点的提取和分割，并对剩余点云执行基于主成分分析（PCA）的几何和强度特征提取；提取出的地面点和特征点 被输入到雷达里程计模块执行三阶段直接特征匹配，在相似性约束和IMU约束的作用下，输出高频率且相对精确的帧间变换估计；在雷达建图模块，以雷达里程计的输出 作为初始猜测，通过强度增强的几何特征匹配构建雷达帧间匹配因子，通过前端提取的地面点评估地面的平坦度和连续性以构建地面约束因子，通过IMU的预积分解算构 建IMU旋转约束因子和重力约束因子，最后与回环检测因子一并构建因子图执行全局优化，实时输出精确的机器人状态估计和全局地图。在使用地面机器人采集了多个具 有挑战性的序列上，与多个算法进行比较的结果表明，本文算法可以得到更精确的机器人状态估计，构建全局一致的地图，并且在地形变化、大尺度、非结构化的场景中 更具鲁棒性。
本文系统主要分为4个模块： 1）特征提取模块：输入激光雷达收集的原始点云，通过KD树搜索每个点的5个最近点，并计算协方差矩阵，对协方差矩阵进行特征值分解后构建直线度、平面度、曲率、法线等判别指标，根据这些指标提取边缘、平面、地面等几何特征，并完成无超参数地面初始化；然后，对强度进行校准后提取强度角点特征作为边缘特征的补充，以提高系统在非结构化场景中的鲁棒性。 2）雷达里程计模块：接收提取到的特征，执行三阶段帧到滑动窗口内点云的直接特征匹配，并附加几何、强度约束、以及IMU约束（如果IMU可用），实现高频的帧间状态估计，输出6自由度位姿作为雷达建图模块优化的初始值。 3）雷达建图模块：分别构建当前帧与局部子图的点到直线残差和点到平面残差，并结合局部特征值表示的几何相似性以及KL散度表示的强度相似性，构建雷达里程计约束；利用提取的地面点进行地面拟合，构建地面约束，若IMU可用，则再构建IMU旋转约束和重力约束，一并放入因子图中进行优化，输出精确的机器人状态估计，根据机器人移动距离和旋转角度选取关键帧，放入数据库。 4）全局优化模块：当有新关键帧添加到数据库时，会执行基于距离搜索的回环检测模块，当检测到候选回环后，进行基于ICP算法的几何验证，若匹配成功，则将回环因子加入到因子图，并执行全局优化减小累计误差，实现全局一致的状态估计。
为验证本文算法的通用性、精度和鲁棒性，搭建了地面特种机器人平台用于数据采集。该平台配备Velodyne VLP16旋转式机械 激光雷达和Livox Mid 360混合固态激光雷达或Livox Avia混合固态激光雷达，三者均以10Hz的频率获取点云数据；IMU型号 为Xsens MTi 100，以100Hz的频率获取9轴测量值；使用北斗星通RTK，以1Hz的卫星数据作为SLAM定位真值；同时还配备Rea lsense d455深度相机以30Hz的频率采集图像。
在我们自己录制的序列上对算法进行评估，并与多种经典算法进行比较，如表1、2、3所示，本文算法均取得了优秀的结果。其中，在GR05这个高动态、大尺度的序列上，大多数算法均产生了巨大的累计误差，甚至导致算法失效，而本文算法即使不启用回环检测，仍实现了最高的精度， 当启用回环检测后，本文算法实现了仅1.51m的误差。
为了更直观的体现本文算法的效果，选择了GR05、GR09、GR12、GR15、GR17五个典型序列，绘制了每个比较算法的轨迹图，本文算法在每个序列上都实现了与真值轨迹最贴合的轨迹，尤其是在z方向上，通过地面约束，本文算法实现了极小的误差。
为了更全面地展现本文算法的优越性，本节定性地展示本文的建图效果。 在GR05序列上构建的点云地图，可以看出该地图与卫星地图街景高度贴合。通过右侧四个局部放大图可以看出，本文提出的算法构建出了精确的街道点云地图。
在GR09序列上构建的沈自所园区点云地图，从三个局部放大图像可以看出，本文算法对于A区域的非结构化林区环境实现了精确的地图构建，对于B区域和C区域的楼宇环境，对窗户、棱角等细节都实现了精细的还原。
在GR12序列上构建的点云地图，地面机器人配备Livox Mid 360固态雷达穿越了局部放大图所示的地面崎岖不平的非结构化林区环境。由局部放大图可以看出，对林区的树木、电线杆，以及园区中的公交车、建筑都实现了精细的建图效果。
在GR19序列上构建的点云地图，该序列为使用Livox Avia录制的室内环境，相比于室外，室内环境存在典型的双面问题， 这通常使得室外好用的SLAM算法普遍失效。而本文算法即使在室内也可以实现精确地定位和建图，根据局部放大细节和显示照片的比对可以看出，本文算法对室内的植物和建筑结构细节也实现了精确的测绘。
GR-LOAM++