使用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 = dataFolder +“scenario1.zip”；在scenario1文件夹中获取PNG文件的完整文件路径。pointCloudFilePattern = fullfile(dataFolder，“scenario1”，“扫描* . png”）;numExpectedFiles = 2513;folderExists = exists (dataFolder，“dir”）;如果~ folderExists在临时目录中创建一个文件夹来保存下载的压缩包。%的文件。mkdir (dataFolder);disp (“正在下载scenario .zip (153 MB)…”) websave(zipFileName, baseDownloadURL, options);%解压下载的文件解压缩(zipFileName dataFolder);elseiffolderExists && numel(dir(pointcloudfileppattern)) < numExpectedFiles如果数据在临时目录中减少，则重新下载。disp (“正在下载scenario .zip (153 MB)…”) websave(zipFileName, baseDownloadURL, options);%解压下载的文件。解压缩(zipFileName dataFolder)结束
下载scenario .zip (153 MB)…
使用helperReadDataset方法从创建的文件夹中读取数据时间表．激光雷达捕获的点云以PNG图像文件的形式存储。中的点云文件名列表pointCloudTable变量。要从映像文件读取点云数据，请使用helperReadPointCloudFromFile函数。这个函数接受一个图像文件名并返回一个pointCloud对象。INS读数直接从配置文件中读取并存储在insDataTable变量。
datasetttable = helperReadDataset(dataFolder, pointcloudfileppattern);pointCloudTable = datasetttable (:， 1);insDataTable = datasetttable (:， 2:end);
读取第一个点云，并在MATLAB®命令提示符中显示它
ptCloud = helperReadPointCloudFromFile(pointCloudTable.PointCloudFileName{1});disp (ptCloud)
pointCloud属性: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 ____________________ _______ ________ _________ _______ _______ ______ 截止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 = helperProcessPointCloud(ptcloudorigin);%对处理过的点云进行下采样ptCloud = pcdownsample(ptCloud，“随机”, downsamplePercent);firstFrame = (n==1);如果firstFrame%将第一个点云扫描作为视图添加到视图集vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);viewId = viewId + 1;ptCloudPrev = ptCloud;继续；结束使用INS估计注册的初始转换initTform = helperComputeInitialEstimateFromINS.．.insDataTable (n-skipFrames: n));计算注册当前点云的刚性变换%以前点云relTform = pcregisterndt(ptCloud, ptCloudPrev, regGridSize，.．.“InitialTransform”, initTform);%更新绝对转换到参考系(第一个点云)absTform = rigid3d;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的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;abposes = vSet.Views.AbsolutePose;mapGridSize = 0.2;ptCloudMap = pcalign(ptClouds, abposses, 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;连续扫描之间的相对转换maxtolerance ablermse = 3;%接受循环闭合候选项的最大允许RMSEviewId = 1;为n = 1: skipFrames: numFrames%读点云fileName = pointCloudTable.PointCloudFileName{n};ptCloudOrig = helperReadPointCloudFromFile(fileName);%流程点云% -分割并移除接地平面% -分割并移除自我载具ptCloud = helperProcessPointCloud(ptcloudorigin);%对处理过的点云进行下采样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 = helperComputeInitialEstimateFromINS.．.insDataTable (n-skipFrames: n));计算注册当前点云的刚性变换%以前点云relTform = pcregisterndt(ptCloud, ptCloudPrev, regGridSize，.．.“InitialTransform”, initTform);%更新绝对转换到参考系(第一个点云)absTform = rigid3d;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) loopViewId = loopViewId(1);从视图集中检索点云loopView = findView(vSet, loopViewId);ptCloudOrig = loopView.PointCloud;%流程点云ptCloudOld = helperProcessPointCloud(ptcloudorigin);%下采样点云ptCloudOld = pcdownsample(ptCloudOld，“随机”, downsamplePercent);使用配准估计相对位姿[relTform， ~， rmse] = pcregisterndt(ptCloud, ptCloudOld，.．.regGridSize,“MaxIterations”, 50);acceptLoopClosure = rmse <= maxtolerance ablermse;如果acceptLoopClosure为简单起见，使用恒定的小信息矩阵。%环闭合边infoMat = 0.01 *眼(6);添加一个闭环对应的连接vSet = addConnection(vSet, loopViewId, viewId, 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)
边:[539×3 table]节点:[503×2 table]
除了连续视图之间的里程数连接，视图集现在还包括环路闭合连接。例如，请注意第二个循环遍历和第一个循环遍历之间的新连接。这些是环路闭合连接。这些可以被识别为图中端点节点不连续的边。
更新轴限制，以关注循环闭合连接xlim(hAxBefore， [-50 50]);ylim(hAxBefore， [-100 50]);查找并突出显示循环闭包连接loopEdgeIds = find(abs(diff(g.h edgeids))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;abposes = vSetOptim.Views.AbsolutePose;ptCloudMap = pcalign(ptClouds, abposses, mapGridSize);hFigAfter = figure(“名字”，“视图集显示(优化后)”）;hAxAfter = axes(hFigAfter);pcshow (ptCloudMap“父”, hAxAfter);覆盖视图集显示持有在情节(vSetOptim“父”, hAxAfter);helperMakeFigurePublishFriendly (hFigAfter);
虽然精确度仍然可以提高，但这个点云图明显更加准确。
参考文献
Kim，“基于自中心空间描述子的三维点云地图位置识别”2018 IEEE/RSJ智能机器人与系统国际会议(IROS)， 2018, pp. 4802-4809。
金宝app辅助功能和类
helperReadDataset将数据从指定文件夹读入时间表。
函数datasetttable = helperReadDataset(dataFolder, pointcloudfileppattern)helperReadDataset将Velodyne SLAM数据集数据读取到时间表中% datasetttable = helperReadDataset(dataFolder)从将dataFolder中指定的%文件夹转换为时间表。这个函数%期望来自Velodyne SLAM数据集的数据。％%参见fileDatastore, helperReadINSConfigFile。创建一个文件数据存储，以正确的顺序读入文件fileDS = fileDatastore(pointcloudfileppattern，“ReadFcn”，.．.@helperReadPointCloudFromFile);从数据存储中提取文件列表pointCloudFiles = fileDS.Files;imuConfigFile = fullfile(dataFolder，“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”});datasetTable = addvars(datasetTable, pointCloudFiles，“之前”, 1.．.“NewVariableNames”，“PointCloudFileName”）;结束
helperProcessPointCloud通过删除属于地平面和自我载体的点来处理点云。
函数ptCloud = helperProcessPointCloud(ptCloudIn，方法)进程pointCloud删除地面和自我车辆% ptCloud = helperProcessPointCloud(ptCloudIn，方法)进程移除地平面和自我载体。方法可以是“planefit”或“rangefloodfill”。％%参见pcfitplane, pointCloud/findNeighborsInRadius。参数ptCloudIn(1,1) pointCloud方法string {mustBeMember(method， ["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;其他的索引= find(~groundFixed & ~egoFixed);结束ptCloud = select(ptCloudIn, indexes);结束
helperComputeInitialEstimateFromINS估计从INS读数进行注册的初始转换。
函数initTform = helperComputeInitialEstimateFromINS(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(quaternion([insData.Heading(end) 0 0]，“欧拉”，“ZYX股票”，“点”)，“点”）;Rbef = rotmat(quaternion([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)如果~是空的(hFig) &&是有效的(hFig)HandleVisibility =“回调”；结束结束
另请参阅
功能
对象
相关的话题
- 根据激光雷达数据构建地图
- 基于激光雷达的地面和障碍物检测(自动驾驶工具箱)
外部网站
您也可以从以下列表中选择网站:
