适用于多地形的多传感器融合的2Dslam方法及其系统技术方案

本发明专利技术公开了一种适用于多地形的多传感器融合的2Dslam方法，通过读取激光雷达和编码器数据，解算编码器数据和处理雷达数据；扫描线特征分割点云，得到地面点云和非地面点云，并计算出坡度信息；依据坡度信息将三维非地面点云投影到二维；编码器和激光雷达二维点云粒子滤波融合。还公开了一种适用于多地形的多传感器融合的2Dslam系统。本发明专利技术所述方法适用于平坦、颠簸和斜坡地形，突破传统2Dslam适用场合，进一步扩展应用场景，融合多传感器提高了算法的定位和建图精度。

【技术实现步骤摘要】
适用于多地形的多传感器融合的2Dslam方法及其系统
本专利技术涉及定位导航与地图构建
，特别是涉及一种适用于多地形的多传感器融合的2Dslam方法及其系统。
技术介绍
SLAM(SimultaneousLocalizationAndMapping，即时定位与地图构建)主要用于解决移动设备在未知环境中运行时进行定位导航与地图构建的问题。要进行定位和建图，首先需要采集数据，目前大多数是采用编码器和激光雷达传感器来进行数据采集。然而，常规采用编码器解算位置和单线激光雷达检测障碍物的slam方法，仅适用于平坦地形。此外，颠簸和斜坡地形等复杂地形采用常规方法建图和定位，精度差。因此，复杂地形使用编码器和激光雷达同时建图与定位是非常值得研究的问题。
技术实现思路
本专利技术所要解决的技术问题是提供一种适用于多地形的多传感器融合的2Dslam方法及其系统，适用于平坦、颠簸和斜坡地形，突破传统2Dslam适用场合。为解决上述技术问题，本专利技术采用的一个技术方案是：提供一种适用于多地形的多传感器融合的2Dslam方法，包括以下步骤：S1：获取编码器脉冲值，依据脉冲值解算运动模型参数；读取激光雷达点云，网格均值降采样处理点云数据并根据运动模型去除点云畸变；S2:基于扫描线特征分割激光点云地面点和非地面点，计算地面点的坡度，根据坡度将非三维地面点投影为二维地面点；S3：根据上一时刻的粒子位姿以及编码器数据运动模型，估计得到当前时刻的粒子位姿；S4:依据处理后的二维激光雷达点云，以及上一时刻的粒子地图，采用梯度下降的方法扫描匹配优化粒子位姿，更新权重；S5:根据得到粒子的姿态参考当前激光雷达扫描数据对各个粒子的地图进行更新，将权重最高的粒子的地图作为当前地图。在本专利技术一个较佳实施例中，步骤S1的具体步骤包括：S1.1：获取编码器脉冲值，解算出自身运动速度V、角速度ω；S1.2：读取激光雷达探测范围所有三维点云存放于容器A，对A中点云采取长宽高均为d的网格，取每个网格内所有点云坐标(x,y,z)和时间戳的均值，将所有网格均值存放在容器B；S1.3：依据速度V和角速度ω的匀速运动模型，根据容器B中点云时间戳将坐标转换到t起始时刻，将转换后的点云存放在容器C。进一步的，所述d的取值范围为0.005—0.001m。在本专利技术一个较佳实施例中，步骤S2的具体步骤包括：S2.1：依据容器C中，假设相邻两扫面线同一射线角的点分别为(x1,y1,z1)和(x2,y2,z2)，倾斜角倾斜角小于10°认为是地面点，以此分割地面点和非地面点；S2.2：依据计算得到的地面点云扫描线间的相邻线段间距倾斜度α、绝对高度差H＝|z1-z2|，将非地面三维点(x,y,z)投影到二维坐标二维点云存放在容器D。在本专利技术一个较佳实施例中，步骤S4的具体步骤包括：S4.1：依据容器D中的激光点云和(t-1)时刻粒子地图采用梯度下降法扫描匹配，优化粒子位姿得到该粒子最优估计位姿S4.2：根据(t-1)时刻粒子权重粒子地图和t时刻的容器D的激光点云Zt、粒子位姿依据更新得到t时刻粒子权重在本专利技术一个较佳实施例中，步骤S5的具体步骤包括：S5.1：计算粒子的离散程度若Neff小于阈值T(取值为2/3n),则进行粒子重采样，否则执行步骤S5.2；S5.2：依据t时刻容器D中的激光点云和粒子位姿以及t-1时刻粒子地图更新得到t时刻粒子地图S5.3：判断数据是否读完，若未读完，则返回步骤S1；若数据处理结束，则遍历所有粒子，输出权重最大粒子地图作为当前地图。为解决上述技术问题，本专利技术采用的另一个技术方案是：提供一种适用于多地形的多传感器融合的2Dslam系统，主要包括：编码器数据模块，用于获取编码器脉冲值并依据脉冲值解算运动模型参数；激光雷达数据模块，用于读取激光雷达点云，采用网格均值降采样处理点云数据并根据运动模型去除点云畸变；激光点云分割模块，用于分割激光点云地面点和非地面点，计算地面点的坡度，根据坡度将非三维地面点投影为二维地面点；粒子位姿获取模块，用于根据上一时刻的粒子位姿以及编码器数据运动模型，估计得到当前时刻的粒子位姿；粒子位姿优化模块，用于依据所述激光雷达数据模块输出的二维激光雷达点云，以及上一时刻的粒子地图，采用梯度下降的方法扫描匹配优化粒子位姿，更新权重；粒子地图输出模块，用于根据所述粒子位姿优化模块得到粒子的姿态参考当前激光雷达扫描数据对各个粒子的地图进行更新，将权重最高的粒子的地图作为当前地图。。为解决上述技术问题，本专利技术还提供一种适用于多地形的多传感器融合的2Dslam设备，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的适用于多地形的多传感器融合的2Dslam方法程序；所述适用于多地形的多传感器融合的2Dslam方法程序被所述处理器执行时实现如上任一项所述的适用于多地形的多传感器融合的2Dslam方法的步骤。为解决上述技术问题，本专利技术还提供一种计算机介质，所述计算机介质上存储有适用于多地形的多传感器融合的2Dslam方法程序；所述适用于多地形的多传感器融合的2Dslam方法程序被所述处理器执行时实现如上任一项所述的适用于多地形的多传感器融合的2Dslam方法的步骤。本专利技术的有益效果是：本专利技术所述方法通过读取激光雷达和编码器数据，解算编码器数据和处理雷达数据；扫描线特征分割点云，得到地面点云和非地面点云，并计算出坡度信息；依据坡度信息将三维非地面点云投影到二维；编码器和激光雷达二维点云粒子滤波融合。本方法适用于平坦、颠簸和斜坡地形，突破传统2Dslam适用场合，进一步扩展应用场景，融合多传感器提高了算法的定位和建图精度。附图说明图1是本专利技术适用于多地形的多传感器融合的2Dslam方法的流程图；图2是所述适用于多地形的多传感器融合的2Dslam系统的结构框图。具体实施方式下面结合附图对本专利技术的较佳实施例进行详细阐述，以使本专利技术的优点和特征能更易于被本领域技术人员理解，从而对本专利技术的保护范围做出更为清楚明确的界定。请参阅图1，本专利技术实施例包括：一种适用于多地形的多传感器融合的2Dslam方法，包括以下步骤：S1：获取编码器脉冲值，依据脉冲值解算运动模型参数；读取激光雷达点云，网格均值降采样处理点云数据并根据运动模型去除点云畸变；具体步骤包括：S1.1：获取左右编码器脉冲值，解算出自身运动速度V、角速度ω；S1.2：读取激光雷达探测范围所有三维点云存放于容器A，对A中点云采取长宽高均为d的网格，取每个网格内所有点云坐标(x,y,z)和时间戳的均值，将所有网格均值存放在容器B；优选的，所述d的取值为0.005—0.001m；S1.3：依据速度V和角速度ω的匀速运动模型，根据容器B中点云时间戳将坐标转换到t起本文档来自技高网...

