从文件加载激光扫描数据

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

的offlineSlamData.mat文件包含扫描变量，它包含本例中使用的所有激光扫描

负载(“offlineSlamData.mat”）;

给出了机器人的平面布置图和近似路径。这个图像显示了被映射的相对环境和机器人的近似轨迹。

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

创建一个lidarSLAM目标，并设置地图分辨率和最大激光雷达范围。本例使用Clearpath Robotics™的Jackal™机器人。机器人配备了SICK™TiM-511激光扫描仪，最大射程为10米。设置最大激光雷达范围略小于最大扫描范围(8m)，因为激光读数在最大范围附近不太准确。将网格地图分辨率设置为每米20个单元格，这将提供5cm的精度。

maxLidarRange = 8;mapResolution = 20;slamAlg = lidarSLAM(mapResolution, maxLidarRange);

下面的闭环参数是经验设置的。在环路闭合识别过程中，采用较高的环路闭合阈值有助于排除误报。然而，记住高分匹配可能仍然是糟糕的匹配。例如，在具有相似或重复特征的环境中收集的扫描更有可能产生假阳性。使用更高的环闭搜索半径允许算法搜索更大范围的地图周围的环闭的当前姿态估计。

slamAlg。LoopClosureThreshold = 210;slamAlg。LoopClosureSearchRadius = 8;

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

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

为i=1:10 [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});如果isScanAccepted流(添加扫描%d \n',我);结束结束

新增scan 1新增scan 2新增scan 3新增scan 4新增scan 5新增scan 6新增scan 7新增scan 8新增scan 9新增scan 10

通过绘制扫描图和被跟踪的姿态来重建场景slamAlg．

图;显示(slamAlg);标题({“环境地图”，“最初10次扫描的姿势图”}）;

观察闭环的效果和优化过程

继续循环添加扫描。当机器人移动时，应该自动检测回路闭合。只要确定了一个闭环，就进行姿态图优化。输出optimizationInfo有一个领域,IsPerformed，表示当姿态图优化发生时…

绘制扫描和姿势，无论何时识别环路闭合，并验证结果可视化。这个图显示了重叠扫描和优化的姿态图的第一个循环闭包。一个闭环边缘作为一个红色链接被添加。

firstTimeLCDetected = false;图;为i=10:length(scans) [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});如果~ isScanAccepted继续；结束%可视化第一个检测到的循环闭包，如果您想查看%完成地图建造过程，去掉下面的if条件如果optimizationInfo。IsPerformed&& ~firstTimeLCDetected show(slamAlg,“姿势”，“关闭”）;持有在；显示(slamAlg.PoseGraph);持有从；firstTimeLCDetected = true;drawnow结束结束标题(“第一循环关闭”）;

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

在所有扫描添加到slamAlg对象。前面的为Loop应该添加所有的扫描，尽管只绘制初始循环闭包。

图显示(slamAlg);标题({“环境最终建成地图”，《机器人的轨迹》}）;

视觉检查已建成的地图与原始平面图

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

建立占用网格地图

优化的扫描和姿态可以用来生成一个occupancyMap，将环境表示为一个概率占用网格。

[scans, optimizedpose] = scansandpose (slamAlg);map = buildMap(扫描，优化的姿势，地图分辨率，maxLidarRange);

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

图;显示(地图);持有在显示(slamAlg。PoseGraph,“id”，“关闭”）;持有从标题(“使用激光雷达SLAM构建的居住网格地图”）;