一种三维激光雷达点云匹配的快速ICP方法技术

本发明专利技术公开了一种三维激光雷达点云匹配的快速ICP方法，针对三维激光雷达原始点云数据的特点，在极坐标系下快速地计算两个点云之间的最近点方法。该方法提高了三维激光雷达点云配准的速度。使得三维激光雷达原始点云数据配准速度大幅度提高，即将时间复杂度从经典ICP方法的O(DNcNm)降为O(DNc)，其中，Nc为当前点云数据个数，Nm为模型点集数据个数。

【技术实现步骤摘要】
一种三维激光雷达点云匹配的快速ICP方法
本专利技术涉及一种快速ICP方法，具体地说，涉及一种三维激光雷达点云匹配的快速ICP方法。
技术介绍
三维激光雷达提供了环境(或物体)的三维深度信息，在机器人导航、物体重三维建、医学图像分析、地形测绘、文物保护等诸多应用中被广泛使用。三维激光雷达提供的点云数据需要进行配准才能用于后续的环境建模、物体重建、地形构建等工作。目前，广为使用的三维激光雷达数据匹配方法是最近迭代算法(ICP,IterativeClosePoint)及其改进方法。然而，ICP算法具有以下不足：(1)要求初始估计较为准确，(2)大量的点云数据使得ICP算法效率低，(3)ICP算法可能陷入局部最优解。现有技术中提出了一种经典的迭代最近点(IterativeClosestPoint,ICP)技术，可以对两个三维点集进行配准，迭代最近点(ICP)的主要缺点为：1)如果初始矩阵选择不当，可能导致陷入局部最优点；2)计算最近点的过程计算复杂度较大，为O(NcNm)(其中，Nc为当前点云数据个数，Nm为模型点集数据个数)。假设算法迭代次数为D，则算法复杂度为：O(DNcNm)。由于配准算法通常要被多次调用，而实际问题的数据点集规模也通常非常大，因此该方法难以获得实时结果。现有技术中还提出了一种在极坐标系下配准二维激光雷达点云数据的方法。其基本思路是利用激光雷达原始扫描数据的结构信息，利用扫描投影的方法计算当前扫描在参考坐标系中的期望点云。该方法首先对原始点云数据进行适当的预处理以去除异常数据。此后，在初始配准估计的基础上，迭代地进行扫描投影(scanprojection)、平移估计、旋转估计。该方法不能处理三维激光雷达点云数据。
技术实现思路
本专利技术的目的在于克服上述技术存在的缺陷，提供一种三维激光雷达点云匹配的快速ICP方法，使得三维激光雷达原始点云数据配准速度大幅度提高，即将时间复杂度从经典ICP方法的O(DNcNm)降为O(DNc)。该专利技术的核心是针对三维激光雷达原始点云数据的特点，在极坐标系下快速地计算两个点云之间的最近点方法，其中，Nc为当前点云数据个数，Nm为模型点集数据个数。其具体技术方案为：一种三维激光雷达点云匹配的快速ICP方法，包括以下步骤：输入：激光雷达原始点云数据i∈[1,Km],j∈[1,Lm]，表示模型点集；激光雷达原始点云数据i∈[1,Kc],j∈[1,Lc]，表示当前点集；其中,表示Rm的第i行的第j列射线,表示Rc的第i行的第j列射线,Km为Rm的射线行数,Kc为Rc的射线行数,Lm为Rm的每行射线数,Lc为Rc的每行射线数,表示Rm的第i行第j列射线测量距离,表示Rc的第i行第j列射线测量距离,表示Rm的第i行射线的方向，表示Rc的第i行射线的方向，表示Rm的第j列射线的方向,表示Rc的第j列射线的方向,分别表示行、列方向角度偏移,分别表示行、列方向角度分辨率；输出：点云数据Rc相对于点云数据Rm的齐次变换T；步骤1：计算模型点集Rm的平面直角坐标表示Pm其中步骤2:计算点集Rc的平面直角坐标表示Pc；其中，步骤3：初始化迭代次数k＝0,T0＝I4×4，误差阈值τ；步骤4：计算中的点在Pm中的最近点及其距离i∈[1,Kc],j∈[1,Lc]；其中表示中第(i,j)点在Pm中的最近点编号，其距离为步骤5：利用奇异值分解(SVD,SingularValueDecomposition)方法计算配准及误差：步骤6：应用配准，步骤7：如果dk-1-dk＜τ，则停止迭代，输出Tk，否则，k＝k+1，转步骤4。优选地，所述步骤4的具体过程如下：(4.1)对于中的每一个点计算其方向(4.2)计算方向在Pm中最近点编号及距离；(4.3)根据距离阈值选择用于配准的点集与现有技术相比，本专利技术的有益效果为：本专利技术使得三维激光雷达原始点云数据配准速度大幅度提高，即将时间复杂度从经典ICP方法的O(DNcNm)降为O(DNc)。该专利技术的核心是针对三维激光雷达原始点云数据的特点，在极坐标系下快速地计算两个点云之间的最近点方法，其中，Nc为当前点云数据个数，Nm为模型点集数据个数。该方法提高了三维激光雷达点云配准的速度。附图说明图1是当前点集与模型点集的最近点匹配示意图；图2是常规ICP方法的最近点与快速ICP方法的最近点比对结果图。具体实施方式为了使本专利技术实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合附图和具体实例进一步阐述本专利技术。本专利技术应用于移动机器人三维位姿估计Γt相对于Γt-1的位姿表示为xt＝(xt,yt,zt,αt,βt,γt)T，其中xt，yt，zt分别表示Γt坐标系的原点在坐标系Γt-1中的坐标，γt，βt，αt分别表示Γt相对于Γt-1在X,Y,Z轴的旋转角度。则平移向量为，pt＝(xt,yt,zt)T旋转矩阵为，从Γt-1到Γt的齐次变换T输入：(1)t-1时刻激光雷达点云数据i∈[1,Kt-1],j∈[1,Lt-1]，表示模型点集。(2)t时刻激光雷达原始点云数据i∈[1,Kt],j∈[1,Lt]，表示当前点集。其中,表示Rt-1的第i行的第j列射线,表示Rt的第i行的第j列射线,Kt-1为Rt-1的射线行数,Kt为Rt的射线行数,Lt-1为Rt-1的每行射线数,Lt为Rt的每行射线数,表示Rt-1的第i行第j列射线测量距离,表示Rt的第i行第j列射线测量距离,表示Rt-1的第i行射线的方向，表示Rt的第i行射线的方向，表示Rt-1的第j列射线的方向,表示Rt的第j列射线的方向,分别表示行、列方向角度偏移,分别表示行、列方向角度分辨率。输出：点云数据Rt相对于点云数据Rt-1的齐次变换T。步骤1：计算模型点集Rt-1的平面直角坐标表示Pt-1其中步骤2:计算点集Rt的平面直角坐标表示Pt。其中，步骤3：初始化迭代次数k＝0,T0＝I4×4，误差阈值τ。步骤4：计算中的点在Pt-1中的最近点及其距离i∈[1,Kt],j∈[1,Lt]。其中表示中第(i,j)点在Pt-1中的最近点编号，其距离为具体过程如下：(4.1)对于中的每一个点计算其方向(4.2)计算方向在Pt-1中最近点编号及距离。(4.3)根据距离阈值选择用于配准的点集步骤5：利用奇异值分解(SVD,SingularValueDecomposition)方法计算配准及误差：步骤6：应用配准，步骤7：如果dk-1-dk＜τ，则停止迭代，输出Tk，否则，k＝k+1，转第4步。以上所述，仅为本专利技术最佳实施方式，任何熟悉本
的技术人员在本专利技术披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本专利技术的保护范围内。本文档来自技高网...

【技术保护点】
一种三维激光雷达点云匹配的快速ICP方法，其特征在于，包括以下步骤：输入：激光雷达原始点云数据i∈[1..Km],j∈[1..Lm]，表示模型点集；激光雷达原始点云数据i∈[1..Kc],j∈[1..Lc]，表示当前点集；其中,表示Rs的第i行的第j条(列)射线(s＝m或c),Ks为射线行数,Ls为每行射线数,表示第i行第j(列)射线测量距离,表示第i行射线的方向，表示第j列射线的方向,分别表示行、列方向角度偏移,分别表示行、列方向角度分辨率；输出：点云数据Rc相对于点云数据Rm的齐次变换T；步骤1：计算模型点集Rm的平面直角坐标表示PmPm={(xmi,j,ymi,j,zmi,j)},i∈[1..Km],j∈[1..Lm]]]>其中zmi,j=ρmi,jsinφmi;]]>步骤2:计算点集Rc的平面直角坐标表示Pc；Pc={(xci,j,yci,j,zci,j)},i∈[1..Kc],j∈[1..Lc]]]>其中，zci,j=ρci,jsinφci;]]>步骤3：初始化迭代次数k＝0,T0＝I4×4，误差阈值τ；步骤4：计算中的点在Pm中的最近点及其距离(Γci,Γcj,Dci,j),i∈[1..Kc],j∈[1..Lc];]]>其中表示中第(i,j)点在Pm中的最近点编号，其距离为步骤5：利用奇异值分解方法计算配准及误差：(Tk,dk)=R{(PmΓci,Γcj,Pci,j),Dci,j≤Dth};]]>步骤6：应用配准，步骤7：如果dk‑1‑dk＜τ，则停止迭代，输出Tk，否则，k＝k+1，转步骤4。...

【技术特征摘要】
1.一种三维激光雷达点云匹配的快速ICP方法，其特征在于，包括以下步骤：输入：激光雷达原始点云数据表示模型点集；激光雷达原始点云数据表示当前点集；其中,表示Rm的第i行的第j列射线,表示Rc的第i行的第j列射线,Km为Rm的射线行数,Kc为Rc的射线行数,Lm为Rm的每行射线数,Lc为Rc的每行射线数,表示Rm的第i行第j列射线测量距离,表示Rc的第i行第j列射线测量距离,表示Rm的第i行射线的方向，表示Rc的第i行射线的方向，表示Rm的第j列射线的方向,表示Rc的第j列射线的方向,分别表示行、列方向角度偏移,分别表示行、列方向角度分辨率；输出：点云数据Rc相对于点云数据Rm的齐次变换T；步骤1：计算模型点集Rm的平面直角坐标表示Pm其中步骤2:计算点集Rc的平面直角坐标表示Pc；

【专利技术属性】
技术研发人员：段琢华，梁瑞仕，马慧，邹昆，
申请(专利权)人：电子科技大学中山学院，
类型：发明
国别省市：广东;44