一种用于二维激光雷达点云匹配的快速ICP方法

摘要

本发明公开了一种用于二维激光雷达点云匹配的快速ICP方法，它涉及一种ICP方法。本发明的方法步骤为：步骤1：计算模型点集R_m的平面直角坐标表示P_m；步骤2：通过分段分析方法构建当前激光雷达点云数据R_c的有效射线集合W_c，以及点集W_c的平面直角坐标表示P_c。步骤3：初始化迭代次数k＝0，T₀＝I_3×3，误差阈值τ。步骤4：计算中的点在P_m中的最近点及其距离其中表示中第j点在P_m中的最近点编号为其距离为步骤5：应用配准，步骤6：如果d_k‑1‑d_k＜τ，则停止迭代，输出T_k，否则，k＝k+1，转第4步。本发明使得二维激光雷达原始点云数据配准速度大幅度提高，即将时间复杂度从经典ICP方法的O(DN_PN_X)降为O(DN_P)，其中N_P为待配准点集的规模，N_X为模型点集的规模，D为迭代次数。

主权项

一种用于二维激光雷达点云匹配的快速ICP方法，其特征在于，其技术方案为：输入：(1)模型点集j∈[1..L_m]，R_m设置为激光雷达原始时刻点云数据；(2)当前激光雷达点云数据j∈[1..L_c]，表示当前点集；其中，表示R_m的第j条射线，表示R_c的第j条射线，，L_m表示点集R_m的射线总数，L_c表示点集R_c的射线总数，表示射线的测量距离，表示射线的测量距离，表示射线的方向，表示射线的方向，表示角度偏移，表示角度分辨率；输出：当前激光雷达点云数据R_c相对于模型点集R_m的齐次变换T；步骤1：计算模型点集R_m的平面直角坐标表示P_m$P_m=\left\{(x_m^j,y_m^j)\right\},j\in \left[\mathrm{1..}{L}_m\right]---\left(1\right)$其中步骤2：通过分段分析方法构建当前激光雷达点云数据R_c的有效射线集合W_c，以及有效射线集合W_c的平面直角坐标表示P_c；步骤(2.1)通过式(2)对R_c进行分段分析，$R_c=\underset{k=1}{\overset{c_t}{\cup }}{G}_c^k---\left(2\right)$其中，表示第k段点云数据，c_t表示分段数目，gap表示一个阈值，步骤(2.2)通过式(3)计算有效射线集合，$W_c=\underset{k=1}{\overset{c_t}{\cup }}{G}_c^k,d_c^k>\gamma ,{\lambda }_2\ge {\tilde{\rho }}_c^k\ge {\lambda }_1---\left(3\right)$其中，W_c表示R_c的有效射线集合，表示第k段射线数目，表示第k段射线平均长度，γ，λ₂，λ₁分别表示阈值；步骤(2.3)，计算有效射线集合W_c的平面直角坐标表示P_c；$P_c=\left\{(x_c^j,y_c^j)\right\},b_c^j\in W_c$其中步骤3：初始化迭代次数k＝0，T₀＝I_3×3，误差阈值τ；计算中的点在P_m中的最近点及其距离j∈[1..|P_c|]；其中表示中第j点在P_m中的最近点编号为其距离为步骤4：利用奇异值分解(SVD，Singular Value Decomposition)方法计算配准及误差：步骤5：应用配准，步骤6：如果d_k‑1‑d_k＜τ，则停止迭代，输出T_k，否则，k＝k+1，转步骤4。