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

这个例子使用:

打开实时脚本

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

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

加载数据和设置可调参数

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

负载pClouds.mat

点云配准算法参数

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

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 Posegraph对象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];%自上次姿态图优化和地图细化以来添加的闭环边数numloopcloseressincelastoptimization = 0;姿态图形优化后，直到下一次扫描时为TruemapUpdated = false;如果扫描被接受，%等于1scanAccepted = 0;% 3D占用网格对象，用于创建和可视化3D地图mapResolution = 8;每米细胞数%omap = occupancyMap3D(mapResolution);

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

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

创建用于显示的变量。

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

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

rng (0);

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

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

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

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

点云滤波
点云降采样
点云配准
循环闭包查询
位姿图优化

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

Count = 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,outliers，“OutputSize”，“全部”）;

在环形区域中选择点。

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

点云降采样

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

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

点云配准

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

如果Count == 0%第一罐form = [];scanAccepted = 1;其他的如果count == 1 tform = pcregisterndt(pcl_wogrd_sampling,prevPc,gridStep);其他的tform = pcregisterndt(pcl_wogrd_sampling,prevPc,gridStep，…“InitialTransform”, prevTform);结束relPose = [tform2trvec(tform.T') tform2quat(tform.T')];如果sqrt(norm(relPose(1:3))) > distanceMovedThreshold addRelativePose(pGraph,relPose);scanAccepted = 1;其他的scanAccepted = 0;结束结束

循环闭包查询

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

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

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

位姿图优化

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

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

在构建过程中可视化地图和姿态图。这种可视化是昂贵的，所以只在必要时通过设置启用它viewMap为1。如果启用了可视化，则每增加15次扫描后就更新一次绘图。

pcToView = pcdownsample(pcl_wogrd_sampling，“随机”, 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_sampling;prevTform = tform;结束结束

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

建立和可视化三维占用图

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

如果(viewMap ~= 1)||(numLoopClosuresSinceLastOptimization>) 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);结束