一种激光雷达定位系统的全局位置初始化方法及系统与流程

1.本发明涉及智能机器人技术领域，具体地，涉及一种激光雷达定位系统的全局位置初始化方法及系统，同时提供了一种相应的终端及存储介质。


背景技术：

2.随着自主移动底盘技术发展，将自主移动底盘放入环境地图已知的房间，从已知位置开始移动成为了当下常见的应用场景。
3.基于激光雷达的定位算法都会遇到一个问题，就是如何建立自主移动底盘初始位置与环境地图的联系。目前的解决方法，是通过人为干预，手动输入自主移动底盘的初始位置估计，之后自主移动底盘才可以开始进行自动定位及导航活动。
4.此外，如果在定位过程中受到了其他因素干扰发生了定位漂移的情况，往往需要人为二次干预，对系统重新进行初始化全局位置，恢复移动底盘的定位及导航功能。
5.因此，目前的定位导航系统，由于存在不能进行全局位置初始化这一问题，全局位置初始化这一过程严重依赖操作人的参与，导致了自主移动底盘常常需要人为的干预及调试，增加了自主移动底盘的使用及维护成本。
6.经过检索发现：
7.授权公告号为cn103412565b，授权公告日为2016年01月27日的中国发明专利《一种具有全局位置快速估计能力的机器人及其定位方法》，其定位过程为：事先对环境空间进行3d扫描得到完整的全局地图，将环境空间栅格化，计算每个栅格到与该栅格最近的障碍物点的距离值；启动所述3d扫描装置完成一帧3d扫描帧，同时采用粒子滤波框架模拟一定量的粒子按照所述机器人平台的动力学模型运动，将3d扫描帧按照各粒子的位置和姿态对环境地图进行模拟扫描得到各粒子的激光扫描点，利用栅格图计算得到激光扫描点与机器人激光扫描点最近的粒子，该粒子位置即为机器人全局位置最终的估计结果。该方法存在如下缺陷：
8.1、该方法中涉及的栅格地图为二维地图，无需使用三维传感器进行扫描及匹配；
9.2、该方法中利用粒子滤波框架计算机器人全局位置时需要前置条件，即，在系统初始化时有一个对于机器人初始位置的良好估计，这一部分工作仍需要人工手动输入系统。
10.综上所述，包括上述专利文献在内的现有技术仍无法避免人为的参与，目前没有发现同本发明类似技术的说明或报道，也尚未收集到国内外类似的资料。


技术实现要素：

11.本发明针对现有技术中存在的上述不足，提供了一种激光雷达定位系统的全局位置初始化方法及系统，同时提供了一种相应的终端及存储介质。
12.根据本发明的一个方面，提供了一种激光雷达定位系统的全局位置初始化方法，包括：
13.加载全局地图及相关参数；
14.根据所述相关参数，在所述全局地图中，切分局部网格子地图；
15.在切分的每一个所述局部网格子地图中，生成随机估计的n个初始估计位置；
16.对于每一个所述初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵；
17.根据所述匹配程度得分和所述位置转换矩阵，对全局位置初始化进行确定；
18.判断再次触发全局位置初始化情况，重新执行上述步骤，进行重定位。
19.优选地，所述全局地图，是指：包含全局坐标系的已知环境地图。
20.优选地，所述全局地图的地图类型包括：pcd、ply、obj、stl以及pgm。
21.优选地，所述相关参数，包括：网格子地图切分尺度参数、每张子地图中生成的随机位置个数、得分阈值参数以及重定位触发相关参数。
22.优选地，所述根据所述相关参数，在所述全局地图中，切分局部网格子地图，包括：
23.根据所述相关参数中的网格子地图切分尺度参数，计算网格横纵个数及网格中心点坐标；
24.根据所述网格中心点坐标，将所述全局地图分割成多个局部网格子地图。
25.优选地，将得到的多个所述局部网格子地图存储在容器中。
26.优选地，所述在切分的每一个所述局部网格子地图中，生成随机估计的n个初始估计位置，包括：
27.遍历每一个所述局部网格子地图，以所述局部网格子地图的中心点、x轴和y轴为范围，生成n个系统随机估计的初始估计位置，得到对应的集合为p
guess
{p
guess_1
,p
guess_2
,
…
,p
guess_n
}。
28.优选地，集合p
guess
中的每一个所述初始估计位置，均包括x轴位置、y轴位置以及偏航角θ，记作p(x,y,θ)。
29.优选地，x,y∈submap，θ∈(3.14，3.14]。
30.优选地，所述对于每一个所述初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵，包括：
31.遍历移动底盘的所有可能初始位置，在每一个所述可能初始位置处，以所述初始估计位置为计算初值，利用迭代最近点算法，计算所述可能初始位置处的匹配程度得分及其位置转换矩阵，分别得到对应的集合s{s1,s2,
…
,s
n
}和p
correct
{p
correct_1
,p
correct_2
,
…
,p
correct_n
}。
32.优选地，所述根据所述匹配程度得分和所述位置转换矩阵，对全局位置初始化进行确定，包括：
33.遍历所述匹配程度得分，获取最小得分所对应的位置转换矩阵中的可能初始位置，若最小得分小于设定阈值a，则视为全局位置初始化成功，雷达定位系统直接开始定位。
34.优选地，设定阈值a为：0.01。
35.优选地，所述判断再次触发全局位置初始化情况，重新执行上述步骤，进行重定位，包括：
36.对一个线程或一个进程进行实时监控定位，当发生如下任意一种或任意多种异常情况时，判断为再次触发全局位置初始化情况，进行重定位：
37.‑
两次定位结果异常跳变；
38.‑
两次定位结果与里程计或惯性测量单元数据相差过大；
39.‑
当前位置定位匹配程度得分大于设定阈值b。
40.优选地，所述设定阈值b为：1.0。
41.根据本发明的另一个方面，提供了一种激光雷达定位系统的全局位置初始化系统，包括：
42.加载模块，该模块用于加载全局地图及相关参数；
43.网格子地图切分模块，该模块根据加载模块加载的所述相关参数，在加载模块加载的所述全局地图中，切分局部网格子地图；
44.初始估计位置生成模块，该模块在网格子地图切分模块切分的每一个所述局部网格子地图中，生成随机估计的n个初始估计位置；
45.匹配得分及位置转换矩阵生成模块：该模块对于初始估计位置生成模块生成的每一个所述初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵；
46.全局位置初始化模块，该模块根据匹配得分及位置转换矩阵生成模块生成的所述匹配程度得分和所述位置转换矩阵，对全局位置初始化进行确定；
47.重定位模块，该模块用于判断再次触发全局位置初始化情况，进行重定位。
48.根据本发明的第三个方面，提供了一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时可用于执行上述任一项所述的方法。
49.根据本发明的第四个方面，提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可用于执行上述任一项所述的方法。
50.由于采用了上述技术方案，本发明与现有技术相比，具有如下至少一项的有益效果：
51.1、本发明提供的激光雷达定位系统的全局位置初始化方法及系统、终端、存储介质，自动进行全局位置初始化，避免了人工输入初始位置的繁琐操作。
52.2、本发明提供的激光雷达定位系统的全局位置初始化方法及系统、终端、存储介质，定位丢失时，自动进行全局位置重定位，避免人工介入。
53.3、本发明提供的激光雷达定位系统的全局位置初始化方法及系统、终端、存储介质，实现无人干预的自主移动底盘，以减少人工成本。
54.4、本发明提供的激光雷达定位系统的全局位置初始化方法及系统、终端、存储介质，去除了移动底盘在开始作业之前，根据环境地图，人工推测初始位置并设置这一环节，在简化了工作流程的基础上，能够带来更为精准的定位效果。
55.5、本发明提供的激光雷达定位系统的全局位置初始化方法及系统、终端、存储介质，实现移动底盘的无人操作自启动。
56.6、本发明提供的激光雷达定位系统的全局位置初始化方法及系统、终端、存储介质，实现移动底盘的自动丢失重定位。
附图说明
57.通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
58.图1为本发明一实施例中激光雷达定位系统的全局位置初始化方法流程图；
59.图2为本发明一优选实施例中激光雷达定位系统的全局位置初始化方法流程图；
60.图3为本发明一优选实施例中全局地图示意图；
61.图4为本发明一优选实施例中切分局部网格子地图示意图；
62.图5为本发明一优选实施例中随机生成的初始估计位置(以第一格为例)示意图；
63.图6为本发明一优选实施例中局部网格子地图中可能初始位置和初始估计位置示意图；
64.图7为本发明一优选实施例中全局位置初始化成功示意图；
65.图8为本发明一具体应用实例中采用webots软件搭建的环境地图(即全局地图)示意图；
66.图9为本发明一具体应用实例中的可视化界面示意图；
67.图10为本发明一实施例中激光雷达定位系统的全局位置初始化系统的组成模块示意图。
具体实施方式
68.下面对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。
69.图1为本发明一实施例中激光雷达定位系统的全局位置初始化方法流程图。
70.如图1所示，该实施例提供的激光雷达定位系统的全局位置初始化方法，可以包括如下步骤：
71.s100，加载全局地图及相关参数；
72.s200，根据相关参数，在全局地图中，切分局部网格子地图；
73.s300，在切分的每一个局部网格子地图中，生成随机估计的n个初始估计位置；
74.s400，对于每一个初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵；
75.s500，根据匹配程度得分和位置转换矩阵，对全局位置初始化进行确定；
76.s600，判断再次触发全局位置初始化情况，重新执行上述步骤，进行重定位。
77.在该实施例的s100中，作为一优选实施例，全局地图，可以为：包含全局坐标系的已知环境地图。
78.在该实施例的s100中，作为一优选实施例，全局地图的地图类型可以包括：pcd、ply、obj、stl以及pgm。
79.在该实施例的s100中，作为一优选实施例，相关参数，可以包括：网格子地图切分尺度参数、每张子地图中生成的随机位置个数、得分阈值参数以及重定位触发相关参数。
80.在该实施例的s200中，作为一优选实施例，根据相关参数，在全局地图中，切分局
部网格子地图，可以包括如下步骤：
81.s201，根据相关参数中的网格子地图切分尺度参数，计算网格横纵个数及网格中心点坐标；
82.s202，根据网格中心点坐标，将全局地图分割成多个局部网格子地图。
83.在该实施例的s202中，作为一优选实施例，将得到的多个局部网格子地图存储在容器中。
84.在该实施例的s300中，作为一优选实施例，在切分的每一个局部网格子地图中，生成随机估计的n个初始估计位置，可以包括如下步骤：
85.遍历每一个局部网格子地图，以局部网格子地图的中心点、x轴和y轴为范围，生成n个系统随机估计的初始估计位置，得到对应的集合为p
guess
{p
guess_1
,p
guess_2
,
…
,p
guess_n
}。
86.在该实施例的s300中，作为一优选实施例，集合p
guess
中得每一个初始估计位置，均可以包括x轴位置、y轴位置以及偏航角θ，记作p(x,y,θ)。
87.在该实施例的一具体应用实例中，x,y∈submap，θ∈(3.14，3.14]。
88.在该实施例的s400中，作为一优选实施例，对于每一个初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵，可以包括如下步骤：
89.遍历移动底盘的所有可能初始位置，在每一个可能初始位置处，以初始估计位置为计算初值，利用迭代最近点算法，计算可能初始位置处的匹配程度得分及其位置转换矩阵，分别得到对应的集合s{s1,s2,
…
,s
n
}和p
correct
{p
correct_1
,p
correct_2
,
…
,p
correct_n
}。
90.在该实施例的s500中，作为一优选实施例，根据匹配程度得分和位置转换矩阵，对全局位置初始化进行确定，可以包括如下步骤：
91.遍历匹配程度得分，获取最小得分s
correct_min
所对应的集合p
correct
中的可能初始位置p
correct_min
，若最小得分s
correct_min
小于设定阈值a，则视为全局位置初始化成功，雷达定位系统直接开始定位。
92.在该实施例的一具体应用实例中，设定阈值a可以为：0.01
93.在该实施例的s600中，判断再次触发全局位置初始化情况，重新执行上述步骤，进行重定位，可以包括如下步骤：
94.对一个线程或一个进程进行实时监控定位，当发生如下任意一种或任意多种异常情况时，判断为再次触发全局位置初始化情况，进行重定位：
95.‑
两次定位结果异常跳变；
96.‑
两次定位结果与里程计或惯性测量单元数据相差过大；
97.‑
当前位置定位匹配程度得分大于设定阈值b。
98.在该实施例的一具体应用实例中，设定阈值b为：1.0
99.图2为本发明一优选实施例提供的激光雷达定位系统的全局位置初始化方法流程图。
100.如图2所示，该优选实施例提供的激光雷达定位系统的全局位置初始化方法，可以包括如下步骤：
101.步骤1：加载全局地图及相关参数；
102.步骤2：基于加载的全局地图及相关参数，切分局部网格子地图；
103.步骤3：在切分的每一个局部网格子地图中，生成随机估计的初始估计位置，得到
该位置的集合p
guess
{p
guess_1
,p
guess_2
,
…
,p
guess_n
}；
104.步骤4：对于每一个初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵，得到相应的匹配程度得分集合s和转换后的初始估计位置集合p
correct
；
105.步骤5：根据匹配程度得分和位置转换矩阵，进行全局位置最优估计；
106.步骤6：根据定位情况监控，自动重定位。
107.作为一优选实施例，步骤1包括如下步骤：
108.步骤1.1：加载全局地图，支持的地图类型包括pcd、ply、obj、stl、pgm等，为包含全局坐标系的已知环境地图，如图3所示；
109.步骤1.2：加载相关参数，包括：网格子地图切分尺度参数、每张子地图中生成的随机位置个数、得分阈值参数、重定位触发相关参数。
110.作为一优选实施例，如图4所示，步骤2包括以下步骤：
111.步骤2.1：根据网格子地图切分尺度参数计算网格横纵个数及网格中心点坐标。
112.步骤2.2：根据网格中心坐标，将全局地图分割成局部网格子地图，并储存在容器中。
113.作为一优选实施例，如图5所示，步骤3包括如下步骤：
114.遍历各局部网格子地图，以网格的中心点及网格的x、y轴为范围，生成n个系统随机估计的初始估计位置，得到相应的集合p
guess
{p
guess_1
,p
guess_2
,
…
,p
guess_n
}。
115.作为一优选实施例，对于集合p
guess
中的每个初始估计位置，由三部分组成，包括：x轴位置、y轴位置及偏航角(yaw)θ，可以记作p(x,y,θ)。
116.图5中，随机初始位置估计即为随机估计的初始估计位置。
117.进一步地，x,y∈submap,θ∈(3.14，3.14]。
118.作为一优选实施例，如图6所示，步骤4包括如下步骤：
119.遍历移动底盘的所有可能初始位置，在每一个可能初始位置处，以初始估计位置为计算初值，利用迭代最近点算法，计算可能初始位置处的匹配程度得分及其位置转换矩阵，分别得到对应的集合s{s1,s2,
…
,s
n
}和p
correct
{p
correct_1
,p
correct_2
,
…
,p
correct_n
}。
120.图6中，随机初始位置估计即为随机估计的初始估计位置。
121.作为一优选实施例，如图7所示，步骤5包括如下步骤：
122.遍历集合s，寻找匹配程度最大、得分最小s
correct_min
对应的初始估计位置p
correct_min
，其中，得分物理含义为匹配点云数据之间的偏差值之和，分数越小匹配程度越好，也就是说，得分代表最近邻点之间的距离，距离越小，概率越大；若s
correct_min
小于设定阈值a，视为全局位置初始化成功，雷达定位系统可以直接开始定位。
123.进一步地，设定阈值a为：0.01。
124.图7中，随机初始位置估计即为随机估计的初始估计位置。
125.作为一优选实施例，步骤6包括如下步骤：
126.保证一个线程或进程实时监控定位结果，当发生以下任意一种或任意多种异常情况时，判断为再次触发全局位置初始化，进行系统的重定位。
127.异常情况包括：
128.‑
两次定位结果异常跳变；
129.‑
两次定位结果与里程计或惯性测量单元数据相差过大；
130.‑
当前位置定位匹配程度得分(概率过低)大于设定阈值b。
131.进一步地，设定阈值b为：1.0。
132.下面结合附图及一具体应用实例，对本发明上述实施例所提供的技术方案进行进一步的详细说明。
133.在该具体应用实例中，利用c++中std标准容器存储根据参数切割生成的局部网格子地图、随机初始估计位置、匹配程度得分等数据结构。
134.使用webots、gazebo等机器人仿真软件，模仿雷达数据及环境地图，对本发明上述实施例所提供的方法进行验证，可使用pcl点云库自带的可视化类进行动态的结果显示，方法实例如图8所示。
135.图9为该具体应用实例中可视化界面示意图，如图9所示，包括雷达原始数据、环境地图、当前子地图以及匹配结果这些方法关键要素的可视化示意。
136.本发明另一实施例，提供了一种激光雷达定位系统的全局位置初始化系统，如图10所示，可以包括如下模块：
137.加载模块、网格子地图切分模块、初始估计位置生成模块、匹配得分及位置转换矩阵生成模块、全局位置初始化模块以及重定位模块。
138.其中：
139.加载模块，该模块用于加载全局地图及相关参数；
140.网格子地图切分模块，该模块根据加载模块加载的相关参数，在加载模块加载的全局地图中，切分局部网格子地图；
141.初始估计位置生成模块，该模块在网格子地图切分模块切分的每一个局部网格子地图中，生成随机估计的n个初始估计位置；
142.匹配得分及位置转换矩阵生成模块：该模块对于初始估计位置生成模块生成的每一个初始估计位置，生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵；
143.全局位置初始化模块，该模块根据匹配得分及位置转换矩阵生成模块生成的匹配程度得分和位置转换矩阵，对全局位置初始化进行确定；
144.重定位模块，该模块用于判断再次触发全局位置初始化情况，进行重定位。
145.本发明第三个实施例提供了一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时可用于执行本发明上述实施例中任一项的方法。
146.可选地，存储器，用于存储程序；存储器，可以包括易失性存储器(英文：volatile memory)，例如随机存取存储器(英文：random
‑
access memory，缩写：ram)，如静态随机存取存储器(英文：static random
‑
access memory，缩写：sram)，双倍数据率同步动态随机存取存储器(英文：double data rate synchronous dynamic random access memory，缩写：ddr sdram)等；存储器也可以包括非易失性存储器(英文：non
‑
volatile memory)，例如快闪存储器(英文：flash memory)。存储器用于存储计算机程序(如实现上述方法的应用程序、功能模块等)、计算机指令等，上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。
147.上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。
148.处理器，用于执行存储器存储的计算机程序，以实现上述实施例涉及的方法中的各个步骤。具体可以参见前面方法实施例中的相关描述。
149.处理器和存储器可以是独立结构，也可以是集成在一起的集成结构。当处理器和存储器是独立结构时，存储器、处理器可以通过总线耦合连接。
150.本发明第四个实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可用于执行本发明上述实施例中任一项的方法。
151.本发明上述实施例提供的激光雷达定位系统的全局位置初始化方法及系统、终端、存储介质，自动进行全局位置初始化，避免了人工输入初始位置的繁琐操作；当定位丢失时，自动进行全局位置重定位，避免人工介入；实现无人干预的自主移动底盘，以减少人工成本；去除了移动底盘在开始作业之前，根据环境地图，人工推测初始位置并设置这一环节，在简化了工作流程的基础上，能够带来更为精准的定位效果；实现移动底盘的无人操作自启动；实现移动底盘的自动丢失重定位。
152.需要说明的是，本发明提供的方法中的步骤，可以利用系统中对应的模块、装置、单元等予以实现，本领域技术人员可以参照方法的技术方案实现系统的组成，即，方法中的实施例可理解为构建系统的优选例，在此不予赘述。
153.本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
154.以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。