阅读说明：本技术 激光雷达定位方法及其相关装置 (Laser radar positioning method and related device thereof ) 是由 石鹏 林辉 卢维 殷俊 于 2020-06-18 设计创作，主要内容包括：本申请提供激光雷达定位方法及其相关装置。该激光雷达定位方法包括：估计激光雷达的预测位姿；基于预测位姿和获取到的当前帧激光点云数据确定当前帧激光点云数据中各个激光点的第一坐标；基于第一坐标筛除匹配程度差的激光点,得到第一激光点集合；基于预测位姿将第一激光点集合与栅格地图进行匹配,以得到第一激光点集合中激光点的第二坐标；基于第二坐标筛除匹配程度差的激光点,得到第二激光点集合；将第二激光点集合与地图进行匹配,以确定激光雷达的最终位姿。本申请的方法可以去除干扰点,以提高激光雷达的定位的准确性。(The application provides a laser radar positioning method and a related device thereof. The laser radar positioning method comprises the following steps: estimating the predicted pose of the laser radar; determining first coordinates of each laser point in the current frame laser point cloud data based on the predicted pose and the obtained current frame laser point cloud data; screening out laser points with poor matching degree based on the first coordinates to obtain a first laser point set; matching the first laser point set with the grid map based on the predicted pose to obtain a second coordinate of the laser point in the first laser point set; screening out laser points with poor matching degree based on the second coordinate to obtain a second laser point set; and matching the second laser point set with the map to determine the final pose of the laser radar. The method can remove the interference points so as to improve the positioning accuracy of the laser radar.)

激光雷达定位方法及其相关装置

技术领域

本申请涉及定位技术领域，特别是涉及激光雷达定位方法及其相关装置。

背景技术

目前，基于激光雷达的定位方法中，基于先验地图的定位方法应用最为普遍。利用激光雷达实时采集的环境点云信息与地图进行匹配来确定激光雷达位姿，这类方法的主要优点是定位过程不会累计误差。但是，这类方法的一大弊端是：当事先构建好地图的环境发生变化时，例如移动物体和局部环境结构发生改变(堆叠了一些物体等情况)，其定位性能会受到较大的影响。

发明内容

本申请提供激光雷达定位方法及其相关装置，可以去除干扰点，以提高激光雷达的定位的准确性。

为达到上述目的，本申请提供一种激光雷达定位方法，该方法包括：

估计激光雷达的预测位姿；

基于预测位姿和获取到的当前帧激光点云数据确定当前帧激光点云数据中各个激光点的第一坐标；

基于第一坐标筛除匹配程度差的激光点，得到第一激光点集合；

基于预测位姿将第一激光点集合与栅格地图进行匹配，以得到第一激光点集合中激光点的第二坐标；

基于第二坐标筛除匹配程度差的激光点，得到第二激光点集合；

将第二激光点集合与地图进行匹配，以确定激光雷达的最终位姿。

其中，基于第二坐标筛除匹配程度差的激光点，得到第二激光点集合；

基于第二坐标将第一激光点集合中距离相近的激光点进行聚类，得到多个类；

从每一个类筛选出距离第二匹配占据栅格大于第一阈值的激光点的点集，其中，每个激光点的第二匹配占据栅格是基于第二坐标确定的最靠近每个激光点的占据栅格；

确认点集中激光点与第二匹配占据栅格的匹配程度，筛除匹配程度差的激光点；和/或，

在点集的激光点总数小于第二阈值，且点集的激光点总数与点集所属的类中激光点的总数的比值小于第三阈值时，筛除小于第三阈值的点集中所有激光点。

其中，确认点集中激光点与第二匹配占据栅格的匹配程度，筛除匹配程度差的激光点，包括：

确认点集中对应于同一第二匹配占据栅格的激光点的数目，在数目超过第四阈值，且对应于同一第二匹配占据栅格的激光点中相距最远的两个激光点之间的距离超过第五阈值时，将对应于同一第二匹配占据栅格的激光点筛除；和/或，

将激光点与点集中激光点在点集所属类中相邻的多个激光点作为特征计算点集，基于特征计算点集计算出点集中激光点所属几何图形的几何特征；并基于特征计算点集中所有激光点的第二匹配占据栅格计算出参考几何特征，基于几何特征和参考几何特征的相似程度将相似程度低的激光点筛除。

其中，基于特征计算点集计算出点集中激光点所属几何图形的几何特征，包括：利用特征计算点集分布的协方差的特征值和特征向量近似求解出激光点的法向量和曲率半径；

基于特征计算点集中所有激光点的第二匹配占据栅格计算出参考几何特征，包括：基于特征计算点集中所有激光点的第二匹配占据栅格的中心点分布的协方差的特征值和特征向量近似求解出激光点的参考法向量和参考曲率半径；

基于几何特征和参考几何特征的相似程度将相似程度低的激光点筛除，包括：在激光点的法向量和参考方向量之间的夹角大于第六阈值且小于第七阈值时，将大于第六阈值且小于第七阈值的激光点筛除；

在激光点的曲率半径和参考曲率半径的差值的绝对值大于第八阈值时，将大于第八阈值的激光点筛除。

其中，从每一个类筛选出距离第一占据栅格大于第一阈值的激光点的点集，之前包括：根据激光点的第二坐标和激光点的第一占据栅格的中心点的坐标计算出激光点的残差向量；

从每一个类筛选出距离第一占据栅格大于第一阈值的激光点的点集，包括：从每一个类筛选出残差向量模长大于第一阈值的激光点的点集，其中，激光点残差向量模长是由激光点的残差向量计算得到的。

其中，将第二激光点集合与地图进行匹配，以确定激光雷达的最终位姿，之前包括：根据激光点的第二坐标和激光点的第一占据栅格的中心点的坐标计算出激光点的残差向量；

将第二激光点集合与地图进行匹配，以确定激光雷达的最终位姿，之前包括：确定第二激光点集合中每一激光点的权重系数，根据第二激光点集合中每一激光点的权重系数和残差向量确定激光雷达的最终位姿。

其中，第二激光点集合中每一激光点的权重系数初始为1，确定第二激光点集合中每一激光点的权重系数，包括：

根据第二激光点集合中激光点的残差向量计算出激光点的残差向量方向角度；

基于残差向量方向角度将残差向量方向相似的激光点聚类，得到多个子集；

将残差向量方向相反的子集对中的子集逐一与栅格地图匹配，以确认子集的匹配评分，基于匹配评分更新子集中所有激光点的权重系数。

其中，将残差向量方向相反的子集对中的子集逐一与栅格地图匹配，包括：

确认残差向量方向相反的子集对中每一子集中所有激光点的残差向量模长和，并计算第二激光点集合中所有激光点的残差向量模长和；

确认残差向量方向相反的子集对中所有子集的残差向量模长和与第二激光点集合的残差向量模长和的比值是否大于第九阈值；

在比值大于第九阈值，且残差向量方向相反的子集对中所有子集中激光点的数目与第二激光点集合中激光点的总数的比值超过第十阈值时，将残差向量方向相反的子集对中的子集逐一与栅格地图匹配。

其中，基于第一坐标筛除匹配程度差的激光点，包括：

将基于第一坐标确定的最靠近每个激光点的占据栅格作为每个激光点的第一匹配占据栅格；

将距离第一匹配占据栅格大于第十一阈值的激光点筛除。

其中，估计激光雷达的预测位姿，包括：

基于其他传感器信息计算激光雷达的预测位姿，或，

基于由前几帧激光点云数据确定的激光雷达的最终位姿估计激光雷达的线速度和偏航角速度，基于线速度和偏航角速度和当前帧激光点云数据和前一帧激光点云数据之间的时间间隔确定激光雷达的预测位姿。

为达到上述目的，本申请提供一种激光雷达定位装置，该激光雷达定位装置包括存储器和处理器；存储器中存储有计算机程序，处理器用于执行计算机程序以实现上述方法的步骤。

为达到上述目的，本申请提供一种计算机可读存储介质，其上存储有计算机程序，程序被处理器执行时实现上述方法的步骤。

本申请的方法是：先基于激光雷达的预测位姿确定各个激光点的第一坐标，并基于第一坐标确定激光点与栅格地图的匹配程度，将匹配程度差的激光点筛除，这样在匹配前筛除匹配程度差的激光点，可以提高匹配精度，使得第一激光点集合中激光点的第二坐标更接近真实坐标；在匹配后进一步基于第二坐标筛除匹配程度差的激光点，可以进一步去除干扰点，以提高激光雷达的定位的准确性。

附图说明

为了更清楚地说明本申请实施方式中的技术方案，下面将对实施方式描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施方式，对本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请激光雷达定位方法一实施方式的流程示意图；

图2是本申请激光雷达定位方法中基于第二坐标筛除匹配程度差的激光点的步骤一实施方式的流程示意图；

图3是本申请激光雷达定位装置一实施方式的结构示意图；

图4是本申请计算机可读存储介质一实施方式的结构示意图。

具体实施方式

为使本领域的技术人员更好地理解本申请的技术方案，下面结合附图和具体实施方式对本申请所提供的激光雷达定位方法及其相关装置做进一步详细描述。

具体请参阅图1，图1是本申请激光雷达定位方法一实施方式的流程示意图。本实施方式激光雷达定位方法包括以下步骤。

S110：估计激光雷达的预测位姿。

首先，在激光雷达定位过程中，可以先预测激光雷达的预测位姿，以便基于预测位姿筛除一些匹配程度差的激光点。

激光雷达的位姿信息包括激光雷达的位置信息和偏航角信息。其中激光雷达的位置信息可以指激光雷达的二维位置信息或三维位置信息。例如，激光雷达的位姿信息可以用表示。其中，激光雷达的预测位姿和准确位姿、初始位姿均可作为激光雷达的预测位姿。

在一实现方式中，可以基于其他传感器信息计算激光雷达的预测位姿。其他传感器可以是IMU，里程计等传感器。

在另一实现方式中，基于由前几帧激光点云数据确定的激光雷达的准确位姿，确定激光雷达的预测位姿。具体地，在位姿信息包括激光雷达的位置信息和偏航角信息的情况下，可以由前几帧激光点云数据确定的激光雷达的准确位姿估计激光雷达的线速度和偏航角速度，基于线速度和偏航角速度、当前帧激光点云数据和前一帧激光点云数据之间的时间间隔确定激光雷达的预测位姿。

示例性地，利用前两帧激光点云数据S_k-1和S_k-2确定的激光雷达的准确位姿估计当前时刻k的线速度和偏航角速度使用估计的线速度

和偏航角速度

和时间间隔Δt进行计算来预测当前时刻的激光雷达的预测位姿：

其中，

和

分别指线速度和偏航角速度，(x_k-1,y_k-1，ψ_k-1)是由前一帧激光点云数据确定的激光雷达的准确位姿，而

是当前帧激光点云数据的激光雷达的预测位姿。

当然，为了能够通过本申请激光雷达的定位方法得到更加准确的激光雷达的位姿，在估计激光雷达的预测位姿之前，要利用一帧激光点云数据，根据先验的初值范围，使用CSM(相关性匹配)和分支定界方法求解激光雷达的初始位姿。

S120：基于预测位姿和获取到的当前帧激光点云数据确定当前帧激光点云数据中各个激光点的第一坐标。

在预测位姿下计算当前帧激光点云数据S_k中每一激光点p_i(i＝1,2,…,N)在地图坐标系中的坐标，将该坐标作为每一激光点的第一坐标。

S130：基于第一坐标筛除匹配程度差的激光点，得到第一激光点集合。

基于每个激光点的第一坐标确定每个激光点与栅格地图的匹配程度，将匹配程度差的激光点筛除，保留匹配程度好的激光点，得到第一激光点集合。

可以理解的是，栅格地图上一般会存在与激光点云数据中激光点对应的占据栅格，所以可以基于每个激光点的第一坐标确定每个激光点的第一匹配占据栅格。可选地，可以先基于激光点的第一坐标，确定栅格地图上与激光点对应的栅格，然后将距离激光点对应的栅格最近的占据栅格作为激光点的第一匹配占据栅格。其中，栅格地图上激光点的坐标所在的栅格即为栅格地图上与激光点对应的栅格。激光点的第一坐标和激光点的第二坐标可以作为激光点的坐标。

其中，激光点和其第一匹配占据栅格之间的匹配程度可以指两者之间的位置相似度和两者之间的形状相似度等。相应的，与第一匹配占据栅格位置相似度和/或形状相似度很低的激光点可作为匹配程度差的激光点。

例如，激光点与其第一匹配占据栅格之间的匹配程度可以用激光点与其第一匹配占据栅格之间的距离表示。这样与第一匹配占据栅格之间的距离超过第十一阈值的激光点可作为匹配程度差的激光点。可以理解的是，两个栅格之间的距离可以由两个栅格的坐标计算得到。栅格的坐标可以用栅格的固定点的坐标表示。其中栅格的固定点可为栅格中心点、栅格左上顶点、栅格右上顶点等栅格中任意点。

当然，还可以通过其他方式确定激光点与其第一匹配占据栅格之间的距离。例如，在确定激光点与其第一匹配占据栅格之前，建立栅格地图对应的距离映射地图。在距离映射地图中，与占据栅格对应的距离为0，与非占据栅格对应的距离为与非占据栅格最近的占据栅格与非占据栅格的距离。然后在确定激光点与其第一匹配占据栅格之间的距离时，先确定激光点对应的栅格，基于距离映射地图确定激光点对应的栅格对应的距离。

S140：基于预测位姿将第一激光点集合与栅格地图进行匹配，以得到第一激光点集合中激光点的第二坐标。

以预测位姿作为匹配初值，将第一激光点集合与栅格地图进行匹配，以得到第一激光点集合中激光点的第二坐标。其中，匹配方法可以为ICP、NDT或相关性匹配方法等。

S150：基于第二坐标筛除匹配程度差的激光点，得到第二激光点集合。

基于每个激光点的第二坐标确定第一激光点集合中每个激光点与栅格地图的匹配程度，将匹配程度差的激光点筛除，保留第一激光点集合中匹配程度好的激光点，得到第二激光点集合。

可选地，可以基于每个激光点的第二坐标确定每个激光点的第二匹配占据栅格，然后确定第一激光点集合中每个激光点与其第二匹配占据栅格之间的匹配程度，将第一激光点集合中匹配程度差的激光点筛除，得到第二激光点集合。

其中，可以先基于激光点的第二坐标，确定栅格地图上与激光点对应的栅格，然后将距离激光点对应的栅格最近的占据栅格作为激光点的第二匹配占据栅格。

另外，每一激光点和其第二匹配占据栅格之间的匹配程度可以指两者之间的位置相似度和/或两者之间的形状相似度等。相应的，与第二占据栅格位置相似度和/或形状相似度很低的激光点可作为匹配程度差的激光点。

进一步地，可以将与第二匹配占据栅格的位置相似度或形状相似度较低的部分激光点作为匹配程度差的激光点。例如，将与第二匹配占据栅格的位置相似度的部分激光点作为匹配程度差的激光点。基于此，如图2所示，步骤S150可以包括以下步骤。

S151：基于第二坐标将第一激光点集合中距离相近的激光点进行聚类，得到多个类。

具体地，利用位置相邻的两个激光点的第二坐标计算位置相邻的两个激光点之间的距离，如果位置相邻的两个激光点之间的距离小于第十二阈值，则位置相邻的两个激光点同属一个类；而如果位置相邻的两个激光点之间的距离大于第十二阈值，则这位置相邻的两个激光点不属于同一个类。这样依次判断位置相邻的两个激光点之间的距离是否小于第十二阈值，将小于阈值的位置相邻的两个激光点归于同一个类，以得到若干个类。

其中，每个激光点都有对应的序号，并且序号相邻的两个激光点之间一般在位置上相邻，因此可以直接判断序号相邻的两个激光点之间的距离是否小于第十二阈值，以判断位置相邻的两个激光点之间的距离是否小于第十二阈值，简化计算过程。

S152：从每一个类筛选出距离第二匹配占据栅格大于第一阈值的激光点的点集。

其中，可以先计算出每个类中各个激光点与其第二匹配占据栅格之间的距离，然后从每一个类筛选出距离第二匹配占据栅格大于第一阈值的激光点，以得到每一类中距离大于第一阈值的激光点的点集。

可以理解的是，计算出每个类中各个激光点与其第二匹配占据栅格之间的距离的步骤可以在步骤S151之前执行。

在一实现方式中，可以直接计算每个类中各个激光点对应的栅格与各个激光点的第二匹配占据栅格之间的距离。

在另一实现方式中，可以根据激光点的第二坐标和激光点的第二匹配占据栅格的坐标计算出激光点的残差向量，然后计算各个激光点的残差向量的模长。其中，各个激光点的残差向量的模长即为各个激光点与各个激光点的第二匹配占据栅格之间的距离。可以理解的是，可以先利用激光点的第二坐标确定激光点对应的栅格，然后利用每个类中各个激光点的第二匹配占据栅格的坐标与各个激光点对应的栅格的坐标计算出各个激光点的残差向量。并且，各个激光点的残差向量的具体计算公式如下所示：

其中，指各个激光点的第二匹配占据栅格的坐标；是指各个激光点对应的栅格的坐标；是指各个激光点的残差向量。

S153：在点集的激光点总数小于第二阈值，且点集的激光点总数与点集所属的类中激光点的总数的比值小于第三阈值时，筛除小于第三阈值的点集中所有激光点。

通过步骤S153剔除类中离散的干扰点。

S154：确认点集中激光点与第二匹配占据栅格的匹配程度，筛除匹配程度差的激光点。

确定每一类的距离大于第一阈值的激光点的点集中各个激光点与其第二匹配占据栅格的匹配程度，然后筛除匹配程度差的激光点。

在一实现方式中，若点集中对应于同一第二匹配占据栅格的激光点的数目大于第四阈值，则可以说明这些激光点的至少部分存在占据栅格匹配出错的情况。其中，第四阈值可以根据激光扫描精度调节。可选地，在超过第四阈值的情况下，可以将与同一匹配占据栅格匹配的点集中的多个激光点全部删除。或者，在超过第四阈值的情况下，可以进一步从与同一匹配占据栅格匹配的点集中的所有激光点选取出占据栅格匹配出错的激光点，然后将占据栅格匹配出错的激光点筛除。又或者，在超过第四阈值的情况下，可以进一步确认与同一匹配占据栅格匹配的点集中的所有激光点中相距最远的两个激光点的距离是否超过第五阈值，如果超过第五阈值，将对应于同一第二匹配占据栅格的激光点筛除。可以理解的是，可以从同一匹配占据栅格匹配的点集中的所有激光点的序号中取最大值和最小值；将序号最大值对应的激光点和序号最小值对应的激光点之间的距离，作为与同一匹配占据栅格匹配的点集中的所有激光点中相距最远的两个激光点的距离。

在另一实现方式中，可以进一步确认每个点集中各个激光点与其第二匹配占据栅格之间的形状相似度，将形状相似度差的激光点筛除。具体地，可以通过比较激光点和其第二匹配占据栅格的几何特征，确认激光点及其第二匹配占据栅格的形状相似度：若激光点和其第二匹配占据栅格的几何特征相差较大，则激光点及其第二匹配占据栅格的形状相似度差；反之形状相似度好。其中，几何特征可以是曲率半径、法向量或其他值。

具体地，通过比较激光点和其第二匹配占据栅格的几何特征，确认激光点及其第二匹配占据栅格的匹配程度，将匹配程度差的激光点筛除的步骤，可以包括：将激光点与点集中激光点在点集所属类中相邻的多个激光点作为特征计算点集，基于特征计算点集计算出点集中激光点所属几何图形的几何特征；并基于特征计算点集中所有激光点的第二匹配占据栅格计算出参考几何特征，基于几何特征和参考几何特征的相似程度将相似程度低的激光点筛除。因为，序号相邻基本上代表着激光点位置相邻，因此可以将激光点在点集所属类中与其序号相邻的多个点作为激光点在点集所属类中相邻的多个激光点。例如，一个类包括序号分别为2、5、6、7、8、12、15、19、22和26等数个点，其中7为距离大于第一阈值的激光点，在计算7的几何特征时，可以将5、6、7、8、12作为特征计算点集，利用特征计算点集计算出7的几何特征，并利用5、6、7、8、12各自的第二匹配占据栅格的坐标计算出参考几何特征。

