[发明专利]激光雷达定位方法及其相关装置

权利要求书

1.一种激光雷达定位方法，其特征在于，所述方法包括：

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

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

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

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

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

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

2.根据权利要求1所述的激光雷达定位方法，其特征在于，所述基于所述第二坐标筛除匹配程度差的激光点，得到第二激光点集合；

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

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

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

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

3.根据权利要求2所述的激光雷达定位方法，其特征在于，所述确认点集中激光点与第二匹配占据栅格的匹配程度，筛除匹配程度差的激光点，包括：

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

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

4.根据权利要求3所述的激光雷达定位方法，其特征在于，所述基于所述特征计算点集计算出点集中激光点所属几何图形的几何特征，包括：利用所述特征计算点集分布的协方差的特征值和特征向量近似求解出激光点的法向量和曲率半径；

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

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

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

5.根据权利要求2所述的激光雷达定位方法，其特征在于，所述从每一个类筛选出距离第一占据栅格大于第一阈值的激光点的点集，之前包括：根据激光点的第二坐标和激光点的第一占据栅格的中心点的坐标计算出激光点的残差向量；

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

6.根据权利要求1所述的激光雷达定位方法，其特征在于，所述将第二激光点集合与地图进行匹配，以确定激光雷达的最终位姿，之前包括：根据激光点的第二坐标和激光点的第一占据栅格的中心点的坐标计算出激光点的残差向量；

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