CN114280583A - 无gps信号下激光雷达定位精度验证方法及系统 - Google Patents

Abstract

本发明公开一种无GPS信号下激光雷达定位精度验证方法及系统，方法包括：获取目标路线的栅格地图、特征地图并进行存储；获取车辆所处道路环境的点云数据，以及车辆的运动数据；基于点云数据分别构建实时的栅格地图和特征地图；基于点云数据和车辆的运动数据进行车辆实时定位，确定定位精度验证的有效区域；将实时的栅格地图和特征地图分别与存储的栅格地图和特征地图进行有效区域的对比，得到栅格地图相似度和特征地图相似度；将栅格地图相似度和特征地图相似度进行融合，输出定位精度验证结果。本发明为开发者或使用者在地下矿区、无人区等信号差或无信号的区域提供了精度验证方法，提高了定位精度验证的适应性、鲁棒性与精准性，并具有实时性。

Description

无GPS信号下激光雷达定位精度验证方法及系统

技术领域

本发明属于自动驾驶技术领域，特别涉及一种针对无GPS信号场景中动态或静态条件下其车载激光雷达定位精度的验证方法，具体为一种无GPS信号下激光雷达定位精度验证方法及系统。

背景技术

汽车定位是汽车自动驾驶的核心技术之一，也是实现环境感知、路径规划与控制等后续功能的基础。汽车定位的精度验证又是汽车定位的关键技术之一，动态或静态条件下用户较难验证其车载激光雷达定位精度，但只有在验证了定位的精度满足实际场景及预期需求的条件下，定位方法才能被应用。

目前的定位精度验证方法仅仅是通过仿真试验或真实实验对比，定位精度验证的效率与精确性很难得到保证，例如L4以上级别的自动驾驶要求厘米级定位精度，这对定位精度验证的效率与精确性都有很高的要求。而且对于用户来说, 要直接验证所使用的车载激光雷达定位的定位精度较为困难。因为在动态条件下, 尤其是在无GPS信号的情况下，通信较为困难, 所以定位精度的验证也就难以保证。定位精度验证方法的提高能间接地更好地满足智慧城市发展新需求。

现有研究中，激光雷达的自主定位可分为建图和定位2个步骤，较早的研发中建图主要通过卡尔曼滤波器等方法实现，较近的研发中已经逐步采用基于非线性优化的SLAM算法，即同时定位与建图的方法。至于定位精度的验证，目前为止，有关针对无GPS信号场景中动态或静态条件下车载激光雷达定位精度验证方法的文献较少。

发明内容

为克服上述现有技术的不足，本发明提供一种无GPS信号下激光雷达定位精度验证方法及系统，用于为开发者或使用者在地下矿区、无人区等信号差或无信号的区域提供一种精度验证方法，提高定位精度验证的适应性、鲁棒性与精准性，并使其具有实时性。

根据本发明说明书的一方面，提供一种无GPS信号下激光雷达定位精度验证方法，包括：

获取目标路线的栅格地图、特征地图并进行存储；

获取车辆所处道路环境的点云数据，以及车辆的运动数据；

基于点云数据分别构建实时的栅格地图和特征地图；

基于点云数据和车辆的运动数据进行车辆实时定位，确定定位精度验证的有效区域；

将实时的栅格地图和特征地图分别与存储的栅格地图和特征地图进行有效区域的对比，得到栅格地图相似度和特征地图相似度；

将栅格地图相似度和特征地图相似度进行融合，输出定位精度验证结果。

上述技术方案适用于辅助开发者及使用者在无GPS信号场景下实时验证激光雷达的定位精度，该方法在准确获取车辆周围道路环境点云数据和车辆位姿的同时，构建栅格地图与特征地图，并进行车辆实时定位以确定精度验证的有效区域；然后将实时构建的栅格地图、特征地图与存储的两者原始地图分别进行对比，获取两种地图各自的相似度，并通过对两地图相似度的融合计算，验证车辆在无GPS信号场景下使用车载激光雷达定位的精度，且使定位精度验证能够确保定位的有效性、可参考性和实用性。

当精度验证结果不符合预设目标时，使用深度学习中预训练后的模型，通过梯度下降法不断降低定位精度作为输入得到的代价函数值，优化对比过程，直至精度验证结果符合预设目标。

作为进一步的技术方案，基于点云数据构建实时的栅格地图，进一步包括：基于SLAM算法通过点云匹配获取车辆的相对位姿；通过图优化方法对车辆的相对位姿进行后端调整；输出自适应分辨率的环境信息平面栅格地图。

SLAM算法通常分为前端和后端，前端使用激光雷达点云数据与当前利用Gmapping算法构建完成的部分栅格地图即子地图进行对比，得到车辆的相对位姿变化。由于栅格地图本来是离散的，为了保证匹配算法的目标函数在进行迭代计算时能够稳定且收敛，需要通过插值处理来生成光滑的概率地图。选用双三次插值方法进行处理，产生的地图更加平滑且稳定。

后端优化用于修正SLAM过程产生的累积误差，提高车辆定位和地图构建的准确度，后端优化通过构造非线性最小二乘问题来实现。

作为进一步的技术方案，基于点云数据，使用EKF-SLAM算法构建实时的特征地图。通过构建特征地图能够提供更多的几何特征信息，以提高定位精度验证的准确性。

作为进一步的技术方案，基于点云数据和车辆的运动数据进行车辆实时定位，进一步包括：基于点云数据，通过车辆的运动模型确定车辆当前时刻的先验位姿分布；基于车辆的运动数据，确定当前时刻的车辆角度；利用当前时刻的车辆角度替换先验位姿分布中的车辆角度；在先验位姿周围投下大量的带权重随机粒子，粒子的权重代表着该位置与栅格地图的匹配程度；通过重采样过程得到车辆位姿的后验概率分布。通过蒙特卡洛定位方法实现车辆的实时定位，并利用车辆运动数据中所获取的车辆角度对基于运动模型得到的先验位姿分布中的车辆角度进行修正，以提高车辆实时定位精度。

作为进一步的技术方案，实时的栅格地图和特征地图分别与存储的栅格地图和特征地图进行对比，进一步包括：分别提取实时栅格地图、实时特征地图、存储栅格地图及存储特征地图的特征三角形；基于法向量匹配提取各特征三角形中的有效区域；基于动态演化抽取各有效区域的多尺度特征，得到各特征三角形的特征向量；采用最近邻的原则将实时栅格地图与存储栅格地图的特征向量以及实时特征地图与存储特征地图的特征向量分别进行对比，输出栅格地图相似度及特征地图相似度。

进一步地，首先，采用Crust算法得到两地图的三角网格

，其中

为网格中的单个三角形，为了明确对目标观测的边界，首先提取T的网格边缘，这里的网格边缘指只存在于唯一三角形中的边，网格边缘构成了当前的观测边界，若某边仅属于一个三角形，不作为任意两个三角形的公共边出现，则将此边作为网格边缘进行提取，得到两地图的特征三角形集合F1，对于存储的高精地图模块中的两地图同理得到为F2。（1）对于 F1中的任一三角形

，其法向量为

，以其内心为球心，以

为半径在 F1中搜索内心位于球体范围内的近邻三角形集合，（2）若近邻集合为空，则将

记为空匹配，若集合不为空，则以法向量夹角最小化为原则，找到其最近邻三角形

，将两者作为匹配对并记录其最小夹角

。（3）经过步骤（2），将所有非空匹配对中包含的三角形记为特征三角形中的初始有效区域；利用动态演化方法中现成的Pre-trainModel计算F1、F2的特征向量；对于F1与F2的特征向量，采用最近邻的原则进行特征匹配，为了避免单向匹配带来的问题，对两F1、F2中各个描述子进行双向最近匹配；因此通过两地图与原始高精地图的重合情况对定位精度有一个大致的判断。

根据本发明说明书的另一方面，提供一种无GPS信号下激光雷达定位精度验证系统，用于实现所述的方法，所述系统包括：

存储模块，用于获取目标路线的栅格地图、特征地图并进行存储；

采集模块，用于获取车辆所处道路环境的点云数据，以及车辆的运动数据；

实时构图模块，用于基于点云数据分别构建实时的栅格地图和特征地图；

实时定位模块，用于基于点云数据和车辆的运动数据进行车辆实时定位，确定定位精度验证的有效区域；

对比模块，用于将实时的栅格地图和特征地图分别与存储的栅格地图和特征地图进行有效区域的对比，得到栅格地图相似度和特征地图相似度；

输出模块，用于将栅格地图相似度和特征地图相似度进行融合，输出定位精度验证结果。

上述技术方案对无GPS信号环境下的待开发路线或待使用路线，通过目标车行驶该环境下所有路线进行激光雷达数据获取，生成目标路线的高精地图，并据此得到目标路线的栅格地图、特征地图且存储在车载计算装置或单独的存储装置内；当这些路线在实际使用时，通过车辆的激光雷达点云数据构建实时的栅格地图和特征地图，以及进行车辆实时定位，并将实时构建的地图与存储的地图进行对比，对车辆在有效区域的定位精度进行验证；且通过栅格地图与特征地图的分别对比、对比结果的再融合，提高精度验证的准确性。

