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

权利要求书

1.一种三维激光雷达点云匹配的快速ICP方法，其特征在于，包括以下步骤：

输入：激光雷达原始点云数据表示模型点集；

激光雷达原始点云数据表示当前点集；

其中,表示R_m的第i行的第j列射线,表示R_c的第i行的第j列射线,K_m为R_m的射线行数,K_c为R_c的射线行数,L_m为R_m的每行射线数,L_c为R_c的每行射线数,表示R_m的第i行第j列射线测量距离,表示R_c的第i行第j列射线测量距离,表示R_m的第i行射线的方向，表示R_c的第i行射线的方向，表示R_m的第j列射线的方向,表示R_c的第j列射线的方向,分别表示行、列方向角度偏移,分别表示行、列方向角度分辨率；

输出：点云数据R_c相对于点云数据R_m的齐次变换T；

步骤1：计算模型点集R_m的平面直角坐标表示P_m

$P_m=\left\{(x_m^{(i,j)},y_m^{(i,j)},z_m^{(i,j)})\right\},i\∈\[1,K_m\],j\∈\[1,L_m\]$

其中

步骤2:计算点集R_c的平面直角坐标表示P_c；

$P_c=\left\{(x_c^{(i,j)},y_c^{(i,j)},z_c^{(i,j)})\right\},i\∈\[1,K_c\],j\∈\[1,L_c\]$

其中，

步骤3：初始化迭代次数k＝0,T₀＝I_4×4，误差阈值τ；

步骤4：计算中的点在P_m中的最近点及其距离其中表示中第(i,j)点在P_m中的最近点编号，其距离为具体过程如下：

(4.1)对于中的每一个点计算其方向

${\mathrm{\φ}}_c^{(i,j)}=\arctan \left(\frac{z_c^{(i,j)}}{\sqrt{{\left(x_c^{(i,j)}\right)}^2+{\left(y_c^{(i,j)}\right)}^2}}\right);$

(4.2)计算方向在P_m中最近点编号及距离；

$D_c^{\left(i,j\right)}=\sqrt{{\left(x_c^{\left(i,j\right)}-x_m^{\left({\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j\right)}\right)}^2+{\left(y_c^{\left(i,j\right)}-y_m^{\left({\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j\right)}\right)}^2+{\left(z_c^{\left(i,j\right)}-z_m^{\left({\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j\right)}\right)}^2}$

(4.3)根据距离阈值选择用于配准的点集

步骤5：利用奇异值分解方法计算配准及误差：

步骤6：应用配准，

步骤7：如果d_k-1-d_k＜τ，则停止迭代，输出T_k，否则，k＝k+1，转步骤4。