使用FPFH描述符的航空激光雷达SLAM
本例演示了如何利用三维特征实现航空测绘的同步定位与测绘(SLAM)算法。本例的目标是估计机器人的轨迹，并创建其环境的点云图。
本例中的SLAM算法通过在下采样的接受扫描之间寻找粗对齐来估计轨迹，使用在每个点提取的快速点特征直方图(FPFH)描述符，然后应用迭代最近点(ICP)算法微调对齐。导航工具箱™的三维姿态图优化，减少了估计轨迹的漂移。
创建和可视化数据
从OpenTopography网站下载的航空数据中创建合成激光雷达扫描[1]．加载一个mat文件，其中包含定义机器人轨迹的航空数据上的路径点序列。从每个航路点的数据计算激光雷达扫描helperCreateDataset函数。函数将在每个路径点计算的激光雷达扫描输出为的数组pointCloud物体，机器人覆盖的原始地图和地面真相路径点。
外部=“aerialMap.tar.gz”；wayPointsfile =“gTruthWayPoints.mat”；使用辅助功能在每个航路点生成一个激光雷达扫描。[pClouds, orgMap gTruthWayPts] = helperCreateDataset(丢失,wayPointsfile);
在机器人覆盖的原始地图上可视化地面真相路径点。
创建一个图形窗口来可视化地面真值图和路径点。hFigGT =图;axGT =轴(父= hFigGT颜色=“黑”）;形象化地面真理路径点pcshow (gTruthWayPts“红色”MarkerSize = 150,父母= axGT)在想象机器人覆盖的原始地图。pcshow (orgMap MarkerSize = 10,父= axGT)从%自定义轴标签轴在包含(axGT“X (m)”) ylabel (axGT“Y (m)”) zlabel (axGT“Z (m)”)标题(axGT“地面真相地图和机器人轨迹”）
可视化提取的激光雷达扫描使用pcplayer对象。
为玩家指定限制Xlimits = [-90 90];Ylimits = [-90 90];Zlimits = [-625 -587];创建一个pcplayer对象来可视化激光雷达扫描。lidarPlayer = pcplayer (xlimits ylimits zlimits);自定义pcplayer轴标签。包含(lidarPlayer。轴,“X (m)”) ylabel (lidarPlayer。轴,“Y (m)”) zlabel (lidarPlayer。轴,“Z (m)”)标题(lidarPlayer。轴,激光雷达扫描的）遍历并可视化数据为l = 1:长度(pClouds)提取激光雷达扫描ptCloud = pClouds (l);更新激光雷达显示视图(lidarPlayer ptCloud)暂停(0.05)结束
设置可调参数
指定轨迹和闭环估计的参数。针对特定的机器人和环境调整这些参数。
点云注册参数
指定要在接受注册的每个扫描之间跳过的激光雷达扫描的数量。由于连续扫描有很高的重叠，跳过几帧可以提高算法速度而不影响精度。
skipFrames = 3;
下采样激光雷达扫描可以提高算法速度。盒网格过滤器通过将每个单元格中的所有点平均为一个点对点云进行下采样。指定箱形网格过滤器的单个单元的大小，以米为单位。
gridStep = 1.5;%在米
为下采样点云中的每个有效点提取FPFH描述符。指定用于计算描述符的k近邻(KNN)搜索方法的邻域大小。
邻居= 60;
为匹配FPFH描述符设置阈值和比率，用于识别点对应关系。
matchThreshold = 0.1;matchRatio = 0.97;
为连续扫描之间的相对位姿估计设置最大距离和追踪数。
maxDistance = 1;maxNumTrails = 3000;
指定微调相对位姿时要考虑的内嵌器百分比。
inlierRatio = 0.1;
指定框网格过滤器的每个单元格的大小，用于通过对齐激光雷达扫描创建地图。
alignGridStep = 1.2;
闭环估计的参数
指定以当前机器人位置为中心的半径来搜索闭环候选机器人。
loopClosureSearchRadius = 7.9;
闭环算法基于三维子图的创建和匹配。子映射由指定数量的可接受扫描组成nScansPerSubmap．循环闭合算法还忽略最近扫描的指定数量subMapThresh，同时搜索循环候选项。指定均方根误差(RMSE)阈值loopClosureThreshold,接受子映射作为匹配。分数越低表示匹配越好，但根据传感器数据和预处理，分数会有所不同。
nScansPerSubmap = 3;subMapThresh = 15;loopClosureThreshold = 0.6;
指定可接受的最大均方根误差(RMSE)来估计环路候选之间的相对位姿rmseThreshold．选择一个较低的值可以得到更精确的闭环边缘，这对位姿图优化有很大的影响，但分数根据传感器数据和预处理的不同而不同。
rmseThreshold = 0.6;
初始化变量
创建一个姿势图，使用poseGraph3D(导航工具箱)对象，以存储接受的激光雷达扫描之间的估计相对姿态。
pGraph = poseGraph3D;6 × 6信息矩阵的默认序列化右上角三角形infoMat = [1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1];
预分配和初始化计算所需的变量。
分配内存存储子映射submap =细胞(地板(长度(pClouds) / (skipFrames * nScansPerSubmap)), 1);分配内存来存储每个子映射姿势subMapPoses = 0(圆(长度(pClouds) / (skipFrames * nScansPerSubmap)), 3);初始化变量以存储已接受的扫描及其转换submap创建百分比pcAccepted = pointCloud.empty (0);tformAccepted = rigidtform3d.empty (0);初始化变量来存储基于特征的方法的相对姿势%没有位姿图优化fpfhTform = rigidtform3d.empty (0);计数器来跟踪添加的扫描数数= 1;
创建变量可视化处理激光雷达扫描和估计的轨迹。
%设置为1可在构建过程中可视化处理过的激光雷达扫描viewPC = 0;创建一个pcplayer对象来查看激光雷达扫描。%按顺序处理它们，如果启用了viewPC如果viewPC == 1 pplayer = pcplayer(xlimits,ylimits,zlimits);自定义播放器轴标签。包含(pplayer。轴,“X (m)”) ylabel (pplayer。轴,“Y (m)”) zlabel (pplayer。轴,“Z (m)”)标题(pplayer。轴,“处理扫描”）结束创建一个图形窗口来可视化估计的轨迹。hFigTrajUpdate =图;axTrajUpdate =轴(父= hFigTrajUpdate颜色=“黑”）;标题(axTrajUpdate,“传感器构成的轨迹”）
轨迹估计与优化
机器人的轨迹是其姿态的集合，包括其在三维空间中的位置和方向。利用三维激光雷达SLAM算法从三维激光雷达扫描实例中估计机器人的姿态。使用这些过程执行3-D SLAM估计:
点云将采样
点云注册
Submap创建
关闭循环查询
构成图优化
迭代处理激光雷达扫描来估计机器人的轨迹。
rng (“默认”）设置随机种子以保证pcmatchfeatures的结果一致为FrameIdx = 1: skipFrames:长度(pClouds)
点云将采样
点云下采样可以提高点云配准算法的速度。下采样应该根据您的特定需求进行调整。
%对当前扫描进行下采样curScan = pcdownsample (pClouds (FrameIdx) gridAverage = gridStep);如果viewPC = = 1可视化下采样点云视图(pplayer curScan)结束
点云注册
点云配准估计当前扫描和前一次扫描之间的相对位姿。该示例使用以下步骤进行注册:
方法从每次扫描中提取FPFH描述符
extractFPFHFeatures函数方法比较描述符来标识点对应关系
pcmatchfeatures函数从点对应估计相对姿态使用
estgeotform3d函数微调相对姿势使用
pcregistericp函数
提取FPFH特征curFeature = extractFPFHFeatures (curScan NumNeighbors =邻居);如果FrameIdx = = 1更新验收扫描及其表格。pcAccepted(计数,1)= curScan;tformAccepted(计数,1)= rigidtform3d;更新初始姿态到地面真理的第一个路径点。%的比较fpfhTform(count,1) = rigidtform3d([0 0 0]，gTruthWayPts(1，:));其他的%通过匹配当前扫描与前一次扫描来识别通信indexPairs = pcmatchfeatures (curFeature prevFeature、curScan prevScan,.．.MatchThreshold = MatchThreshold RejectRatio = matchRatio);matchedPrevPts =选择(prevScan indexPairs (:, 2));matchedCurPts =选择(curScan indexPairs (: 1));估计当前扫描与前一次扫描之间的相对位姿%使用通讯tform1 = estgeotform3d (matchedCurPts。的位置,.．.matchedPrevPts。的位置,“刚性”MaxDistance = MaxDistance,.．.MaxNumTrials = maxNumTrails);执行ICP注册以微调相对姿势tform = pcregistericp (curScan、prevScan InitialTransform = tform1,.．.InlierRatio = InlierRatio);
把刚变换转化为anxyz -位置和四元数定位，以将其添加到姿态图中。
relPose = [tform2trvec(tform.A) tform2quat(tform.A)];%添加相对姿势到姿势图addRelativePose (pGraph relPose);
存储接受的扫描和它们的姿势，以创建子地图。
更新计数器并存储接受的扫描及其姿势。Count = Count + 1;pcAccepted(计数,1)= curScan;accumPose = pGraph.nodes(高度(pGraph.nodes));tformAccepted(count,1) = rigidtform3d((trvec2tform(伏位(1:3)))*.．.quat2tform (accumPose (4))));%更新估计姿势fpfhTform(数)= rigidtform3d (fpfhTform(把1)。* tform.A);结束
Submap创建
通过在指定大小的组中相互对齐顺序的、接受的扫描来创建子映射nScansPerSubmap,使用pcalign函数。使用子映射可以导致更快的循环关闭查询。
currSubMapId =地板(数/ nScansPerSubmap);如果快速眼动(计数,nScansPerSubmap) = = 0对齐激光雷达扫描数组以创建子映射。submap {currSubMapId} = pcalign (.．.pcAccepted((count - nScansPerSubmap + 1):count,1)，.．.tformAccepted((count - nScansPerSubmap + 1):count,1)，.．.alignGridStep);指定中心扫描位姿为子图位姿submappose (currSubMapId，:) = tformAccepted(count - . submapid.．.地板(nScansPerSubmap / 2), 1) .Translation;结束
关闭循环查询
使用一个关闭循环查询识别以前去过的地方。循环闭包查询包括查找当前扫描和以前的子映射之间的相似性。如果找到类似的扫描，则当前扫描是一个循环关闭候选扫描。循环闭合候选估计由以下步骤组成:
方法估计以前的子映射与当前扫描之间的匹配
helperEstimateLoopCandidates函数。如果当前扫描与子映射之间的RMSE小于的指定值，则子映射为匹配loopClosureThreshold。前面由所有匹配子映射表示的扫描是循环关闭候选。使用ICP配准算法计算当前扫描和环路闭合候选点之间的相对位姿。RMSE最小的循环关闭候选项是当前扫描的最佳可能匹配项。
只有当RMSE低于指定的阈值时，才接受一个循环关闭候选作为有效的循环关闭。
如果currSubMapId > subMapThresh mostRecentScanCenter = pGraph.nodes(height(pGraph.nodes));通过匹配电流来估计可能的闭环候选电路%扫描与子映射[loopSubmapIds, ~] = helperEstimateLoopCandidates (curScan submap,.．.subMapPoses、mostRecentScanCenter currSubMapId subMapThresh,.．.loopClosureSearchRadius loopClosureThreshold);如果~isempty(loopSubmapIds) rmseMin = inf;从匹配的子映射id中估计当前扫描的最佳匹配为k = 1:长度(loopSubmapIds)检查子映射中的每次扫描为kf = 1:nScansPerSubmap probableLoopCandidate =.．.loopSubmapIds(k)*nScansPerSubmap - kf + 1;[pose_Tform ~, rmse] = pcregistericp (curScan,.．.pcAccepted (probableLoopCandidate));更新最佳循环关闭候选如果rmse < rmseMin;matchingNode = probableLoopCandidate;构成= [tform2trvec (pose_Tform.A).．.tform2quat (pose_Tform.A)];结束结束结束检查循环关闭候选程序是否有效如果rmseMin < rmseThreshold将闭环候选对象的相对位姿添加到位姿图中addRelativePose (pGraph姿势,infoMat matchingNode,.．.pGraph.NumNodes);结束结束结束更新以前的点云和特性prevScan = curScan;prevFeature = curFeature;可视化从接受的扫描估计轨迹。显示(pGraph id =“关闭”父母= axTrajUpdate);drawnow结束
构成图优化
减小估计轨迹的漂移optimizePoseGraph(导航工具箱)函数。通常，位姿图中第一个节点的位姿代表原点。为了比较估计的轨迹与地面真相路径点，指定第一个地面真相路径点作为第一个节点的位姿。
pGraph = optimizePoseGraph(pGraph,FirstNodePose=[gTruthWayPts(1，:) 1 0 0 0]);
想象轨迹比较
使用没有位姿图优化的特征、有位姿图优化的特征和地面真实数据来可视化估计的轨迹。
%从姿态图中得到估计的轨迹pGraphTraj = pGraph.nodes;从基于特征的注册中获得估计的轨迹，而不需要姿态%图优化fpfhEstimatedTraj = 0(统计,3);为i = 1:count fpfhEstimatedTraj(i，:) = fpfhTform(i).Translation;结束创建一个图形窗口来可视化地面真相和估计。%的轨迹hFigTraj =图;axTraj =轴(父= hFigTraj颜色=“黑”）;plot3 (fpfhEstimatedTraj (: 1) fpfhEstimatedTraj (:, 2), fpfhEstimatedTraj (:, 3),.．.的r *、家长= axTraj)在plot3 (pGraphTraj (: 1) pGraphTraj (:, 2), pGraphTraj (:, 3),“b”。、家长= axTraj) plot3 (gTruthWayPts (: 1), gTruthWayPts (:, 2), gTruthWayPts (:, 3),“去”、家长= axTraj)从轴平等的视图(axTraj 2)包含(axTraj“X (m)”) ylabel (axTraj“Y (m)”) zlabel (axTraj“Z (m)”)标题(axTraj“轨迹比较”)传说(axTraj“没有位姿图优化的估计轨迹”，.．.“用姿态图优化估计轨迹”，.．.“地面实况轨迹”，“位置”，“bestoutside”）
构建和可视化生成的地图
利用估计的姿态转换和合并激光雷达扫描，以创建累积的点云图。
从姿势中得到估计的轨迹estimatedTraj = pGraphTraj (:, 1:3);将相对姿态转换为刚性变换。estimatedTforms = rigidtform3d.empty (0);为idx = 1: pGraph。NumNodes pose = pGraph.nodes(idx);rigidPose = rigidtform3d((trvec2tform(pose(1:3)) * quat2tform(pose(4:7))));estimatedTforms (idx, 1) = rigidPose;结束从处理过的点云和它们的相对姿态创建全局地图globalMap = pcalign (pcAccepted estimatedTforms alignGridStep);创建一个图形窗口来可视化估计的地图和轨迹。hFigTrajMap =图;axTrajMap =轴(父= hFigTrajMap颜色=“黑”）;pcshow (estimatedTraj“红色”MarkerSize = 150,父母= axTrajMap)在pcshow (globalMap MarkerSize = 10,父= axTrajMap)从%自定义轴标签轴在包含(axTrajMap“X (m)”) ylabel (axTrajMap“Y (m)”) zlabel (axTrajMap“Z (m)”)标题(axTrajMap“估计机器人轨迹并生成地图”）
将地面真值图和估计图可视化。
创建一个图形窗口来显示地面真值图和估计图。hFigMap = figure(Position=[0 0 700 400]);axMap1 =次要情节(1、2、1,颜色=“黑”父母= hFigMap);axMap1。位置= [0.08 0.2 0.4 0.55];pcshow (orgMap、家长= axMap1)轴在包含(axMap1“X (m)”) ylabel (axMap1“Y (m)”) zlabel (axMap1“Z (m)”)标题(axMap1“地面实况图”) axMap2 = subplot(1,2,2,Color=“黑”父母= hFigMap);axMap2。位置= [0.56 0.2 0.4 0.55];pcshow (globalMap、家长= axMap2)轴在包含(axMap2“X (m)”) ylabel (axMap2“Y (m)”) zlabel (axMap2“Z (m)”)标题(axMap2“估计地图”）
另请参阅
功能
readPointCloud|insertPointCloud(导航工具箱)|rayIntersection(导航工具箱)|addRelativePose(导航工具箱)|optimizePoseGraph(导航工具箱)|显示(导航工具箱)|extractFPFHFeatures|pcmatchfeatures|estgeotform3d|pcdownsample|pctransform|pcregistericp|pcalign|tform2trvec(导航工具箱)|tform2quat(导航工具箱)
对象
lasFileReader|pointCloud|pcplayer|occupancyMap3D(导航工具箱)|poseGraph3D(导航工具箱)|rigidtform3d
参考文献
[1]斯塔尔,斯科特。塔斯卡卢萨，美国:季节性洪水动态和无脊椎动物群落。国家机载激光测绘中心，2011年12月1日。OpenTopography (https://doi.org/10.5069/G9SF2T3K）.
您也可以从以下列表中选择网站: