利用激光雷达扫描实现同步定位和绘图（SLAM）

从文件加载激光扫描数据

加载下采样数据集，该数据集由室内环境中从移动机器人采集的激光扫描组成。每两次扫描之间的平均位移约为0.6米。

这个脱机SlamData.mat文件包含扫描变量，其中包含本例中使用的所有激光扫描。

负载('offlineSlamData.mat');

为了便于说明，提供了机器人的平面图和近似路径。此图显示了正在映射的相对环境和机器人的近似轨迹。

运行SLAM算法，构建优化地图并绘制机器人轨迹

创建一个激光雷达SLAM对象并设置地图分辨率和最大激光雷达范围。本例使用Clearpath Robotics™中的Jackal™机器人。该机器人配备了最大射程为10米的SICK™TiM-511激光扫描仪。将最大激光雷达范围设置为略小于最大扫描范围（8m），因为激光读数在最大范围附近不太准确。将栅格地图分辨率设置为每米20个单元格，这样可以获得5厘米的精度。

最大激光雷达范围=8；mapResolution=20；slamAlg=lidarSLAM（mapResolution，maxLidarRange）；

以下回路闭合参数是根据经验设定的。使用较高的环路闭合阈值有助于在环路闭合识别过程中拒绝误报。然而，请记住，高分比赛可能仍然是一场糟糕的比赛。例如，在具有类似或重复特征的环境中收集的扫描更有可能产生假阳性。使用较高的环路闭合搜索半径，该算法可以围绕环路闭合的当前姿势估计搜索更大范围的地图。

砰砰声。环路关闭阈值=210；砰砰声。环路闭合搜索半径=8；

通过最初的10次扫描观察地图构建过程

将扫描增量添加到slamAlg公司对象。如果将扫描号添加到地图中，则会打印扫描号。如果扫描之间的距离太小，对象将拒绝扫描。首先添加前10个扫描以测试算法。

对于i=1:10[isScanAccepted，loopClosureInfo，optimizationInfo]=添加扫描（slamAlg，扫描{i}）；如果isScanAccepted（扫描接受）fprintf（打印）('添加了扫描%d\n'，i）；结束
结束

添加了扫描1添加扫描2添加扫描3已添加扫描4添加了扫描5添加了扫描6添加扫描7添加了扫描8添加了扫描9添加扫描10

通过绘制由斯拉姆阿尔格.

图形；显示（slamAlg）；标题({“环境地图”,'初始10次扫描的姿势图'});

观察回路闭合的效果和优化过程

继续在循环中添加扫描。机器人移动时，应自动检测回路闭合。只要确定了回路闭合，就会执行位姿图优化。输出优化信息有一个字段，已执行，指示何时进行姿势图优化。

每当识别出环路闭合时，绘制扫描图和姿势图，并目测验证结果。该图显示了覆盖扫描和第一个回路闭合的优化姿势图。循环闭合边作为红色链接添加。

firstTimeLCDetected=false；图形；对于i=10：长度（扫描）[isScanAccepted，loopClosureInfo，optimizationInfo]=addScan（slamAlg，扫描｛i｝）；如果~已扫描接受持续;结束
    %如果您想查看
    %完成地图构建过程，删除下面的if条件
    如果optimizationInfo。已执行&&~firstTimeLCDetected显示（slamAlg，“姿势”,“关闭”);持有在;显示（slamAlg.PoseGraph）；持有远离的;firstTimeLCDetected=true；刷新屏幕结束
结束标题('第一个循环闭合');

机器人构造的地图和轨迹可视化

将所有扫描添加到斯拉姆阿尔格对象。尽管之前对于loop只绘制了初始闭合图，所有扫描都被添加。

图形显示（slamAlg）；标题({“最终环境地图”,“机器人轨迹”});

目视检查与原始平面图相比的已建地图

扫描图像和姿势图覆盖在原始平面图上。添加所有扫描并优化姿势图后，可以看到地图与原始平面图很好地匹配。

构建占用率网格图

优化的扫描和姿势可用于生成占用地图，将环境表示为概率占用网格。

[scans，optimizedPoses]=扫描和姿势（slamAlg）；map=buildMap（扫描、优化姿势、mapResolution、maxLidarRange）；

可视化由激光扫描和优化姿势图填充的占用网格图。

图形；显示（地图）；持有在显示（slamAlg.PoseGraph，“ID”,“关闭”);持有远离的标题(“使用激光雷达SLAM构建的占用网格图”);