使用SLAM从激光雷达数据建立地图
这个例子展示了如何处理安装在车辆上的传感器的3-D激光雷达数据，逐步建立地图，并使用同步定位和测绘(SLAM)估计车辆的轨迹。除了3-D激光雷达数据，惯性导航传感器(INS)也用于帮助构建地图。这种方法构建的地图可以方便车辆导航的路径规划，也可以用于定位。
概述
的用激光雷达数据构建地图示例使用3-D LIDAR数据和IMU读数逐步构建车辆遍历的环境的地图。虽然这种方法构建了局部一致的地图，但它仅适用于映射小区域。在更长的序列中，漂移累积成显着的错误。为了克服这种限制，该示例识别先前访问的位置，并尝试使用图形堆积方法来校正累积漂移。
加载和探索记录数据
在此示例中使用的数据是其中的一部分调速发电机大满贯数据集，表示近6分钟的记录数据。将数据下载到临时目录。
注意:此下载可能需要几分钟。
BrienceOdloadURL =.“https://www.mrt.kit.edu/z/publ/download/velodyneslam/data/scenario1.zip”；datafolder = fullfile（tempdir，“kit_velodyneslam_data_scenario1”, filesep);选择= weboptions ('暂停'、正);zipFileName = dataFolder +“scenario1.zip”；％获取“方案”文件夹中的PNG文件的完整文件路径。pointCloudFilePattern = fullfile (dataFolder,“scenario1”，“扫描* . png”）;numExpectedFiles = 2513;folderExists =存在(dataFolder,“dir”）;如果~ folderExists%在临时目录中创建一个文件夹来保存下载的zip文件%的文件。mkdir（Datafolder）;DISP（'下载方案1.zip（153 MB）......') websave(zipFileName, baseDownloadURL，选项);解压下载的文件解压缩(zipFileName dataFolder);eleesif.&& numel(dir(pointcloudfileppattern)) < numExpectedFiles%如果在临时目录中减少了数据，请重新下载。DISP（'下载方案1.zip（153 MB）......') websave(zipFileName, baseDownloadURL，选项);解压下载的文件。解压缩(zipFileName dataFolder)结束
下载scenario .zip (153mb)…
使用helperReadDataset函数从创建的文件夹中读取数据时间表．激光雷达捕捉到的点云以PNG图像文件的形式存储。的点云文件名列表pointCloudTable变量。要从图像文件读取点云数据，请使用helperReadPointCloudFromFile函数。这个函数接受一个图像文件名并返回pointCloud对象。直接从配置文件读取INS读数并存储在insDataTable变量。
datasetTable = helperReadDataset(dataFolder, pointcloudfileppattern);pointCloudTable = datasetTable(:， 1);insDataTable = datasetTable(:， 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] Color: [] Normal: [] Intensity: []
显示第一个INS读数。的时间表持有标题，球场，卷，X，Y,Z移民局的信息。
: disp (insDataTable (1))
时间戳标题节距滚X Y Z ^ ____________________ _______ ________ _________ _______ _______ ______ 6月13日2010 6时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读点云文件名= pointCloudTable.PointCloudFileName {n};ptCloud = helperReadPointCloudFromFile(文件名);%可视化点云视图(lidarPlayer ptCloud);暂停(0.01)结束
使用里程计绘制地图
首先，使用中解释的方法用激光雷达数据构建地图示例来构建一个地图。该方法包括以下步骤:
结合激光雷达扫描:使用点云配准技术对齐连续的激光雷达扫描。这个示例使用
pcregisterndt注册扫描。通过依次组合这些变换，每个点云被转换回第一个点云的参考系。结合对齐扫描:通过合并所有转换的点云生成一张地图。
这种增量构建地图和估计车辆轨迹的方法被称为测程法．
用一个PCViewSet.对象来存储和管理跨多个视图的数据。视图集由一组连接的视图组成。
每个视图都存储与单个视图相关联的信息。该信息包括视图的绝对姿势，该视图捕获的点云传感器数据，以及视图的唯一标识符。向查看视图添加视图
addView．建立视图之间的连接
addConnection．连接存储诸如连接视图之间的相对转换、计算此度量(表示为信息矩阵)和相关视图标识符所涉及的不确定性等信息。使用
情节方法可视化视图集建立的连接。这些连接可以用来可视化车辆所经过的路径。
隐藏(lidarPlayer)%设置随机种子，确保重现性rng (0);创建一个空视图集vSet = pcviewset;创建一个图形用于视图集显示hFigBefore =图(“名字”，“视图设置显示”）;hAxBefore =轴(hFigBefore);%初始化点云处理参数downsamplePercent = 0.1;regGridSize = 3;％初始化转换absTform = rigid3d;%对参考系的绝对变换relTform = rigid3d;%连续扫描之间的相对转换viewId = 1;skipFrames = 5;numFrames =身高(pointCloudTable);displayRate = 100;%每100帧更新一次为n = 1: skipFrames: numFrames读点云文件名= pointCloudTable.PointCloudFileName {n};ptCloudOrig = helperReadPointCloudFromFile(文件名);%过程点云% -切割并拆除接地面％ - 段和移除自我车辆ptCloud = helperProcessPointCloud (ptCloudOrig);%向下采样处理的点云ptCloud = pcdownsample (ptCloud,“随机”, downsamplePercent);firstFrame = (n = = 1);如果firstFrame％将第一点云扫描添加为视图集的视图vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);viewId = viewId + 1;ptCloudPrev = ptCloud;继续；结束%使用INS估计注册的初始转换inittform = HelperComputeInitialEstimateFromins（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，“父”, hAxBefore);drawnow更新结束结束
视图集对象vSet，现在拥有视图和连接。在Vset的视图表中，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 ViewId2RelativePose 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;assposs = vset.views.absolutepose;mapGridSize = 0.2;ptCloudMap = pcalign(ptClouds, abspses, mapGridSize);
请注意，使用此方法所遍历的路径会随时间变化。虽然沿着第一个循环回到起点的路径似乎是合理的，但第二个循环从起点开始漂移明显。累积的漂移导致第二圈在离起点几米远的地方结束。
仅用里程计绘制的地图是不准确的。显示构建的点云地图与遍历路径。注意，第二个循环的映射和遍历路径与第一个循环不一致。
持有(hAxBefore“上”）;pcshow（ptcloudmap）;持有(hAxBefore“关闭”）;关上(hAxBefore.Parent)
使用姿态图优化校正漂移
图大满贯在里程计中是一种广泛应用的解决漂移的方法。图SLAM方法以增量的方式创建一个图，其中节点对应于车辆的姿态，边缘代表约束连接姿态的传感器测量值。这样的图称为a构成图．由于测量中的噪声或不准确性，姿势图包含编码矛盾信息的边缘。然后优化构造图中的节点以找到最佳地解释测量的车辆姿势。这种技术被称为构成图优化．
要从视图集创建姿势图形，可以使用createPoseGraph函数。此函数为每个视图创建一个节点，以及视图集中的每个连接的边缘。要优化姿势图，可以使用optimizePoseGraph(导航工具箱)函数。
图SLAM在纠正漂移方面的有效性的一个关键方面是精确检测环路，即之前访问过的地方。这就是所谓的环路闭合检测或位置识别．在闭合环对应的位姿图中添加边，为被连接节点的位姿提供了一个矛盾测量方法，可以在位姿图优化过程中解决这个问题。
环路闭合可以使用描述符检测，描述符描述了激光雷达传感器可见的本地环境。的扫描上下文描述符[1]就是这样一个描述符，它可以使用scancontextdescriptor.函数。这个例子使用了scanContextLoopDetector对象来管理与每个视图对应的扫描上下文描述符。它使用detectLoop用两阶段描述符搜索算法检测循环闭包的目标函数。在第一阶段，它计算环密钥子描述符来寻找潜在的循环候选。在第二阶段，它通过设置扫描上下文距离的阈值将视图分类为循环闭包。
%设置随机种子，确保重现性rng (0);创建一个空视图集vSet = pcviewset;创建一个循环闭包检测器loopDetector = scanContextLoopDetector;创建一个图形用于视图集显示hFigBefore =图(“名字”，“视图设置显示”）;hAxBefore =轴(hFigBefore);％初始化转换absTform = rigid3d;%对参考系的绝对变换relTform = rigid3d;%连续扫描之间的相对转换maxTolerableRMSE = 3;允许接受一个循环闭包候选的最大RMSEviewId = 1;为n = 1: skipFrames: numFrames读点云文件名= pointCloudTable.PointCloudFileName {n};ptCloudOrig = helperReadPointCloudFromFile(文件名);%过程点云% -切割并拆除接地面％ - 段和移除自我车辆ptCloud = helperProcessPointCloud (ptCloudOrig);%向下采样处理的点云ptCloud = pcdownsample (ptCloud,“随机”, downsamplePercent);firstFrame = (n = = 1);如果firstFrame％将第一点云扫描添加为视图集的视图vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);%从第一个点云提取扫描上下文描述符描述符= scanContextDescriptor (ptCloudOrig);％将第一个描述符添加到环路闭合检测器addDescriptor（LoopDetector，查看区，描述符）查看id= viewid + 1;ptCloudPrev = ptCloud;继续；结束%使用INS估计注册的初始转换inittform = HelperComputeInitialEstimateFromins（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);将描述符添加到循环闭包检测器中addDescriptor (loopDetector viewId描述符)%检测候选循环闭包loopViewId = detectLoop (loopDetector);找到一个循环候选如果〜isempty（loopViewID）循环viewId = LoopViewID（1）;从视图集检索点云loopView = findView(vSet, loopViewId);ptCloudOrig = loopView.PointCloud;%过程点云ptCloudOld = helperProcessPointCloud (ptCloudOrig);下采样点云ptCloudOld = pcdownsample (ptCloudOld,“随机”, downsamplePercent);％使用注册来估计相对姿势[relTform， ~， rmse] = pcregisterndt(ptCloud, ptCloudOld，．..regGridSize,“发光”, 50);acceptLoopClosure = rmse <= maxTolerableRMSE;如果acceptLoopClosure为简便起见，使用一个常数，小的信息矩阵％环闭合边缘infoMat = 0.01 * eye(6);添加一个与循环闭包相对应的连接vset = addConnection（Vset，LoopViewID，查看id，Reltform，Infomat）;结束结束viewId = viewId + 1;ptCloudPrev = ptCloud;initTform = relTform;如果n>1 && mod(n, displayRate) == 1 hG = plot(vSet，“父”, hAxBefore);drawnow更新结束结束
通过使用使用的视图创建姿势图createPoseGraph方法。姿态图是a有向图对象:
节点包含每个视图的绝对姿态
包含每个连接的相对位姿约束的边
G = createPoseGraph (vSet);disp (G)
具有属性的有向图:Edges: [539×3 table] Nodes: [503×2 table]
除了连续视图之间的里程连接外，视图集现在还包括环路闭合连接。例如，注意第二个循环遍历和第一个循环遍历之间的新连接。这些是闭环连接。这些可以被识别为图中的边，其结束节点不是连续的。
%更新轴限制将重点放在循环闭包连接上xlim (hAxBefore 50 [-50]);ylim (hAxBefore 50 [-100]);找到并突出显示循环闭包连接loopEdgeIds =找到(abs (diff (G.Edges。EndNodes, 1, 2)) > 1);突出(hG,“边缘”loopEdgeIds,“EdgeColor”，“红色”，'行宽'3，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, abspses, mapGridSize);hFigAfter =图(“名字”，“视图设置显示(优化后)”）;hAxAfter =轴(hFigAfter);pcshow (ptCloudMap“父”, hAxAfter);％覆盖视图集显示显示持有在情节(vSetOptim“父”, hAxAfter);helperMakeFigurePublishFriendly (hFigAfter);
虽然精度仍然可以提高，但这个点云图明显更准确。
参考文献
G. Kim和A. Kim，“扫描上下文:三维点云地图中位置识别的自我中心空间描述符”，2018 IEEE/RSJ智能机器人与系统国际会议(IROS)，马德里，2018，pp。4802-4809。
金宝app支持函数和类
helperReadDataset从指定的文件夹中读取数据到时间表中。
函数datasetTable = helperReadDataset(dataFolder, pointcloudfileppattern)％HelperReadDataset将Velodyne Slam数据集数据读入时间段% datasetTable = helperReadDataset(dataFolder)读取数据将dataFolder中指定的%文件夹放入时间表中。这个函数%期望来自Velodyne SLAM数据集的数据。％％另见filedataStore，HelperReadinsConfigfile。%创建文件数据存储以按正确的顺序读取文件提起= fileDatastore (pointCloudFilePattern,“ReadFcn”，．..@helperReadPointCloudFromFile);%从数据存储中提取文件列表pointCloudFiles = fileDS.Files;imuConfigFile = fullfile (dataFolder,“scenario1”，'imu.cfg'）;insDataTable = helperReadINSConfigFile (imuConfigFile);%从INS配置文件中删除坏行indatatable (1447，:) = [];%删除不使用的列datasettable = removevars（InsdataTable，．..｛“Num_Satellites”，“纬度”，“经”，“高度”，'omega_heading'，．..“Omega_Pitch”，'omega_roll'，“V_X”，“V_Y”，“V_ZDown”}）;datasetTable = addvars(datasetTable, pointCloudFiles，'前'，1，．..“NewVariableNames”，“pointcloudfilename”）;结束
helperProcessPointCloud通过去除属于地平面和自我载体的点来处理点云。
函数ptcloud = helperprocesspointpointcloud（ptcloudin，方法）%helperProcessPointCloud处理pointCloud移除地面和自我车辆％ptcloud = helperprocesspointcloud（ptcloudin，方法）流程% ptCloudIn通过移除地平面和自我飞行器。%方法可以是“planefit”或“rangefloodfill”。％%参见pcfitplane, pointCloud/findNeighborsInRadius。参数ptCloudIn（1,1）POINTCLOUD方法String {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));其他的(ptCloudIn groundFixed = false。统计,1);结束groundFixed (groundFixedIdx) = true;在给定的传感器半径内，以点的形式分割自我车辆传感器位置= [0 0 0];半径= 3.5;egoFixedIdx = findNeighborsInRadius(ptCloudIn, sensorLocation, radius);如果isOrganized egoFixed = false(size(ptCloudIn.Location,1)，size(ptCloudIn.Location,2));其他的(ptCloudIn egoFixed = false。统计,1);结束egoFixed (egoFixedIdx) = true;%保留无地面和自我车辆的点云子集如果isOrganized indices = ~groundFixed & ~egoFixed;其他的索引= find(~groundFixed & ~egoFixed);结束ptCloud = select(ptCloudIn, indices);结束
helperComputeInitialEstimateFromINS估计从INS读数注册的初始转型。
函数initTform = helperComputeInitialEstimateFromINS(initTform, insData)%如果没有可用的INS读数，返回如果isempty (insData)返回；结束% INS读数X指向前面，Y指向左边%和Z向上。下面的翻译说明了转换为激光雷达%框架。instolidaroffset = [0 -0.79 -1.73];%见DATAFORMAT.txttnow = [-insdata.y（结束），insdata.x（结束），insdata.z（end）]。'+ instolidaroffset';tbef = [-insdata.y（1），insdata.x（1），insdata.z（1）]。'+ instolidaroffset';由于车辆预计在地面上移动，滚转的变化%和音高是最小的。忽略横摇和俯仰的变化，只使用航向。Rnow = rotmat(quaternion([insData.Heading(end) 0 0])，'euler'，“ZYX股票”，'观点'），'观点'）;Rbef = rotmat(四元数([insData.Heading(1) 0 0])，'euler'，“ZYX股票”，'观点'），'观点'）;T = [Rbef Tbef;0 0 0 1] \ [Rnow Tnow;0 0 0 1];initTform = rigid3d (T。');结束
helperMakeFigurePublishFriendly调整数字，以便发布捕获的屏幕截图是正确的。
函数helperMakeFigurePublishFriendly (hFig)如果〜Isempty（HFIG）&& IsValid（HFig）HFig.HandLevisibility =“回调”；结束结束
另请参阅
功能
对象
相关的话题
外部网站
你也可以从以下列表中选择一个网站:
