[发明专利]一种三维激光雷达点云匹配的快速ICP方法

摘要

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

主权项

一种三维激光雷达点云匹配的快速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的平面直角坐标表示PmPm={(xm(i,j),ym(i,j),zm(i,j))},i∈[1,Km],j∈[1,Lm]]]>其中步骤2:计算点集Rc的平面直角坐标表示Pc；Pc={(xc(i,j),yc(i,j),zc(i,j))},i∈[1,Kc],j∈[1,Lc]]]>其中，步骤3：初始化迭代次数k＝0,T0＝I4×4，误差阈值τ；步骤4：计算中的点在Pm中的最近点及其距离其中表示中第(i,j)点在Pm中的最近点编号，其距离为具体过程如下：(4.1)对于中的每一个点计算其方向φc(i,j)=arctan(zc(i,j)(xc(i,j))2+(yc(i,j))2);]]>(4.2)计算方向在Pm中最近点编号及距离；Dc(i,j)=(xc(i,j)-xm(Γci,Γcj))2+(yc(i,j)-ym(Γci,Γcj))2+(zc(i,j)-zm(Γci,Γcj))2]]>(4.3)根据距离阈值选择用于配准的点集步骤5：利用奇异值分解方法计算配准及误差：步骤6：应用配准，步骤7：如果dk‑1‑dk＜τ，则停止迭代，输出Tk，否则，k＝k+1，转步骤4。