使用三维激光雷达点云执行SLAM

这个例子使用了:

打开实时脚本

此示例演示如何实现同时定位和映射(SLAM)算法对采集的三维激光雷达传感器数据采用点云处理算法和位姿图优化。本例的目标是估计机器人的轨迹，并根据三维激光雷达点云和估计的轨迹创建环境的三维占用图。

所演示的SLAM算法使用基于正态分布变换(NDT)的点云配准算法估计轨迹，并在机器人重新访问某个地方时使用可信区域求解器使用SE3姿态图优化来减少漂移。

加载数据和设置可调参数

在停车场加载从Clearpath™Husky机器人收集的3-D激光雷达数据。激光雷达数据包含的单元格数组n-by-3矩阵，其中n是捕获的激光雷达数据中的数字3-D点，列表示xyz-与每个捕获点相关联的坐标。

负载pClouds.mat

点云配准算法参数

指定使用点云配准算法估计轨迹的参数。maxLidarRange三维激光扫描仪的最大扫描范围。

maxLidarRange = 20;

在室内环境中捕获的点云数据包含位于地面和天花板平面上的点，这使得点云配准算法混淆。用这些参数从点云中移除一些点:

referenceVector—垂直于地平面。
maxDistance-拆除地面和天花板平面时，衬垫的最大距离。
maxAngularDistance-拟合地面和天花板平面时与参考矢量的最大角度偏差。

referenceVector = [0 0 1];maxDistance = 0.5;maxAngularDistance = 15;

为提高配准算法的效率和精度，点云采用随机抽样的方式进行下采样randomSampleRatio．

randomSampleRatio = 0.25;

gridStep指定NDT配准算法中使用的体素网格大小。只有当机器人移动的距离大于distanceMovedThreshold．

gridStep = 2.5;distanceMovedThreshold = 0.3;

针对特定的机器人和环境调整这些参数。

闭环估计算法参数研究

指定环路闭合估计算法的参数。所指定的当前机器人位置周围的半径内只搜索循环闭包loopClosureSearchRadius．

loopClosureSearchRadius = 3;

环路闭合算法基于二维子映射和扫描匹配。在每个之后创建子映射nScansPerSubmap(每个子地图的扫描数)接受的扫描。闭环算法忽略了最近的subMapThresh在搜索循环候选时进行扫描。

nScansPerSubmap = 3;subMapThresh = 50;

带的环形区域z-由annularRegionLimits是从点云中提取的。点云平面拟合算法识别出感兴趣的区域后，在地板和天花板上这些限制之外的点被删除。

annularRegionLimits = [-0.75 0.75];

最大可接受的均方根误差(RMSE)估计的相对位姿之间的循环候选人指定rmseThreshold．选择一个较低的值来估计准确的环路闭合边，这对位姿图优化有很大影响。

rmseThreshold = 0.26;

所指定的接受循环关闭的扫描匹配分数的阈值loopClosureThreshold．插入后调用姿态图优化optimizationInterval将闭环边放入姿态图中。

loopClosureThreshold = 150;optimizationInterval = 2;

初始化变量

建立一个姿态图，占用图，和必要的变量。

% 3D姿态对象用于存储估计的相对姿态pGraph = poseGraph3D;默认序列化6 × 6信息矩阵的右上角三角形infoMat = [1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 1];自上次姿态图优化和地图优化以来增加的循环闭合边的百分比numLoopClosuresSinceLastOptimization = 0;% True在姿态图优化后，直到下一次扫描mapUpdated = false;如果扫描被接受，%等于1scanAccepted = 0;用于创建和可视化3D地图的占用网格对象mapResolution = 8;每米细胞百分比omap = occuancymap3d (mapResolution);

为处理过的点云、激光雷达扫描和子地图预分配变量。创建一个向下采样的点云集，以便快速可视化地图。

pcProcessed = cell(1,length(pClouds));lidarScans2d = cell(1,length(pClouds));submaps = cell(1,length(pClouds)/nScansPerSubmap);pcsToView = cell(1,length(pClouds));

创建用于显示的变量。

%设置为1以在构建过程中可视化创建的地图和姿态图viewMap = 1;%设置为1可在构建过程中可视化处理过的点云viewPC = 0;

设置随机种子，保证随机抽样的一致性。

rng (0);

如果需要，初始化图形窗口。

