实现同步定位和映射(大满贯)与激光雷达扫描
这个例子演示了如何实现同步定位和映射(大满贯)算法使用带来一系列收集的激光雷达扫描图的优化。这个例子的目的是建立一个使用激光雷达扫描的地图环境和检索机器人的轨迹。
建立环境地图,SLAM算法逐步处理激光雷达扫描和构建一个姿势图链接这些扫描。机器人通过扫描匹配和识别之前访问过的地方可以建立一个或多个循环闭包在其移动路径。SLAM算法利用循环关闭信息更新地图和调整估计机器人轨迹。
激光扫描数据加载文件
加载一个down-sampled数据集组成的激光扫描收集从一个移动机器人室内环境。每两个之间的平均位移扫描大约是0.6米。
的offlineSlamData.mat
文件包含扫描
变量,其中包含所有在本例中使用的激光扫描。
负载(“offlineSlamData.mat”);
机器人的平面图和近似路径提供用于演示目的。这张图片显示了相对环境映射和机器人的近似轨迹。
运行SLAM算法,构造优化机器人的轨迹地图和阴谋
创建一个lidarSLAM
对象和设置地图的分辨率和马克斯激光雷达范围。这个例子使用一个豺狼™机器人从Clearpath机器人™。机器人配备了一个生病的™蒂姆- 511激光扫描仪和最大范围的10米。设置最大激光雷达扫描范围略小于最大范围(8美元),随着激光读数不准确附近最大范围。将网格地图的分辨率设置为每米20个细胞,使一个5厘米精度。
maxLidarRange = 8;mapResolution = 20;slamAlg = lidarSLAM (mapResolution maxLidarRange);
以下循环关闭参数设置经验。使用更高的闭合循环阈值有助于拒绝关闭循环识别过程中假阳性。然而,请记住,高分匹配可能仍然是一个糟糕的比赛。例如,扫描收集在一个有相似或重复特性的环境更容易产生假阳性。使用更高的循环关闭搜索允许算法搜索半径更大范围的地图在当前姿势估计for循环闭包。
slamAlg。LoopClosureThreshold = 210;slamAlg。LoopClosureSearchRadius = 8;
观察地图构建过程最初10扫描
逐步添加扫描slamAlg
对象。如果添加到地图扫描数字印刷。对象拒绝扫描如果扫描之间的距离太小。添加前10扫描测试算法。
为我= 1:10 isScanAccepted、loopClosureInfo optimizationInfo] = addScan (slamAlg扫描{我});如果isScanAccepted流(“添加扫描% d \ n”,我);结束结束
添加扫描1添加扫描2添加扫描3添加扫描4添加扫描5添加扫描6添加扫描7添加扫描8添加扫描9添加扫描10
重建场景的绘制扫描和跟踪的slamAlg
。
图;显示(slamAlg);标题({“环境地图”,“姿势图最初10扫描”});
观察循环闭包和优化过程的影响
继续添加扫描在一个循环中。关闭循环应该自动检测机器人的动作。构成图优化循环关闭时执行确认。输出optimizationInfo
有一个领域,IsPerformed
,这表明构成图优化发生时。
情节扫描和循环闭合时造成视觉识别和验证结果。这个图显示覆盖扫描和优化构成图的第一循环关闭。一个循环闭合边缘添加红色链接。
firstTimeLCDetected = false;图;为我= 10:长度(扫描)[isScanAccepted、loopClosureInfo optimizationInfo] = addScan (slamAlg扫描{我});如果~ isScanAccepted继续;结束%可视化首次检测到循环关闭,如果你想看到的%完成地图构建过程,删除下面的if条件如果optimizationInfo。IsPerformed&& ~firstTimeLCDetected show(slamAlg,“姿势”,“关闭”);持有在;显示(slamAlg.PoseGraph);持有从;firstTimeLCDetected = true;drawnow结束结束标题(“第一循环关闭”);
可视化构建地图和机器人的轨迹
情节最后建毕竟扫描被添加到地图slamAlg
对象。虽然前面的为
循环只绘制初始关闭,所有的扫描都是补充道。
图显示(slamAlg);标题({最后建立环境地图的,机器人的轨迹});
视觉检查建立地图相比,最初的平面图
图像的扫描和姿势图覆盖原来的平面布置图。你可以看到地图匹配原始平面图后添加所有扫描和优化构成图。
建设占用网格地图
优化的扫描和姿势可以用来生成一个occupancyMap
代表作为概率占用网格环境。
[扫描,optimizedPoses] = scansAndPoses (slamAlg);地图= buildMap(扫描,optimizedPoses、mapResolution maxLidarRange);
可视化占用网格地图的激光扫描和优化构成图。
图;显示(地图);持有在显示(slamAlg.PoseGraph“id”,“关闭”);持有从标题(“占用网格地图使用激光雷达大满贯”);