【技术保护点】
1.一种适用于多地形的多传感器融合的2Dslam方法，其特征在于，包括以下步骤：/nS1：获取编码器脉冲值，依据脉冲值解算运动模型参数；读取激光雷达点云，网格均值降采样处理点云数据并根据运动模型去除点云畸变；/nS2:基于扫描线特征分割激光点云地面点和非地面点，计算地面点的坡度，根据坡度将非三维地面点投影为二维地面点；/nS3：根据上一时刻的粒子位姿以及编码器数据运动模型，估计得到当前时刻的粒子位姿；/nS4:依据处理后的二维激光雷达点云，以及上一时刻的粒子地图，采用梯度下降的方法扫描匹配优化粒子位姿，更新权重；/nS5:根据得到粒子的姿态参考当前激光雷达扫描数据对各个粒子的地图进行更新，将权重最高的粒子的地图作为当前地图。/n

【技术特征摘要】
1.一种适用于多地形的多传感器融合的2Dslam方法，其特征在于，包括以下步骤：
S1：获取编码器脉冲值，依据脉冲值解算运动模型参数；读取激光雷达点云，网格均值降采样处理点云数据并根据运动模型去除点云畸变；
S2:基于扫描线特征分割激光点云地面点和非地面点，计算地面点的坡度，根据坡度将非三维地面点投影为二维地面点；
S3：根据上一时刻的粒子位姿以及编码器数据运动模型，估计得到当前时刻的粒子位姿；
S4:依据处理后的二维激光雷达点云，以及上一时刻的粒子地图，采用梯度下降的方法扫描匹配优化粒子位姿，更新权重；
S5:根据得到粒子的姿态参考当前激光雷达扫描数据对各个粒子的地图进行更新，将权重最高的粒子的地图作为当前地图。


