一种基于点云的大规模建图场景的快速重定位方法与流程

1.本发明属于机器人自动驾驶技术领域，尤其涉及一种基于点云的大规模建图场景的快速重定位方法。


背景技术：

2.自动驾驶车辆或移动机器人实现自主定位，需要以自动驾驶车辆或移动机器人作为载具，载具上安装有激光雷达、摄像头等传感器来进行建图(mapping)和定位(localization)，该过程在行业内叫做slam(simultaneous localization and mapping)技术。
3.建图完成后，使用建立好的既有地图，通过当前实时采集的传感器数据，确定载具在既有地图中的位置，实现实时定位。
4.实时定位在实际使用中，存在两种应用场景，一种是载具启动的位置可能是未知的情况，所以需要找到载具在既有地图中的位置；另一种是在实时定位的过程中出现定位突然失效的情况，需要再次找到载具在既有地图中的位置，并以这个位置作为初始位置，重新开始实时定位。这两种情况都需要重定位。如果系统没有自动重定位功能，就只能依赖人工提供初始位置信息。所以，重定位技术是定位系统智能化和自动化的关键。
5.实时定位的难点在于：在不依赖于外界条件的情况下，能够达到快速定位：现有技术必须依赖于外部环境的变化才可以实现快速定位，例如，或者依赖于gps定位从而实现进一步的精细定位；或者依赖于辅助传感器，从而对当前传感器进行补偿，从而实现精细定位；或者依赖于载具的移动获得精细定位。第一种依赖于gps定位从而实现快速定位，其不足在于gps只能用于室外，而且精度为5
‑
10米，对于室内家庭服务机器人精度为厘米级别的场合，或gps信号很弱的地方，例如楼宇、地下室的应用场合，则gps定位无能为力；第二种依赖于辅助传感器实现快速定位，例如视觉传感器辅助激光传感器，但受限于光照条件，其技术的鲁棒性受限于环境；第三种方法需要载具在没有定位信息的情况下移动载具，给规划和运动控制模块增加了困难，同时也给系统的安全性增加了挑战；
6.第四种方法采用多分辨率子地图节点的分数判定方法，该方法虽然并不依赖于外界条件，但运算量很大，不适合大规模建图的场景。该方法在重新定位时，获取当前激光传感器关键帧的3d点云、再将每个3d点云投影为2d点云，建立基于2d点云的多分辨率子地图的树形结构，用3d点云和多分辨率子地图的树形结构每个节点依次做对比，从而找出与当前3d点云最为接近的节点的位姿，作为重定位的位姿。虽然该方法重定位并不依赖于外界条件，但其节点分数判断的运算量很大。因为其在没有找到与当前3d点云相似度大于80％的节点以前，必须逐个计算每个节点的分值，一直到找到适合的节点为止。对于大规模建图的场合，假设关键帧有1万个，则节点也有一万个，假设一万个节点的树结构层次足够多，并且与当前3d点云相似度大于80％的节点位于树形结构的末层节点，则该算法几乎从头到尾要遍历树结构的大部分节点，大大降低了重定位的速度。


技术实现要素：

7.本发明针对现有技术的不足，提出一种基于点云的大规模建图场景的快速重定位方法，目的在于解决现有技术进行载具重定位时，或者必须依赖于外部环境的支持、或者实时定位时的运算数据量太大，不适合大规模建图场景的实时定位问题。
8.本发明为解决其技术问题，提出以下技术方案：
9.一种基于点云的大规模建图场景的快速重定位方法，其特点是：包括以下步骤，
10.步骤一、使用slam技术进行建图，建图过程中，生成3d点云的关键帧和对应环
‑
域特征向量库、多分辨率子地图库、以及进行数据预处理，所述数据预处理为：计算多分辨率子地图树形结构中的每个节点的分数，生成节点
‑
分数数据库；
11.步骤二、开始重定位，使用当前时刻传感器数据，进行环
‑
域特征向量匹配，得到候选子地图库；
12.步骤三、从候选子地图中得到最佳子地图和载具的位姿初值；
13.步骤四、在最佳子地图中进行精匹配，得到最终重定位结果。
14.所述步骤一的具体过程如下：
15.1)建图的过程中，生成3d点云关键帧库和2d栅格子地图库；该2d栅格子地图库的每个2d栅格子地图由多个关键帧组成；
16.2)生成3d点云关键帧对应的环
‑
域特征向量库；
17.3)计算2d栅格子地图中每个子地图中的自由空间，该自由空间为每个子地图中非障碍物的可行走区域；
18.4)建立2d栅格子地图中的多分辨率子地图的树形结构，生成多分辨率子地图库；该多分辨率子地图中的树形结构从顶层到底层分辨率由粗到细；
19.5)计算多分辨率子地图树形结构中的每个节点的分数，生成节点分数数据库。
20.所述步骤二的具体过程如下：
21.1)生成环
‑
域信息和特征向量；
22.2)计算目标环
‑
域特征向量在历史环
‑
域特征向量库中的差异度，筛选出差异度较小的关键帧；
23.3)筛选出差异度较小的关键帧所对应的多分辨率子地图作为候选子地图库；
24.所述步骤三的具体过程如下：
25.1)生成2d点云数据，
26.包括以下过程：
27.a、获取传感器当前3d点云数据；
28.b、生成2d点云数据；
29.c、获取目标点点云数据；
30.d、读取候选子地图数据；
31.e、采用分支定界的相关匹配算法；
32.f、获得最佳子地图和载具位姿初始值；
33.2)分支定界的相关匹配算法计算，
34.具体过程如下：
35.a.生成2d点云数据；
36.b.获取当前候选子地图；
37.c.用当前的2d点云数据和当前候选子地图做对比，所述候选子地图为多分辨率子地图，并且在预处理阶段已经完成当前候选子地图的自由空间的标注、并且当前候选子地图的每个节点都预算了分值；
38.d.设n＝1；
39.e.用当前2d点云数据和候选子地图的第n层节点做运算，获取分数值；
40.f.获取n 1层的子节点分数历史统计值，用当前2d点云分数值和n 1层的子节点分数历史统计值对比，小于历史统计值的80％被剪枝，大于80％的子节点作为候选节点；
41.g.判断n 2是否等于w,如果等于w返回过程e，否则继续过程h；
42.h.计算候选节点的子节点的分数，取分数最大的子地图极其位姿；
43.所述步骤四的具体过程如下：
44.1)释放掉最佳子地图中的自由空间约束；
45.2)通过ndt算法进行精匹配。
46.本发明的优点效果
47.1、利用旋转不变性，快速过滤掉大量差异较大的数据。现有2d栅格子地图相关匹配算法计算开销较大，即便是使用了分支定界，在大规模场景地图中，由于分支数据量随之增大，还是需要大量的计算时间。本技术首先使用了3d点云信息，通过3d点云的环
‑
域特征向量，利用旋转不变性，快速过滤掉大量差异较大的数据，同时确保所有可能包含正确结果的数据。
48.2、使用自由空间作为2d栅格子地图相关匹配算法的计算约束，减小了在每个子地图中的搜索范围，进一步减小了整体计算量。并在最后精匹配的过程释放自由空间约束，来避免场景变化导致的非最优结果。
49.3、使用节点分数数据库作为分支定界过程中剪枝策略的一部分，改善了现有分支定界算法在大量分支和场景相似度较高的情况下剪枝不足的问题，做分支时，不用计算中间层的所有node的匹配值，只对其中一个节点分数作为依据去做减值，避免了对所有节点计算。
50.4、本发明通过分时运算和实时运算相结合、历史分支定界和当前分支定界相结合、环
‑
域特征向量和自由空间约束相结合，解决了现有技术或者必须依赖于外部环境的支持才能实时定位、或者实时定位时的运算数据量太大，不适合大规模建图场景的实时定位问题。
附图说明
51.图1为本发明定位方法主框架流程图s100；
52.图2为本发明图1的s110细化流程图；
53.图3为本发明划分环
‑
域特征向量示意图；
54.图4为多分辨率子地图的树形结构示意图；
55.图5为本发明图1的s120细化流程图；
56.图6为本发明图1的s130细化流程图；
57.图7为本发明图6的s133细化流程图；
58.图8为本发明图1的s140细化流程图。
具体实施方式
59.下面结合附图对本发明作出进一步解释：
60.本发明设计原理
61.1、分时运算和实时运算相结合的设计原理：所述分时运算，就是在建图过程中进行数据预处理，生成节点分数数据库；所述实时运算，就是进行重定位时，不完全依赖于历史数据库，而是有选择地进行节点的实时计算，包括第一层的节点的实时计算、中间层和最末层节点的有选择进行计算，中间层节点实时计算的条件是：凡是和历史库没有匹配上的节点进行剪枝，已经匹配上的节点进行实时分数计算；最末层节点进行实时计算的条件是：如果末层节点的父节点是没有被剪枝的节点，则重定位时，这些节点进行实时计算，如果末层节点的父节点是已经被剪枝的节点，则重定位时，这些节点不进行实时计算。
62.2、历史分支定界和当前分支定界相结合的设计原理。本发明数据预处理过程中的分支定界算法不包括剪枝，而重定位时的分支界定算法包括剪枝；数据预处理过程中的分支定界算法由于没有时间限制，所以，传感器的当前3d点云能够遍历2d栅格子地图树结构的所有节点，并且每个节点都保存一个最大值；实时定位过程中，由于受到时间限制，所以传感器的当前3d点云无需遍历全部节点，没有被匹配上的节点被剪枝。
63.3、环
‑
域特征向量和自由空间约束相结合的设计原理。利用3d点云的环
‑
域特征向量，利用旋转不变性，快速过滤掉大量差异较大的数据，并确保所有可能包含正确结果的数据；同时，使用自由空间作为2d栅格子地图相关匹配算法的计算约束，减小了在每个子地图中的搜索范围，进一步减小了整体计算量。
64.一种基于点云的大规模建图场景的快速重定位方法，如图1、图2、图3、图4、图5、图6、图7、图8所示，包括以下步骤，
65.步骤一、使用slam技术进行建图，建图过程中，生成3d点云的关键帧和对应环
‑
域特征向量库、多分辨率子地图库、以及进行数据预处理，所述数据预处理为：计算多分辨率子地图树形结构中的每个节点的分数，生成节点分数数据库；
66.补充说明：
67.步骤一要做的是：载具在建图过程中并行完成三项工作：生成3d点云的关键帧和对应环
‑
域特征向量库、2d栅格子地图库、以及进行数据预处理。其中，数据预处理是本发明的特色之一。建图过程和确定载具当前起始点位置，这是两个不同阶段，由于载具进行起始点定位时，需要运算的数据量很大且耗时很长，所以将车辆定位时的大部分运算提前做完，称为数据预处理；
68.数据预处理的核心是建立节点分数数据库，建立节点分数数据库之前需要生成3d点云关键帧库、2d栅格子地图库、生成关键帧对应的环
‑
域特征向量库、计算每个子地图中的自由空间、建立子地图中的多分辨率地图的树形结构，具体如下：
69.1)数据预处理的第1步是在建图的过程中，建图的过程中，生成3d点云关键帧库和2d栅格子地图库；该2d栅格子地图库的每个2d栅格子地图由多个关键帧组成；
70.补充说明：
71.载具建图过程中，传感器的数据以帧的形式读取，随着车辆的行驶，每一帧为某一
时刻下的一包完整的传感器数据；该一包完整的传感器数据称为一帧数据，一个关键帧数据包括3d点云、以及载具当前的位姿，这两个合起来称为一个关键帧，所述的3d点云就是由多个x、y、z空间点组成的、用于描述环境中某个特征物体形状的3d点云，3d点云可以是汽车形状、也可以是飞机形状；所述每个2d栅格子地图库是由每个3d点云投影为x
‑
y平面的2d点云后的2d栅格子地图，将建图过程中的所有3d点云关键帧汇集一起组成了3d点云关键帧库，将所有2d栅格子地图汇集一起组成2d栅格子地图库。
72.以上所涉及的名词解释如下：
73.帧：传感器的数据以帧的形式读取，每一帧为某一时刻下的一包完整的传感器数据；
74.载具的位姿：pose＝{x,y,theta}，其中x，y，theta分别为载具在既有地图坐标系中的x、y方向的坐标和方位角；
75.3d点云：3d点云在关键帧中的点，3d点云中包含其在关键帧坐标系中的x，y，z三维空间坐标信息，表示为：3d点云:{x,y,z}；
76.2d点云：2d点云在2d空间中的点，2d点云中包含其在2d坐标系中的x，y坐标信息，表示为：2d点云:{x,y}；
77.关键帧：关键帧，关键帧是在slam建图过程中产生的，每个关键帧包含id、关键帧的位姿和在关键帧坐标系下的传感器数据；本技术中的关键帧数据为关键帧所在时刻的一帧3d点云数据；关键帧位姿为某一时刻载具在地图中的位姿。
78.点云投影：将3d点云转换为2d点云，将：3d点云:{x,y,z}取x,y信息，转换为2d点云:{x,y}；
79.2d栅格子地图：由大小相同的2d栅格单元子地图组成，每个2d栅格单元子地图组代表一个关键帧，所以2d栅格子地图包含数个关键帧；；
80.2)数据预处理的第2步是生成3d点云关键帧对应的环
‑
域特征向量库；
81.补充说明：
82.第一、划分环域特征的目的：划分为环域特征的目的是大大减少计算数据量，这一步是为后面的车辆定位当前起始位置时使用，定位起始位置时要求定位速度很快，定位时包括对环境特征的识别，识别出和当前车辆位姿最为相近的特征，而抛弃和当前定位无关的环境特征，就可以大大加快定位速度。再从环
‑
域特征向量库历史库找到最为相近的特征的位置，也就大致确定了当前车辆的位置。
83.第二、从3d点云降维到环域特征，数据运算量减少到原来的1％：设每个关键帧中的3d点云集合为p,将p进行环
‑
域划分：环域特征如图3所示，i代表每个环面，j代表每个扇区，所述环面，就是按照距离激光发射点相同距离为一个圆环平面、不同距离为另一个圆环平面；所述扇区，就是0到360度划分扇区，图3中，一共3个环面，扇区为30度一个，一个环面上12个扇区，环域一共划分为3*12＝36个环域。每个圆环平面上再划分若干的扇区，用每个扇区所具有的点数和所在的环面来描述该扇区的特性，这样就大大降低的数据量，假设一个3d点云有一万个点，每个点用x、y、z表示，当采用环领域特征划分以后，这一万个点用10个环面、每个环面10个扇区描述，10*10就是100个特征值，用p
ji
来表示,其中，i代表第几个环、j代表第几个扇区，采用环领域特征划分以后，1万个数据的计算量就压缩成100个数据的计算量。但是，环域特征中没有x、y信息，所以，环域特征不能用于最终定位车辆的启示位
置，而只能用于筛选环境中不相干的特征。定位车辆的位置还需要使用2d栅格子地图。
84.第三、划分环域特征的具体方法为：
85.i、定义3d点云中的点3d点云在关键帧中的距离为是在2d空间中的欧氏距离。
86.ii、设关键帧中3d点云距关键帧原点的有效距离为l；
87.环为绕关键帧坐标中心一圈，某个2d空间中距离区间的一个环形区间；域为关键帧坐标中心某角度区间，2d空间中距离从0到l的扇形区域。任意环和任意域可交叠出空间中唯一的一个区域，该区域定义为环
‑
域区域。
88.iii、设每一个关键帧中的环的个数为n
a
,则每个环的区域的范围为设域的个数为n
s
，则每个域的角度区间为
89.设[n
a
]为集合：{1,2,...,n
a
}，设[n
s
]为集合：{1,2,...,n
s
}；
[0090]
环
‑
域划分后的3d点云集合p表示为：
[0091][0092]
其中∪表示并集，p
ij
为在第i个环和第j个域构成的环
‑
域区域所包含的点的集合，如图3所示。
[0093]
iv、设环
‑
域区域中点的数量为n(p
ij
)；设环
‑
域区域中点的z坐标值最大值为m(p
ij
)。每个环的点云数量均值为环上所有环
‑
域区域点数量的均值，第i个环的点云数量均值为n
i
：
[0094][0095]
v、每个环的点云高度均值为所有环
‑
域区域点云z坐标值最大值的均值，第i个环的点云高度均值为m
i
：
[0096][0097]
关键帧的环
‑
域特征向量定义为：
[0098][0099]
关键帧的环
‑
域特征向量是由其每个环的点云均值和点云高度均值组成的。
[0100]
vi、计算所有关键帧对应的环
‑
域特征向量，建立环
‑
域特征向量库(d130)。
[0101]
3)数据预处理的第3步是计算2d栅格子地图中每个子地图中的自由空间，该自由空间为每个子地图中非障碍物的可行走区域；
[0102]
补充说明：
[0103]
i、本实施例计算每个子地图中的自由空间是车辆定位时的第二次数据筛选，环域特征方法是第一步数据筛选，自由空间方法是第二步数据筛选：后面的过程4)的多分辨率
子地图就是完成2d栅格子地图自由空间约束基础上建立的多分辨率子地图。
[0104]
ii、计算每个子地图的自由空间的方法为：使用泛洪算法，以子地图的中心为起点，子地图中的占据栅格作为障碍物，进行泛洪，泛洪算法遍历从起点开始可达的所有连通的非障碍物区域，即自由空间，自由空间为载具可以出现的位置；经过泛洪计算后，记录自由空间。子地图中的自由空间由建图过程中的传感器采集的环境数据决定的，随着时间推移场景发生变化，并不能保证真实场景中的自由空间是一成不变的。除非重新建图，否则场景的变化将不会在既有地图的子地图中体现。正因为如此，自由空间的使用需要采取一些策略，后续步骤将具体阐述。
[0105]
4)数据预处理的第4步是建立2d栅格子地图中的多分辨率子地图的树形结构，生成多分辨率子地图库；该多分辨率子地图中的树形结构从顶层到底层分辨率由粗到细；
[0106]
补充说明：
[0107]
i、树形结构设计原则。树形结构如图4所示，建立树形结构的原则是：最末层的节点代表一个2d栅格子地图：假设当前的2d栅格子地图包括16个关键帧，这16个关键帧对应16个2d栅格子地图，树结构第一层为1个节点，第二层为4个节点。第三层为16个节点，到此为止就不能再细分了，因为再细分就会把第三层的每个2d栅格子地图拆分开，而拆分以后就不便于识别了，所以树结构的最末层的节点不能小于一个2d栅格子地图。
[0108]
重要说明：第一层的节点数量为1，但是第一层的关键帧和第三层的关键帧数量都是16，区别在于节点的数量。第三层节点的数量为16，第一层节点的数量为1.
[0109]
ii、使用分支定界中的分支方法，建立每个子地图的多分辨率地图树形结构。设每个子地图的原始分辨率为r，树形结构一共有w层。树形结构的每个节点节点代表一个2d栅格单元子地图；树形结构的第一层，即顶层，为原始物理空间尺寸的子地图的栅格地图，地图分辨率最低，为r*1/2
(w
‑
1)
；每个节点作为根节点，在其下一层分支出4个子节点，4个子节点将根节点平均分成4部分，并且分辨率是根节点的2倍；最底层，即w层的节点不再分支子节点，其分辨率最高，是子地图的原始分辨率r，物理空间尺寸最小，为原子地图物理空间尺寸的1/2
(w
‑
1)
。
[0110]
5)数据预处理的第5步是计算多分辨率子地图树形结构中的每个节点的分数，生成节点分数数据库。
[0111]
补充说明：
[0112]
i、预处理阶段的节点分数计算。假设16个点云，分别取每个点云作为当前点云，拿当前点云到多分辨率子地图的树形结构中去查找，假设三层树结构21个节点，则每个点云都要遍历21个节点，或者每个节点都要经过16个点云在当前节点的分值计算，然后每个节点从16个点云值挑出分数最大的1个保留，但不保留最大值的关键帧的id号，因为将来重定位时的关键帧的点云，由于周围环境的变化，不保证和历史的点云完全一样，只是近似相同，所以，这个最大值将来对应将来哪一个点云是没有关联关系的。16个点云每个遍历21个节点，总的运算次数＝16*21＝336次。
[0113]
ii、本发明节点分数计算和原始节点分数计算的区别：原始节点分数计算是有剪枝过程的，本发明节点分值计算是不包括剪枝过程的，有剪枝过程并非每个节点都有分数，而不包括剪枝过程则21个节点每个节点均有分数。原始分数计算方法：当发现下一层的节点的分值大于上一层时，例如上一层为第2层，第2层有a、b、c、d四个节点，当a、b、c、d四个节
点的分值都算了一遍以后，发现a的分值最高，所以将a展开，a展开后为第三层，第三层对应a的有4个节点，当其中一个节点的分值大于b、c、d的分值时，上一层的b、c、d节点就不展开了，如果a的第三层的四个节点的分数还是不能大于b、c、d的分数，那就再展开b的第三层，一直找到下一层大于上一层的节点为止。由此看出，传统的分支计算方法包括了剪枝过程，被剪断的分枝的节点的值一定是小于下一层的节点的分数。本发明预处理中，即使a的第三层展开后的4个节点其中一个大于b、c、d的分支，也继续展开b的第三层、c的第三层、d的第三层，使得每个节点都有分值，这样准确率更高。
[0114]
iii、节点分数判定步骤：从计算多分辨率子地图库(d101)的第一层开始，使用相关匹配算法，计算每一层的节点和关键帧库中(d120)每个关键帧的得分，直到最后一层；一共产生n
m
*n
k
个结果；每个节点有n
k
个得分结果，记录每个节点的最大的得分作为一对数值，保存下来，形成节点分数数据库。
[0115]
没有剪枝的遍历节点过程需要消耗大量计算时间，但仍属于数据预处理的阶段，不需要实时运行，计算完毕生成节点分数数据库d140后无需重复计算。并且，本发明是利用了建图的过程，载具行走的同时进行计算为并行计算，并行计算不会影响建图的过程，所以没有运算时间上的要求，这样，就可以在预处理过程中，将所有2d点云遍历所有节点。由于提前运算，提高了重定位过程的效率，使得节点分数数据库在重定位分支定界的剪枝加速过程中，大大减少了计算量，提高了计算效率。
[0116]
总结：到此为止，步骤一完成了数据预处理工作。如图2、图3、图4所示，一共生成5个数据库：子地图库d100、关键帧库d120、环域特征向量库d130、多分辨率子地图库d101、节点分数库d140；
[0117]
步骤二、开始重定位，使用当前时刻传感器数据，进行环
‑
域特征向量匹配，得到候选子地图库；
[0118]
1)开始重定位，生成目标环
‑
域信息和特征向量；
[0119]
补充说明：
[0120]
所述目标环
‑
域特征向量，即为当前载具进行起始位置重定位时，其传感器读入的当前3d点云数据，将该3d点云数据转换为环
‑
域信息，即目标环
‑
域特征向量；
[0121]
2)开始重定位，如图5所示，计算目标环
‑
域特征向量在历史环
‑
域特征向量库中的差异度，筛选出差异度较小的关键帧；
[0122]
补充说明：
[0123]
i、用目标环
‑
域特征向量到步骤一的历史环
‑
域特征向量库中查找，由于压缩后的每个点云的环
‑
域特征向量数据量很小，所以能够很快找出目标环
‑
域特征向量和历史环
‑
域特征向量库中的哪些环
‑
域特征向量差异度小；
[0124]
ii、两个环
‑
域特征向量的差异度的计算：
[0125]
设两个环
‑
域特征向量的差异度为d：
[0126][0127]
其中ψ
q
，ψ
c
分别为两个环
‑
域特征向量；和分别为ψ
q
和ψ
c
的第j个环的点云数
量均值；和分别为ψ
q
和ψ
c
的第j个环的；的第j个环的；分别为的向量模值，使用余弦相似度计算，将两个特征向量的2个行维度结合在一起。得到ψ
q
，ψ
c
的差异度d(ψ
q
,ψ
c
)。
[0128]
iii、本步骤利用了环
‑
域特征向量的旋转不变性，可在保证载具在不同姿态角度下的重定位都能和既有地图中的信息对比出差异度。
[0129]
iv、计算目标环
‑
域特征向量ψ
t
的列向量和环
‑
域特征向量库中每个向量的差异度，对差异度进行排序，选出差异度最小的前30％的环
‑
域特征向量所对应的关键帧，该差异度最小的关键帧，定义为p关键帧集合。
[0130]
3)开始重定位，筛选出差异度较小的关键帧所对应的多分辨率子地图作为候选子地图库；
[0131]
补充说明：
[0132]
所述候选子地图就是与筛选出的30％的环域特征相对应的多分辨率子地图，该多分辨率子地图就是在完成自由空间约束的2d栅格子地图基础上建立的多分辨率子地图。假如100个环域特征，经过筛选保留了30个环域特征，每10个环域特征构成一张多分辨率子地图，那就保留了3张多分辨率子地图，这3张多分辨率子地图称为候选子地图。
[0133]
步骤三、从候选子地图中得到最佳子地图和位姿初值；
[0134]
1)生成2d点云数据如图6所示；
[0135]
包括以下过程：
[0136]
a、获取传感器当前3d点云数据；
[0137]
b、生成2d点云数据；
[0138]
c、获取目标点点云数据；
[0139]
d、读取候选子地图数据；
[0140]
e、采用分值定界的相关匹配算法；
[0141]
f、获得最佳子地图和载具位姿初始值；
[0142]
2)分支定界的相关匹配算法计算如图7所示，
[0143]
具体过程如下：
[0144]
a.生成2d点云数据；
[0145]
b.获取当前候选子地图；
[0146]
c.用当前的2d点云数据和当前候选子地图做对比，所述候选子地图为多分辨率子地图，并且在预处理阶段已经完成当前候选子地图的自由空间的标注、并且当前候选子地图的每个节点都预算了分值；
[0147]
d.设n＝1；
[0148]
e.用当前2d点云数据和候选子地图的第n层节点做运算，获取分数值；
[0149]
f.获取n 1层的子节点分数历史统计值，用当前2d点云分数值和n 1层的子节点分数历史统计值对比，小于历史统计值的80％被剪枝，大于80％的子节点作为候选节点；
[0150]
补充说明：
[0151]
i、本发明和传统节点分数的区别在于：本发明只计算第1层节点和最末层节点、以
及和上一层相比分值大于80％的节点，最末层节点只计算被展开的节点。本发明无需计算中间层节点分数的原因在于，中间层节点的分数在预处理时已经计算完毕，可以直接拿来做对比，假如树结构一共10层，中间层为2到9层，如果第9层以前每层的节点的分数都不能大于第一层的80％，则本发明实施例的第2到8层的节点数据都不需要计算，从历史数据库获取即可，一直到当前2d点云和第9层的某个节点的历史数据相比，相似度大于80％时，再将该节点向下展开；而传统分值计算方法中间层也必须计算，如果当前2d点云数据和第9层以前每层的节点的分数相比，都不能大于第一层的80％，则从第2层到第8层每个节点都会计算一遍，一直到第9层的某个节点的分数和当前2d点云数据相比，当前2d点云的分值和第9层的某个节点的分数相比大于80％时，才停止继续计算。
[0152]
ii、本发明在重定位时，只计算第1层节点和最末层被展开的节点的分数的原理：用当前2d点云和当前候选子地图的第一层节点做运算，如果分数小于80％，则直接舍弃掉这张候选子地图，换下一个候选子地图，但是，这种情况不容易发生，因为采用环域特征已经筛选了大部分子地图，而保留下来的候选子地图不会发生上述情况；最末层被展开的节点的数量已经很少，所以，在重定位时，这部分节点不依赖于历史数据，而是重新计算一遍，也就是拿当前2d点云数据和末层每个被展开的节点的数据做相似度对比，并得出分值，这是因为当前定位和环境和历史定位的环境大体上不会发生变化，比如周围的特征明显的建筑物或树木，但是，环境中的任何车的疏密度会有变化，所以重新计算一遍是更为安全的，最重要的是，此时的数据量已经很少，重新计算不会影响重定位的效率。
[0153]
g.判断n 2是否等于w,如果等于w返回过程e，否则继续过程h；
[0154]
h.计算候选节点的子节点的分数，取分数最大的子地图极其位姿；
[0155]
步骤四、在最佳子地图中进行精匹配，得到最终重定位结果；
[0156]
具体过程如下：
[0157]
1)释放掉最佳子地图中的自由空间约束；
[0158]
补充说明：
[0159]
当定位完成以后，需要释放自由空间约束，下次重定位时的算法将计算子地图中的所有可能的行走区域，避免了建图时场景中的物体在重定位时被移动了，而重定位时载具处于原来物体所在的区域，而得不到真实的重定位位姿的问题；
[0160]
2)通过ndt算法进行精匹配。
[0161]
通过ndt算法进行精匹配。
[0162]
补充说明：
[0163]
当完成重定位以后，采用ndt算法可以对重定位进行精匹配。
[0164]
以上所述并非是对本发明的限制，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明实质范围的前提下，还可以做出若干变化、改型、添加或替换，这些改进和润饰也应视为本发明的保护范围。