[发明专利]一种三维激光点云快速重定位方法

权利要求书

1.一种三维激光点云快速重定位方法，其特征在于，包括以下步骤：

步骤1、对分割好的当前帧点云进行二维栅格化处理，具体为：

步骤1-1、将分割好的当前帧原始点云进行地面点云以及雪面点云的剔除；

步骤1-2、对点云中心化处理：

式中，投影栅格大小为N×N，N＝range/res，激光雷达观测范围为range，二维栅格分辨率为res，原始点云中某点坐标为(p_x,p_y,p_z)，投影后为坐标(x,y,z)；

步骤1-3、将当前帧内点云描述为p₁＝(x₁,y₁,z₁)，p₂＝(x₂,y₂,z₂)，...，p_n＝(x_n,y_n,z_n)，确定当前单帧剩余点云的质心

步骤1-4、确定当前帧内点云的协方差矩阵：

步骤1-5、对cov(x,y,z)进行奇异值分解得到特征值λ_X,λ_Y,λ_Z，取其中最大值代表的特征向量作为数据主方向，计算数据主方向与当前激光雷达局部坐标系主方向之间的夹角θ：

其中，为数据主方向，当前激光雷达局部坐标系主方向为x轴方向；

步骤1-6、对点云进行重映射；

其中，(x_i,y_i,z_i)为点云中第i个点，(x′_i,y′_i)为重映射得到的点，在完成重映射后，仅保留其x与y两个维度信息，完成当前帧内点云的二维栅格化；

步骤2、基于Jaccard系数对先验地图中关键帧进行初筛得到候选场景集合，具体为：

步骤2-1、将原始N×N栅格简化为一维向量，分量为N×N个；

步骤2-2、计算当前帧与先验地图中关键帧的Jaccard系数；

其中，n＝N×N，A_i表示当前帧三维场景经过二维投影后的第i个分量的栅格占据情况，B_ki表示为先验地图关键帧中，帧ID为k的历史帧第i个分量的占栅格据情况，栅格占据情况取值为：占据取1，非占据取0；

步骤2-3、对计算得到的Jaccard系数与阈值J_pre比较，若大于该阈值则将该关键帧放入候选场景集合中；

步骤3、基于数据主方向与Pearson相关系数对候选场景集合进行进一步筛选，具体为：

步骤3-1、根据步骤1的方法计算当前场景与候选场景集合中关键帧的数据的主方向夹角：

其中，g^c与g^r分别为当前帧与某候选场景帧的主方向， 为二者的余弦计算，ε为阈值，计算二者之间的余弦值，若某个候选场景帧满足则予以剔除；

步骤3-2、计算当前帧与某候选场景帧的Pearson相关系数对候选场景集合进一步筛选：

其中，X_i为当前帧二维栅格化后的质心坐标，Y_ki为某候选场景帧中第k帧二维栅格化后的质心坐标，n＝N×N，若r_k在阈值内则予以保留完成进一步筛选；

步骤4、对原始帧与步骤3筛选出的候选场景帧进行基于欧几里得聚类算法的三维点云聚类，得到聚类集合；

步骤5、对步骤4得到的聚类集合基于Hungarian匹配构建当前帧与候选场景集合中每一帧的最大对应关系，得到对应的集合；

步骤6、基于余弦相似度进行唯一候选场景筛选；

步骤7、基于唯一候选场景帧，由3D-NDT算法得到变换矩阵，并依此确定定位矩阵，实现三维点云配准重定位。