使用SLAM从激光雷达数据建立地图
这个例子展示了如何处理来自安装在车辆上的传感器的3-D激光雷达数据，以逐步构建地图，并使用同步定位和测绘(SLAM)估计车辆的轨迹。除了3-D激光雷达数据外，惯性导航传感器(INS)也被用于帮助构建地图。以这种方式构建的地图可以促进车辆导航的路径规划，也可以用于本地化。
概述
的从激光雷达数据构建地图示例使用三维激光雷达数据和IMU读数逐步构建车辆所经过的环境地图。虽然这种方法构建了一个局部一致的映射，但它只适用于映射小区域。在较长的序列中，漂移累积成一个显著的误差。为了克服这一限制，本示例识别以前访问过的地方，并尝试使用图SLAM方法纠正累积漂移。
加载和探索记录的数据
本例中使用的数据是Velodyne SLAM数据集，代表近6分钟的记录数据。将数据下载到临时目录。
注意:这个下载过程可能需要几分钟。
baseDownloadURL =“https://www.mrt.kit.edu/z/publ/download/velodyneslam/data/scenario1.zip”；dataFolder = fullfile(tempdir，“kit_velodyneslam_data_scenario1”, filesep);选项= weboptions(“超时”、正);zipFileName =数据文件夹+“scenario1.zip”；获取scenario o1文件夹中PNG文件的完整文件路径。pointcloudfileppattern = fullfile(数据文件夹，“scenario1”，“扫描* . png”）;numExpectedFiles = 2513;folderExists = exist(数据文件夹“dir”）;如果~ folderExists在临时目录中创建一个文件夹来保存下载的zip文件%的文件。mkdir (dataFolder);disp (“正在下载scenario .zip (153 MB)…”) websave(zipFileName, baseDownloadURL, options);解压缩下载的文件解压缩(zipFileName dataFolder);elseiffolderExists && numel(dir(pointCloudFilePattern)) < numExpectedFiles如果数据在临时目录中减少，则重新下载。disp (“正在下载scenario .zip (153 MB)…”) websave(zipFileName, baseDownloadURL, options);解压缩下载的文件。解压缩(zipFileName dataFolder)结束
下载scenario .zip (155mb)…
使用helperReadDataset函数以格式从创建的文件夹中读取数据时间表．激光雷达捕获的点云以PNG图像文件的形式存储。中提取点云文件名的列表pointCloudTable变量。若要从映像文件读取点云数据，请使用helperReadPointCloudFromFile函数。此函数接受一个图像文件名并返回pointCloud对象。INS读数直接从配置文件读取并存储在insDataTable变量。
datasetttable = helperReadDataset(数据文件夹，pointCloudFilePattern);pointCloudTable = datasetttable (:， 1);insDataTable = datasetttable (:， 2:end);
读取第一个点云并在MATLAB®命令提示符中显示它
ptCloud = helperReadPointCloudFromFile(pointCloudTable.PointCloudFileName{1});disp (ptCloud)
pointCloud with properties: Location: [64×870×3 single] Count: 55680 XLimits: [-78.4980 77.7062] YLimits: [-76.8795 74.7502] ZLimits:[-2.4839 2.6836]颜色:[]正常:[]强度:[]
显示第一个INS读数。的时间表持有标题，球场，卷，X，Y,Z来自移民局的信息。
: disp (insDataTable (1))
时间戳向距辊X Y Z ____________________ _______ ________ _________ _______ _______ ______ 截止2010年6月13 06:27:31 1.9154 0.007438 -0.019888 -2889.1 -2183.9 116.47
可视化使用的点云pcplayer，流点云显示。车辆穿过由两个环路组成的路径。在第一个循环中，车辆进行一系列转弯并返回起点。在第二个循环中，车辆沿着另一条路线进行一系列转弯，然后再次返回起点。
指定玩家的限制Xlimits = [-45 45];%米Ylimits = [-45 45];Zlimits = [-10 20];创建流点云显示对象lidarPlayer = pcplayer(xlimits, ylimits, zlimits);%自定义玩家轴标签包含(lidarPlayer。轴,“X (m)”) ylabel (lidarPlayer。轴,“Y (m)”) zlabel (lidarPlayer。轴,“Z (m)”)标题(lidarPlayer。轴,“激光雷达传感器数据”）跳过每一帧，因为这是一个很长的序列skipFrames = 2;numFrames = height(pointCloudTable);为n = 1: skipFrames: numFrames读取点云fileName = pointCloudTable.PointCloudFileName{n};ptCloud = helperReadPointCloudFromFile(fileName);%可视化点云视图(lidarPlayer ptCloud);暂停(0.01)结束
使用里程计绘制地图
首先，使用中解释的方法从激光雷达数据构建地图构建地图的示例。该方法包括以下步骤:
对齐激光雷达扫描:使用点云配准技术对准连续的激光雷达扫描。这个例子使用了
pcregisterndt用于注册扫描。通过依次组合这些变换，每个点云被变换回第一个点云的参考系。合并对齐扫描:通过组合所有转换后的点云生成一个地图。
这种逐步构建地图并估计车辆轨迹的方法被称为测程法．
使用一个pcviewset对象跨多个视图存储和管理数据。视图集由一组相互连接的视图组成。
每个视图存储与单个视图相关的信息。这些信息包括视图的绝对姿态、在该视图中捕获的点云传感器数据以及视图的唯一标识符。将视图添加到视图集
addView．建立视图之间的连接使用
addConnection．连接存储的信息包括连接视图之间的相对转换、计算该测量所涉及的不确定性(表示为信息矩阵)以及相关的视图标识符。使用
情节方法以可视化由视图集建立的连接。这些连接可以用来可视化车辆所经过的路径。
隐藏(lidarPlayer)%随机设置种子，确保重现性rng (0);创建一个空视图集vSet = pcviewset;为视图集显示创建一个图形hFigBefore = figure(“名字”，“视图集显示”）;hAxBefore = axes(hFigBefore);初始化点云处理参数downsamplePercent = 0.1;regGridSize = 3;初始化转换absTform = rigid3d;到参考系的绝对变换relTform = rigid3d;%连续扫描之间的相对转换viewId = 1;skipFrames = 5;numFrames = height(pointCloudTable);displayRate = 100;每100帧更新显示一次为n = 1: skipFrames: numFrames%读点云fileName = pointCloudTable.PointCloudFileName{n};ptCloudOrig = helperReadPointCloudFromFile(fileName);%过程点云% -分割并移除接地平面分割并移除自我车ptCloud = helpprocesspointcloud (ptCloudOrig);对处理过的点云进行下采样ptCloud = pcdownsample(ptCloud，“随机”, downsamplePercent);firstFrame = (n==1);如果firstFrame将第一个点云扫描作为视图添加到视图集中vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);viewId = viewId + 1;ptCloudPrev = ptCloud;继续；结束使用INS估计注册的初始转换initTform = helpcomputeinitialestimatefromins (relTform，.．.insDataTable (n-skipFrames: n));计算注册当前点云的刚性变换前点云%relTform = pcregisterndt(ptCloud, ptCloudPrev, regGridSize，.．.“InitialTransform”, initTform);更新到参考系的绝对转换(第一个点云)absTform = rigid3d(relTform。T * absTform。T);将当前点云扫描作为视图添加到视图集中vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);将前一个视图中的连接添加到当前视图，表示它们之间的相对变换vSet = addConnection(vSet, viewId-1, viewId, relTform);viewId = viewId + 1;ptCloudPrev = ptCloud;initTform = relTform;如果n>1 && mod(n, displayRate) == 1 plot(vSet, displayRate)“父”, hAxBefore);drawnow更新结束结束
视图集对象vSet，现在可以保存视图和连接。在vSet的Views表中AbsolutePose变量指定了每个视图相对于第一个视图的绝对姿态。在连接表vSet,RelativePose变量指定连接视图之间的相对约束InformationMatrix变量为每条边指定与连接相关的不确定性。
显示前几个视图和连接头(vSet.Views)头(vSet.Connections)
ans = 8×3表ViewId AbsolutePose PointCloud ______ ____________ ______________ 1 1×1 rigid3d 1×1 pointCloud 2 1×1 rigid3d 1×1 pointCloud 3 1×1 rigid3d 1×1 pointCloud 4 1×1 rigid3d 1×1 pointCloud 5 1×1 rigid3d 1×1 pointCloud 6 1×1 rigid3d 1×1 pointCloud 7 1×1 rigid3d 1×1 pointCloud 8 1×1 rigid3d 1×1 pointCloud ans = 8×4表ViewId1 ViewId2 RelativePose InformationMatrix _______ _______ ____________ _________________ 1 2 1×1 rigid3d{6×6双}2 3 1×1 rigid3d{6×6双}3 4 1×1 rigid3d{6×6双}4 5 1×1 rigid3d{6×6双}5 6 1×1 rigid3d{6×6双}6 7 1×1 rigid3d双}{6×6 7 8 1×1 rigid3d{6×6双}8 9 1×1 rigid3d{6×6双}
现在，使用创建的视图集构建一个点云图。使用将视图绝对姿态与视图集中的点云对齐pcalign．指定一个网格大小来控制地图的分辨率。映射返回为pointCloud对象。
ptClouds = vSet.Views.PointCloud;absPoses = vSet.Views.AbsolutePose;mapGridSize = 0.2;ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize);
注意，使用这种方法遍历的路径会随着时间而漂移。虽然沿着第一个循环返回起点的路径似乎合理，但第二个循环从起点明显漂移。累积的漂移导致第二个环在距离起点几米远的地方终止。
仅用里程计绘制的地图是不准确的。显示已构建的点云图和遍历路径。注意，第二个循环的映射和遍历路径与第一个循环不一致。
持有(hAxBefore“上”）;pcshow (ptCloudMap);持有(hAxBefore“关闭”）;关上(hAxBefore.Parent)
使用姿态图优化修正漂移
图大满贯是一种广泛应用于测程中解决漂移的技术。图SLAM方法增量地创建一个图，其中节点对应于车辆姿态，边表示约束连接姿态的传感器测量值。这样的图称为a构成图．由于测量中的噪声或不准确，姿态图包含编码矛盾信息的边。然后对构造图中的节点进行优化，以找到最优解释测量结果的车辆姿态集。这种技术叫做姿态图优化．
要从视图集创建姿态图，可以使用createPoseGraph函数。这个函数为每个视图创建一个节点，并为视图集中的每个连接创建一条边。要优化姿态图，可以使用optimizePoseGraph(导航工具箱)函数。
图SLAM在纠正漂移方面的有效性的一个关键方面是对环路的准确检测，即以前访问过的地方。这叫做环路闭合检测或位置识别．在与环路闭包相对应的位姿图中添加边可以为连接节点位姿提供一个矛盾的测量，可以在位姿图优化过程中解决。
环路闭合可以使用描述符来检测，这些描述符描述了激光雷达传感器可见的本地环境。的扫描环境描述符[1]就是这样一个描述符，可以从点云使用scanContextDescriptor函数。本例使用scanContextLoopDetector对象来管理与每个视图对应的扫描上下文描述符。它使用detectLoop对象函数使用两阶段描述符搜索算法检测循环闭包。在第一阶段，它计算环键子描述符以找到潜在的循环候选。在第二阶段，它通过阈值扫描上下文距离将视图分类为循环闭包。
%随机设置种子，确保重现性rng (0);创建一个空视图集vSet = pcviewset;创建一个循环关闭检测器loopDetector = scanContextLoopDetector;为视图集显示创建一个图形hFigBefore = figure(“名字”，“视图集显示”）;hAxBefore = axes(hFigBefore);初始化转换absTform = rigid3d;到参考系的绝对变换relTform = rigid3d;%连续扫描之间的相对转换maxTolerableRMSE = 3;接受循环闭包候选的最大允许RMSEviewId = 1;为n = 1: skipFrames: numFrames%读点云fileName = pointCloudTable.PointCloudFileName{n};ptCloudOrig = helperReadPointCloudFromFile(fileName);%过程点云% -分割并移除接地平面分割并移除自我车ptCloud = helpprocesspointcloud (ptCloudOrig);对处理过的点云进行下采样ptCloud = pcdownsample(ptCloud，“随机”, downsamplePercent);firstFrame = (n==1);如果firstFrame将第一个点云扫描作为视图添加到视图集中vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);从第一个点云提取扫描上下文描述符描述符= scanContextDescriptor(ptCloudOrig);将第一个描述符添加到循环关闭检测器addDescriptor(loopDetector, viewId, descriptor) viewId = viewId + 1;ptCloudPrev = ptCloud;继续；结束使用INS估计注册的初始转换initTform = helpcomputeinitialestimatefromins (relTform，.．.insDataTable (n-skipFrames: n));计算注册当前点云的刚性变换前点云%relTform = pcregisterndt(ptCloud, ptCloudPrev, regGridSize，.．.“InitialTransform”, initTform);更新到参考系的绝对转换(第一个点云)absTform = rigid3d(relTform。T * absTform。T);将当前点云扫描作为视图添加到视图集中vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);从前一个视图添加连接到当前视图表示它们之间的相对变换vSet = addConnection(vSet, viewId-1, viewId, relTform);从点云中提取扫描上下文描述符描述符= scanContextDescriptor(ptCloudOrig);将描述符添加到循环关闭检测器添加描述符(loopDetector, viewId，描述符)检测循环关闭候选loopViewId = detectLoop(loopDetector);找到一个循环候选如果~isempty(loopViewId) loopViewId = loopViewId(1);从视图集中检索点云loopView = findView(vSet, loopViewId);ptCloudOrig = loopView.PointCloud;%过程点云ptCloudOld = helpprocesspointcloud (ptCloudOrig);下采样点云ptCloudOld = pcdownsample(ptCloudOld，“随机”, downsamplePercent);使用配准估计相对位姿[relTform， ~， rmse] = pcregisterndt(ptCloud, ptCloudOld，.．.regGridSize,“MaxIterations”, 50);acceptLoopClosure = rmse <= maxTolerableRMSE;如果acceptLoopClosure为简单起见，使用一个常量，小的信息矩阵%环闭合边infoMat = 0.01 * eye(6);添加一个与循环关闭相对应的连接vSet = addConnection(vSet, loopViewId, viewId, relTform, infoMat);结束结束viewId = viewId + 1;ptCloudPrev = ptCloud;initTform = relTform;如果n>1 && mod(n, displayRate) == 1 hG = plot(vSet, displayRate)“父”, hAxBefore);drawnow更新结束结束
方法从视图集中创建一个姿态图createPoseGraph方法。姿态图是a有向图对象:
包含每个视图的绝对姿态的节点
包含每个连接的相对姿态约束的边
G = createPoseGraph(vSet);disp (G)
有向图属性:边:[539×3 table]节点:[503×2 table]
除了连续视图之间的里程计连接外，视图集现在还包括环路闭合连接。例如，请注意第二个循环遍历和第一个循环遍历之间的新连接。这些是闭环连接。这些可以被识别为图中端点节点不连续的边。
更新轴限制以专注于环路闭合连接xlim(hAxBefore， [-50 50]);ylim(hAxBefore， [-100 50]);查找并突出显示循环闭合连接。loopEdgeIds = find(abs(diff(G.Edges. ids .)EndNodes, 1,2)) > 1);突出(hG,“边缘”loopEdgeIds,“EdgeColor”，“红色”，“线宽”3)
优化姿态图使用optimizePoseGraph．
optimG = optimizePoseGraph(G，“g2o-levenberg-marquardt”）;vSetOptim = updateView(vSet, optimG.Nodes);
显示具有优化姿势的视图集。注意，检测到的循环现在合并了，从而得到了更准确的轨迹。
情节(vSetOptim“父”hAxBefore)
优化视图集中的绝对姿态现在可以用来构建更精确的地图。使用pcalign函数将视图集点云与优化的视图集绝对姿态对齐为单个点云图。指定一个网格大小来控制创建的点云图的分辨率。
mapGridSize = 0.2;ptClouds = vSetOptim.Views.PointCloud;absPoses = vSetOptim.Views.AbsolutePose;ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize);hFigAfter = figure(“名字”，“视图集显示(优化后)”）;hAxAfter = axes(hFigAfter);pcshow (ptCloudMap“父”, hAxAfter);覆盖视图集显示持有在情节(vSetOptim“父”, hAxAfter);helperMakeFigurePublishFriendly (hFigAfter);
虽然精度仍然可以提高，但这个点云图明显更准确。
参考文献
G. Kim和A. Kim，“扫描上下文:三维点云图中位置识别的自我中心空间描述符”，2018 IEEE/RSJ智能机器人与系统国际会议(IROS)，马德里，2018，第4802-4809页。
金宝app支持函数和类
helperReadDataset将数据从指定的文件夹读入时间表。
函数datasetttable = helperReadDataset(数据文件夹，pointCloudFilePattern)%helperReadDataset将Velodyne SLAM Dataset数据读入时间表% datasetttable = helperReadDataset(dataFolder)从在dataFolder中指定的%文件夹放入时间表。这个函数%期望来自Velodyne SLAM数据集的数据。％参见fileDatastore, helperReadINSConfigFile。创建一个文件数据存储，以正确的顺序读入文件fileDS = fileDatastore(pointcloudfileppattern，“ReadFcn”，.．.@helperReadPointCloudFromFile);从数据存储中提取文件列表pointCloudFiles = files . files;imuConfigFile = fullfile(数据文件夹，“scenario1”，“imu.cfg”）;insDataTable = helperReadINSConfigFile(imuConfigFile);从INS配置文件中删除坏行insDataTable(1447，:) = [];删除不使用的列datasetttable = removevars(insDataTable，.．.｛“Num_Satellites”，“纬度”，“经”，“高度”，“Omega_Heading”，.．.“Omega_Pitch”，“Omega_Roll”，“V_X”，“V_Y”，“V_ZDown”});datasetttable = addvars(datasetttable, pointCloudFiles，“之前”, 1.．.“NewVariableNames”，“PointCloudFileName”）;结束
helperProcessPointCloud通过移除属于地平面和自我载体的点来处理点云。
函数ptCloud = helpprocesspointcloud (ptCloudIn，方法)进程pointCloud删除地面和自我车辆% ptCloud = helpprocesspointcloud (ptCloudIn, method)进程% ptCloudIn通过删除地平面和自我车辆。方法可以是“planefit”或“rangflood fill”。％参见pcfitplane, pointCloud/findNeighborsInRadius。参数ptCloudIn(1,1) pointCloud方法字符串{mustBeMember(方法，["planefit"，"rangefloodfill"])}＝“rangefloodfill”结束isOrganized = ~ismatrix(ptCloudIn.Location);如果(方法= =“rangefloodfill”& & isOrganized)在范围图像上使用泛洪填充的分段地面groundFixedIdx = segmentGroundFromLidarData(ptCloudIn，.．.“ElevationAngleDelta”11);其他的以参考法线分割地面为主导平面%指向正z方向的向量maxDistance = 0.4;maxAngularDistance = 5;referenceVector = [0 0 1];[~， groundFixedIdx] = pcfitplane(ptCloudIn, maxDistance，.．.referenceVector maxAngularDistance);结束如果isOrganized groundFixed = false(size(ptCloudIn.Location,1)，size(ptCloudIn.Location,2));其他的groundFixed = false(ptCloudIn。统计,1);结束groundFixed(groundFixedIdx) = true;在传感器的给定半径内，将自我车辆分割为点sensorLocation = [0 0 0];半径= 3.5;egoFixedIdx = findNeighborsInRadius(ptCloudIn, sensorLocation, radius);如果isOrganized egoFixed = false(size(ptCloudIn.Location,1)，size(ptCloudIn.Location,2));其他的egoFixed = false(ptCloudIn。统计,1);结束egoFixed(egoFixedIdx) = true;保留无地点云子集和自我载体。如果isOrganized indexes = ~groundFixed & ~egoFixed;其他的indices = find(~groundFixed & ~egoFixed);结束ptCloud = select(ptCloudIn, indexes);结束
helperComputeInitialEstimateFromINS估计从INS读数进行登记的初始转换。
函数initTform = helpcomputeinitialestimatefromins (initTform, insData)%如果没有可用的INS读数，返回如果isempty (insData)返回；结束INS的读数是X指向前方，Y指向左侧%和Z向上。下面的翻译说明了转换到激光雷达%框架。insToLidarOffset = [0 -0.79 -1.73];参见DATAFORMAT.txtTnow = [-insData.Y(end)， insData.X(end)， insData.Z(end)]。' + insToLidarOffset';Tbef = [-insData.Y (1) insData.X (1) insData.Z(1)]。' + insToLidarOffset';由于车辆预计将沿着地面移动，因此在滚转方面发生变化。%和pitch最小。忽略横摇和俯仰的变化，只使用航向。Rnow = rotmat(四元数([insData.Heading(end) 0 0]，“欧拉”，“ZYX股票”，“点”)，“点”）;Rbef = rotmat(四元数([insData.Heading(1) 0 0]，“欧拉”，“ZYX股票”，“点”)，“点”）;T = [Rbef Tbef;0 0 0 1] \ [Rnow Tnow;0 0 0 1];initTform = rigid3d(T.');结束
helperMakeFigurePublishFriendly调整数字，以便发布捕获的屏幕截图是正确的。
函数helperMakeFigurePublishFriendly (hFig)如果~isempty(hFig) && isvalid(hFig)HandleVisibility =“回调”；结束结束
另请参阅
功能
对象
相关的话题
外部网站
您也可以从以下列表中选择一个网站:
