CN112767456A - 一种三维激光点云快速重定位方法 - Google Patents

Abstract

本发明提出一种三维激光点云快速重定位方法，在先验地图的基础上，基于二维栅格化减少数据运算量提高实时性，同时基于Jaccard系数得到初步候选场景集合，在此基础上基于数据主方向与Pearson相关系数对初步候选场景集合进行筛选后得到候选场景集合，提高了实时性，再基于欧几里得聚类法进行三维聚类后对候选场景集合中每一历史帧场景构建二分图，并基于Hungarian匹配算法寻找到最大匹配关系后基于余弦相似性完成三维相似性度量，最终得到唯一候选场景，使得得到的对应关系求解更可靠，鲁棒性更高，配准结果更准确，方法整体实时性高，最后利用3D‑NDT算法得到两帧间的姿态变换关系矩阵完成重定位，本发明基于余弦相似度进行唯一候选场景筛选。

Description

一种三维激光点云快速重定位方法

技术领域

本发明属于同步定位与建图技术，特别是一种三维激光点云快速重定位方法。

背景技术

重定位算法是指基于先验地图完成定位的过程。重定位技术直接利用先验地图与当前三维激光雷达所获取的场景信息进行全局定位，其允许机器人在不用重新构图的基础上完成定位。

传统的三维激光点云重定位算法通常基于三维场景原始点云与关键帧进行匹配以期获得候选场景，这导致算法实时性差，往往需要对重定位算法或者三维点云输入降频以达到实时性的要求；

同时，传统重定位算法基于激光里程计的匹配结果，需要在其设定的欧氏距离范围内进行候选场景搜索，这往往使得重定位结果高度依赖激光里程计结果，一旦激光里程计配准失败则会导致重定位结果不可信，由此基于重定位结果进行优化的里程计结果也将继续发散，最终整个构图与重定位算法失败。

发明内容

本发明的目的在于提出一种三维激光点云快速重定位方法，以解决三维激光点云在先验地图中快速重定位的问题。

实现本发明的技术解决方案为：一种三维激光点云快速重定位方法，具体步骤为：

步骤1、对分割好当前场景的原始激光图点云进行二维栅格化处理；

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

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

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

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

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

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

本发明与现有技术相比，其显著优点为：

(1)本发明基于Jaccard系数、数据主方向与Pearson相关系数对当前场景进行二维化处理提高了实时性。

(2)本发明基于二分图构建基当前场景与候选场景之间的对应关系，使得得到的对应关系求解更可靠，鲁棒性更高。

(3)本发明基于余弦相似度进行唯一候选场景筛选，使得配准结果更准确，方法整体实时性高。

下面结合附图对本发明做进一步详细的描述。

附图说明

图1为本发明中三维激光点云快速重定位方法整体步骤流程图。

图2为本发明中原始点云中心化处理效果示意图，其中图2(a)为原始点云图，图2(b)为剔除地面-雪面后的点云图，图2(c)为点云中心化处理后的图。

具体实施方式

一种三维激光点云快速重定位方法，包括以下步骤：

步骤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筛选出的候选场景帧进行基于欧几里得聚类算法的三维点云聚类，得到聚类集合，具体为：

对当前帧和步骤3筛选出的候选场景帧依据下式进行中心化处理：

其中，θ为步骤1-5中得到的数据主方向与当前激光雷达局部坐标系主方向之间的夹角，(x_j,y_j,z_j)为点云中第i个点，(x′_j,y′_j,z_j)为点云中第i个点重映射得到的点，

为点云质心

基于欧几里得聚类法将中心化处理后的点云进行聚类处理，得到当前帧与候选场景中每一帧的聚类结果；

欧几里得聚类算法中，点云中n个点P₁,P₂,P₃...P_n为输入，通过欧几里得算法得到m个聚类为c₁,c₂,c₃...c_m，算法过程中设定好T_n为将点视为同一簇的阈值。

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

步骤5-1、计算当前帧中聚类得到的点云簇集合与候选场景中每一帧聚类后得到的点云簇集合中任意两个点云簇之间的三维相似度：

其中，K_ab表示点云簇a与点云簇b之间的关联相似系数，p_a与p_b表示点云簇a与点云簇b的质心坐标，c_a与c_b表示表示点云簇a与点云簇b中激光点的数目，o_a与o_b分别表示点云簇a与点云簇b的数据主方向，|| ||表示点云簇a与点云簇b之间的差异度，其中，p_a与p_b的差异度为两个坐标间的欧氏距离，c_a与c_b差异度为数目差，o_a与o_b差异度为二者数据主方向的夹角γ，w₁、w₂和w₃分别为三者的权重系数；

步骤5-2、构建当前帧点云簇聚类集合与候选场景中每一帧点云簇聚类集合的对应二分图，当满足K_ab＜K_max时，则认为二者属于同一聚类，并在二分图中构建边；

步骤5-3、基于构建好的二分图利用Hungarian算法找当前帧聚类集合与每一候选场景帧聚类集合的最大匹配，得到具有最大对应关系的集合P与多个集合Q。

步骤6、基于余弦相似度进行唯一候选场景筛选，具体为：

步骤6-1、统计集合P与集合Q中每个点云簇中激光点数目，建立统计矩阵S_P和S_Q；

步骤6-2、计算集合P与集合Q之间的余弦相似度d(S_P,S_Q)为：

其中，

与

为统计矩阵S_P和S_Q的对应第γ个分量，n＝a×b，其中S_P大小为m₁×n₁，S_Q大小为m₂×n₂，则：

步骤6-3、统计所有候选场景与当前场景的d(S_P,S_Q)，根据d(S_P,S_Q)取相似度最大的场景作为唯一候选场景帧。

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

步骤7-1、对当前场景点云P^c与为匹配得到的唯一候选场景帧点云P^r分别进行中心化处理：

其中，T^c和T^r分别为二者的中心化处理矩阵；其中，(u_x,u_y,u_z)表示当前帧点云质心坐标，θ为步骤1中数据主方向与当前激光雷达局部坐标系主方向之间的夹角；

步骤7-2、将两帧点云作为输入，由3D-NDT算法计算得到二者的位姿变换关系后得到变换矩阵T_NDT，由此得到定位矩阵T_loc，完成重定位：

T_loc＝(T^r)^-1T_NDTT^c。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

步骤1、对分割好当前场景的原始激光图点云进行二维栅格化处理；

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

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

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

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

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

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

一种计算机可存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

步骤1、对分割好当前场景的原始激光图点云进行二维栅格化处理；

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

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

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

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

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

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

下面结合实施例对本发明作进一步说明。

实施例

如图1所示，一种三维激光点云快速重定位方法，包括以下步骤：

步骤1、对分割好当前场景的原始激光图点云进行二维栅格化处理，具体为：

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

步骤1-2、如图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筛选出的候选场景帧进行基于欧几里得聚类算法的三维点云聚类，得到聚类集合，具体为：

对当前帧和步骤3筛选出的候选场景帧依据下式进行中心化处理：

其中，θ为步骤1-5中得到的数据主方向与当前激光雷达局部坐标系主方向之间的夹角，(x_j,y_j,z_j)为点云中第i个点，(x′_j,y′_j,z_j)为点云中第i个点重映射得到的点，

为点云质心

基于欧几里得聚类法将中心化处理后的点云进行聚类处理，得到当前帧与候选场景中每一帧的聚类结果；

欧几里得聚类算法中，点云中n个点P₁,P₂,P₃...P_n为输入，通过欧几里得算法得到m个聚类为c₁,c₂,c₃...c_m，算法过程中设定好T_n为将点视为同一簇的阈值。

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

步骤5-1、计算当前帧中聚类得到的点云簇集合与候选场景中每一帧聚类后得到的点云簇集合中任意两个点云簇之间的三维相似度：

其中，K_ab表示点云簇a与点云簇b之间的关联相似系数，p_a与p_b表示点云簇a与点云簇b的质心坐标，c_a与c_b表示表示点云簇a与点云簇b中激光点的数目，o_a与o_b分别表示点云簇a与点云簇b的数据主方向，|| ||表示点云簇a与点云簇b之间的差异度，其中，p_a与p_b的差异度为两个坐标间的欧氏距离，c_a与c_b差异度为数目差，o_a与o_b差异度为二者数据主方向的夹角γ，w₁、w₂和w₃分别为三者的权重系数；

步骤5-2、构建当前帧点云簇聚类集合与候选场景中每一帧点云簇聚类集合的对应二分图，当满足K_ab＜K_max时，则认为二者属于同一聚类，并在二分图中构建边；

步骤5-3、基于构建好的二分图利用Hungarian算法找当前帧聚类集合与每一候选场景帧聚类集合的最大匹配，得到具有最大对应关系的集合P与多个集合Q。

步骤6、基于余弦相似度进行唯一候选场景筛选，具体为：

步骤6-1、统计集合P与集合Q中每个点云簇中激光点数目，建立统计矩阵S_P和S_Q；

步骤6-2、计算集合P与集合Q之间的余弦相似度d(S_P,S_Q)为：

其中，

与

为统计矩阵S_P和S_Q的对应第γ个分量，n＝a×b，其中S_P大小为m₁×n₁，S_Q大小为m₂×n₂，则：

步骤6-3、统计所有候选场景与当前场景的d(S_P,S_Q)，根据d(S_P,S_Q)取相似度最大的场景作为唯一候选场景帧。

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

步骤7-1、对当前场景点云P^c与为匹配得到的唯一候选场景帧点云P^r分别进行中心化处理：

其中，T^c和T^r分别为二者的中心化处理矩阵；其中，(u_x,u_y,u_z)表示当前帧点云质心坐标，θ为步骤1中数据主方向与当前激光雷达局部坐标系主方向之间的夹角；

步骤7-2、将两帧点云作为输入，由3D-NDT算法计算得到二者的位姿变换关系后得到变换矩阵T_NDT，由此得到定位矩阵T_loc，完成重定位：

T_loc＝(T^r)^-1T_NDTT^c。

本发明在先验地图的基础上，基于二维栅格化减少数据运算量提高实时性，同时基于Jaccard系数得到初步候选场景集合，在此基础上基于数据主方向与Pearson相关系数对初步候选场景集合进行筛选后得到候选场景集合。再基于欧几里得聚类法进行三维聚类，后对候选场景集合中每一历史帧场景构建二分图，并基于Hungarian匹配算法寻找到最大匹配关系后基于余弦相似性完成三维相似性度量，最终得到唯一候选场景。最后利用3D-NDT算法得到两帧间的姿态变换关系矩阵完成重定位。

Claims (10)

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

步骤1、对分割好当前场景的原始激光图点云进行二维栅格化处理；

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

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

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

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

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

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

2.根据权利要求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两个维度信息，完成当前帧内点云的二维栅格化。

3.根据权利要求1所述的三维激光点云快速重定位方法，其特征在于，所述步骤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比较，若大于该阈值则将该关键帧放入候选场景集合中。

4.根据权利要求2所述的三维激光点云快速重定位方法，其特征在于，所述步骤3中的对候选场景集合进行进一步筛选具体为：

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

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

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

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

5.根据权利要求2所述的三维激光点云快速重定位方法，其特征在于，所述步骤4中的于欧几里得算法的三维点云聚类成集合具体为：

对当前帧和步骤3筛选出的候选场景帧依据下式进行中心化处理：

其中，θ为步骤1-5中得到的数据主方向与当前激光雷达局部坐标系主方向之间的夹角，(x_j,y_j,z_j)为点云中第i个点，(x′_j,y′_j,z_j)为点云中第i个点重映射得到的点，

为点云质心

基于欧几里得聚类法将中心化处理后的点云进行聚类处理，得到当前帧与候选场景中每一帧的聚类结果。

6.根据权利要求5所述的三维激光点云快速重定位方法，其特征在于，所述步骤5中的基于Hungarian算法匹配构建对应关系具体为：

步骤5-1、计算当前帧中聚类得到的点云簇集合与候选场景中每一帧聚类后得到的点云簇集合中任意两个点云簇之间的三维相似度：

其中，K_ab表示点云簇a与点云簇b之间的关联相似系数，p_a与p_b表示点云簇a与点云簇b的质心坐标，c_a与c_b表示表示点云簇a与点云簇b中激光点的数目，o_a与o_b分别表示点云簇a与点云簇b的数据主方向，|| ||表示点云簇a与点云簇b之间的差异度，其中，p_a与p_b的差异度为两个坐标间的欧氏距离，c_a与c_b差异度为数目差，o_a与o_b差异度为二者数据主方向的夹角γ，w₁、w₂和w₃分别为三者的权重系数；

步骤5-2、构建当前帧点云簇聚类集合与候选场景中每一帧点云簇聚类集合的对应二分图，当满足K_ab＜K_max时，则认为二者属于同一聚类，并在二分图中构建边；

步骤5-3、基于构建好的二分图利用Hungarian算法找当前帧聚类集合与每一候选场景帧聚类集合的最大匹配，得到具有最大对应关系的集合P与多个集合Q。

7.根据权利要求6所述的三维激光点云快速重定位方法，其特征在于，所述步骤6中的基于余弦相似度筛选唯一候选场景具体为：

步骤6-1、统计集合P与集合Q中每个点云簇中激光点数目，建立统计矩阵S_P和S_Q；

步骤6-2、计算集合P与集合Q之间的余弦相似度d(S_P,S_Q)为：

其中，

与

为统计矩阵S_P和S_Q的对应第γ个分量，n＝a×b，其中S_P大小为m₁×n₁，S_Q大小为m₂×n₂，则：

步骤6-3、统计所有候选场景与当前场景的d(S_P,S_Q)，根据d(S_P,S_Q)取相似度最大的场景作为唯一候选场景帧。

8.根据权利要求2所述的三维激光点云快速重定位方法，其特征在于，所述步骤7中的三维点云配准实现重定位具体为：

步骤7-1、对当前场景点云P^c与为匹配得到的唯一候选场景帧点云P^r分别进行中心化处理：

其中，T^c和T^r分别为二者的中心化处理矩阵；其中，(u_x,u_y,u_z)表示当前帧点云质心坐标，θ为步骤1中数据主方向与当前激光雷达局部坐标系主方向之间的夹角；

步骤7-2、将两帧点云作为输入，由3D-NDT算法计算得到二者的位姿变换关系后得到变换矩阵T_NDT，由此得到定位矩阵T_loc，完成重定位：

T_loc＝(T^r)^-1T_NDTT^c

9.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1-8中任一项所述方法的步骤。

10.一种计算机可存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1-8中任一项所述的方法的步骤。

Images