当定位精度验证结果满足预设目标时，输出的当前车辆实时定位的准确性更高。

作为进一步的技术方案，所述采集模块包括激光雷达、轮速传感器和转矩传感器。转矩传感器，用于获得横向转向角速度，作为先验位姿分布中的车辆角度。

作为进一步的技术方案，所述实时构图模块、实时定位模块、对比模块和输出模块均通过车载计算装置实现。

与现有技术相比，本发明的有益效果在于：

（1）本发明提供一种方法，该方法适用于辅助开发者及使用者在无GPS信号场景下实时验证激光雷达的定位精度，该方法在准确获取车辆周围道路环境点云数据和车辆位姿的同时，构建栅格地图与特征地图，并进行车辆实时定位以确定精度验证的有效区域；然后将实时构建的栅格地图、特征地图与存储的两者原始地图分别进行对比，获取两种地图各自的相似度，并通过对两地图相似度的融合计算，验证车辆在无GPS信号场景下使用车载激光雷达定位的精度，且使定位精度验证能够确保定位的有效性、可参考性和实用性。

（2）本发明提供一种系统，该系统对无GPS信号环境下的待开发路线或待使用路线，通过目标车行驶该环境下所有路线进行激光雷达数据获取，生成目标路线的高精地图，并据此得到目标路线的栅格地图、特征地图且存储在车载计算装置或单独的存储装置内；当这些路线在实际使用时，通过车辆的激光雷达点云数据构建实时的栅格地图和特征地图，以及进行车辆实时定位，并将实时构建的地图与存储的地图进行对比，对车辆在有效区域的定位精度进行验证；且通过栅格地图与特征地图的分别对比、对比结果的再融合，提高精度验证的准确性。

附图说明

图1为根据本发明实施例的无GPS信号下激光雷达定位精度验证方法的流程示意图。

图2为根据本发明实施例的网格化地图示意图。

图3为根据本发明实施例的地图对比步骤示意图。

图4为根据本发明实施例的栅格地图构建示意图。

图5为根据本发明实施例的无GPS信号下激光雷达定位精度验证系统的示意图。

具体实施方式

以下将结合附图对本发明各实施例的技术方案进行清楚、完整的描述，显然，所描述发实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所得到的所有其它实施例，都属于本发明所保护的范围。

本发明提供一种无GPS信号下激光雷达定位精度验证方法，如图1所示，包括：

步骤1，原始地图构建步骤。对无GPS信号环境下的待开发路线或待使用路线，通过目标车行驶该环境下所有路线进行激光雷达数据获取，生成目标路线的高精地图，并据此得到目标路线的栅格地图、特征地图且存储在车载计算装置或单独的存储装置内。

步骤2，数据获取步骤。将四个激光雷达（可采用Livox Horizon）分别布置于车辆的前后两端，用来检测、识别和获取道路环境信息；激光雷达采集的数据传输至工控机进行消噪、运算处理。

在车辆运动数据采集步骤中，利用安装在智能车上的轮速传感器获得车辆的纵向车速信息，利用转矩传感器获得车辆的横向转向角速度信息等进行车辆的运动数据采集。

为便于直观理解车辆运动数据，可进行道路场景网格划分，以自车为中心建立直角坐标系，横坐标方向为自车的速度方向，纵坐标方向是与自车速度方向垂直的方向；建立网格化地图，如图2所示。

步骤3，实时构图步骤。该步骤利用SLAM算法基于点云数据分别构建实时的栅格地图和特征地图。

SLAM算法通常分为前端和后端，前端使用激光雷达点云数据与当前利用Gmapping算法构建完成的部分栅格地图即子地图进行对比，得到车辆的相对位姿变化；通过图优化方法对车辆的相对位姿进行后端调整；输出自适应分辨率的环境信息平面栅格地图。

由于栅格地图本来是离散的，为了保证匹配算法的目标函数在进行迭代计算时能够稳定且收敛，需要通过插值处理来生成光滑的概率地图。选用双三次插值方法进行处理，产生的地图更加平滑且稳定。

后端优化用于修正SLAM过程产生的累积误差，提高车辆定位和地图构建的准确度，后端优化通过构造非线性最小二乘问题作为目标函数来实现。

如图3所示，概率占栅格地图 M 的取值被限制在

