本发明涉及机器人slam，尤其涉及一种基于关键帧地图的三维激光雷达重定位方法。
背景技术：
1、当前机器人领域在进行基于先验地图的定位的时候，需要人工给定一个初始的大概位置，也叫重定位过程，目前行业内大都是采用人工进行操作或者是在机器人运行过程中实时记录当前位姿，这样在机器人再次开机运行时，加载最后一次保存的pose进行初始化重定位，如果机器人在关机状态被移动到另一个地方，则会导致初始化重定位失败，这个时候则需要人工进行干预，不能保证重定位的准确性和快速性，而且大大影响机器人自动工作的效率。
技术实现思路
1、本发明提出的一种基于关键帧地图的三维激光雷达重定位方法，解决了现有的问题。
2、为了实现上述目的，本发明采用了如下技术方案：一种基于关键帧地图的三维激光雷达重定位方法，使用自研的地图格式，首先采用3d激光雷达帧与帧之间匹配度的计算方式，然后采用2d二值栅格地图二次筛选的重定位方法，得到更加准确的初始化重定位位姿，具体包括以下步骤：
3、步骤一、加载地图：加载已经建立好的地图，建图采用本公司自研的建图算法并保存为本公司自研的地图格式；
4、步骤二、解析地图：提取关键帧所需信息，拼接形成全局点云地图；
5、步骤三、输入激光帧lasersacn：输入当前激光雷达扫描的激光帧lasersacn；
6、步骤四、遍历所有关键帧：对当前帧激光点云体素滤波降采样，然后让当前帧激光点云和每一个关键帧进行匹配，最终可以得到多个后选位姿；
7、步骤五、构造二值栅格地图：构造栅格大小为0.1m的二值栅格地图，即栅格地图中只存在占据点和空闲点，初始栅格地图中全为空闲点；
8、步骤六、点云转换：将当前帧点云按照步骤五中的方式由3d点云转换到2d点云；
9、步骤七、计算匹配度：将2d点云转化到栅格地图坐标系下，计算两者匹配度；
10、步骤八、选取匹配度最高的pose：选取步骤七中匹配度最高的那个作为当前重定位的位姿。
11、优选的，所述地图基于本公司自研的地图格式，已经建立好了先验地图，并且在地图中保存了关键帧的点云和关键帧的pose。
12、优选的，所述步骤二的具体步骤为：将其中的关键帧点云以及关键帧的位姿提取出来，并将关键帧点云全部拼接起来，形成一张大的全局点云地图。
13、优选的，所述激光帧lasersacn为机器人操作系统中常用的信息类型之一，用于传输和处理激光雷达数据。
14、优选的，所述步骤四的具体计算方法为：假设当前帧一共有a个点，其中地面点有b个点，天花板点有c个点，那么当前参与计算匹配度的点一共为a-b-c个点，匹配后得到位姿p，将当前帧的点云通过位姿转换和关键帧对齐，然后在参与匹配度计算的点中寻找匹配上的点，如果当前帧中的一个点周围l距离内能够找到关键帧中的点，则记为一个有效点，距离l为体素滤波的区域大小，这样一共可以选取d个有效点，则最终的两帧之间的匹配度为
15、优选的，首先判断匹配度是否大于阈值，提前设置阈值为0.7，根据当前帧和关键帧匹配出来的位姿p和关键帧的位姿pt可以得出当前帧在全局地图坐标系下的位姿姿pt′＝pt+p，也就是当前帧点云在全局地图下的坐标，即当前定位的候选位姿，遍历所有的关键帧可以得到多个后选位姿。
16、优选的，所述步骤五的具体步骤为：根据点云坐标的z轴进行过滤，只使用-0.5＜z＜0.5之间的点，然后将点云坐标的z轴设为0，将3d点云转化为2d点云，初始栅格地图中全为空闲点，当点云打到栅格中，则将栅格状态设为占据。
17、优选的，所述二值栅格地图不存在未知状态的栅格和ray casting过程。
18、优选的，所述步骤七的具体步骤为：遍历步骤四中得到的所有的候选位姿，将步骤六中的2d点云转化到步骤五中的栅格地图坐标系下，计算2d点云与栅格地图的匹配度。
19、优选的，所述匹配度的具体计算方法为：假设2d点云中一共有a个点，如果有一个点打到栅格地图的占据栅格中，则记为一个有效点，一共存在b个有效点，则匹配度为
20、本发明的有益效果为：使得机器人在关机状态被移动到另一个地方，也不会导致初始化重定位失败，基于先验地图的重定位方式，更为高效的自动重定位流程，经过两次筛选后得到更加准确的初始化重定位位姿，不再需要人工进行干预，保证重定位的准确性和快速性，大大提高机器人自动工作的效率。
1.一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，首先采用3d激光雷达帧与帧之间匹配度的计算方式，然后采用2d二值栅格地图二次筛选的重定位方法，得到更加准确的初始化重定位位姿，具体包括以下步骤：
2.根据权利要求1所述的一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，所述地图基于自研的地图格式，已经建立好了先验地图，并且在地图中保存了关键帧的点云和关键帧的pose。
3.根据权利要求1所述的一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，所述步骤二的具体步骤为：将其中的关键帧点云以及关键帧的位姿提取出来，并将关键帧点云全部拼接起来，形成一张大的全局点云地图。
4.根据权利要求1所述的一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，所述激光帧lasersacn为机器人操作系统中常用的信息类型之一，用于传输和处理激光雷达数据。
5.根据权利要求1所述的一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，所述步骤四的具体计算方法为：假设当前帧一共有a个点，其中地面点有b个点，天花板点有c个点，那么当前参与计算匹配度的点一共为a-b-c个点，匹配后得到位姿p，将当前帧的点云通过位姿转换和关键帧对齐，然后在参与匹配度计算的点中寻找匹配上的点，如果当前帧中的一个点周围l距离内能够找到关键帧中的点，则记为一个有效点，距离l为体素滤波的区域大小，这样一共可以选取d个有效点，则最终的两帧之间的匹配度为
6.根据权利要求5所述的一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，首先判断匹配度是否大于阈值，提前设置阈值为0.7，根据当前帧和关键帧匹配出来的位姿p和关键帧的位姿pt可以得出当前帧在全局地图坐标系下的位姿姿pt′＝pt+p，也就是当前帧点云在全局地图下的坐标，即当前定位的候选位姿，遍历所有的关键帧可以得到多个后选位姿。
7.根据权利要求1所述的一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，所述步骤五的具体步骤为：根据点云坐标的z轴进行过滤，只使用-0.5＜z＜0.5之间的点，然后将点云坐标的z轴设为0，将3d点云转化为2d点云，初始栅格地图中全为空闲点，当点云打到栅格中，则将栅格状态设为占据。
8.根据权利要求1所述的一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，所述二值栅格地图不存在未知状态的栅格和ray casting过程。
9.根据权利要求1所述的一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，所述步骤七的具体步骤为：遍历步骤四中得到的所有的候选位姿，将步骤六中的2d点云转化到步骤五中的栅格地图坐标系下，计算2d点云与栅格地图的匹配度。
10.根据权利要求9所述的一种基于关键帧地图的三维激光雷达重定位方法，其特征在于，所述匹配度的具体计算方法为：假设2d点云中一共有a个点，如果有一个点打到栅格地图的占据栅格中，则记为一个有效点，一共存在b个有效点，则匹配度为
