本发明涉及导航定位，具体是一种基于面元匹配的定位方法及系统。
背景技术：
1、在机器人/汽车/自动驾驶行业，对机器人/汽车自身的实时建图和定位是极其重要的，这是机器人/自动驾驶车辆感知、规划、控制的上游算法模块。其中，激光雷达作为常见传感器，在自动驾驶领域受到了广泛的运用，以雷达作为主要传感器的定位建图算法，是行业内的主要采用的技术方案之一。
2、激光里程计算法的核心在于当前时刻雷达传感器获取的一帧点云数据与历史地图进行匹配从而计算当前时刻机器在地图中的位姿。目前常见的匹配算法基本为基于点云的icp(iterative closest point，迭代最近点)、ndt(normal distributiontransformation，正态分布变换)等匹配算法。对于常见的点云匹配算法，例如icp，它通过最小化匹配点之间的距离实现点云匹配并求出相对位姿。但是由于不能知道点云准确的对应关系，往往有精度较低的缺点。对于另一种常见的点面icp匹配算法，通过最小化点面之间的距离实现定位，但是这种依然不能很好的处理退化场景。
3、这些匹配算法往往需要使用大量的点云，如果点云数量不足则容易出现匹配失败，数量过多则会造成过大的计算负担，影响里程计算法的实时性，或者要求更高级的硬件设备增加成本。此外，这些常见的匹配算法往往没有充分利用点云的局部形状特征，从而在退化场景中不够稳定，例如在隧道场景中，激光里程计匹配往往会发生错误。如果周围环境中缺乏规律性特征，那么也存在精度不高的问题。如果出现错误定位则会造成严重后果。
技术实现思路
1、针对上述现有技术中的不足，本发明提供一种基于面元匹配的定位方法及系统，可应用于移动机器人、智能驾驶车辆、测绘、三维重建设备、激光雷达、tof相机里程计、即时定位与地图构建等。本发明以面元的形式进行点云匹配，不仅能够增加匹配精度，实现更高精度的定位，而且对算力的消耗显著更少，具有良好的实时性，减少对硬件设备的要求，降低了硬件成本。
2、为实现上述目的，本发明提供一种基于面元匹配的定位方法，包括如下步骤：
3、步骤1，获取载体当前帧的点云数据，并提取所述点云数据中的所有第一面元；
4、步骤2，对于所述点云数据中的所有第一面元，匹配地图中的第二面元，得到若干匹配的面元对，并基于所述面元对得到载体当前的位姿。
5、在其中一个实施例，所述提取所述点云数据中的所有第一面元，具体为：
6、步骤1.1，令i＝1，j＝1；
7、步骤1.2，判断i>imax是否成立，其中，imax为所述点云数据中点云序列中的总行数或总环数；
8、若是，则输出提取的第一面元；
9、否则，进行步骤1.3；
10、步骤1.3，判断j>jmax是否成立，其中，jmax为所述点云数据中点云序列每一行或每一环的中列数：
11、若是，令i＝i+1，j＝1后再次进行步骤1.2；
12、否则，进行步骤1.4；
13、步骤1.4，判断所述点云数据中序列坐标为(i，j)的点是否被占用：
14、若是，令j＝j+1后再次进行步骤1.3；
15、否则，进行步骤1.5
16、步骤1.5，将点(i，j)、(i+1，j)、(i，j+1)组成当前第一面元，并将点(i，j)、(i+1，j)、(i，j+1)标记为已占用；
17、步骤1.6，将所述点云数据中与当前第一面元任意一点相邻、与当前第一面元共面且未被占用的点加入当前第一面元，进行当前第一面元的一次更新，并将所有加入当前第一面元的点标记为已占用；
18、步骤1.7，重复步骤1.6直至当前第一面元不再更新后提取当前第一面元，并令j＝j+1，返回步骤1.3。
19、在其中一个实施例，步骤2中，所述地图为在线地图或离线地图。
20、在其中一个实施例，当所述地图为在线地图时，将载体获取第一帧点云数据的位置作为原点，并将第一帧点云数据中提取的第一面元作为第二面元，构建初始的地图；
21、当载体获取新的点云数据时，将新点云数据中提取的第一面元作为第二面元加入地图中，完成地图的在线更新。
22、在其中一个实施例，当所述地图为离线地图时，所述地图包括若干预存的第二面元。
23、在其中一个实施例，在所述地图中，以体素化的形式存储各第二面元，并对每一个体素内的第二面元构造搜索树。
24、在其中一个实施例，步骤2具体为：
25、步骤2.1，在地图中筛选与各第一面元距离最近的第二面元，并将第一面元与距离最近的第二面元组成一组面元对；
26、步骤2.2，以所有面元对中第一面元、第二面元间的距离最小、夹角最小、重合度最高为优化目标，得到载体的位姿；
27、步骤2.3，基于步骤2.2所得位姿中的旋转矩阵旋转各第一面元后，再次进行步骤2.1至步骤2.2得到更新后载体的位姿，判断当前位姿是否符合精度要求，若是则将其作为定位结果并输出，否则继续更新位姿直至符合精度要求。
28、在其中一个实施例，步骤2.2中，所述优化目标为：
29、
30、其中，f(x)表示目标函数，x表示载体的位姿，a、b表示一组面元对中的第一面元与第二面元，dis(a,b)表示一组面元对中第一面元与第二面元的距离，na、nb表示一组面元对中第一面元与第二面元的法向量，arccos(na,nb)表示一组面元对中第一面元与第二面元的夹角，s(a,b)表示一组面元对中第一面元与第二面元的重叠面积，δs为避免分母为0的小量，pdis、pn、ps为权重。
31、在其中一个实施例，步骤2.3中，位姿的精度要求为：当前次得到的位姿与上一次得到的位姿之间的差值小于精度阈值。
32、为实现上述目的，本发明还提供一种基于面元匹配的定位系统，采用上述的方法进行定位，所述定位系统包括：
33、点云获取单元，用于获取载体当前帧的点云数据；
34、面元提取单元，用于提取所述点云数据中的所有第一面元；
35、面元匹配单元，用于为所有第一面元匹配地图中的第二面元，组成面元对；
36、定位输出单元，用于根据所述面元对得到载体的位姿并输出。
37、本发明具有如下有益技术效果：
38、1.本发明根据当前帧的点云数据提取面元，并基于面元进行特征匹配，能够提取更多的局部点云特征，增加了点云匹配精度，实现更高精度的定位；
39、2.本发明能够有效地减少算力消耗，加快匹配速度，具有良好的实时性，减少对硬件设备的要求，降低了硬件成本。
1.一种基于面元匹配的定位方法，其特征在于，包括如下步骤：
2.根据权利要求1所述的基于面元匹配的定位方法，其特征在于，所述提取所述点云数据中的所有第一面元，具体为：
3.根据权利要求1或2所述的基于面元匹配的定位方法，其特征在于，步骤2中，所述地图为在线地图或离线地图。
4.根据权利要求3所述的基于面元匹配的定位方法，其特征在于，当所述地图为在线地图时，将载体获取第一帧点云数据的位置作为原点，并将第一帧点云数据中提取的第一面元作为第二面元，构建初始的地图；
5.根据权利要求3所述的基于面元匹配的定位方法，其特征在于，当所述地图为离线地图时，所述地图包括若干预存的第二面元。
6.根据权利要求4或5所述的基于面元匹配的定位方法，其特征在于，在所述地图中，以体素化的形式存储各第二面元，并对每一个体素内的第二面元构造搜索树。
7.根据权利要求1或2所述的基于面元匹配的定位方法，其特征在于，步骤2具体为：
8.根据权利要求7所述的基于面元匹配的定位方法，其特征在于，步骤2.2中，所述优化目标为：
9.根据权利要求7所述的基于面元匹配的定位方法，其特征在于，步骤2.3中，位姿的精度要求为：当前次得到的位姿与上一次得到的位姿之间的差值小于精度阈值。
10.一种基于面元匹配的定位系统，其特征在于，采用权利要求1至9任一项所述的方法进行定位，所述定位系统包括：
