使用三维激光雷达点云执行SLAM
这个例子演示了如何实现同时定位和绘图(SLAM)算法对采集的三维激光雷达传感器数据进行点云处理和位姿图优化。本例的目标是估计机器人的轨迹，并根据3-D激光雷达点云和估计的轨迹创建环境的3-D占据地图。
演示的SLAM算法使用基于正态分布变换(NDT)的点云配准算法估计轨迹，并在机器人重新访问某个地方时使用信任区域求解器使用SE3位姿图优化减少漂移。
加载数据并设置可调参数
加载从停车场的Clearpath™Husky机器人收集的3-D激光雷达数据。激光雷达数据包含一个单元阵列n3矩阵,n在捕获的激光雷达数据中，数字和列代表三维点吗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 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;%在姿态图优化后直到下一次扫描时为真mapUpdated = false;如果接受扫描，%等于1scanAccepted = 0;% 3D占用网格对象，用于创建和可视化3D地图mapResolution = 8;每米细胞百分比omap = occupancyMap3D (mapResolution);
预分配处理过的点云、激光雷达扫描和子地图的变量。创建一个下采样点云集合，以快速可视化地图。
pcProcessed =细胞(1、长度(pClouds));lidarScans2d =细胞(1、长度(pClouds));submap =细胞(1、长度(pClouds) / nScansPerSubmap);pcsToView =细胞(1、长度(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算法的步骤如下:
点云滤波
点云将采样
点云注册
关闭循环查询
构成图优化
迭代处理点云来估计轨迹。
数= 0;跟踪添加的扫描次数的计数器disp (“估计机器人轨迹…”）;
估计机器人轨迹……
为i = 1:长度(pClouds)%按顺序读取点云我电脑= pClouds {};
点云滤波
点云滤波从获取的扫描图像中提取感兴趣的区域。在这个例子中，感兴趣的区域是去掉地面和天花板的环形区域。
去除最大范围外的无效点和机器人后面与人类驾驶员对应的不必要点。
maxLidarRange = (-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(印第安纳州,:));
拆除接地面上的点。
[~， ~， outliers] =．..pcfitplane (pcl、maxDistance referenceVector maxAngularDistance);pcl_wogrd =选择(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．所有表示接受子映射的扫描都被认为是可能的循环候选。估计可能的循环候选和当前扫描之间的相对姿态。相对位姿仅在RMSE小于时才被接受为闭环约束
rmseThreshold．
如果scanAccepted == 1 count = count + 1;pcProcessed{数}= pcl_wogrd_sampled;lidarScans2d{数}= exampleHelperCreate2DScan (pcl_wogrd_sampled);%创建子映射是为了更快的循环闭包查询。如果rem(count,nScansPerSubmap)==0 submap {count/nScansPerSubmap} = exampleHelperCreateSubmap(lidarScans2d，．..计数,pGraph nScansPerSubmap maxLidarRange);结束% loopSubmapIds包含匹配的子映射id，如果有其他空的子映射id。如果(floor(count/nScansPerSubmap)>subMapThresh) [loopSubmapIds，~] = exampleHelperEstimateLoopCandidates(pGraph，．..数、submap lidarScans2d {}, nScansPerSubmap,．..loopClosureSearchRadius、loopClosureThreshold subMapThresh);如果~isempty(loopSubmapIds) rmseMin = inf;估计与当前扫描的最佳匹配为k = 1:长度(loopSubmapIds)%用于子映射内的每次扫描为j = 1:nScansPerSubmap probableLoopCandidate =．..loopSubmapIds(k)*nScansPerSubmap - j + 1;[loopTform ~, rmse] = pcregisterndt (pcl_wogrd_sampled,．..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 = numLoopClosuresSinceLastOptimization + 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;网格在；结束numLoopClosuresSinceLastOptimization = 0;%优化后减少优化频率%的trjectoryoptimizationInterval = optimizationInterval * 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;结束结束
进行姿态图优化以减少漂移。
建立和可视化三维入住地图
点云被插入occupancyMap3D使用估计的全局姿态。通过对所有节点的迭代，可以得到完整的地图和估计的车辆轨迹。
如果(viewMap ~= 1)||(numLoopClosuresSinceLastOptimization>0) nodesPositions =节点(pGraph);%创建3D占用网格omapToView = occupancyMap3D (mapResolution);为i = 1:(size(nodesPositions,1)-1) pc = pcsToView{i};位置= nodesPositions(我:);%将点云插入到正确的位置insertPointCloud (omapToView、位置、pc.removeInvalidPoints maxLidarRange);结束图;axisFinal = newplot;exampleHelperVisualizeMapAndPoseGraph (omapToView pGraph axisFinal);结束
你也可以从以下列表中选择一个网站: