具体实施方式

下面对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

图1为本发明一实施例中激光雷达定位系统的全局位置初始化方法流程图。

如图1所示，该实施例提供的激光雷达定位系统的全局位置初始化方法，可以包括如下步骤：

S100，加载全局地图及相关参数；

S200，根据相关参数，在全局地图中，切分局部网格子地图；

S300，在切分的每一个局部网格子地图中，生成随机估计的n个初始估计位置；

S400，对于每一个初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵；

S500，根据匹配程度得分和位置转换矩阵，对全局位置初始化进行确定；

S600，判断再次触发全局位置初始化情况，重新执行上述步骤，进行重定位。

在该实施例的S100中，作为一优选实施例，全局地图，可以为：包含全局坐标系的已知环境地图。

在该实施例的S100中，作为一优选实施例，全局地图的地图类型可以包括：pcd、ply、obj、stl以及pgm。

在该实施例的S100中，作为一优选实施例，相关参数，可以包括：网格子地图切分尺度参数、每张子地图中生成的随机位置个数、得分阈值参数以及重定位触发相关参数。

在该实施例的S200中，作为一优选实施例，根据相关参数，在全局地图中，切分局部网格子地图，可以包括如下步骤：

S201，根据相关参数中的网格子地图切分尺度参数，计算网格横纵个数及网格中心点坐标；

S202，根据网格中心点坐标，将全局地图分割成多个局部网格子地图。

在该实施例的S202中，作为一优选实施例，将得到的多个局部网格子地图存储在容器中。

在该实施例的S300中，作为一优选实施例，在切分的每一个局部网格子地图中，生成随机估计的n个初始估计位置，可以包括如下步骤：

遍历每一个局部网格子地图，以局部网格子地图的中心点、x轴和y轴为范围，生成n个系统随机估计的初始估计位置，得到对应的集合为P_guess{P_{guess_1},P_{guess_2},…,P_{guess_n}}。

在该实施例的S300中，作为一优选实施例，集合P_guess中得每一个初始估计位置，均可以包括x轴位置、y轴位置以及偏航角θ，记作P(x,y,θ)。

在该实施例的一具体应用实例中，x,y∈submap，θ∈(3.14，3.14]。

在该实施例的S400中，作为一优选实施例，对于每一个初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵，可以包括如下步骤：

遍历移动底盘的所有可能初始位置，在每一个可能初始位置处，以初始估计位置为计算初值，利用迭代最近点算法，计算可能初始位置处的匹配程度得分及其位置转换矩阵，分别得到对应的集合S{S₁,S₂,…,S_n}和P_correct{P_{correct_1},P_{correct_2},…,P_{correct_n}}。

在该实施例的S500中，作为一优选实施例，根据匹配程度得分和位置转换矩阵，对全局位置初始化进行确定，可以包括如下步骤：

遍历匹配程度得分，获取最小得分S_{correct_min}所对应的集合P_correct中的可能初始位置P_{correct_min}，若最小得分S_{correct_min}小于设定阈值a，则视为全局位置初始化成功，雷达定位系统直接开始定位。

在该实施例的一具体应用实例中，设定阈值a可以为：0.01

在该实施例的S600中，判断再次触发全局位置初始化情况，重新执行上述步骤，进行重定位，可以包括如下步骤：

对一个线程或一个进程进行实时监控定位，当发生如下任意一种或任意多种异常情况时，判断为再次触发全局位置初始化情况，进行重定位：

-两次定位结果异常跳变；

-两次定位结果与里程计或惯性测量单元数据相差过大；

-当前位置定位匹配程度得分大于设定阈值b。

在该实施例的一具体应用实例中，设定阈值b为：1.0

图2为本发明一优选实施例提供的激光雷达定位系统的全局位置初始化方法流程图。

如图2所示，该优选实施例提供的激光雷达定位系统的全局位置初始化方法，可以包括如下步骤：

步骤1：加载全局地图及相关参数；

步骤2：基于加载的全局地图及相关参数，切分局部网格子地图；

步骤3：在切分的每一个局部网格子地图中，生成随机估计的初始估计位置，得到该位置的集合P_guess{P_{guess_1},P_{guess_2},…,P_{guess_n}}；

步骤4：对于每一个初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵，得到相应的匹配程度得分集合S和转换后的初始估计位置集合P_correct；

步骤5：根据匹配程度得分和位置转换矩阵，进行全局位置最优估计；

步骤6：根据定位情况监控，自动重定位。

作为一优选实施例，步骤1包括如下步骤：

步骤1.1：加载全局地图，支持的地图类型包括pcd、ply、obj、stl、pgm等，为包含全局坐标系的已知环境地图，如图3所示；

步骤1.2：加载相关参数，包括：网格子地图切分尺度参数、每张子地图中生成的随机位置个数、得分阈值参数、重定位触发相关参数。

作为一优选实施例，如图4所示，步骤2包括以下步骤：

步骤2.1：根据网格子地图切分尺度参数计算网格横纵个数及网格中心点坐标。

步骤2.2：根据网格中心坐标，将全局地图分割成局部网格子地图，并储存在容器中。

作为一优选实施例，如图5所示，步骤3包括如下步骤：

遍历各局部网格子地图，以网格的中心点及网格的x、y轴为范围，生成n个系统随机估计的初始估计位置，得到相应的集合P_guess{P_{guess_1},P_{guess_2},…,P_{guess_n}}。

作为一优选实施例，对于集合P_guess中的每个初始估计位置，由三部分组成，包括：x轴位置、y轴位置及偏航角(yaw)θ，可以记作P(x,y,θ)。

图5中，随机初始位置估计即为随机估计的初始估计位置。

进一步地，x,y∈submap,θ∈(3.14，3.14]。

作为一优选实施例，如图6所示，步骤4包括如下步骤：

遍历移动底盘的所有可能初始位置，在每一个可能初始位置处，以初始估计位置为计算初值，利用迭代最近点算法，计算可能初始位置处的匹配程度得分及其位置转换矩阵，分别得到对应的集合S{S₁,S₂,…,S_n}和P_correct{P_{correct_1},P_{correct_2},…,P_{correct_n}}。

图6中，随机初始位置估计即为随机估计的初始估计位置。

作为一优选实施例，如图7所示，步骤5包括如下步骤：

遍历集合S，寻找匹配程度最大、得分最小S_{correct_min}对应的初始估计位置P_{correct_min}，其中，得分物理含义为匹配点云数据之间的偏差值之和，分数越小匹配程度越好，也就是说，得分代表最近邻点之间的距离，距离越小，概率越大；若S_{correct_min}小于设定阈值a，视为全局位置初始化成功，雷达定位系统可以直接开始定位。

进一步地，设定阈值a为：0.01。

图7中，随机初始位置估计即为随机估计的初始估计位置。

作为一优选实施例，步骤6包括如下步骤：

保证一个线程或进程实时监控定位结果，当发生以下任意一种或任意多种异常情况时，判断为再次触发全局位置初始化，进行系统的重定位。

异常情况包括：

-两次定位结果异常跳变；

-两次定位结果与里程计或惯性测量单元数据相差过大；

-当前位置定位匹配程度得分(概率过低)大于设定阈值b。

进一步地，设定阈值b为：1.0。

下面结合附图及一具体应用实例，对本发明上述实施例所提供的技术方案进行进一步的详细说明。

在该具体应用实例中，利用C++中std标准容器存储根据参数切割生成的局部网格子地图、随机初始估计位置、匹配程度得分等数据结构。

使用webots、gazebo等机器人仿真软件，模仿雷达数据及环境地图，对本发明上述实施例所提供的方法进行验证，可使用pcl点云库自带的可视化类进行动态的结果显示，方法实例如图8所示。

图9为该具体应用实例中可视化界面示意图，如图9所示，包括雷达原始数据、环境地图、当前子地图以及匹配结果这些方法关键要素的可视化示意。

本发明另一实施例，提供了一种激光雷达定位系统的全局位置初始化系统，如图10所示，可以包括如下模块：

加载模块、网格子地图切分模块、初始估计位置生成模块、匹配得分及位置转换矩阵生成模块、全局位置初始化模块以及重定位模块。

其中：

加载模块，该模块用于加载全局地图及相关参数；

网格子地图切分模块，该模块根据加载模块加载的相关参数，在加载模块加载的全局地图中，切分局部网格子地图；

初始估计位置生成模块，该模块在网格子地图切分模块切分的每一个局部网格子地图中，生成随机估计的n个初始估计位置；

匹配得分及位置转换矩阵生成模块：该模块对于初始估计位置生成模块生成的每一个初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵；

全局位置初始化模块，该模块根据匹配得分及位置转换矩阵生成模块生成的匹配程度得分和位置转换矩阵，对全局位置初始化进行确定；

重定位模块，该模块用于判断再次触发全局位置初始化情况，进行重定位。

本发明第三个实施例提供了一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时可用于执行本发明上述实施例中任一项的方法。

可选地，存储器，用于存储程序；存储器，可以包括易失性存储器(英文：volatilememory)，例如随机存取存储器(英文：random-access memory，缩写：RAM)，如静态随机存取存储器(英文：static random-access memory，缩写：SRAM)，双倍数据率同步动态随机存取存储器(英文：Double Data Rate Synchronous Dynamic Random Access Memory，缩写：DDR SDRAM)等；存储器也可以包括非易失性存储器(英文：non-volatile memory)，例如快闪存储器(英文：flash memory)。存储器用于存储计算机程序(如实现上述方法的应用程序、功能模块等)、计算机指令等，上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。

上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。

处理器，用于执行存储器存储的计算机程序，以实现上述实施例涉及的方法中的各个步骤。具体可以参见前面方法实施例中的相关描述。

处理器和存储器可以是独立结构，也可以是集成在一起的集成结构。当处理器和存储器是独立结构时，存储器、处理器可以通过总线耦合连接。

本发明第四个实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可用于执行本发明上述实施例中任一项的方法。

本发明上述实施例提供的激光雷达定位系统的全局位置初始化方法及系统、终端、存储介质，自动进行全局位置初始化，避免了人工输入初始位置的繁琐操作；当定位丢失时，自动进行全局位置重定位，避免人工介入；实现无人干预的自主移动底盘，以减少人工成本；去除了移动底盘在开始作业之前，根据环境地图，人工推测初始位置并设置这一环节，在简化了工作流程的基础上，能够带来更为精准的定位效果；实现移动底盘的无人操作自启动；实现移动底盘的自动丢失重定位。

需要说明的是，本发明提供的方法中的步骤，可以利用系统中对应的模块、装置、单元等予以实现，本领域技术人员可以参照方法的技术方案实现系统的组成，即，方法中的实施例可理解为构建系统的优选例，在此不予赘述。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。