一种面向港口自动驾驶的同时制图与定位专利检索-嵌入式系统电脑零配件专利检索查询-专利查询网

一种面向港口自动驾驶的同时制图与定位

阅读：855发布：2020-05-11

专利汇可以提供一种面向港口自动驾驶的同时制图与定位专利检索，专利查询，专利分析的服务。并且本发明公开了一种面向港口自动驾驶的同时制图与定位，包括三维激光SLAM、LeGO-LOAM建图与NDT-MATCH定位，所述三维激光SLAM包括多线激光雷达数据，所述LeGO-LOAM建图包括点云分割、点云特征提取、激光里程计与激光建图。本发明所述的一种面向港口自动驾驶的同时制图与定位，采用了三维激光SLAM，接收三维激光数据并结合IMU信息使得建图和定位算法更加鲁棒，建图方面主要在开源框架 LeGO-LOAM 基础上进行优化改进，定位方面主要采用了NDT-MATCH的方案，LeGO-LOAM是一个轻量化且经过地面优化的激光里程和建图方法，其在低功耗嵌入式系统上可以达到实时建图的效果，NDT-MATCH是主要的定位框架，具有算法对环境适应性强，定位稳定的特点，带来更好的使用前景。，下面是一种面向港口自动驾驶的同时制图与定位专利的具体信息内容。

权利要求

1.一种面向港口自动驾驶的同时制图与定位，包括三维激光SLAM、LeGO-LOAM建图与NDT-MATCH定位，其特征在于：所述三维激光SLAM包括多线激光雷达数据，所述LeGO-LOAM建图包括点云分割、点云特征提取、激光里程计与激光建图，所述NDT-MATCH定位包括点云地图、点云配准。
2.根据权利要求1所述的一种面向港口自动驾驶的同时制图与定位，其特征在于：所述点云分割是将点云分为地面和非地面数据，地面检测模型需要适应平面、坡道以及两者的过度过程，地面检测过的过程如下：segment(pi)＝首先在xy平面通过角度分辨率Δα和公式(1)、(2)将点云分为若干块；
xy平面根据公式(3)，将Ps分为若干块
每个块选取z轴最小值作为地面上的点，同时作为该块的代表点
对每个分块从近到远，从左到右，获得直线方程y＝mx+b；
地面上的点需要满足如下条件：
(1)直线的坡度m不能超过合理的阈值Tm；
(2)如果坡度m(3)P的直线拟合均方差不能超过一个合理的阈值TRMSE；
(4)第一个点到前一条直线的距离不能超过一个合理的阈值Tdpre；
然后对非地面数据进行聚类，采用基于深度图像的聚类方法；同一类别的点云数据会标记唯一的标签；通过滤除点云数量比较小的类别，提高处理速度，加快点云的特征提取；
所述点云特征提取为从聚类后的点云和地面点云中提取特征点云，首先设定S为连续的点云数据，利用点云的距离信息r，根据公式(4)计算点Pi在点击S的粗糙程度c；设S的点云数目|S|为10；
通过c与设定阈值cth的大小判定点云数据的特征类型；c大于cth则为边缘点，c小于cth则为平面点；S中边缘点c最大的点作为边缘特征点，同理平面点中c最小的作为平面特征点。
3.根据权利要求1所述的一种面向港口自动驾驶的同时制图与定位，其特征在于：所述激光里程计是通过相邻帧点云数据估计激光雷达的姿态变化；相邻帧的变换关系通过点边和点面匹配得到；第一步进行标签匹配，平面特征点只采用了地面上的平面特征点，用来做匹配，这样可以提高匹配的精度和速度，第二步两阶段的L-M优化，最优转换矩阵同过两步获得，首先ts，θroli，θpitch通过平面特征匹配获得，剩下的通过边缘特征匹配获得；值得注意的是，第一步的ts，θroli，θpitch可以作为第二步的输入优化匹配；
所述激光建图是对当前激光特征再次与前一时刻的地图进行匹配，通过L-M 算法优化转换关系；更新关键帧及关键帧姿态；当前地图即为到最新时刻为止全部关键帧的点云数据，通过关键帧姿态进行融合，一般情况下会降采样为稀疏点云地图。
4.根据权利要求1所述的一种面向港口自动驾驶的同时制图与定位，其特征在于：所述点云地图采用NDT可以用来紧凑的描述一个平面，首次提出时用来配准二维激光点云，之后被广泛应用在点云的配准和建图中；通过概率密度函数(PDFs)描述局部栅格平面的形状特性，首先将点云数据栅格化，然后计算每个栅格的概率密度函数，如公式(4)；
其中为栅格点云，D为点云的维度，和∑为均值和方差向量；
在二维和三维点云中，协方差矩阵的特征向量和特征值可以用来评估表面的取向和光滑度；
所述点云配准使用NDT进行，目标是找到和参考点云匹配度最高的点云的姿态；当前点云集合空间变换函数通过姿态移动点计算概率密度函数PDF
最优的姿态应该获得最大的相似概率；
或者最小的-log(Ψ)，