之间，表示栅格被占据的概率，每个栅格在作为图片显示时，也是一个像素，当每一帧激光雷达扫描数据被插入到概率栅格中时，都有一组表示命中与一组表示未命中的点集被计算。如果命中，就将最接近激光雷达扫描点的栅格加入到命中点集中。如果未命中，就将激光雷达扫描的原点与命中点连线，将连线上所经过的栅格都加入到未命中点集中。

按照激光雷达位姿的变换公式进行匹配，激光雷达数据与子图之间的匹配通过构造一个非线性最小二乘问题得到解决，目标函数为

，式中：K为一次扫描数据的点集总数，k为点集K中的第 k 个扫描点；

为第 k 个扫描点的空间坐标，

为坐标系的变换矩阵；

为经过插值处理的概率占栅格地图。

为提高定位精度验证的准确性，还基于点云数据，使用EKF-SLAM算法构建实时的特征地图，以提供更多的几何特征信息。

步骤4，车辆实时定位步骤。该步骤基于点云数据和车辆的运动数据进行车辆实时定位，确定定位精度验证的有效区域。

车辆实时定位通过蒙特卡洛定位方法予以实现。首先，通过运动模型确定车辆当前时刻的先验位姿分布，位姿分布会使用车辆当前时刻的角度，一般使用通过运动模型计算得到的角度，本发明使用车辆直接采集的运动数据中的当前时刻车辆角度，达到了对位姿数据的修正效果；然后，在先验位姿周围投下大量的带权重随机粒子，粒子的权重代表着该位置与地图的匹配程度，匹配程度越高的粒子就越有可能在重采样过程中被选入后验概率分布的粒子集；最后，通过重采样过程得到车辆位姿的后验概率分布，即车辆实时定位。粒子滤波器通过迭代方式不断更新车辆位姿的后验概率分布，以逐渐消除误差，最终达到精确定位车辆的目的。

步骤5，相似度对比步骤，将实时的栅格地图和特征地图分别与存储的栅格地图和特征地图进行有效区域的对比，得到栅格地图相似度和特征地图相似度。

如图4所示，该步骤先提取两地图中的特征三角形加快后续计算，再基于法向量匹配的方法提取两地图中的有效区域，即数据有效区域，然后基于复杂网络的动态演化抽取两地图有效区域的多尺度特征，最终分别得到两地图与原始高精地图的相似度。

进一步来说，首先，采用Crust算法得到两地图的三角网格

，其中

为网格中的单个三角形，为了明确对目标观测的边界，首先提取T 的网格边缘，这里的网格边缘指只存在于唯一三角形中的边，网格边缘构成了当前的观测边界，若某边仅属于一个三角形，不作为任意两个三角形的公共边出现，则将此边作为网格边缘进行提取，得到两地图的特征三角形集合F1，对于高精地图模块中的两地图同理得到为F2；利用现成的Pre-train Model计算F1、F2的高维特征向量，然后对特征向量，计算余玄距离；对于F1与F2的特征向量，采用最近邻的原则进行特征匹配，为了避免单向匹配带来的问题，对两F1、F2中各个描述子进行双向最近匹配；因此通过两地图与原始高精地图的重合情况对定位精度有一个大致的判断。

步骤6，相似度融合步骤，将栅格地图相似度和特征地图相似度进行融合，输出定位精度验证结果。具体地，分别将两地图的定位精度结果作为输入通过特定带参数的求平均函数进行计算，输出最终精度验证结果。

进一步地，还可记录存储对比过程，利用深度学习使对比过程不断进化，进而不断提高定位精度验证的适应性、鲁棒性、精准性与效率。

本发明还提供一种无GPS信号下激光雷达定位精度验证系统，用于实现所述的方法，如图5所示，所述系统包括：存储模块，用于获取目标路线的栅格地图、特征地图并进行存储；采集模块，用于获取车辆所处道路环境的点云数据，以及车辆的运动数据；实时构图模块，用于基于点云数据分别构建实时的栅格地图和特征地图；实时定位模块，用于基于点云数据和车辆的运动数据进行车辆实时定位，确定定位精度验证的有效区域；对比模块，用于将实时的栅格地图和特征地图分别与存储的栅格地图和特征地图进行有效区域的对比，得到栅格地图相似度和特征地图相似度；输出模块，用于将栅格地图相似度和特征地图相似度进行融合，输出定位精度验证结果。

所述采集模块包括激光雷达和车载传感器，车载传感器包括轮速传感器和转矩传感器。激光雷达设有四个，分别布置于车辆的前后两端，用来检测、识别和获取道路环境信息；激光雷达采集的数据传输至工控机进行消噪、运算处理。

