一种公开道路激光特征匹配定位方法、系统及存储介质

技术领域

本发明涉及无人驾驶技术领域，尤其是涉及一种公开道路激光特征匹配定位方法、系统及存储介质。

背景技术

正态分布变换Normal Distributions Transform(NDT)方法最早由Biber于2003年提出。该方法最早用于解决SLAM问题中，激光扫描数据的匹配问题。该方法的核心思路是通过建立数据基于概率密度的表示形式，构建一个对匹配的连续的评估函数，并且连续可微。这样，匹配问题就转换成了对一个连续函数的极值优化问题。

通过不断的比对实时扫描到的点云和已经建好的全局点云地图,我们就可以持续获得我们当前的位置。ICP(迭代最近点)等配准算法通过对所有的点或者提取的特征点进行匹配配准以确定当前的位置,但是这样就有一个问题:我们所处的环境是在不断变化的,比如树木的稀疏程度,或者环境中车辆及行人的移动,乃至固有的测量误差,这些都会导致我们实时扫描到的点云与已建立的点云地图有些许的差别,从而导致较大匹配误差。

而NDT可以在很大程序上消除这种不确定性。NDT没有计算两个点云中点与点之间的差距,而是先将参考点云(即高精度地图)转换为多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法,比如牛顿法,求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。NDT匹配定位广泛应用于自动驾驶车辆上，但其中存在点云地图过大，无法放在车载控制器中，只适用小规模场景，无法进行大场景定位，点云地图没有网格化和动态加载，影响匹配效率等问题。没有利用惯性传感器对激光雷达运动畸变进行处理，导致在高速和转弯情况下匹配误差较大，针对NDT匹配定位的一系列问题，进行了优化改进。

发明内容

鉴于以上现有技术的不足，本发明提供了一种公开道路激光特征匹配定位方法、系统及存储介质，不仅解决原始点云离线降采样和网格化问题和点云地图动态加载问题，而且解决点云动态配准问题。

为了实现上述目的及其他相关目的，本发明提供的技术方案如下：

一种公开道路激光特征匹配定位方法，包括以下步骤：

Q1：基于原始点云地图，采用点云预处理算法对原始点云地图进行预处理，再将预处理后的原始点云地图进行切割点云数据、点云拼接和降采样处理，输出原始点云数据集；

Q2：基于车载激光雷达，获取目标区域的实时点云数据，并对实时点云数据进行点云拼接、降采样处理和移除非固定特征点云，输出实时特征点云数据集；

Q3：基于所述原始点云数据集和所述实时特征点云数据集，根据NDT匹配算法，将所述原始点云数据集和所述实时特征点云数据集进行匹配和优化，输出实际点云数据集；

Q4：基于所述实际点云数据集，对IMU获取的角速度和加速度信息进行匹配和修正，对组合导航通过车辆位置、航向、速度和置信度信息得到的初始化位姿进行纠正，输出全局车辆位姿和置信度信息。

进一步的，在步骤Q1中，所述点云预处理算法包括：

Q11：基于所述原始点云地图，获取原始点云地图的点云数据信息，采用点云库PCL中的体素网格法对点云数据信息进行处理，在点云数据信息上创建一个三维体素网格；

Q12：基于所述三维体素网格，获取所述三维体素网格中的所有点，将所有点进行质心近似，输出预处理后的原始点云地图。

进一步的，在步骤Q3中，所述NDT匹配算法包括：

Q31：基于所述原始点云数据集，将空间划分成各个格子，在各个格子中输入所述原始点云数据集，计算每个格子的正态分布PDF参数；

Q32：基于所述实时特征点云数据集，通过转移变换对所述实时特征点云数据集中的点云数据进行变换，将变换后的点云数据输入到空间划分成各个格子中，进行匹配，输出匹配后的点云数据；

Q33：基于所述匹配后的点云数据，建立目标函数，

Q34：基于所述目标函数，对所述原始点云数据集和所述实时特征点云数据集的所有点云数据进行优化，输出实际点云数据集。

进一步的，所述的概率密度函数f为

进一步的，根据所述IMU获取的角速度和加速度信息，基于IMU积分预测激光雷达帧间位移，并采用scan to map对激光雷达帧间位移进行匹配和修正，结合所述实际点云数据集，得到车辆的位姿轨迹。

进一步的，基于组合导航的车辆位置、航向、速度和置信度信息，设置预设阀值，若组合导航结果高于预设阀值则表示初始化成功，输出车辆的初始化位姿。

为了实现上述目的及其他相关目的，本发明还提供了一种公开道路激光特征匹配定位系统，所述系统包括原始点云地图预处理模块，用于对原始点云地图进行预处理；

点云数据处理模块，用于对点云数据进行拼接、降采样和切割；

点云数据采集模块，用于实时获取路面的点云数据；

IMU，用于实时获取车辆的角速度和加速度；

组合导航模块，用于获取车辆的车辆位置、航向、速度和置信度信息；

数据处理模块，与所述点云数据处理模块、IMU和组合导航模块连接，用于处理车辆的各种信息。

进一步的，所述点云数据采集模块包括前激光雷达和后激光雷达。

进一步的，所述原始点云地图预处理模块、所述点云数据采集模块与所述点云数据处理模块连接，用于对点云数据进行切割、拼接、降采样和移除非固定特征点云。

为了实现上述目的及其他相关目的，本发明还提供了一种计算机可读存储介质，该计算机可读存储介质上存储有被编程或配置以执行任意一项所述公开道路激光特征匹配定位方法的计算机程序。

本发明具有以下积极效果：

1)本发明通过原始点云地图和原始点云地图预处理模块，解决了原始点云离线降采样和网格化问题。

2)本发明通过激光雷达和点云数据处理模块，可以实时获取路面点云数据并通过点云数据处理模块对点云进行拼接、降采样和移除非固定点云，解决了点云地图动态加载问题。

3)本发明通过IMU和组合导航模块，对目标点云数据进行匹配和校准，解决点云动态配准问题。

附图说明

图1为本发明方法流程示意图；

图2为本发明点云地图降采样与拼接的示意图；

图3为本发明原始点云地图示意图；

图4为本发明原始点云地图中单片地图示意图；

图5为本发明点云地图拼接和降采样后效果图；

图6为本发明激光雷达点云处理效果图；

图7为本发明NDT点云匹配效果；

图8为本发明NDT点云匹配精度效果。

具体实施方式

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

实施例1：如图1所示，一种公开道路激光特征匹配定位方法，包括以下步骤：

Q1：基于原始点云地图，采用点云预处理算法对原始点云地图进行预处理，再将预处理后的原始点云地图进行切割点云数据、点云拼接和降采样处理，输出原始点云数据集；

Q2：基于车载激光雷达，获取目标区域的实时点云数据，并对实时点云数据进行点云拼接、降采样处理和移除非固定特征点云，输出实时特征点云数据集；

Q3：基于所述原始点云数据集和所述实时特征点云数据集，根据NDT匹配算法，将所述原始点云数据集和所述实时特征点云数据集进行匹配和优化，输出实际点云数据集；

Q4：基于所述实际点云数据集，对IMU获取的角速度和加速度信息进行匹配和修正，对组合导航通过车辆位置、航向、速度和置信度信息得到的初始化位姿进行纠正，输出全局车辆位姿和置信度信息。

如图2、3、4或5所示，在步骤Q1中，所述点云预处理算法包括：

Q11：基于所述原始点云地图，获取原始点云地图的点云数据信息，采用点云库PCL中的体素网格法对点云数据信息进行处理，在点云数据信息上创建一个三维体素网格；

Q12：基于所述三维体素网格，获取所述三维体素网格中的所有点，将所有点进行质心近似，输出预处理后的原始点云地图。

其中，在步骤Q3中，所述NDT匹配算法包括：

Q31：基于所述原始点云数据集，将空间划分成各个格子，在各个格子中输入所述原始点云数据集，计算每个格子的正态分布PDF参数；

Q32：基于所述实时特征点云数据集，通过转移变换对所述实时特征点云数据集中的点云数据进行变换，将变换后的点云数据输入到空间划分成各个格子中，进行匹配，输出匹配后的点云数据；

Q33：基于所述匹配后的点云数据，建立目标函数，

Q34：基于所述目标函数，对所述原始点云数据集和所述实时特征点云数据集的所有点云数据进行优化，输出实际点云数据集。

具体地，如图7或图8所示，图7中给出了通过NDT算法进行点云数据匹配后的状态图，道路的形状和障碍物及其他物体都能够清楚的显示出来，图8为NDT点云匹配精度效果，可从图中可知本申请点云匹配精度高，如下表所示

实施例2：在实施例1的基础上的一种公开道路激光特征匹配定位系统，我们对本申请作进一步的描述，

本发明还提供了一种公开道路激光特征匹配定位系统，所述系统包括原始点云地图预处理模块，用于对原始点云地图进行预处理；

点云数据处理模块，用于对点云数据进行拼接、降采样和切割；

点云数据采集模块，用于实时获取路面的点云数据；

IMU，用于实时获取车辆的角速度和加速度；

组合导航模块，用于获取车辆的车辆位置、航向、速度和置信度信息；

数据处理模块，与所述点云数据处理模块、IMU和组合导航模块连接，用于处理车辆的各种信息。

其中，所述点云数据采集模块包括前激光雷达和后激光雷达。

其中，所述原始点云地图预处理模块、所述点云数据采集模块与所述点云数据处理模块连接，用于对点云数据进行切割、拼接、降采样和移除非固定特征点云。

为了实现上述目的及其他相关目的，本发明还提供了一种计算机可读存储介质，该计算机可读存储介质上存储有被编程或配置以执行任意一项所述公开道路激光特征匹配定位方法的计算机程序。

如图6所示，首先将从前后激光雷达获取的点云数据进行拼接操作，然后移除非固定特征点云数据，最后对点云数据进行降采样处理，得到静态特征点云；根据所述IMU获取的角速度和加速度信息，基于IMU积分预测激光雷达帧间位移，并采用scan to map对激光雷达帧间位移进行匹配和修正，结合所述实际点云数据集，得到车辆的位姿轨迹，基于组合导航的车辆位置、航向、速度和置信度信息，设置预设阀值，若组合导航结果高于预设阀值则表示初始化成功，输出车辆的初始化位姿。

综上所述，本发明不仅解决原始点云离线降采样和网格化问题和点云地图动态加载问题，而且解决点云动态配准问题。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。