CN104615880A - 一种三维激光雷达点云匹配的快速icp方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及一种快速ICP方法，具体地说，涉及一种三维激光雷达点云匹配的快速ICP方法。

背景技术

三维激光雷达提供了环境(或物体)的三维深度信息，在机器人导航、物体重三维建、医学图像分析、地形测绘、文物保护等诸多应用中被广泛使用。

三维激光雷达提供的点云数据需要进行配准才能用于后续的环境建模、物体重建、地形构建等工作。目前，广为使用的三维激光雷达数据匹配方法是最近迭代算法(ICP,Iterative Close Point)及其改进方法。然而，ICP算法具有以下不足：(1)要求初始估计较为准确，(2)大量的点云数据使得ICP算法效率低，(3)ICP算法可能陷入局部最优解。

现有技术中提出了一种经典的迭代最近点(Iterative Closest Point,ICP)技术，可以对两个三维点集进行配准，迭代最近点(ICP)的主要缺点为：1)如果初始矩阵选择不当，可能导致陷入局部最优点；2)计算最近点的过程计算复杂度较大，为O(N_cN_m)(其中，N_c为当前点云数据个数，N_m为模型点集数据个数)。假设算法迭代次数为D，则算法复杂度为：O(DN_cN_m)。由于配准算法通常要被多次调用，而实际问题的数据点集规模也通常非常大，因此该方法难以获得实时结果。

现有技术中还提出了一种在极坐标系下配准二维激光雷达点云数据的方法。其基本思路是利用激光雷达原始扫描数据的结构信息，利用扫描投影的方法计算当前扫描在参考坐标系中的期望点云。该方法首先对原始点云数据进行适当的预处理以去除异常数据。此后，在初始配准估计的基础上，迭代地进行扫描投影(scan projection)、平移估计、旋转估计。该方法不能处理三维激光雷达点云数据。

发明内容

本发明的目的在于克服上述技术存在的缺陷，提供一种三维激光雷达点云匹配的快速ICP方法，使得三维激光雷达原始点云数据配准速度大幅度提高，即将时间复杂度从经典ICP方法的O(DN_cN_m)降为O(DN_c)。该发明的核心是针对三维激光雷达原始点云数据的特点，在极坐标系下快速地计算两个点云之间的最近点方法，其中，N_c为当前点云数据个数，N_m为模型点集数据个数。

其具体技术方案为：

一种三维激光雷达点云匹配的快速ICP方法，包括以下步骤：

输入：激光雷达原始点云数据i∈[1..K_m],j∈[1..L_m]，表示模型点集；

激光雷达原始点云数据i∈[1..K_c],j∈[1..L_c]，表示当前点集；

其中,表示R_s的第i行的第j条(列)射线(s＝m或c),K_s为射线行数,L_s为每行射线数,表示第i行第j(列)射线测量距离,表示第i行射线的方向， 表示第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中的最近点及其距离

$({\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j,D_c^{i,j}),i\∈[1..K_c],j\∈[1..L_c];$ 其中表示中第(i,j)点在P_m中的最近点编号，其距离为

步骤5：利用奇异值分解(SVD,Singular Value Decomposition)方法计算配准及误差： $(T_k,d_k)=R\{(P_m^{{\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j},P_c^{i,j}),C_c^{i,j}\≤D_{\mathrm{th}}\};$

步骤6：应用配准，

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

优选地，所述步骤4的具体过程如下：

(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^{i,j}=\sqrt{{(x_c^{i,j}-x_m^{{\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j})}^2+{(y_c^{i,j}-y_m^{{\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j})}^2+{(z_c^{i,j}-z_m^{{\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j})}^2}$

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

与现有技术相比，本发明的有益效果为：

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

附图说明

图1是当前点集雨模型点集的最近点匹配示意图；

图2是常规ICP方法的最近点与快速ICP方法的最近点比对结果图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合附图和具体实例进一步阐述本发明。

本发明应用于移动机器人三维位姿估计

Γ_t相对于Γ_t-1的位姿表示为x_t＝(x_t,y_t,z_t,α_t,β_t,γ_t)^T，其中x_t，y_t，z_t分别表示Γ_t坐标系的原点在坐标系Γ_t-1中的坐标，γ_t，β_t，α_t分别表示Γ_t相对于Γ_t-1在X,Y,Z轴的旋转角度。

则平移向量为，

p_t＝(x_t,y_t,z_t)^T

旋转矩阵为，

$H_t=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_t\cos {\mathrm{\β}}_t& \cos {\mathrm{\α}}_t\sin {\mathrm{\β}}_t\sin {\mathrm{\γ}}_t-\sin {\mathrm{\α}}_t\cos {\mathrm{\γ}}_t& \cos {\mathrm{\α}}_t\sin {\mathrm{\β}}_t\cos {\mathrm{\γ}}_t+\sin {\mathrm{\α}}_t\sin {\mathrm{\γ}}_t\\ \sin {\mathrm{\α}}_t\cos {\mathrm{\β}}_t& \sin {\mathrm{\α}}_t\sin {\mathrm{\β}}_t\sin {\mathrm{\γ}}_t+\cos {\mathrm{\α}}_t\cos {\mathrm{\γ}}_t& \sin {\mathrm{\α}}_t\sin {\mathrm{\β}}_t\cos {\mathrm{\γ}}_t-\cos {\mathrm{\α}}_t\sin {\mathrm{\γ}}_t\\ -\sin {\mathrm{\β}}_t& \cos {\mathrm{\β}}_t\sin {\mathrm{\γ}}_t& \cos {\mathrm{\β}}_t\cos {\mathrm{\γ}}_t\end{array}\right]$

从Γ_t-1到Γ_t的齐次变换T

$T=\left[\begin{array}{cc}{H}_t& p_t\\ 0& 1\end{array}\right]---\left(3\right)$

输入：(1)t-1时刻激光雷达点云数据i∈[1..K_t-1],j∈[1..L_t-1]，表示模型点集。

(2)t时刻激光雷达原始点云数据i∈[1..K_t],j∈[1..L_t]，表示当前点集。

其中,表示R_s的第i行的第j条(列)射线(s＝t-1或t),K_s为射线行数,L_s为每行射线数,表示第i行第j(列)射线测量距离,表示第i行射线的方向， 表示第j列射线的方向,分别表示行、列方向角度偏移,分别表示行、列方向角度分辨率)

输出：点云数据R_t相对于点云数据R_t-1的齐次变换T。

步骤1：计算模型点集R_t-1的平面直角坐标表示P_t-1

$P_{t-1}=\{x_{t-1}^{i,j},y_{t-1}^{i,j},z_{t-1}^{i,j}\},i\∈[1..K_{t-1}],j\∈[1..L_{t-1}]---\left(1\right)$

其中

步骤2:计算点集R_t的平面直角坐标表示P_t。

$P_t=\left\{(x_t^{i,j},y_t^{i,j},z_t^{i,j})\right\},i\∈[1..K_t],j\∈[1..L_t]$

其中，

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

步骤4：计算中的点在P_t-1中的最近点及其距离

$({\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j,D_c^{i,j}),i\∈[1..K_t],j\∈[1..L_t].$ 其中表示中第(i,j)点在P_t-1中的最近点编号，其距离为具体过程如下：

(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_t-1中最近点编号及距离。

$D_t^{i,j}=\sqrt{{(x_t^{i,j}-x_{t-1}^{{\mathrm{\Γ}}_t^i,{\mathrm{\Γ}}_t^j})}^2+{(y_t^{i,j}-y_{t-1}^{{\mathrm{\Γ}}_t^i,{\mathrm{\Γ}}_t^j})}^2+{(z_t^{i,j}-z_{t-1}^{{\mathrm{\Γ}}_t^i,{\mathrm{\Γ}}_t^j})}^2}$

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

步骤5：利用奇异值分解(SVD,Singular Value Decomposition)方法计算配准及误差： $(T_k,d_k)=R\{(P_{t-1}^{{\mathrm{\Γ}}_t^i,{\mathrm{\Γ}}_t^j},P_t^{i,j}),C_t^{i,j}\≤D_{\mathrm{th}}\}$

步骤6：应用配准，

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

参照图1，其中，虚线网格的交点为当前点集的点云，实线网格的交点为模型点集的点云。图示当前点集中第i行第j列的扫描点与模型点集的最近点位其第行第列的点。

在图2中，横轴为当前点集的编号，纵轴为两种方法获得的当前点在模型点集中的最近点的编号。

以上所述，仅为本发明最佳实施方式，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。

Claims (2)

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

输入：激光雷达原始点云数据i∈[1..K_m],j∈[1..L_m]，表示模型点集；

激光雷达原始点云数据i∈[1..K_c],j∈[1..L_c]，表示当前点集；

其中,表示R_s的第i行的第j条(列)射线(s＝m或c),K_s为射线行数,L_s为每行射线数,表示第i行第j(列)射线测量距离,表示第i行射线的方向， 表示第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]$

其中 $z_m^{i,j}={\mathrm{\ρ}}_m^{i,j}\sin {\mathrm{\φ}}_m^i;$

步骤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]$

其中， $z_c^{i,j}={\mathrm{\ρ}}_c^{i,j}\sin {\mathrm{\φ}}_c^i;$

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

步骤4：计算中的点在P_m中的最近点及其距离 $({\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j,D_c^{i,j}),i\∈[1..K_c],j\∈[1..L_c];$ 其中表示中第(i,j)点在P_m中的最近点编号，其距离为

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

$(T_k,d_k)=R\{(P_m^{{\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j},P_c^{i,j}),D_c^{i,j}\≤D_{\mathrm{th}}\};$

步骤6：应用配准，

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

2.根据权利要求1所述的三维激光雷达点云匹配的快速ICP方法，其特征在于，所述步骤4的具体过程如下：

(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^{i,j}=\sqrt{{(x_c^{i,j}-x_m^{{\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j})}^2+{(y_c^{i,j}-y_m^{{\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j})}^2+{(z_c^{i,j}-z_m^{{\mathrm{\Γ}}_c^i,{\mathrm{\Γ}}_c^j})}^2}$

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