%如果您想在顺序处理点云时查看点云如果viewPC==1 pplayer = pcplayer([-50 50]，[-50 50]，[-10 10]，“MarkerSize”10);结束%如果你想在构建过程中查看创建的地图和姿态图如果viewMap==1 ax = newplot;图轴手柄视图(20、50);网格在；结束

基于位姿图优化的轨迹估计与优化

机器人的轨迹是机器人姿态(三维空间中的位置和方向)的集合。利用三维激光雷达SLAM算法对每个三维激光雷达扫描采集实例进行机器人姿态估计。三维激光雷达SLAM算法的步骤如下:

点云滤波
点云下采样
点云配准
环路闭合查询
姿态图优化

迭代处理点云来估计轨迹。

计数= 0;添加的扫描轨道数的%计数器disp (“估计机器人的轨迹……”）;

估计机器人轨迹…

为i = 1:长度(pClouds)按顺序读取点云pc = pClouds{i};

点云过滤

点云滤波是从获取的扫描中提取感兴趣的区域。在这个例子中，感兴趣的区域是去掉地面和天花板的环形区域。

去除最大范围外的无效点和机器人后面对应人类驾驶员的不必要点。

ind = (-maxLidarRange < pc(:，1) & pc(:，1) < maxLidarRange.．.& -maxLidarRange < pc(:，2) & pc(:，2) < maxLidarRange.．.& (abs(pc(:，2))>abs(0.5*pc(:，1)) | pc(:，1)>0));pcl = pointCloud(pc(ind，:));

移除地平面上的点。

[~， ~， outliers] =.．.pcfitplane (pcl、maxDistance referenceVector maxAngularDistance);Pcl_wogrd = select(pcl,outliers，“OutputSize”，“全部”）;

去除天花板平面上的点。

[~， ~， outliers] =.．.pcfitplane (pcl_wogrd, 0.2, referenceVector maxAngularDistance);Pcl_wogrd = select(Pcl_wogrd，异常值，“OutputSize”，“全部”）;

选择环形区域内的点。

ind = (pcl_wogrd.Location(:，3)annularRegionLimits(1));Pcl_wogrd = select(Pcl_wogrd,ind，“OutputSize”，“全部”）;

点云下采样

点云下采样提高了点云配准算法的速度和精度。下采样应针对特定需求进行调整。随机抽样算法是根据经验从下面的下抽样变量中选择的。

Pcl_wogrd_sampled = pcdownsample(pcl_wogrd，“随机”, randomSampleRatio);如果viewPC = = 1可视化下采样点云视图(pplayer pcl_wogrd_sampled);暂停(0.001)结束

点云配准

点云配准估计当前扫描和之前扫描之间的相对姿态(旋转和平移)。第一个扫描始终被接受(进一步处理并存储)，但其他扫描仅在翻译超过指定阈值后才被接受。poseGraph3D用于存储估计的可接受的相对姿态(轨迹)。

如果计数== 0%先可以Tform = [];scanAccepted = 1;其他的如果count == 1 tform = pcregisterndt(pcl_wogrd_sampled,prevPc,gridStep);其他的tform = pcregisterndt(pcl_wogrd_sampled,prevPc,gridStep，.．.“InitialTransform”, prevTform);结束relPose = [tform2trvec(tform.T') tform2quat(tform.T')];如果> distanceMovedThreshold addRelativePose(pGraph,relPose);scanAccepted = 1;其他的scanAccepted = 0;结束结束

环路闭合查询

循环闭包查询确定当前机器人位置之前是否被访问过。所指定的当前机器人位置周围的小半径内，通过将当前扫描与以前的扫描相匹配来执行搜索loopClosureSearchRadius．由于激光雷达测程的低漂移，在小半径内搜索就足够了，因为搜索所有之前的扫描都是耗时的。闭环查询由以下步骤组成:

创建子映射nScansPerSubmap连续扫描。
对象中的子映射与当前扫描匹配loopClosureSearchRadius．
如果匹配得分大于loopClosureThreshold．所有表示接受的子映射的扫描都被认为是可能的循环候选。
估计可能的循环候选和当前扫描之间的相对位置。只有当RMSE小于时，相对位姿才被接受为闭环约束rmseThreshold．

如果scanAccepted == 1 count = count + 1;pcProcessed{count} = pcl_wogrd_sampled;lidarScans2d{count} = exampleHelperCreate2DScan(pcl_wogrd_sampled);为更快的循环关闭查询创建子映射。如果rem(count,nScansPerSubmap)==0 submaps{count/nScansPerSubmap} = exampleHelperCreateSubmap(lidarScans2d，.．.计数,pGraph nScansPerSubmap maxLidarRange);结束% loopSubmapIds包含匹配的子映射id，否则为空。如果(floor(count/nScansPerSubmap)>subMapThresh) [loopSubmapIds，~] = examplehelperestimateloopcandidate (pGraph，.．.数、submap lidarScans2d {}, nScansPerSubmap,.．.loopClosureSearchRadius、loopClosureThreshold subMapThresh);如果~isempty(loopSubmapIds) rmseMin = inf;估计与当前扫描的最佳匹配为k = 1:length(loopSubmapIds)%用于子映射中的每次扫描为j = 1:nScansPerSubmap probableLoopCandidate =.．.loopSubmapIds(k)*nScansPerSubmap - j + 1;[loopTform，~，rmse] = pcregisterndt(pcl_wogrd_sampled，.．.pcProcessed {probableLoopCandidate}, gridStep);更新最佳循环关闭候选如果rmse < rmseMin loopCandidate = probabileloopcandidate;rmseMin = rmse;结束如果rmseMin < rmseThreshold打破；结束结束结束检查循环候选是否有效如果rmseMin < rmseThreshold%闭环约束relPose = [tform2trvec(loopTform.T') tform2quat(loopTform.T')];addRelativePose (pGraph relPose infoMat,.．.loopCandidate数);numLoopClosuresSinceLastOptimization = numLoopClosuresSinceLastOptimization + 1;结束结束结束

姿态图优化

姿态图优化在接受足够数量的环路边后进行，以减少轨迹估计中的漂移。每次闭环优化后，由于姿态估计的不确定性减小，闭环搜索半径减小。

如果(numLoopClosuresSinceLastOptimization == optimizationInterval)||.．.((numLoopClosuresSinceLastOptimization > 0) & & (i = =长度(pClouds)))如果loopClosureSearchRadius ~=1 disp(“进行姿态图优化以减少漂移。”）;结束%位姿图优化pGraph = optimizePoseGraph(pGraph);loopClosureSearchRadius = 1;如果viewMap == 1 position = pGraph.nodes;姿态图优化后重建地图omap = occuancymap3d (mapResolution);为n = 1:(pGraph.NumNodes-1) insertPointCloud(omap,position(n，:)，pcsToView{n}.removeInvalidPoints,maxLidarRange);结束mapUpdated = true;Ax = newplot;网格在；结束numLoopClosuresSinceLastOptimization = 0;减少优化后的优化频率%轨迹optimizationInterval = optimizationInterval*7;结束

在构建过程中可视化地图和姿态图。此可视化的开销很大，因此仅在必要时通过设置启用它viewMap为1。如果启用可视化，则每增加15次扫描后，绘图就会更新。

pcToView = pcdownsample(pcl_wogrd_sampled，“随机”, 0.5);pcsToView{count} = pcToView;如果viewMap = = 1将点云插入到占用图的正确位置position = pGraph.nodes(count);insertPointCloud (omap、位置、pcToView.removeInvalidPoints maxLidarRange);如果(rem(count-1,15)==0)||mapUpdated exampleHelperVisualizeMapAndPoseGraph(omap, pGraph, ax);结束mapUpdated = false;其他的%提供反馈，以知道示例正在运行如果(快速眼动(把1、15)= = 0)流(“。”）;结束结束

更新之前的相对姿态估计和点云。

prevPc = pcl_wogrd_sampled;prevTform = tform;结束结束

进行姿态图优化以减少漂移。

建立和可视化3-D占用地图

点云被插入occupancyMap3D使用估计的全局姿态。在遍历所有节点后，将显示完整的地图和估计的车辆轨迹。

如果(viewMap ~= 1)||(numLoopClosuresSinceLastOptimization>0) nodesPositions = nodes(pGraph);创建3D占用网格omapToView = occuancymap3d (mapResolution);为i = 1:(size(nodesPositions,1)-1) pc = pcsToView{i};position = nodesPositions(i，:);将点云插入到占用图的正确位置insertPointCloud (omapToView、位置、pc.removeInvalidPoints maxLidarRange);结束图;axisFinal = newplot;exampleHelperVisualizeMapAndPoseGraph(omapToView, pGraph, axisFinal);结束