说明书全文

一种面向港口自动驾驶的同时制图与定位

技术领域

[0001] 本发明涉及自动驾驶的同时制图与定位领域，特别涉及一种面向港口自动驾驶的同时制图与定位。

背景技术

[0002] 移动机器人的自主导航问题可以分成三大块：定位，建图；定位就是精确确定机器人在环境中的位姿，建图就是将环境的多个不完整观测集成到单一且一致的模型中；最初，建图和定位是单独研究的，后来人们意识到他们是相互关联，相互依赖的，换句话说，为了得到环境中精确的定位需要正确无误的地图，但是，为了建立好的地图需要在新元素添加到地图时能够准确地被定位，这就是SLAM；SLAM是Simultaneous Localization and Mapping 的简写，中文译作“同时定位与地图构建”，它是指搭载特定传感器的主体，在没有环境先验的情况下，在运动过程中建立环境的模型(地图)，同时估计自己的运动。

[0003] 目前上市面主流的SLAM方案是基于激光和基于相机这两种，对这两种方案进行对比：应用场景：激光SLAM根据使用的激光雷达的档次泾渭分明分为室内应用和室外应用；而视觉SLAM在室内室外都有丰富的应用，视觉SLAM 工作稳定性不如激光雷达，激光雷达不擅长动态环境中的定位，比如动态遮挡；而视觉SLAM在纹理缺失环境(白墙壁等)，以及光照较弱的环境中表现较差，成本激光雷达成本整体都高于视觉传感器，为此，我们提出一种面向港口自动驾驶的同时制图与定位。

发明内容

[0004] 本发明的主要目的在于提供一种面向港口自动驾驶的同时制图与定位，可以有效解决背景技术中的问题。

[0005] 为实现上述目的，本发明采取的技术方案为：

[0006] 一种面向港口自动驾驶的同时制图与定位，包括三维激光SLAM、 LeGO-LOAM建图与NDT-MATCH定位，所述三维激光SLAM包括多线激光雷达数据，所述LeGO-LOAM建图包括点云分割、点云特征提取、激光里程计与激光建图，所述NDT-MATCH定位包括点云地图、点云配准。

[0007] 优选的，所述点云分割是将点云分为地面和非地面数据，地面检测模型需要适应平面、坡道以及两者的过度过程，地面检测过的过程如下：

[0008] 首先在xy平面通过角度分辨率Δa和公式(1)、(2)将点云分为若干块；

[0009]

[0010] Ps＝{pi∈P|segment(pi)＝s} (2)

[0011] xy平面根据公式(3)，将Ps分为若干块

[0012]

[0013] 每个块选取z轴最小值作为地面上的点，同时作为该块的代表点

[0014] 对每个分块从近到远，从左到右，获得直线方程y＝mx+b；

[0015] 地面上的点需要满足如下条件：

[0016] (1)直线的坡度m不能超过合理的阈值Tm；

[0017] (2)如果坡度m

[0018] (3)P的直线拟合均方差不能超过一个合理的阈值TRMSE；

[0019] (4)第一个点到前一条直线的距离不能超过一个合理的阈值Tdpre；

[0020] 然后对非地面数据进行聚类，采用基于深度图像的聚类方法；同一类别的点云数据会标记唯一的标签；通过滤除点云数量比较小的类别，提高处理速度，加快点云的特征提取；

[0021] 所述点云特征提取为从聚类后的点云和地面点云中提取特征点云，首先设定S为连续的点云数据，利用点云的距离信息r，根据公式(4)计算点Pi 在点击S的粗糙程度c；设S的点云数目|S|为10；

[0022]

[0023] 通过c与设定阈值cth的大小判定点云数据的特征类型；c大于cth则为边缘点，c小于cth则为平面点；S中边缘点c最大的点作为边缘特征点，同理平面点中c最小的作为平面特征点。

[0024] 优选的，所述激光里程计是通过相邻帧点云数据估计激光雷达的姿态变化；相邻帧的变换关系通过点边和点面匹配得到；第一步进行标签匹配，平面特征点只采用了地面上的平面特征点，用来做匹配，这样可以提高匹配的精度和速度，第二步两阶段的L-M优化，最优转换矩阵同过两步获得，首先tz，通过平面特征匹配获得，剩下的tx，ty，θyaw通过边缘特征匹配获得；值得注意的是，第一步的tz，θreli，θpirch可以作为第二步的输入优化匹配；

[0025] 所述激光建图是对当前激光特征再次与前一时刻的地图进行匹配，通过 L-M 算法优化转换关系；更新关键帧及关键帧姿态；当前地图即为到最新时刻为止全部关键帧的点云数据，通过关键帧姿态进行融合，一般情况下会降采样为稀疏点云地图。

[0026] 优选的，所述点云地图采用NDT可以用来紧凑的描述一个平面，首次提出时用来配准二维激光点云，之后被广泛应用在点云的配准和建图中；通过概率密度函数(PDFs)描述局部栅格平面的形状特性，首先将点云数据栅格化，然后计算每个栅格的概率密度函数，如公式(4)；

[0027]

[0028] 其中为栅格点云，D为点云的维度，和∑为均值和方差向量；

[0029] 在二维和三维点云中，协方差矩阵的特征向量和特征值可以用来评估表面的取向和光滑度；

[0030] 所述点云配准使用NDT进行，目标是找到和参考点云匹配度最高的点云的姿态；当前点云集合空间变换函数通过姿态移动点计算概率密度函数
最优的姿态应该获得最大的相似概率；

[0031]

[0032] 或者最小的-log(Ψ)，

[0033]

[0034] 与现有技术相比，本发明具有如下有益效果：该一种面向港口自动驾驶的同时制图与定位，采用了三维激光SLAM，接收三维激光数据并结合IMU信息使得建图和定位算法更加鲁棒，建图方面主要在开源框架LeGO-LOAM基础上进行优化改进，定位方面主要采用了NDT-MATCH的方案，LeGO-LOAM是一个轻量化且经过地面优化的激光里程和建图方法，其在低功耗嵌入式系统上可以达到实时建图的效果，NDT-MATCH是主要的定位框架，具有算法对环境适应性强，定位稳定的特点。附图说明

[0035] 图1为本发明一种面向港口自动驾驶的同时制图与定位中LeGO-LOAM建图程序系统图。

[0036] 图2为本发明一种面向港口自动驾驶的同时制图与定位中激光里程计程序示意图；

[0037] 图3为本发明一种面向港口自动驾驶的同时制图与定位中NDT-MATCH定位算法框架示意图；

[0038] 图4为传统的SLAM框架示意图；

[0039] 图5为本发明中xy平面通过角度分辨率和公式(1)、(2)将点云分为若干块示意图；

[0040] 图6为本发明中xy平面根据公式(3)，将Ps分为若干块示意图；

[0041] 图7为本发明中点云数据示意图；

[0042] 图8为本发明中在二维和三维点云中协方差矩阵的特征向量和特征值示意图。

具体实施方式

[0043] 为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

[0044] 如图1-3所示，一种面向港口自动驾驶的同时制图与定位，LeGO-LOAM是一个轻量化且经过地面优化的激光里程和建图方法，其在低功耗嵌入式系统上可以达到实时建图的效果；其首先通过点云分割，去除点云噪声；然后进行特征提取，获得平面和边缘特征；通过平面和边缘特征采用两步莱文贝格- 马夸特(Levenberg-Marquardt)算法优化获得激光时间域六自由度位姿转换信息；系统的框架如附图1所示；

[0045] 其中点云分割是将点云分为地面和非地面数据，地面检测模型需要适应平面、坡道以及两者的过度过程，地面检测过的过程如下：

[0046] 首先在xy平面通过角度分辨率Δa和公式(1)、(2)将点云分为若干块，如下图5所示；

[0047]

[0048] Ps＝{pi∈P|segment(pi)＝s} (2)

[0049] xy平面根据公式(3)，将Ps分为若干块如下图6所示；

[0050]

[0051] 每个块选取z轴最小值作为地面上的点，同时作为该块的代表点

[0052] 对每个分块从近到远，从左到右，获得直线方程y＝mx+b；

[0053] 地面上的点需要满足如下条件：

