使用SLAM从二维激光雷达扫描建立地图
本例展示了如何利用扫描处理和位姿图优化(PGO)在一系列二维激光雷达扫描上实现SLAM算法。本例的目标是估计机器人的轨迹并构建环境地图。
大满贯表示同时定位和映射。
本地化-估计机器人在已知环境中的姿态。
映射-根据已知的机器人姿态和传感器数据构建未知环境的地图。
在SLAM过程中，机器人在定位自己的同时创建一个环境地图。SLAM在机器人、自动驾驶汽车和无人机等领域有着广泛的应用。
在离线SLAM中，机器人在环境中操纵并记录传感器数据。SLAM算法处理这些数据，计算出环境的地图。该地图被存储起来，用于机器人实际操作过程中的定位和路径规划。
本例使用二维脱机SLAM算法。该算法增量处理记录的激光雷达扫描，并建立一个姿势图，以创建一个环境地图。为了克服估计的机器人轨迹中累积的漂移，该示例使用扫描匹配来识别之前访问过的地方，然后使用这个循环闭合信息来优化姿态和更新环境地图。为了优化姿态图，本示例使用Navigation Toolbox™中的二维姿态图优化函数。
在本例中，您将学习如何:
利用扫描配准算法从一系列扫描中估计机器人轨迹。
通过识别以前访问过的地方(环路闭合)，优化估计机器人轨迹中的漂移。
使用扫描和它们的绝对姿势可视化环境地图。
负载激光扫描
本例使用Clearpath Robotics公司的Jackal机器人在室内环境中收集的数据。该机器人配备了SICK™TiM-511激光扫描仪，最大扫描距离为10米。加载wareHouse.mat包含激光扫描到工作空间的文件。
data =负载(“wareHouse.mat”）;扫描= data.wareHouseScans;
估计机器人轨迹
创建一个lidarscanmap对象。使用这个对象，你可以:
存储和添加激光雷达扫描增量。
检测、添加和删除循环闭包。
查找并更新扫描的绝对姿势。
生成并可视化一个姿势图。
指定最大激光雷达范围和网格分辨率值。您可以修改这些值来微调环境的映射。使用这些值创建激光雷达扫描地图。
maxLidarRange = 8;gridResolution = 20;mapObj = lidarscanmap (gridResolution maxLidarRange);
方法将从输入数据的扫描增量添加到激光雷达扫描映射对象中addScan函数。如果扫描太接近连续扫描，此函数将拒绝扫描。
为i = 1:numel(扫描)isScanAccepted = addScan(mapObj，扫描{i});如果~ 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:numel(扫描)isScanAccepted = addScan(mapObjLoop，扫描{i});如果接受扫描，则检测环路关闭如果isScanAccepted [relPose,matchScanId] = detectLoopClosure(mapObjLoop，.．.MatchThreshold = loopClosureThreshold,.．.SearchRadius = loopClosureSearchRadius,.．.NumMatches = loopClosureNumMatches);如果估算了relPose，则向映射对象添加循环闭包如果~ isempty (relPose) addLoopClosure (mapObjLoop, matchScanId relPose);结束结束结束
优化轨迹
方法从漂移校正的激光雷达扫描图中创建一个姿态图对象poseGraph函数。使用optimizePoseGraph(导航工具箱)函数优化姿态图。
pGraph = poseGraph (mapObjLoop);updatedPGraph = optimizePoseGraph (pGraph);
从位姿图中提取最优的绝对位姿nodeEstimates(导航工具箱)函数，并更新轨迹，以建立一个精确的环境地图。
optimizedScanPoses = nodeEstimates (updatedPGraph);updateScanPoses (mapObjLoop optimizedScanPoses);
可视化的结果
可视化姿态图优化前后机器人轨迹的变化。红线表示环路闭合边。
hFigTraj = figure(Position=[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 = figure(Position=[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])“环境和机器人轨迹图”FontWeight =“大胆”）
另请参阅
功能
相关的话题
您也可以从以下列表中选择网站: