使用SLAM从二维激光雷达扫描构建地图
这个示例向您展示了如何使用扫描处理算法和位姿图优化(PGO)在一系列二维激光雷达扫描上实现同步定位和映射(SLAM)算法。这个例子的目标是估计机器人的轨迹，并建立一个环境地图。
在此示例中的SLAM算法递增地处理LIDAR扫描并构建姿势图以创建环境的地图。为了克服估计的机器人轨迹中累积的漂移，该示例通过扫描匹配识别先前访问的位置，并利用循环闭合信息来优化姿势并更新环境的地图。要优化姿势图，此示例使用来自导航工具箱™的2-D姿态图优化。
在这个例子中，您将学习如何:
利用扫描配准算法从一系列扫描中估计机器人的轨迹。
通过识别以前访问过的地方(回路闭合)优化估计机器人轨迹中的漂移。
使用扫描和绝对姿势可视化环境的地图。
负载激光扫描
本示例使用Clearpath Robotics™的Jackal™机器人在室内环境中收集的数据。机器人配备了SICK™TiM-511激光扫描仪，最大扫描距离为10米。加载offlineslamdata.mat.包含激光扫描到工作区的文件。
data = load（“offlineSlamData.mat”）;扫描= data.scans;
机器人轨迹估计
示例使用matchscansgrid.和匹配估计连续扫描之间的相对姿势的功能。这matchscansgrid.函数提供相对姿态的初始估计，其精度可达到指定的分辨率。这匹配函数使用估计作为初始猜测，并改进相对姿态以更好地估计。
%设置最大激光雷达范围略小于最大范围%扫描器，因为激光读数在最大距离附近不太准确maxLidarRange = 8;％将地图分辨率设置为每米的10个单元格，这给出了一种精确度% 10厘米mapResolution = 10;％创建姿势图对象并定义信息矩阵Pgraph =姿势图;Infomat = [1 0 0 1 0 1];%循环遍历每个扫描并估计相对姿态prevscan =扫描{1};为了i = 2：Numel（扫描）Curscan = Scans {i};当前扫描与先前扫描之间的百分比估计相对姿势[rimble，stats] = matchscansgrid（Curscan，prevscan，......“MaxRange”maxLidarRange,“决议”, mapResolution);％精炼相对姿势RelposereFined = MatchScans（Curscan，Prevscan，'initialsing'，有影响）;给姿势图形对象添加相对姿势pgraph.addrelativepose（Relposerefined，Infomat）;x = show（光印，“id”那“关闭”）;标题(ax,“估计机器人轨迹”）绘制Prevscan = Curscan;结尾
请注意，估计的机器人轨迹随时间漂移。漂移可能是由于以下任何原因：
传感器的噪声扫描没有足够的重叠
缺少重要特征
不准确的初始转换，特别是当旋转很大时
估计轨迹的漂移导致了不准确的环境地图。可视化地图和机器人轨迹使用求助者辅助函数，在金宝app支持功能这个例子的一部分。
hFigMap =图;axMap =轴(“父”，hfigmap）;求助者（扫描，Pgraph，maxlidarrange，Axmap）;标题（AXMAP，'环境和机器人轨迹的地图'）
漂移修正
通过准确检测到纠正轨迹的漂移循环，这是机器人先前访问过的地方。将循环闭合边缘添加到姿势图，这有助于在姿势图优化期间校正轨迹的漂移。
环路闭合检测
环路闭合检测确定机器人以前是否访问过当前位置。在指定的半径范围内，将当前的扫描与之前的扫描进行匹配loopClosureSearchRadius。如果匹配分数大于指定的匹配分数，则扫描被接受为匹配项loopClosureThreshold。使用循环闭合使用helperDetectLoop辅助函数，它作为支持文件附加到本示例中。金宝app
根据结果的质量调整环路闭合参数。你可以增加loopClosureThreshold拒绝循环关闭检测中误报的值，但功能仍可能在具有相似或重复功能的环境中返回错误匹配。要解决这个问题，增加loopClosureSearchRadius值来搜索围绕循环闭包当前姿态估计的更大半径，尽管这会增加计算时间。
loopClosureThreshold = 110;loopClosureSearchRadius = 2;[loopClosureEdgeIds, loopClosurePoses] = helperDetectLoop (pGraph扫描,......LoopClosuresearchRadius，Loopclosurethreshold）;
轨迹优化
将检测到的环路闭合边添加到姿态图中，以纠正估计轨迹中的漂移。使用优化术照片.(导航工具箱)函数来优化姿态图。
%在图形中添加循环闭合边如果〜isempty（LoopCloseredIds）为了pGraph.addRelativePose(loopclosurepose (k，:)，infoMat，......LoopCloserCedIds（K，1），LoopCloserCedIds（K，2））;结尾结尾％优化姿势图updatedPGraph = optimizePoseGraph (pGraph);
可视化
可视化机器人在姿态图优化前后的轨迹变化。红线表示环路闭合边。
hfigtraj =图（“位置”，[0 0 900 450]）;%在优化前可视化机器人轨迹Axpropre =子图（1,2,1，“父”, hFigTraj);axPGraph。Position = [0.04 0.1 0.45 0.8];显示(pGraph“id”那“关闭”那“父”，Axpraphy）;标题（AXPRAGH，“PGO之前”）%优化后的机器人轨迹可视化axUpdatedPGraph =次要情节(1、2、2、“父”, hFigTraj);axUpdatedPGraph。Position = [0.54 0.1 0.45 0.8];显示(updatedPGraph“id”那“关闭”那“父”，axupdatepgraph）;标题（axupdatepgraph，“PGO后”）轴（[Axpraphy AxupdatepGraph]，[ - 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'那'胆大'）
金宝app支持功能
这求助者帮助函数可视化机器人环境的地图和轨迹。该函数使用相应的姿势将LIDAR扫描转换为创建环境的地图。
函数helperShow(扫描,pGraph maxRange ax) (ax,'在'）为了i = 1:numel(scans) sc = transformScan(scans{i}.removeInvalidData(“RangeLimits”，[0.02 maxrange]），......pGraph.nodes (i));scPoints = sc.Cartesian;情节(ax, scPoints (: 1), scPoints (:, 2),'。'那“MarkerSize”3,'颜色'那“米”）结尾nds = pgraph.nodes;绘图（AX，NDS（：，1），NDS（：2），'.-'那“MarkerSize”5,'颜色'那'B'）握住（斧头，“关闭”）轴（斧头，“平等”）盒子（斧头，'在')网格(ax,'在')包含(“X”) ylabel (“Y”）结尾
也可以看看
职能
matchscansgrid.|匹配|addRelativePose(导航工具箱)|展示(导航工具箱)|优化术照片.(导航工具箱)
对象
你也可以从以下列表中选择一个网站:
