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

此示例使用：

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

演示的SLAM算法使用基于正态分布变换（NDT）的点云注册算法估计轨迹，并使用SE3位姿图优化（使用信任区域求解器）减少漂移，无论机器人何时重访某地。

加载数据并设置可调参数

加载从停车库中的Clearpath™Husky机器人采集的三维激光雷达数据。激光雷达数据包含n个-乘3矩阵，其中n个是捕获的激光雷达数据中的三维点数量，列表示xyz公司-与每个捕获点关联的坐标。

outputFolder=完整文件（tempdir，'停车场');数据URL=['https://ssd.mathworks.com/supportfiles/lidar/data/' ...
            “NatickParkingLotLidarData.tar”];pClouds=helperDownloadData（输出文件夹，数据URL）；

点云配准算法的参数

指定使用点云注册算法估计轨迹的参数。最大激光雷达范围指定三维激光扫描仪的最大范围。

最大激光雷达范围=20；

在室内环境中捕获的点云数据包含位于地面和天花板平面上的点，这混淆了点云注册算法。使用以下参数可以从点云中删除一些点：

引用向量-垂直于地平面。
最大距离-删除地面和天花板平面时内层的最大距离。
最大角度距离-拟合地面和天花板平面时与参考矢量的最大角度偏差。

referenceVector=[0 0 1]；最大距离=0.5；最大角度距离=15；

为了提高配准算法的效率和准确性，使用随机采样对点云进行降采样，采样率由随机采样比率.

randomSampleRatio=0.25；

gridStep（网格步）指定NDT注册算法中使用的体素栅格大小。只有当机器人移动距离大于距离移动阈值.

gridStep=2.5；distanceMovedThreshold=0.3；

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

环路闭合估计算法的参数

指定回路闭合估计算法的参数。仅在指定的当前机器人位置周围的半径范围内搜索循环闭包循环闭合搜索半径.

loopClosure搜索半径=3；

循环闭合算法基于二维子映射和扫描匹配。在每个nScansPerSubmap（每个子映射扫描）（每个子映射的扫描数）接受的扫描。循环闭包算法忽略最近的subMapThresh（子贴图阈值）在搜索循环候选者时进行扫描。

nScansPerSubmap=3；subMapThresh=50；

环形区域z（z）-由指定的限制年度区域限制是从点云中提取的。在点云平面拟合算法确定感兴趣区域后，将删除地板和天花板上超出这些限制的点。

环形区域界限=【-0.75 0.75】；

估计回路候选之间的相对姿态时可接受的最大均方根误差（RMSE）由以下公式规定rmse阈值。选择一个较低的值来估计精确的循环闭合边，这对姿势图优化有很大影响。

rmseThreshold=0.26；

接受循环闭包的扫描匹配分数阈值由以下内容指定循环关闭阈值.插入后调用Pose Graph Optimization优化间隔将闭合边循环到姿势图中。

环路闭合阈值=150；optimizationInterval=2；

初始化变量

设置姿势图、占用地图和必要的变量。

%用于存储估计相对姿势的3D Posegraph对象pGraph=poseGraph3D；%6×6信息矩阵的默认序列化右上三角信息材料=[1,0,0,0:0,0,1,0,0-0,01,0,00,1,0-0,1,0，0,1,1,0,1]；%自上次姿势图优化和贴图优化以来添加的循环闭合边数numLoopClosuresSinceLastOptimization=0；%姿势图优化后直到下次扫描时为TruemapUpdated=false；%如果扫描被接受，则等于1扫描接受=0；%用于创建和可视化3D地图的3D占用栅格对象mapResolution=8；%每米电池数omap=占用Map3D（mapResolution）；

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

pcProcessed=细胞（1，长度（pClouds））；lidarScans2d=细胞（1，长度（pClouds））；submaps=单元格（1，长度（pClouds）/nScansPerSubmap）；pcsToView=单元格（1，长度（pClouds））；

创建用于显示的变量。

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

设置随机种子以确保一致的随机采样。

rng（0）；

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

%如果要在按顺序处理点云的同时查看它们
如果viewPC==1pplayer=pcplayer（[-50 50]，[-50 50.]，[-10 10]，'标记大小',10);结束

%如果要在构建过程中查看创建的地图和posegraph
如果viewMap==1ax=新图；%图轴手柄视图（20,50）；网格在;结束

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

机器人的轨迹是机器人姿态的集合（三维空间中的位置和方向）。使用三维激光雷达SLAM算法，在每个三维激光雷达扫描采集实例上估计机器人姿态。三维激光雷达SLAM算法有以下步骤：

点云过滤
点云下采样
点云注册
循环结束查询
姿势图优化

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

计数=0；%用于跟踪添加扫描数的计数器显示('估计机器人轨迹…');

正在估计机器人轨迹。。。

对于i=1：长度（pClouds）%按顺序读取点云pc=pClouds{i}；

点云过滤

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

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

ind=（-maxLidarRange<pc（：，1）&pc（：...&-最大激光雷达范围<pc（：，2）和pc（：...&（abs（pc（：，2））>abs（0.5*pc（：；pcl=点云（pc（ind，：））；

删除地平面上的点。

[~，~，离群值]=...pcfitplane（pcl，maxDistance，referenceVector，maxAngularDistance）；pcl_wogrd=选择（pcl，离群值，'输出大小',“已满”);

删除天花板平面上的点。

[~，~，离群值]=...pcfitplane（pcl_wogrd，0.2，referenceVector，maxAngularDistance）；pcl_wogrd=选择（pcl_worgrd，离群值，'输出大小',“已满”);

选择环形区域中的点。

ind=（pcl_wogrd.Location（：，3）<annularRegionLimits（2））&（pcl_0ogrd.Location；pcl_wogrd=选择（pcl_worgrd，ind，'输出大小',“已满”);

点云下采样

点云下采样提高了点云注册算法的速度和准确性。应根据具体需要调整向下采样。对于当前场景，随机抽样算法是从下面的下采样变量中根据经验选择的。

pcl_wogrd_sampled=pc下采样（pcl_wgrd，“随机”，randomSampleRatio）；如果viewPC==1%可视化向下采样的点云视图（pplayer，pcl_wogrd_sampled）；暂停（0.001）结束

点云注册

点云注册估计当前扫描和上一次扫描之间的相对姿势（旋转和平移）。第一次扫描始终被接受（进一步处理并存储），但其他扫描只有在转换超过指定阈值后才被接受。位置图3D用于存储估计的可接受相对姿态（轨迹）。

    如果计数==0%第一个罐子t形式=[]；扫描验收=1；其他的
        如果计数==1tform=pcregisterndt（pcl_wogrd_sampled，prevPc，gridStep）；其他的tform=pcregisterndt（pcl_wogrd_sampled，prevPc，gridStep，...
                '初始转换'，prevTform）；结束relPose=[tform2rvec（tform.T'）tform2quat（tform.T'）]；如果sqrt（norm（relPose（1:3）））>距离移动阈值添加相对位置（pGraph，relPose）；扫描验收=1；其他的扫描接受=0；结束
    结束

循环结束查询

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

