从文件加载激光扫描数据

在室内环境中加载由移动机器人采集的激光扫描组成的降采样数据集。两次扫描之间的平均位移约为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',我);结束结束

新增扫描1新增扫描2新增扫描3新增扫描4新增扫描5新增扫描6新增扫描7新增扫描8新增扫描9新增扫描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对象。前面的为循环应该添加所有的扫描，尽管只是绘制初始循环闭包。

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

目视检查建立的地图与原始平面图比较

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

构建占用网格地图

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

[scans, optimizedpositions] = scanandpositions (slamAlg);map = buildMap(scans, optimizedpose, mapResolution, maxLidarRange);

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

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