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

这个示例使用:

打开直播脚本

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

演示的SLAM算法估计使用基于正态分布变换（NDT）的点云登记算法的轨迹，并且每当机器人重新留置一个地方时，使用信任区域解算器使用SE3姿态图优化来减少漂移。

加载数据并设置可调参数

在停车场中加载从Clearpath™Husky机器人收集到的3d激光雷达数据。激光雷达数据包含一个单元阵列n-B-3矩阵，在哪里n捕获的激光雷达数据中的3-D点的数字，以及列表示的是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;子行为= 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];自上次姿态图优化和地图细化后添加的循环闭包边的数量numLoopClosuresSinceLastOptimization = 0;% True在姿态图优化后直到下一次扫描mapUpdated = false;如果扫描被接受，%等于1scanAccepted = 0;％3D占用网格对象用于创建和可视化3D地图mapResolution = 8;每米电池百分比OMAP =占领ymap3d（MapResolution）;

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

pcProcessed =细胞(1、长度(pClouds));lidarScans2d =细胞(1、长度(pClouds));submap =细胞(1、长度(pClouds) / nScansPerSubmap);pcsToView =细胞(1、长度(pClouds));

为显示目的创建变量。

%设置为1，在构建过程中可视化创建的地图和海图ViewMap = 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)％按顺序读取点云我电脑= pClouds {};

点云滤波

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

删除与人类驱动程序对应的机器人后面的最大范围内的无效点和不必要的点。

ind = (-maxLidarRange < pc(:，1) & pc(:，1) < maxLidarRange．..＆-maxlidarrange ．..＆（ABS（PC（:,2））> ABS（0.5 * PC（：，1））| PC（：，1）> 0））;pcl = pointcloud（pc（ind，:)）;

移除接地面上的点。

[~， ~， outliers] =．..PCFITPLANE（PCL，MAXDISTANCE，参考矢量，MAXANGULARDISTANCE）;pcl_wogrd = select（pcl，异常值，“OutputSize”，'满的'）；

移除天花板上的点。

[~， ~， outliers] =．..pcfitplane (pcl_wogrd, 0.2, referenceVector maxAngularDistance);pcl_wogrd =选择(pcl_wogrd、离群值“OutputSize”，'满的'）；

选择环形区域内的点。

印第安纳州= (pcl_wogrd.Location (:, 3) < annularRegionLimits (2) & (pcl_wogrd.Location (:, 3) > annularRegionLimits (1));pcl_wogrd =选择(pcl_wogrd,印第安纳州,“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')];如果sqrt(norm(relPose(1:3))) > distanceMovedThreshold addRelativePose(pGraph,relPose);scanAccepted = 1;其他的scanAccepted = 0;结束结束

关闭循环查询

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

创建子映射nScansPerSubmap连续扫描。
控件中的子映射匹配当前扫描LoopClosuresearchRadius.．
如果匹配得分大于loopclosurethreshold.．所有表示可接受子映射的扫描都被认为是可能的循环候选。
估计可能的环路候选与当前扫描之间的相对姿态。只有当均方根误差小于时，才接受相对位姿作为闭环约束rmseThreshold．

如果scanAccepted == 1 count = count + 1;pcProcessed{数}= pcl_wogrd_sampled;lidarScans2d{数}= exampleHelperCreate2DScan (pcl_wogrd_sampled);为更快的循环关闭查询创建％子部分。如果rem(count,nScansPerSubmap)==0 submaps{count/nScansPerSubmap} = exampleHelperCreateSubmap(lidarScans2d, nScansPerSubmap);．..计数,pGraph nScansPerSubmap maxLidarRange);结束% loopSubmapIds包含匹配的子映射id(如果有其他空的)。如果(floor(count/nScansPerSubmap)>subMapThresh) [loopSubmapIds，~] = exampleHelperEstimateLoopCandidates(pGraph，．..数、submap lidarScans2d {}, nScansPerSubmap,．..loopClosureSearchRadius、loopClosureThreshold subMapThresh);如果~isempty(loopSubmapIds) rmseMin = inf;%估计与当前扫描的最佳匹配为k = 1：长度（围页覆盆子）%用于子映射中的每次扫描为j = 1:nScansPerSubmap probableeloopcandidate =．..loopSubmapIds(k)*nScansPerSubmap - j + 1;[loopTform ~, rmse] = pcregisterndt (pcl_wogrd_sampled,．..PCProcessed {probableLoopCandidate}，gridstep）;%更新最佳循环闭包候选如果rmse < rmseMin loopCandidate = probleloopcandidate;rmseMin = rmse;结束如果rmseMin < rmseThreshold休息；结束结束结束检查循环候选项是否有效如果rmseMin < rmseThreshold％循环关闭约束relPose = [tform2trvec(loopTform.T') tform2quat(loopTform.T')];addRelativePose (pGraph relPose infoMat,．..loopCandidate数);numLoopClosuresSinceLastOptimization = numLoopClosuresSinceLastOptimization + 1;结束结束结束

构成图优化

姿态图优化在接受足够数量的环路边缘后运行以减少轨迹估计的漂移。在每个环路闭合优化之后，由于姿势估计中的不确定性在优化之后减少了循环闭合搜索半径。

如果（numloopclosuressincelastoptimization ==优化interval）||．..（（numloopclosuressincelactoptimization> 0）&&（i ==长度（pclouds）））如果loopclosuresearchradius〜= 1 disp（"进行姿态图优化以减少漂移"）；结束位姿图优化Pgraph = OptimizePoseGraph（PGraph）;LoopClosuresearchRadius = 1;如果ViewMap == 1位置= pgraph.nodes;在姿态图优化后重建地图OMAP =占领ymap3d（MapResolution）;为n = 1:(pGraph.NumNodes-1) insertPointCloud(omap,position(n，:)，pcsToView{n}.removeInvalidPoints,maxLidarRange);结束mapUpdated = true;ax = newplot;网格上；结束numLoopClosuresSinceLastOptimization = 0;优化后％降低优化频率%的trjectory优化interval =优化interval * 7;结束

在构建过程中可视化地图和姿态图。这种可视化成本很高，所以只有在必要时才通过设置启用它viewmap.1.如果启用可视化，则每15个添加扫描后，绘图会更新。

pcToView = pcdownsample (pcl_wogrd_sampled,“随机”, 0.5);pcsToView{数}= pcToView;如果viewMap = = 1%在正确的位置插入点云到住户地图位置= pGraph.nodes(数);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）||（numloopclosuressincelastoptizimization> 0）NodeSpositions =节点（PGraph）;%创建3D占用网格omapToView = occupancyMap3D (mapResolution);为i = 1 :(大小（nodeSpositions，1）-1）pc = pcstoview {i};位置= nodeSpositions（i，:);%在正确的位置插入点云到住户地图insertPointCloud (omapToView、位置、pc.removeInvalidPoints maxLidarRange);结束数字;AxisFinal = Newplot;examplehelpervisualizemapand2appraph（omaproview，pgraph，axisfinal）;结束