从创建子映射nScansPerSubmap（每个子映射扫描）连续扫描。
将当前扫描与循环闭合搜索半径.
如果比赛分数大于循环关闭阈值。表示接受子映射的所有扫描都被视为可能的循环候选。
估计可能的回路候选和当前扫描之间的相对姿势。只有当RMSE小于rmse阈值.

    如果扫描合格==1计数=计数+1；pcProcessed{count}=pcl_wogrd_sampled；lidarScans2d{count}=exampleHelperCreate2DScan（pcl_wogrd_sampled）；%创建子映射是为了更快地进行循环结束查询。
        如果rem（计数，nScansPerSubmap）==0submps{count/nScansPerSubmap}=exampleHelperCreateSubmap（lidarScans2d，...pGraph、count、nScansPerSubmap、maxLidarRange）；结束
        
        %loopSubmapId包含匹配的子映射id（如果有的话），否则为空。
        如果（楼层（计数/nScansPerSubmap）>subMapThresh）[loopSubmapIds，~]=exampleHelperEstimateLoopCandidates（pGraph，...count，submap，lidarScans2d{count}，nScansPerSubmap，...loopClosure SearchRadius，loopCloseThreshold，subMapThresh）；如果~isempty（loopSubmapIds）rmseMin=inf；%估计与当前扫描的最佳匹配
                对于k=1：长度（loopSubmapIds）%对于子映射中的每个扫描
                    对于j=1:nScansPerSubmap可能的循环候选=...loopSubmapIds（k）*nScansPerSubmap-j+1；[loopT形式，~，rmse]=pcregisterndt（pcl_wogrd_sampled，...pcProcessed{probableLoopCandidate}，gridStep）；%更新最佳循环结束候选
                        如果rmse<rmse最小值loopCandidate=probableLoopCandiate；rmseMin=rmse；结束
                        如果rmse最小值<rmse阈值打破;结束
                    结束
                结束
                
                %检查循环候选是否有效
                如果rmse最小值<rmse阈值%回路闭合约束relPose=[tform2rvec（loopTform.T'）tform2quat（loopT form.T`）]；添加相对位置（pGraph、relPose、infoMat、，...loopCandidate，计数）；numLoopClosuresSineLastOptimization=numLoop ClosuresIneLast最优化+1；结束
                     
            结束
        结束

姿势图优化

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

        如果（numLoopClosuresSinceLastOptimization==优化间隔）||...（（numLoopClosuresSinceLastOptimization>0）&&（i==长度（pClouds））如果loopClosure搜索半径~=1显示(“进行姿势图优化以减少漂移。”);结束
            %位姿图优化pGraph=优化PoseGraph（pGraph）；loopClosure搜索半径=1；如果viewMap==1位置=p图形节点；%姿势图优化后重建贴图omap=占用Map3D（mapResolution）；对于n=1：（pGraph.NumNodes-1）insertPointCloud（omap，位置（n，：），pcsToView{n}.removeInvalidPoints，maxLidarRange）；结束mapUpdated=true；ax=新图；网格在;结束numLoopClosuresSinceLastOptimization=0；%优化后降低优化频率
            %轨迹optimizationInterval=优化间隔*7；结束

在构建过程中可视化地图和姿势图。这种可视化成本很高，因此只有在必要时才能通过设置查看地图至1。如果启用了可视化，则每增加15次扫描，就会更新绘图。

pcToView=pcdownsample（pcl_wogrd_sampled，“随机”, 0.5);pcsToView{count}=pcToView；如果viewMap==1%将点云插入到占用地图的正确位置位置=pGraph.nodes（计数）；插入点云（omap，位置，pcToView.removeInvalidPoints，maxLidarRange）；如果（rem（count-1,15）==0）||map已更新示例HelperVisualizeMapAndPoseGraph（omap、pGraph、ax）；结束mapUpdated=false；其他的
            %提供反馈以了解示例正在运行
            如果（雷姆（计数-1,15）==0）fprintf（打印）('.');结束
        结束

更新以前的相对姿势估计和点云。

prevPc=pcl_wogrd_sampled；prevTform=tform；结束
结束

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

构建并可视化三维入住率地图

将点云插入到占用Map3D使用估计的全局姿势。迭代所有节点后，显示完整地图和估计的车辆轨迹。

如果（viewMap~=1）||（numLoopClosuresSinceLastOptimization>0）nodesPositions=节点（pGraph）；%创建三维占用栅格omapToView=占用Map3D（mapResolution）；对于i=1：（大小（节点位置，1）-1）pc=pcsToView{i}；位置=节点位置（i，：）；%将点云插入到占用地图的正确位置insertPointCloud（omapToView，位置，pc.removeInvalidPoints，maxLidarRange）；结束图形；axisFinal=新图；示例HelperVisualizeMapAndPoseGraph（omapToView、pGraph、axisFinal）；结束

实用程序功能

功能pClouds=helperDownloadData（输出文件夹，lidarURL）%将数据集从给定的URL下载到输出文件夹。lidarDataTarFile=完整文件（outputFolder，“NatickParkingLotLidarData.tar”);如果~存在（lidarDataTarFile，'文件')mkdir（输出文件夹）；显示('正在下载激光雷达数据（472 MB）…');网络保存（lidarDataTarFile，lidarURL）；untar（lidarDataTarFile，outputFolder）；结束    
    %解压缩文件。
    如果（~exist（完整文件（outputFolder，“NatickParkingLotLidarData”),“目录”))untar（lidarDataTarFile，outputFolder）；结束pClouds=加载（完整文件（outputFolder，“NatickParkingLotLidarData”,“NatickParkingLotLidarData.mat”)).pClouds（1:5：结束）；结束