[0054] (1)直线的坡度m不能超过合理的阈值Tm；

[0055] (2)如果坡度m

[0056] (3)P的直线拟合均方差不能超过一个合理的阈值TRMSE；

[0057] (4)第一个点到前一条直线的距离不能超过一个合理的阈值Tdpre；

[0058] 然后对非地面数据进行聚类，采用基于深度图像的聚类方法；同一类别的点云数据会标记唯一的标签；通过滤除点云数量比较小的类别，提高处理速度，加快点云的特征提取；

[0059] 下图7中a为原始点云数据，b为聚类后的点云数据，c为提取的特征点云数据，蓝色和黄色分别代表边缘和平面特征点云；d为关键帧特征点云，绿色和粉色分别代表边缘和平面特征点云；

[0060] 点云特征提取为从聚类后的点云和地面点云中提取特征点云，首先设定S 为连续的点云数据，利用点云的距离信息r，根据公式(4)计算点Pi在点击S的粗糙程度c；设S的点云数目|S|为10；

[0061]

[0062] 通过c与设定阈值cth的大小判定点云数据的特征类型；c大于cth则为边缘点，c小于cth则为平面点；S中边缘点c最大的点作为边缘特征点，同理平面点中c最小的作为平面特征点。

[0063] 进一步的，激光里程计是通过相邻帧点云数据估计激光雷达的姿态变化；相邻帧的变换关系通过点边和点面匹配得到；第一步进行标签匹配，平面特征点只采用了地面上的平面特征点，用来做匹配，这样可以提高匹配的精度和速度，第二步两阶段的L-M优化，最优转换矩阵同过两步获得，首先tz，θroil， θptteh通过平面特征匹配获得，剩下的tx，ty，θyaw通过边缘特征匹配获得；值得注意的是，第一步的tz，θroli，θpitoh可以作为第二步的输入优化匹配，如附图2；

[0064] 激光建图是对当前激光特征再次与前一时刻的地图进行匹配，通过L-M 算法优化转换关系；更新关键帧及关键帧姿态；当前地图即为到最新时刻为止全部关键帧的点云数据，通过关键帧姿态进行融合，一般情况下会降采样为稀疏点云地图。

[0065] 进一步的，NDT-MATCH定位具有算法对环境适应性强，定位稳定的特点，其核心算法采用NDT(The normal-distributions transform)的配准方式，算法框架如附图3所示。

[0066] 优选的，点云地图采用NDT可以用来紧凑的描述一个平面，首次提出时用来配准二维激光点云，之后被广泛应用在点云的配准和建图中；通过概率密度函数(PDFs)描述局部栅格平面的形状特性，首先将点云数据栅格化，然后计算每个栅格的概率密度函数，如公式(4)；

[0067]

[0068] 其中为栅格点云，D为点云的维度，和∑为均值和方差向量；

[0069] 在二维和三维点云中，协方差矩阵的特征向量和特征值可以用来评估表面的取向和光滑度，下图8中，a中特征值大小相等，则表示球面；b中一个特征值远大于另外两个特征值则为直线；c中一个特征值远小于其他两个特征值则为平面；

[0070] 点云配准使用NDT进行，目标是找到和参考点云匹配度最高的点云的姿态；当前点云集合空间变换函数通过姿态移动点计算概率密度函数最优的姿态应该获得最大的相似概率；

[0071]

[0072] 或者最小的-log(Ψ)，

[0073]

[0074] 以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

标题	发布/更新时间	阅读量
基于AES算法和PUF技术的嵌入式系统加密保护装置和方法	2020-05-08	329
一种基于实时嵌入式系统的硬件在环仿真方法	2020-05-08	468
一种便携式超声影像智能处理系统及其辅助分析方法	2020-05-08	176
堆内存块查找方法、装置、设备及存储介质	2020-05-08	589
网络异常自恢复方法	2020-05-08	650
一种基于数据缓存扩充的嵌入式系统数据完整性保护方法	2020-05-12	469
基于FPGA-ARM嵌入式系统的R-LATs系统移动端信号处理方法	2020-05-08	174
基坑水平位移自动监测系统及方法	2020-05-08	750
人脸识别方法及系统	2020-05-11	891
多芯片集成功率管理解决方案	2020-05-08	905

一种面向港口自动驾驶的同时制图与定位

一种面向港口自动驾驶的同时制图与定位

技术领域

背景技术

发明内容

具体实施方式

该功能需要专业版企业版VIP权限，您可以：