2.根据权利要求1所述的适用于多地形的多传感器融合的2Dslam方法，其特征在于，步骤S1的具体步骤包括：
S1.1：获取编码器脉冲值，解算出自身运动速度V、角速度ω；
S1.2：读取激光雷达探测范围所有三维点云存放于容器A，对A中点云采取长宽高均为d的网格，取每个网格内所有点云坐标(x,y,z)和时间戳的均值，将所有网格均值存放在容器B；
S1.3：依据速度V和角速度ω的匀速运动模型，根据容器B中点云时间戳将坐标转换到t起始时刻，将转换后的点云存放在容器C。


3.根据权利要求2所述的适用于多地形的多传感器融合的2Dslam方法，其特征在于，所述d的取值范围为0.005—0.001m。


4.根据权利要求2所述的适用于多地形的多传感器融合的2Dslam方法，其特征在于，步骤S2的具体步骤包括：
S2.1：依据容器C中，假设相邻两扫面线同一射线角的点分别为(x1,y1,z1)和(x2,y2,z2)，倾斜角倾斜角小于10°认为是地面点，以此分割地面点和非地面点；
S2.2：依据计算得到的地面点云扫描线间的相邻线段间距倾斜度α、绝对高度差H＝|z1-z2|，将非地面三维点(x,y,z)投影到二维坐标二维点云存放在容器D。


5.根据权利要求4所述的适用于多地形的多传感器融合的2Dslam方法，其特征在于，步骤S4的具体步骤包括：
S4.1：依据容器D中的激光点云和(t-1)时刻粒子地图采用梯度下降法扫描匹配，优化粒子位姿得到该粒子最优估计位姿
S4.2...

【专利技术属性】
技术研发人员：江如海，王玉龙，袁胜，刘跃，丁骥，
申请(专利权)人：合肥中科智驰科技有限公司，
类型：发明
国别省市：安徽;34