[发明专利]车辆初始化定位方法、系统及计算机可读介质

说明书

本发明公开了一种车辆初始化定位方法，本发明采用点云匹配技术，其将当前雷达输出的点云数据和高精度点云地图中的关键帧进行数据匹配，然后根据关键帧的位姿，最终得到车辆的粗略定位状态。然后将其输入NDT模块，最终得到车辆高精度定位状态。本发明还提供了一种车辆初始化定位系统及计算机可读介质。发明不仅对车辆初始化状态下的定位的精度更高，更准确，而且具有较高的独立性、经济性、鲁棒性，可扩展性。还可以与GPS、UWB、蓝牙等定位模块配合使用。

技术领域

本发明属于自动驾驶领域，特别涉及车辆初始化定位方法、系统及计算机可读介质。

背景技术

初始化定位是自动驾驶领域的关键技术。常用GPS、UWB、蓝牙、地磁等定位模块提供车辆的在全局坐标系的相对位置，进而为后续的定位、感知、规划等模块提供参考数据。

基于GPS的定位方案在地下车库、桥隧、楼宇等有遮挡的环境中无法使用，此外低成本GPS的定位精度较低。高精度GPS价格高昂，而且在静止状态无法提供航向信息。

UWB、蓝牙、地磁等定位方案，需要额外的基站设备搭建，相对投资较高，同时其对电磁环境、遮挡环境等要求较高，其无法输出航向信息，且定位精度相对较低。

发明内容

发明目的：本发明针对现有技术存在的问题，提出了一种使用成本低，定位精准的车辆初始化定位方法。

技术方案：为实现上述目的，本发明提供了一种车辆初始化定位方法，包括以下步骤：

构建slam中的关键帧数据库；

对关键帧数据库中所有的关键帧进行特征提取；得到关键帧特征矩阵；

对激光雷达采集的当前帧的激光点云数据进行特征提取；得到当前帧特征矩阵；

对得到关键帧特征矩阵和当前帧特征矩阵进行匹配，根据公式：

计算关键帧特征矩阵和当前帧特别矩阵的相似度d，式中，p_i表示关键帧特征矩阵或当前帧特别矩阵中第i行的权重，M表示当前帧特征矩阵或关键帧特征矩阵的总行数，当前帧特征矩阵和关键帧特征矩阵的行数和列数均相同，两个矩阵中的p_i相同，i表示每一行的编号，表示关键帧矩阵中第i行行向量，表示当前帧矩阵中第i行行向量；

选择与当前帧相似度最大的多个关键帧分别构建局部点云地图；

将当前帧的点云数据分别与选择的每个关键帧对应的局部点云地图进行NDT匹配，取NDT输出分数最小的位置信息作为初始化定位信息。

进一步，关键帧特征矩阵和当前帧特征矩阵的获取方法相同，主要包括：以激光雷达的原点为中心，激光雷达检测的最远距离L为半径做圆，将圆分别切割M个圆环和N个扇面，共形成M*N个扇区；其中，

L为激光雷达检测的最远距离，D为相邻的圆环之间的距离；A表示每个扇区的角度；计算每个扇区中所有点云的高度值平均值，作为特征矩阵中的元素值。

更进一步，D的长度在0.5m-1.5m之间；A的角度在2°～8°之间。这样筛选出的特征精度更高。更有利于车辆的定位。

进一步，所述p_i的设置方法为：将关键帧特征矩阵或当前帧特别矩阵中1～20％M行的权重设置为0.05，关键帧特征矩阵或当前帧特别矩阵中21％M～80％M行的权重设置为0.9，关键帧特征矩阵或当前帧特别矩阵中81％M～100％M行的权重设置为0.05。这样计算出的相似度更加的准确，使车辆的初始化定位更加准确。