使用SLAM从二维激光雷达扫描构建地图
这个示例向您展示了如何使用扫描处理算法和位姿图优化(PGO)在一系列二维激光雷达扫描上实现同步定位和映射(SLAM)算法。这个例子的目标是估计机器人的轨迹，并建立一个环境地图。
本例中的SLAM算法增量地处理激光雷达扫描，并构建一个姿态图来创建环境地图。为了克服机器人轨迹估计过程中积累的漂移问题，该实例通过扫描匹配来识别之前访问过的位置，并利用环路闭合信息来优化姿态和更新环境地图。为了优化姿态图，本示例使用了Navigation Toolbox™中的2-D姿态图优化。
在这个例子中，您将学习如何:
利用扫描配准算法从一系列扫描中估计机器人的轨迹。
通过识别以前访问过的地方(回路闭合)优化估计机器人轨迹中的漂移。
使用扫描和他们的绝对姿势可视化环境的地图。
负载激光扫描
本示例使用Clearpath Robotics™的Jackal™机器人在室内环境中收集的数据。机器人配备了SICK™TiM-511激光扫描仪，最大扫描距离为10米。加载offlineSlamData.mat包含激光扫描到工作区的文件。
data =负载(“offlineSlamData.mat”）;扫描= data.scans;
机器人轨迹估计
示例使用matchScansGrid和matchScans函数来估计连续扫描之间的相对姿态。的matchScansGrid函数提供相对姿态的初始估计，其精度可达到指定的分辨率。的matchScans函数使用估计作为初始猜测，并改进相对姿态以更好地估计。
%设置最大激光雷达范围略小于最大范围%扫描器，因为激光读数在最大距离附近不太准确maxLidarRange = 8;%设置地图分辨率为每米10个单元格，给出了一个精度% 10厘米分辨率=10；创建一个姿势图形对象并定义信息矩阵pGraph = poseGraph;infoMat = [1 0 0 1 0 1];%循环遍历每个扫描并估计相对姿态prevScan ={1}扫描;为i = 2:numel(scans) currScan = scans{i};估计当前扫描和先前扫描之间的相对位姿[relPose,统计]= matchScansGrid (currScan prevScan,．..“最大范围”，Maxlid，“决议”, mapResolution);优化相对姿势relPoseRefined = matchScans (currScan prevScan,“initialPose”, relPose);给姿势图形对象添加相对姿势pGraph.addRelativePose (relPoseRefined infoMat);ax =显示(pGraph“id”，“关闭”）;标题(ax,“估计机器人轨迹”) drawnow prevScan = currScan;结束
注意，估计的机器人轨迹会随着时间的推移而漂移。漂移可能是由于下列任何一个原因:
传感器的噪声扫描没有足够的重叠
无显著特征
不准确的初始转换，特别是当旋转是重要的
估计轨迹的漂移导致了不准确的环境地图。可视化地图和机器人轨迹使用helperShow辅助函数，在万博1manbetx支持功能这个例子的一部分。
hFigMap =图;axMap =轴(“父”, hFigMap);helperShow(扫描,pGraph maxLidarRange axMap);标题(axMap,“环境与机器人轨迹地图”）
漂移修正
通过精确的检测来纠正弹道上的漂移循环，这些地方是机器人之前去过的地方。在位姿图中加入环闭合边，有助于修正位姿图优化过程中的轨迹漂移。
环路闭合检测
环路闭合检测确定机器人以前是否访问过当前位置。在指定的半径范围内，将当前的扫描与之前的扫描进行匹配loopClosureSearchRadius．如果匹配分数大于指定值，则接受扫描作为匹配循环闭合阈值．方法检测循环闭包helperDetectLoop辅助函数，它作为支持文件附加到本示例中。万博1manbetx
根据结果的质量调整循环闭包参数。你可以增加循环闭合阈值值来拒绝循环闭包检测中的假阳性，但该函数仍可能在具有类似或重复特性的环境中返回坏匹配。为了解决这个问题，增加loopClosureSearchRadius值来搜索围绕循环闭包当前姿态估计的更大半径，尽管这会增加计算时间。
loopClosureThreshold = 110;loopClosureSearchRadius = 2;[loopClosureEdgeIds, loopClosurePoses] = helperDetectLoop (pGraph扫描,．..loopClosureSearchRadius loopClosureThreshold);
轨迹优化
将检测到的回路闭合边添加到姿势图中，以纠正估计轨迹中的漂移。使用optimizePoseGraph(导航工具箱)函数来优化姿态图。
%在图形中添加循环闭合边如果~ isempty (loopClosureEdgeIds)为k=1:size（loopClosureEdgeIds，1）pGraph.addRelativePose（loopClosurePoses（k，：），infoMat，．..loopClosureEdgeIds (k, 1), loopClosureEdgeIds (k, 2));结束结束优化姿态图updatedPGraph = optimizePoseGraph (pGraph);
可视化
将机器人在姿势图优化前后的轨迹变化可视化。红线表示回路闭合边。
hFigTraj =图(“位置”，[0 0 900 450]);%在优化前可视化机器人轨迹axPGraph =次要情节(1、2、1,“父”, hFigTraj);axPGraph。Position = [0.04 0.1 0.45 0.8];显示(pGraph“id”，“关闭”，“父”, axPGraph);标题(axPGraph,“在PGO之前”）%优化后的机器人轨迹可视化axUpdatedPGraph =次要情节(1、2、2、“父”, hFigTraj);axUpdatedPGraph。Position = [0.54 0.1 0.45 0.8];显示(updatedPGraph“id”，“关闭”，“父”, axUpdatedPGraph);标题(axUpdatedPGraph,“PGO后”)轴([axPGraph axUpdatedPGraph]，[-6 10 -7 3]) sgtitle(“机器人轨迹”，“FontWeight”，“大胆”）
可视化环境地图和机器人轨迹的姿态图优化前后。
hFigMapTraj =图(“位置”，[0 0 900 450]);%在优化前可视化地图和机器人轨迹axOldMap =次要情节(1、2、1,“父”, hFigMapTraj);axOldMap。Position = [0.05 0.1 0.44 0.8];helperShow(扫描,pGraph maxLidarRange axOldMap)标题(axOldMap,“在PGO之前”）%优化后可视化地图和机器人轨迹axUpdatedMap =次要情节(1、2、2、“父”, hFigMapTraj);axUpdatedMap。Position = [0.56 0.1 0.44 0.8];helperShow(扫描,updatedPGraph maxLidarRange axUpdatedMap)标题(axUpdatedMap,“PGO后”)轴([axOldMap axUpdatedMap]，[-9 18 -10 9]) sgtitle(“环境与机器人轨迹地图”，“FontWeight”，“大胆”）
万博1manbetx支持功能
的helperShow辅助功能可视化的环境地图和机器人的轨迹。该函数使用激光雷达扫描的相应姿态来创建环境地图。
函数helperShow(扫描,pGraph maxRange ax) (ax,“上”）为i = 1:numel(scans) sc = transformScan(scans{i}.removeInvalidData(“RangeLimits”[0.02 maxRange]),．..p图节点（i）；scPoints=sc.笛卡尔坐标；绘图（ax、scPoints（：，1）、scPoints（：，2），“。”，“MarkerSize”,3，“颜色”，“米”）结束nds = pGraph.nodes;情节(ax, nds (: 1), nds (:, 2),“。”，“MarkerSize”5,“颜色”，“b”)持有(ax,“关闭”)轴(ax,“平等”)框(ax,“上”)网格(ax,“上”)xlabel(“X”) ylabel (“Y”）结束
另请参阅
功能
matchScansGrid|matchScans|addRelativePose(导航工具箱)|显示(导航工具箱)|optimizePoseGraph(导航工具箱)
对象
你也可以从以下列表中选择一个网站:
