使用SLAM从二维激光雷达扫描建立地图
本例展示了如何使用扫描处理和位姿图优化(PGO)在一系列二维激光雷达扫描上实现SLAM算法。本例的目标是估计机器人的轨迹并构建环境地图。
大满贯表示同时定位和映射。
本地化-在已知环境中估计机器人的姿态。
映射-根据已知的机器人姿态和传感器数据构建未知环境的地图。
在SLAM过程中，机器人在本地化自己的同时创建环境地图。SLAM在机器人、自动驾驶汽车和无人机等领域有着广泛的应用。
在离线SLAM中，机器人在环境中导航并记录传感器数据。SLAM算法处理这些数据来计算环境的地图。该地图被存储并用于机器人实际操作期间的定位和路径规划。
本例使用二维离线SLAM算法。该算法逐步处理记录的激光雷达扫描，并构建一个姿态图来创建环境地图。为了克服估计机器人轨迹中累积的漂移，该示例使用扫描匹配来识别之前访问过的地方，然后使用这个闭环信息来优化姿态和更新环境地图。为了优化姿态图，本例使用Navigation Toolbox™中的2-D姿态图优化函数。
在这个例子中，你将学习如何:
利用扫描配准算法从一系列扫描中估计机器人轨迹。
通过识别之前访问过的地方(环路封闭)，优化估计机器人轨迹中的漂移。
使用扫描和它们的绝对姿势来可视化环境地图。
负载激光扫描
本例使用Clearpath Robotics公司的Jackal机器人在室内环境中收集的数据。机器人配备了SICK™TiM-511激光扫描仪，最大范围为10米。加载wareHouse.mat文件包含激光扫描到工作空间。
数据=负载(“wareHouse.mat”）;扫描= data.wareHouseScans;
估计机器人轨迹
创建一个lidarscanmap对象。使用这个对象，你可以:
存储和增加激光雷达扫描增量。
检测、添加和删除循环闭包。
查找和更新扫描的绝对姿势。
生成并可视化一个姿态图。
指定最大激光雷达范围和网格分辨率值。您可以修改这些值来微调环境的映射。使用这些值来创建激光雷达扫描地图。
maxLidarRange = 8;gridResolution = 20;mapObj = lidarscanmap(gridResolution,maxLidarRange);
方法将输入数据中的扫描增量添加到激光雷达扫描映射对象addScan函数。如果扫描与连续扫描的距离太近，该函数将拒绝扫描。
为i = 1:numel(扫描)isScanAccepted = addScan(mapObj，扫描{i});如果~ isScanAccepted继续；结束结束
通过绘制激光雷达扫描地图跟踪的扫描和姿势来重建场景。
hFigMap =图;axMap = axes(Parent=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，扫描{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(导航工具箱)函数，并更新轨迹，以建立一个精确的环境地图。
optimizedscanposed = nodeEstimates(updatedPGraph);updateScanPoses (mapObjLoop optimizedScanPoses);
可视化的结果
可视化姿态图优化前后机器人轨迹的变化。红线代表环路闭合边。
hFigTraj = figure(Position=[0 0 900 450]);在优化前可视化机器人轨迹axPGraph = subplot(1,2,1,Parent=hFigTraj);axPGraph。位置= [0.04 0.1 0.45 0.8];显示(pGraph id =“关闭”父母= axPGraph);标题(axPGraph,“之前PGO”）优化后的机器人轨迹可视化axUpdatedPGraph = subplot(1,2,2,Parent=hFigTraj);axUpdatedPGraph。位置= [0.54 0.1 0.45 0.8];显示(updatedPGraph id =“关闭”父母= axUpdatedPGraph);标题(axUpdatedPGraph,“PGO后”) axis([axPGraph axUpdatedPGraph]，[-6 10 -7 3]) sgtitle(“机器人轨迹”FontWeight =“大胆”）
可视化姿态图优化前后的环境图和机器人轨迹图。
hFigMapTraj = figure(Position=[0 0 900 450]);在优化前可视化地图和机器人轨迹axOldMap = subplot(1,2,1,Parent=hFigMapTraj);axOldMap。位置= [0.05 0.1 0.44 0.8];显示(mapObj、家长= axOldMap);标题(axOldMap,“之前PGO”）优化后可视化地图和机器人轨迹axUpdatedMap = subplot(1,2,2,Parent=hFigMapTraj);axUpdatedMap。位置= [0.56 0.1 0.44 0.8];显示(mapObjLoop、家长= axUpdatedMap);标题(axUpdatedMap,“PGO后”) axis([axOldMap axUpdatedMap]，[-9 18 -10 9])环境与机器人轨迹图FontWeight =“大胆”）
另请参阅
功能
相关的话题
您也可以从以下列表中选择一个网站: