一种基于关键帧地图的三维激光雷达定位方法

技术领域

本发明涉及机器人slam技术领域，尤其涉及一种基于关键帧地图的三维激光雷达定位方法。

背景技术

当前机器人领域中的3D激光雷达定位技术主要依赖于所建立的全局地图，在定位的时候需要将当前帧和全局地图进行匹配，地图越大，所计算的时间也就越长，无法满足实时定位的需求。当前行业内使用比较多的方案主要有两种，第一种是对建立的全局地图进行切割，分成多个小地图，在定位使用的时候当前帧和小地图进行匹配；第二种是对全局地图进行降采样，将全局地图变得很稀疏，然后定位的时候当前帧和降采样后的全局地图进行匹配。前者需要人工介入，后者会影响定位的精度。

发明内容

本发明提出的一种基于关键帧地图的三维激光雷达定位方法，解决了现有的问题。

为了实现上述目的，本发明采用了如下技术方案：一种基于关键帧地图的三维激光雷达定位方法，基于自研的地图格式，地图中保存关键帧的点云和关键帧的pose，在定位时将当前帧和全局地图进行匹配，满足实时定位的需求，具体包括以下步骤：

步骤一、加载地图：地图中保存有关键帧的点云和关键帧的pose；

步骤二、解析地图：将地图中的关键帧点云以及关键帧的位姿提取出来并进行解析；

步骤三、粗位姿推算：输入imu和odometry数据，这两个传感器的数据为可选项，用作当前帧点云定位的粗位姿；

步骤四、生成关键帧子图：根据得出的粗位姿P

步骤五、数据预处理：输入当前帧激光点云，该数据由激光雷达传感器获得，在接收到数据之后首先对数据进行预处理；

步骤六、当前帧和关键帧进行匹配：对输入的激光点云降采样之后就可以和步骤四中选定局部子图进行匹配，采用《Generalized-ICP》论文中提出的GICP为匹配算法；

步骤七、精位姿再循环：根据步骤六得出的精位姿再循环到步骤3更新上一帧的位姿P

优选的，所述地图采用本公司自研的建图算法并保存为本公司自研的地图格式。

优选的，所述imu数据用于定位和导航，是一种用于测量和跟踪物体姿态的传感器，通过对imu输出的姿态信息进行处理，可以在空间中的位置、方向和速度等信息，从而实现自主运动和导航，也可用于姿态控制、运动控制和稳定控制等应用；所述odometry数据可以用于测速和测距，提供对机器人位姿和速度的局部准确估计。

优选的，首先判断是否输入odometry数据，若是，则将odometry数据传递至步骤三；同时判断是否输入imu数据，若是，则将imu数据传递至步骤三，将两项数据作为当前帧点云定位的粗位姿。

优选的，所述步骤三的具体方法为：上一时刻位姿P

优选的，如果不使用imu和odometry数据，则将运动模型考虑为匀速运动模型，上一时刻t-1位姿为P

优选的，所述步骤四的具体方法为：首先以当前位姿P

优选的，所述步骤五的具体方法为：将点云根据激光雷达的外参，即激光雷达在车体中心坐标系下的安装位置，转换到车体坐标系下，该参数由本公司研发的激光雷达标定算法给出，接着在当前激光点云中剔除掉车体的部分，然后根据激光点云的稠密程度对点云做简单的降采样处理，最后将激光点云根据求出的P

优选的，所述步骤六的具体方法为：根据当前帧和局部子图匹配出来的位姿P和粗位姿P

本发明的有益效果为：基于自研的地图格式，地图中保存关键帧的点云和关键帧的pose，并基于关键帧的定位算法，在定位时将当前帧和全局地图进行匹配，满足实时定位的需求，本发明有效地避免了在建图完成后人工进行地图分割，建完地图之后不需要人工对地图进行裁剪，在定位过程中在降低计算量的同时保证定位的精度，不过多的占用cpu的资源，在大地图场景下定位也不增长资源占用，减少了人为错误，提高了定位效率。

附图说明

图1为本发明的定位系统流程图。

图2为本发明的关键帧子图的选取规则。

图3为本发明的该次匹配的目标值。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

参照图1-图3所示，一种基于关键帧地图的三维激光雷达定位方法，基于自研的地图格式，地图中保存关键帧的点云和关键帧的pose，并基于关键帧的搜索算法和定位算法，在定位时将当前帧和全局地图进行匹配，满足实时定位的需求，具体包括以下步骤：

步骤一、加载地图：地图中保存有关键帧的点云和关键帧的pose；

步骤二、解析地图：将地图中的关键帧点云以及关键帧的位姿提取出来并进行解析；

步骤三、粗位姿推算：输入imu和odometry数据，这两个传感器的数据为可选项，用作当前帧点云定位的粗位姿；

步骤四、生成关键帧子图：根据得出的粗位姿选取全局地图中的子图，子图由多个全局地图中的关键帧组成；

步骤五、数据预处理：输入当前帧激光点云，该数据由激光雷达传感器获得，在接收到数据之后首先对数据进行预处理；

步骤六、当前帧和关键帧进行匹配：对输入的激光点云降采样之后就可以和步骤四中选定局部子图进行匹配，采用《Generalized-ICP》论文中提出的GICP为匹配算法；

步骤七、精位姿再循环：根据步骤六得出的精位姿再循环到步骤3更新上一帧的位姿，再对下一帧进行计算。

地图采用本公司自研的建图算法并保存为本公司自研的地图格式，imu数据用于定位和导航，是一种用于测量和跟踪物体姿态的传感器，通过对imu输出的姿态信息进行处理，可以在空间中的位置、方向和速度等信息，从而实现自主运动和导航，也可用于姿态控制、运动控制和稳定控制等应用；odometry数据可以用于测速和测距，可以提供对机器人位姿和速度的局部准确估计，odometry信息可以从各种来源获得，例如imu、radar和车轮编码器等，由于imu会随时间漂移，而车轮编码器会随航行距离漂移，因此它们经常一起使用可以抵消各自的负面特性。

首先判断是否输入odometry数据，若是，则将odometry数据传递至步骤三；同时判断是否输入imu数据，若是，则将imu数据传递至步骤三，将两项数据作为当前帧点云定位的粗位姿，上一时刻位姿P

参照图2和图3，以当前位姿P

GICP方法又称广义迭代最近点方法，GICP算法是一种基于点到面的配准方法，它通过最小化点云之间的距离来实现点云的对齐.与传统的ICP算法相比，GICP算法引入了点云的法线信息,提高了配准的稳定性和精度，然后根据当前帧和局部子图匹配出来的位姿P和粗位姿P

本发明有效地避免了在建图完成后人工进行地图分割和裁剪，在定位过程中在降低计算量的同时保证定位的精度，不过多的占用cpu的资源，在大地图场景下定位也不增长资源占用，减少了人为错误，提高了定位效率。

本发明在工作中，首先基于自研的地图格式，地图中保存关键帧的点云和关键帧的pose，将地图中的关键帧点云以及关键帧的位姿提取出来并进行解析，判断是否输入odometry数据，若是，则将odometry数据传递至粗位姿推算；同时判断是否输入imu数据，若是，则将imu数据传递至粗位姿推算，将两项数据作为当前帧点云定位的粗位姿，根据得出的粗位姿选取全局地图中的子图，子图由多个全局地图中的关键帧组成，然后输入当前帧激光点云并对数据进行预处理，将当前帧和关键帧进行匹配，最后得到当前帧的定位精位姿，将得出的精位姿再循环到粗位姿推算，从而更新上一帧的位姿，重复上述流程对下一帧进行计算。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。