从二维激光雷达扫描构建地图使用大满贯
这个例子展示了如何实现大满贯算法在一系列二维激光雷达扫描使用扫描处理和优化(PGO)构成图。这个例子的目的是估计机器人的轨迹和构建环境地图。
大满贯代表同时定位和地图。
本地化——估计构成一个机器人在已知环境中。
映射——建筑从一个已知的未知环境地图机器人位姿和传感器数据。
在大满贯的过程中,机器人地图创建一个环境而本地化本身。摔在机器人已经广泛应用,自动驾驶汽车,无人机。
在离线大满贯,机器人引导传感器数据通过一个环境和记录。SLAM算法流程这个数据来计算环境的地图。地图存储和用于定位、机器人路径规划在实际操作。
这个例子使用一个2 d算法离线大满贯。算法的增量流程记录激光雷达扫描和构建一个环境的构成图来创建一个地图。克服漂移估计机器人轨迹积累,示例使用扫描匹配识别之前访问过的地方,然后使用这个循环关闭信息环境的优化提出了和更新地图。优化构成图,这个示例使用一个二维的姿势从导航图优化函数工具箱™。
在这个例子中,您将了解如何:
估计机器人轨迹的一系列扫描使用扫描注册算法。
优化的漂移估计机器人轨迹通过识别之前访问过的地方(循环闭包)。
使用扫描及其可视化的地图环境绝对的姿势。
负载激光扫描
这个示例使用收集的数据在一个室内环境中使用豺从Clearpath™机器人机器人™。机器人配备了一个生病的™蒂姆- 511激光扫描仪的最大射程10米。加载wareHouse.mat文件包含激光扫描到工作区中。
data =负载(“wareHouse.mat”);扫描= data.wareHouseScans;
估计机器人轨迹
创建一个lidarscanmap对象。使用这个对象,您可以:
存储和逐步增加激光雷达扫描的。
检测,添加和删除循环闭包。
查找和更新的绝对姿态扫描。
生成和可视化构成图。
指定最大的激光雷达和网格分辨率值范围。您可以修改这些值来调整环境的地图。使用这些值来创建激光雷达扫描地图。
maxLidarRange = 8;gridResolution = 20;mapObj = lidarscanmap (gridResolution maxLidarRange);
逐步从输入数据中添加扫描激光雷达扫描地图对象使用addScan函数。这个函数如果他们拒绝扫描太接近连续扫描。
为i = 1:元素个数(扫描)isScanAccepted = addScan (mapObj扫描{我});如果~ isScanAccepted继续;结束结束
重建扫描通过绘制的场景和姿态跟踪的激光雷达扫描地图。
hFigMap =图;axMap =轴(父= hFigMap);显示(mapObj、家长= axMap);标题(axMap,“环境和机器人轨迹地图”)
注意,估计机器人轨迹漂移。的漂移可以因任何原因:
噪声传感器的扫描,没有足够的重叠。
缺乏显著特征的环境。
一个不准确的初始转换,尤其是旋转是显著的
漂移轨迹估计结果不准确的环境地图。
漂移修正
正确的漂移轨迹的准确检测循环机器人回到的地方,此前访问。添加循环闭合边缘lidarscanmap对象在姿势正确的漂移轨迹图优化。
环路闭合检测
环路闭合检测决定,对于一个给定的扫描,机器人曾参观了当前位置。搜索包含匹配当前扫描对机器人当前位置周围的以前的扫描,在指定的半径loopClosureSearchRadius。接受扫描匹配,如果匹配得分大于指定的阈值loopClosureThreshold。
你可以检测循环使用闭包detectLoopClosure的函数lidarscanmap对象,并将它们添加到映射对象使用addLoopClosure函数。
你可以增加loopClosureThreshold价值在环路闭合检测避免假阳性,但函数仍然可以返回坏比赛环境具有类似或重复功能。为了解决这个问题,增加了loopClosureSearchRadius搜索的价值更大的半径在当前扫描循环闭包,虽然这样会增加计算时间。
您还可以指定循环闭包的数量匹配loopClosureNumMatches。所有这些参数有助于调整环路闭合检测。
loopClosureThreshold = 110;loopClosureSearchRadius = 2;loopClosureNumMatches = 1;mapObjLoop = lidarscanmap (gridResolution maxLidarRange);为i = 1:元素个数(扫描)isScanAccepted = addScan (mapObjLoop扫描{我});%检测回路关闭是否接受扫描如果isScanAccepted [relPose matchScanId] = detectLoopClosure (mapObjLoop,…MatchThreshold = loopClosureThreshold,…SearchRadius = loopClosureSearchRadius,…NumMatches = loopClosureNumMatches);%循环闭包添加到映射对象如果relPose估计如果~ isempty (relPose) addLoopClosure (mapObjLoop, matchScanId relPose);结束结束结束
优化轨迹
创建一个从drift-corrected构成图形对象通过使用激光雷达扫描地图poseGraph函数。使用optimizePoseGraph(导航工具箱)函数优化构成图。
pGraph = poseGraph (mapObjLoop);updatedPGraph = optimizePoseGraph (pGraph);
从提取优化绝对姿势图使用nodeEstimates(导航工具箱)功能,更新环境的轨迹建立一个精确的地图。
optimizedScanPoses = nodeEstimates (updatedPGraph);updateScanPoses (mapObjLoop optimizedScanPoses);
可视化的结果
想象机器人轨迹的变化构成图之前和之后的优化。红线代表循环闭合边缘。
hFigTraj =图(位置= [0 0 900 450]);%之前想象机器人轨迹优化axPGraph =次要情节(1、2、1,父母= hFigTraj);axPGraph。位置= (0.04 - 0.1 0.45 - 0.8);显示(pGraph id =“关闭”父母= axPGraph);标题(axPGraph,“之前PGO”)%可视化后机器人轨迹优化axUpdatedPGraph =次要情节(1、2、2、家长= hFigTraj);axUpdatedPGraph。位置= (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。位置= (0.05 - 0.1 0.44 - 0.8);显示(mapObj、家长= axOldMap);标题(axOldMap,“之前PGO”)%可视化地图后,机器人轨迹优化axUpdatedMap =次要情节(1、2、2、家长= hFigMapTraj);axUpdatedMap。位置= (0.56 - 0.1 0.44 - 0.8);显示(mapObjLoop、家长= axUpdatedMap);标题(axUpdatedMap,“PGO后”)轴([axOldMap axUpdatedMap], [9 18 -10 9]) sgtitle (“环境和机器人轨迹地图”FontWeight =“大胆”)