轮速传感器获得车辆的纵向车速信息；转矩传感器获得车辆的横向转向角速度信息。

作为进一步的技术方案，所述实时构图模块、实时定位模块、对比模块和输出模块均通过车载计算装置实现。

本发明系统对无GPS信号环境下的待开发路线或待使用路线，通过目标车行驶该环境下所有路线进行激光雷达数据获取，生成目标路线的高精地图，并据此得到目标路线的栅格地图、特征地图且存储在车载计算装置或单独的存储装置内；当这些路线在实际使用时，通过车辆的激光雷达点云数据构建实时的栅格地图和特征地图，以及进行车辆实时定位，并将实时构建的地图与存储的地图进行对比，对车辆在有效区域的定位精度进行验证；且通过栅格地图与特征地图的分别对比、对比结果的再融合，提高精度验证的准确性。

当定位精度验证结果满足预设目标时，输出的当前车辆实时定位的准确性更高。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案。

Claims (8)

1.无GPS信号下激光雷达定位精度验证方法，其特征在于，包括：

获取目标路线的栅格地图、特征地图并进行存储；

获取车辆所处道路环境的点云数据，以及车辆的运动数据；

基于点云数据和车辆的运动数据进行车辆实时定位，确定定位精度验证的有效区域；

基于点云数据分别构建实时的栅格地图和特征地图；

将实时的栅格地图和特征地图分别与存储的栅格地图和特征地图进行有效区域的对比，得到栅格地图相似度和特征地图相似度；

将栅格地图相似度和特征地图相似度进行融合，输出定位精度验证结果。

2.根据权利要求1所述无GPS信号下激光雷达定位精度验证方法，其特征在于，基于点云数据构建实时的栅格地图，进一步包括：基于SLAM算法通过点云匹配获取车辆的相对位姿；通过图优化方法对车辆的相对位姿进行后端调整；输出自适应分辨率的环境信息平面栅格地图。

3.根据权利要求1所述无GPS信号下激光雷达定位精度验证方法，其特征在于，基于点云数据，使用EKF-SLAM算法构建实时的特征地图。

4.根据权利要求1所述无GPS信号下激光雷达定位精度验证方法，其特征在于，基于点云数据和车辆的运动数据进行车辆实时定位，进一步包括：基于点云数据，通过车辆的运动模型确定车辆当前时刻的先验位姿分布；基于车辆的运动数据，确定当前时刻的车辆角度；利用当前时刻的车辆角度替换先验位姿分布中的车辆角度；在先验位姿周围投下大量的带权重随机粒子，粒子的权重代表着该位置与栅格地图的匹配程度；通过重采样过程得到车辆位姿的后验概率分布。

5.根据权利要求1所述无GPS信号下激光雷达定位精度验证方法，其特征在于，实时的栅格地图和特征地图分别与存储的栅格地图和特征地图进行对比，进一步包括：分别提取实时栅格地图、实时特征地图、存储栅格地图及存储特征地图的特征三角形；基于法向量匹配提取各特征三角形中的有效区域；基于动态演化抽取各有效区域的多尺度特征，得到各特征三角形的特征向量；采用最近邻的原则将实时栅格地图与存储栅格地图的特征向量以及实时特征地图与存储特征地图的特征向量分别进行对比，输出栅格地图相似度及特征地图相似度。

6.无GPS信号下激光雷达定位精度验证系统，用于实现权利要求1-5任一项所述的方法，其特征在于，所述系统包括：

存储模块，用于获取目标路线的栅格地图、特征地图并进行存储；

采集模块，用于获取车辆所处道路环境的点云数据，以及车辆的运动数据；

实时构图模块，用于基于点云数据分别构建实时的栅格地图和特征地图；

实时定位模块，用于基于点云数据和车辆的运动数据进行车辆实时定位，确定定位精度验证的有效区域；

对比模块，用于将实时的栅格地图和特征地图分别与存储的栅格地图和特征地图进行有效区域的对比，得到栅格地图相似度和特征地图相似度；

输出模块，用于将栅格地图相似度和特征地图相似度进行融合，输出定位精度验证结果。

7.根据权利要求6所述无GPS信号下激光雷达定位精度验证系统，其特征在于，所述采集模块包括激光雷达、轮速传感器和转矩传感器。

8.根据权利要求6所述无GPS信号下激光雷达定位精度验证系统，其特征在于，所述实时构图模块、实时定位模块、对比模块和输出模块均通过车载计算装置实现。

Images