进一步地，在评判形状相似度的几何特征为法向量和曲率向量时，基于特征计算点集计算出点集中激光点所属几何图形的几何特征，可以包括：利用特征计算点集分布的协方差的特征值和特征向量近似求解出激光点的法向量和曲率半径。其中，基于特征计算点集中所有激光点的第二匹配占据栅格计算出参考几何特征，可以包括：基于特征计算点集中所有激光点的第二匹配占据栅格的中心点分布的协方差的特征值和特征向量近似求解出激光点的参考法向量和参考曲率半径。这样在激光点的法向量和参考法向量之间的夹角大于第六阈值且小于第七阈值时，将大于第六阈值且小于第七阈值的激光点筛除；和/或，在激光点的曲率半径和参考曲率半径的差值的绝对值大于第八阈值时，将大于第八阈值的激光点筛除。其中，第六阈值和第七阈值、第八阈值可按实际情况进行选择。优选地，第六阈值接近0°，可以取值5°。优选地，第七阈值接近180°，可以取值175°。

其中，步骤S154和步骤S153的执行先后顺序不限。例如步骤S154可以在步骤S153之前执行，或与步骤S153同时执行。另外，在其他实施方式中，可以只执行步骤S154，或者只执行步骤S153。

S160：将第二激光点集合与地图进行匹配，以确定激光雷达的准确位姿。

在一实现方式中，可以直接将经过两次筛除得到的第二激光点集合与地图进行匹配计算，以得到激光雷达的准确位姿。

在另一实现方式中，可以基于第二激光点集合中各个激光点与栅格地图的匹配评分确定各个激光点的权重系数，然后基于各个激光点的权重系数将第二激光点集合和栅格地图进行加权匹配，匹配得到激光雷达的准确位姿。其中，加权匹配过程的目标函数中每个激光点对应的残差向量需要乘上该激光点的权重系数。另外，加权匹配的方法可以包括ICP、NDT、CSM等匹配方法。

进一步地，可以直接将所有激光点与栅格地图进行匹配，以确定各个激光点与栅格地图的匹配评分。

或者可以将第二激光点集合的所有激光点按照残差向量分为多个子集，需要确认权重系数的子集逐一与栅格地图进行匹配，以得到这些需要确认权重系数的子集的匹配评分，然后基于子集的匹配评分确定子集内所有激光点的权重系数。

可以理解的是，为了避免在与栅格地图匹配时残差向量方向相反的两组激光点互相影响，可以将残差向量方向相反的两组激光点分别归于不同的子集，然后残差向量方向相反的两组激光点分别与地图进行匹配，以降低匹配误差，得到准确的匹配评分。

比较优选地是，可以按照残差向量方向将第二激光点集合的所有激光点分为多个子集，每个子集内的所有激光点的残差向量方向相同或相似，这样残差向量方向相反的两组激光点必然归属于不同的子集。

示例性地，将第二激光点集合的所有激光点分为多个子集的方法，可以包括：根据第二激光点集合中激光点的残差向量计算出激光点的残差向量方向角度；基于残差向量方向角度将残差向量方向相似的激光点聚类，得到多个子集。其中，残差向量方向角度

的计算公式如下所示：

其中，

是激光点的残差向量；是激光点的残差向量方向角度。

可以理解的是，残差向量方向角度在同一个角度区间内的激光点可为残差向量方向相似的激光点，并且第二激光点集合中所有激光点的残差向量方向角度均在所有角度区间的组合内，且不同角度区间互不重叠。角度区间的上限值与角度区间下限值的差值可以为角度窗口阈值。另外，不同角度区间的角度窗口阈值可以相同或不相同。

需要说明的是，并不是所有子集都需要基于匹配评分确认权重系数的，即有部分子集内所有激光点的权重系数可以直接是固定值，例如为1，当然固定值的取值不限于此。

其中，可以将对匹配结果影响大的子集基于匹配评分确认权重系数，而对匹配结果影响小的子集内所有激光点的权重系数直接设为固定值。可以理解的是，存在若干个激光点与其第二匹配占据栅格相距较远、且激光点数量多的子集可以为对匹配结果影响大的子集。可以将子集内所有激光点残差向量的模长和与第二激光点集合内所有激光点残差向量模长和进行对比，确认子集内是否存在若干个激光点与其第二匹配占据栅格相距较远。示例性地，子集的残差向量模长和与第二激光点集合的残差向量模长和的比值大于第九阈值，则代表子集内存在若干个激光点与其第二匹配占据栅格相距较远，此时若子集内激光点总数超过第十阈值，则该子集为对匹配结果影响大的子集。

在其他实现方式中，可以以子集对为判断单位，确认是否需要对子集对内的所有子集基于匹配评分确认权重系数。其中子集对包括残差向量方向相反的两个子集。例如，在子集对的两个子集均为对匹配结果影响大的子集时，需要对子集对的所有子集基于匹配评分确认权重系数；在子集对的两个子集中至少一者为对匹配结果影响小的子集时，无需对子集对的所有子集基于匹配评分确认权重系数，将子集对内所有子集的所有激光点的权重系数设为固定值。

下面为更好说明本申请定位方法中确定第二激光点集合中所有激光点的权重系数的步骤，提供以下具体实施例来示例性说明：

统计第二激光点集合中每个激光点的残差向量，将残差向量方向相似的激光点聚类，提取出残差向量方向相反的激光点集对，包括下述步骤：

1)计算匹配残差向量的方向角度：

遍历第二激光点集合，计算每个激光点的残差向量方向的角度和模长；其中向量指向地图坐标系X轴正向的方向角度为0，逆时针为正；第i个激光点的匹配残差向量方向角度

2)匹配残差向量的聚类：

设置角度窗口阈值β，β＝5°*i(1≤i≤4)，从-180°开始，以β角度范围聚类(如：[-180°,β-180°),[β-180°,2β-180°)…)，根据激光点的匹配残差向量方向角度进行聚类，得到多个子集记为

3)筛选出需要基于匹配评分确认权重系数的、且残差向量方向相反的激光点子集对：

其中，在时，

和

为残差向量方向相反的激光点子集对。

统计

中的残差向量模长和并记为中所有激光点的残差向量模长和记为如果

满足以下两点则为需要基于匹配评分确认权重系数的激光点子集对(记为和

)：

A.设置比例阈值

B.激光点集的残差向量模长和

C.设置比例阈值的激光点总数为

激光点集

和

的点数(0<α<1)量均大于

4)计算各激光点的权重系数

A.初始化每个激光点的权重系数为1。

B.将需要基于匹配评分确认权重系数的所有激光点子集对中的

和分别与地图用CSM(相关性匹配)方法进行匹配，匹配评分较低的点集中激光点的权重系数设为α(0<α<1)。其中，α与匹配评分呈正相关。

在本实施方式中，先基于激光雷达的预测位姿确定各个激光点的第一坐标，并基于第一坐标确定激光点与栅格地图的匹配程度，将匹配程度差的激光点筛除，这样在匹配前筛除匹配程度差的激光点，可以提高匹配精度，使得第一激光点集合中激光点的第二坐标更接近真实坐标；在匹配后进一步基于第二坐标筛除匹配程度差的激光点，可以进一步去除干扰点，以提高激光雷达定位的准确性。

可以理解的是，可以将上述激光雷达定位方法应用到机器人定位中，因为机器人通过激光雷达定位时，激光雷达一般安装在机器人上，所以基于上述激光雷达定位方法确定的激光雷达的准确位姿可作为机器人的准确位姿。

请参阅图3，图3是本申请激光雷达定位装置一实施方式的结构示意图。本申请激光雷达定位装置10包括处理器12和存储器11；存储器11用于存储实现如上述激光雷达定位方法的程序指令，处理器12用于执行存储器11存储的程序指令。

上述激光雷达定位方法的逻辑过程以计算机程序呈现，在计算机程序方面，若其作为独立的软件产品销售或使用时，其可存储在计算机可读存储介质中，因而本申请提出一种计算机可读存储介质。请参阅图4，图4是本申请计算机可读存储介质一实施方式的结构示意图，本实施方式计算机可读存储介质20中存储有计算机程序21，计算机程序被处理器执行时实现上述激光雷达定位方法中的步骤。

该计算机可读存储介质20具体可以为U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory，)、磁碟或者光盘等可以存储计算机程序的介质，或者也可以为存储有该计算机程序的服务器，该服务器可将存储的计算机程序发送给其他设备运行，或者也可以自运行该存储的计算机程序。该计算机可读存储介质20从物理实体上来看，可以为多个实体的组合，例如多个服务器、服务器加存储器、或存储器加移动硬盘等多种组合方式。

以上仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。