CN104166989A - 一种用于二维激光雷达点云匹配的快速icp方法 - Google Patents

Abstract

本发明公开了一种用于二维激光雷达点云匹配的快速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为迭代次数。

Description

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

技术领域

本发明涉及的是一种ICP方法，具体涉及一种用于二维激光雷达点云匹配的快速ICP方法。

背景技术

二维激光雷达广泛应用于移动机器人领域，例如移动机器人避障、环境建模、目标跟踪、位姿估计等。在这些任务中，通常需要对激光雷达点云数据进行配准。如何快速地进行数据配准是实现移动机器人各类任务实时性的关键。

迭代最近点算法(ICP，Iterative Close Point)是实现点云数据匹配的一种经典方法。然而，ICP算法具有以下不足：(1)要求初始估计较为准确，(2)大量的点云数据使得ICP算法效率低，(3)ICP算法可能陷入局部最优解。

现有技术提出了一种经典的迭代最近点(Iterative Closest Point，ICP)技术，可以对两个点集进行配准，其基本过程如下：

1、令和分别代表代配准的两个点集(具体地，将数据点集P配准到模型点集X)。

2、初始化点集P₀＝P，配准向量迭代次数k＝0，误差阈值τ。

3、计算最近点，Y_k＝Closest(P_k，X)(时间复杂度为O(N_PN_X))

4、计算配准及其误差，(时间复杂度为O(N_P))

5、应用配准，(时间复杂度为O(N_P))

6、如果d_k-1-d_k＜τ，则停止迭代，输出否则，k＝k+1，转第3步迭代最近点(ICP)的主要缺点为：1)如果初始矩阵选择不当，可能导致陷入局部最优点；2)计算最近点的过程计算复杂度较大，为O(N_PN_X)。假设算法迭代次数为D，则算法复杂度为：O(DN_PN_X)。由于配准算法通常要被多次调用，而实际问题的数据点集规模也通常非常大，因此该方法难以获得实时结果。

现有技术还提出了一种在极坐标系下配准二维激光雷达点云数据的方法。其基本思路是利用激光雷达原始扫描数据的结构信息，利用扫描投影的方法计算当前扫描在参考坐标系中的期望点云。

该方法首先对原始点云数据进行适当的预处理以去除异常数据。此后，在初始配准估计的基础上，迭代地进行扫描投影(scan projection)、平移估计、旋转估计。具体技术方案如下。

令分别表示参考点云和当前点云，表示当前点云C相对于参考点云R的初始位姿估计，k＝0。

1、扫描投影， 表示当前点云C按位姿 $x_c^k=(x_c^k,y_c^k,{\mathrm{\θ}}_c^k)$

投射到参考点云R后，在参考点云方向的期望距离。

2、平移估计，目的是找到使目标函数最小的平移估计(x_c，y_c)。其中w_i为权重。有 $\left[\begin{array}{c}{\mathrm{\Δx}}_c^k\\ {\mathrm{\Δy}}_c^k\end{array}\right]={\left(H^T\mathrm{WH}\right)}^{-1}{H}^{T}W(r_r-r_c^{\′\′}).$

其中， $H=\left[\begin{array}{cc}\frac{{\∂r}_{c_1}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c_1}^{\′\′}}{{\∂y}_c}\\ \frac{{\∂r}_{c_2}^{\′\′}}{{\∂x}_c}& \frac{{\∂r}_{c_2}^{\′\′}}{{\∂y}_c}\\ ...& ...\\ & \end{array}\right],$ W为权重w_i的对角矩阵。 $r_r=\left\{r_{r_i}\right\},r_c^{\′\′}=\left\{r_{c_i}^{\′\′}\right\}.$

3、方向估计， ${\mathrm{\Δ\θ}}_c^k=\mathrm{orientation}(C,R,x_c^k,y_c^k,{\mathrm{\θ}}_c^k,{\mathrm{\Δx}}_c^k,{\mathrm{\Δy}}_c^k).$ 在平移估计的基础上，以1°的分辨率对[-20°，+20°]范围进行扫描投影，计算使目标函数最小的

4、更新位姿估计 $\left\{\begin{array}{c}{x}_c^{k+1}=x_c^k+{\mathrm{\Δx}}_c^k\\ y_c^{k+1}=y_c^k+{\mathrm{\Δy}}_c^k\\ {\mathrm{\θ}}_c^{k+1}={\mathrm{\θ}}_c^k+{\mathrm{\θy}}_c^k\end{array}\right.,$ 计算k和k+1次迭代的目标函数值，当两者的差距小于设定阈值时终止，否则，k＝k+1，转第1步。

该方法的主要缺点包括：1)要求初始估计较为准确，否则有可能陷入局部最优点。2)角度估计与平移估计分为两步，且角度估计仍需要对[-20°，+20°]范围的窗口进行扫描，降低了算法效率。

发明内容

针对现有技术上存在的不足，本发明目的是在于提供一种用于二维激光雷达点云匹配的快速ICP方法，使得二维激光雷达原始点云数据配准速度大幅度提高，即将时间复杂度从经典ICP方法的O(DN_PN_X)降为O(DN_P)，本发明对二维激光雷达原始点云数据的特点，在极坐标系下快速地计算两个点云之间的最近点方法。

为了实现上述目的，本发明是通过如下的技术方案来实现：一种用于二维激光雷达点云匹配的快速ICP方法，其技术方案为：输入：(1)激光雷达原始点云数据表示模型点集。

(2)激光雷达原始点云数据表示当前点集。

其中，表示R_s的第j条射线(s＝m或c)，L_s分别点集R_s的射线总数，表示射线的测量距离，表示射线的方向， 表示角度偏移，ζ表示角度分辨率；

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

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

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

其中 $x_m^j={\mathrm{\ρ}}_m^j\cos \left({\mathrm{\α}}_m^j\right),y_m^j={\mathrm{\ρ}}_m^j\sin \left({\mathrm{\α}}_m^j\right).$

步骤2：通过分段分析方法构建当前激光雷达点云数据R_c的有效射线集合W_c，以及点集W_c的平面直角坐标表示P_m。

步骤(2.1)通过式(2)对R_c进行分段分析，

$R_c=\underset{k=1}{\overset{c_t}{\∪}}{G}_c^k---\left(2\right)$

其中，表示第k段，c_t表示分段数目， $r_c^{c_t}=L,,f_c^{k+1}=r_c^k+1,|{\mathrm{\&rho;}}_c^{i+1}-{\mathrm{\&rho;}}_c^i|<\mathrm{gap},i+1,i\&Element;[f_c^k..r_c^k],|{\mathrm{\&rho;}}_c^{f_c^{k+1}}-{\mathrm{\&rho;}}_c^{r_c^k}|\&GreaterEqual;\mathrm{gap},$ gap表示一个阈值，

步骤(2.2)通过式(3)有效射线集合，

$W_c=\underset{k=1}{\overset{c_t}{\&cup;}}{G}_c^k,d_c^k>\mathrm{\&gamma;},{\mathrm{\&lambda;}}_2\&GreaterEqual;{\widetilde{\mathrm{\&rho;}}}_c^k\&GreaterEqual;{\mathrm{\&lambda;}}_1---\left(3\right)$

其中，W_c表示R_c的有效射线集合，表示第k段射线数目，表示第k段射线平均长度，根据这两个特征，γ，λ₂，λ₁分别表示阈值。例如，对于LMS291激光雷达，设置γ＝5，λ₂＝81.9m，λ₁＝.03m；

步骤(2.3)，计算点集W_c的平面直角坐标表示P_c。

$P_c=\left\{(x_c^j,y_c^j)\right\},b_c^j\&Element;W_c$

其中，其中 $x_c^j={\mathrm{\&rho;}}_c^j\cos \left({\mathrm{\&alpha;}}_c^j\right),y_c^j={\mathrm{\&rho;}}_c^j\sin \left({\mathrm{\&alpha;}}_c^j\right).$

步骤3：初始化迭代次数k＝0，T₀＝I_3×3，误差阈值τ。计算中的点在P_m中的最近点及其距离其中表示中第j点在P_m中的最近点编号为其距离为具体过程如下：

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

(3.2)计算方向在P_m中最近点编号

${\mathrm{\&Gamma;}}_c^j=\left\{\begin{array}{c}1,\mathrm{idx}<1\\ L_m,\mathrm{idx}>=L_m\\ \mathrm{idx},L_m>\mathrm{idx}\&GreaterEqual;1\end{array}\right.$

$D_c^j=\sqrt{{(x_c^j-x_m^{{\mathrm{\&Gamma;}}_c^j})}^2+{(y_c^j-y_m^{{\mathrm{\&Gamma;}}_c^j})}^2}$

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

步骤4：利用奇异值分解(SVD，Singular Value Decomposition)方法计算配准及误差： $(T_k,d_k)=R\{(P_m^{{\mathrm{\&Gamma;}}_c^j},P_c^j),D_c^j\&le;D_{\mathrm{th}}\}$

步骤5：应用配准，

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

本发明设计了一种快速的方法来计算两个待配准的二维激光雷达点云数据的最近点。将常规的ICP方法在计算最近点时的平方复杂度(O(N_PN_X))降为线性复杂度(O(N_P))(N_P为当前点云数据个数，N_X为模型点集数据个数)。

附图说明

下面结合附图和具体实施方式来详细说明本发明；

图1为本发明的实施例的四种方法的比较(航迹推算传感器正常时，故障前)图；

图2为本发明的实施例的四种方法的比较(航迹推算传感器故障时)图；

图3为本发明的实施例的四种方法的比较(航迹推算传感器正常时，故障排除后)图。

具体实施方式

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

参照图1-3，本具体实施方式采用以下技术方案：一种用于二维激光雷达点云匹配的快速ICP方法，其方法步骤为：

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

$P_m=\left\{(x_m^j,y_m^j)\right\},j\&Element;[1..L_m]---\left(1\right)$

其中 $x_m^j={\mathrm{\&rho;}}_m^j\cos \left({\mathrm{\&alpha;}}_m^j\right),y_m^j={\mathrm{\&rho;}}_m^j\sin \left({\mathrm{\&alpha;}}_m^j\right).$

步骤2：通过分段分析方法构建当前激光雷达点云数据R_c的有效射线集合W_c，以及点集W_c的平面直角坐标表示P_m。

步骤(2.1)通过式(2)对R_c进行分段分析，

$R_c=\underset{k=1}{\overset{c_t}{\&cup;}}{G}_c^k---\left(2\right)$

其中，表示第k段，c_t表示分段数目， $r_c^{c_t}=L,,f_c^{k+1}=r_c^k+1,|{\mathrm{\&rho;}}_c^{i+1}-{\mathrm{\&rho;}}_c^i|<\mathrm{gap},i+1,i\&Element;[f_c^k..r_c^k],|{\mathrm{\&rho;}}_c^{f_c^{k+1}}-{\mathrm{\&rho;}}_c^{r_c^k}|\&GreaterEqual;\mathrm{gap},$ gap表示一个阈值，

步骤(2.2)通过式(3)有效射线集合，

$W_c=\underset{k=1}{\overset{c_t}{\&cup;}}{G}_c^k,d_c^k>\mathrm{\&gamma;},{\mathrm{\&lambda;}}_2\&GreaterEqual;{\widetilde{\mathrm{\&rho;}}}_c^k\&GreaterEqual;{\mathrm{\&lambda;}}_1---\left(3\right)$

其中，W_c表示R_c的有效射线集合，表示第k段射线数目，表示第k段射线平均长度，根据这两个特征，γ，λ₂，λ₁分别表示阈值。例如，对于LMS291激光雷达，设置γ＝5，λ₂＝81.9m，λ₁＝.03m；

步骤(2.3)，计算点集W_c的平面直角坐标表示P_c。

$P_c=\left\{(x_c^j,y_c^j)\right\},b_c^j\&Element;W_c$

其中，其中 $x_c^j={\mathrm{\&rho;}}_c^j\cos \left({\mathrm{\&alpha;}}_c^j\right),y_c^j={\mathrm{\&rho;}}_c^j\sin \left({\mathrm{\&alpha;}}_c^j\right).$

步骤3：初始化迭代次数k＝0，T₀＝I_3×3，误差阈值τ。计算中的点在P_m中的最近点及其距离其中表示中第j点在P_m中的最近点编号为其距离为具体过程如下：

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

(3.2)计算方向在P_m中最近点编号

${\mathrm{\&Gamma;}}_c^j=\left\{\begin{array}{c}1,\mathrm{idx}<1\\ L_m,\mathrm{idx}>=L_m\\ \mathrm{idx},L_m>\mathrm{idx}\&GreaterEqual;1\end{array}\right.$

$D_c^j=\sqrt{{(x_c^j-x_m^{{\mathrm{\&Gamma;}}_c^j})}^2+{(y_c^j-y_m^{{\mathrm{\&Gamma;}}_c^j})}^2}$

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

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

步骤6：应用配准，

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

实施例1：图1-3说明了在机器人导航过程中，激光雷达配准的结果比较。四种方法都以航迹推算为初始估计，图1、3为正常情况下的结果，图2为传感器异常(导致航迹推算有较大误差)的结果。

图1-3说明快速ICP比常规ICP收敛速度快。同时，快速ICP的最近点计算过程为线性复杂度，而常规ICP的最近点计算过程为平方复杂度。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (2)

1.一种用于二维激光雷达点云匹配的快速ICP方法，其特征在于，其技术方案为：输入：(1)激光雷达原始点云数据表示模型点集；

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

其中，表示R_s的第j条射线(s＝m或c)，L_s分别点集R_s的射线总数，表示射线的测量距离，表示射线的方向， 表示角度偏移，ζ表示角度分辨率；

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

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

$P_m=\left\{(x_m^j,y_m^j)\right\},j\&Element;[1..L_m]---\left(1\right)$

其中 $x_m^j={\mathrm{\&rho;}}_m^j\cos \left({\mathrm{\&alpha;}}_m^j\right),y_m^j={\mathrm{\&rho;}}_m^j\sin \left({\mathrm{\&alpha;}}_m^j\right);$

步骤2：通过分段分析方法构建当前激光雷达点云数据R_c的有效射线集合W_c，以及点集W_c的平面直角坐标表示P_m；

步骤(2.1)通过式(2)对R_c进行分段分析，

$R_c=\underset{k=1}{\overset{c_t}{\&cup;}}{G}_c^k---\left(2\right)$

其中，表示第k段，c_t表示分段数目， $r_c^{c_t}=L,,f_c^{k+1}=r_c^k+1,|{\mathrm{\&rho;}}_c^{i+1}-{\mathrm{\&rho;}}_c^i|<\mathrm{gap},i+1,i\&Element;[f_c^k..r_c^k],|{\mathrm{\&rho;}}_c^{f_c^{k+1}}-{\mathrm{\&rho;}}_c^{r_c^k}|\&GreaterEqual;\mathrm{gap},$ gap表示一个阈值，

步骤(2.2)通过式(3)有效射线集合，

$W_c=\underset{k=1}{\overset{c_t}{\&cup;}}{G}_c^k,d_c^k>\mathrm{\&gamma;},{\mathrm{\&lambda;}}_2\&GreaterEqual;{\widetilde{\mathrm{\&rho;}}}_c^k\&GreaterEqual;{\mathrm{\&lambda;}}_1---\left(3\right)$

其中，W_c表示R_c的有效射线集合，表示第k段射线数目，表示第k段射线平均长度，根据这两个特征，γ，λ₂，λ₁分别表示阈值；例如，对于LMS291激光雷达，设置γ＝5，λ₂＝81.9m，λ₁＝.03m；

步骤(2.3)，计算点集W_c的平面直角坐标表示P_c；

$P_c=\left\{(x_c^j,y_c^j)\right\},b_c^j\&Element;W_c$

其中，其中 $x_c^j={\mathrm{\&rho;}}_c^j\cos \left({\mathrm{\&alpha;}}_c^j\right),y_c^j={\mathrm{\&rho;}}_c^j\sin \left({\mathrm{\&alpha;}}_c^j\right);$

步骤3：初始化迭代次数k＝0，T₀＝I_3×3，误差阈值τ；计算中的点在P_m中的最近点及其距离其中表示中第j点在P_m中的最近点编号为其距离为

步骤4：利用奇异值分解(SVD，Singular Value Decomposition)方法计算配准及误差： $(T_k,d_k)=R\{(P_m^{{\mathrm{\&Gamma;}}_c^j},P_c^j),D_c^j\&le;D_{\mathrm{th}}\};$

步骤5：应用配准，

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

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

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

(3.2)计算方向在P_m中最近点编号

${\mathrm{\&Gamma;}}_c^j=\left\{\begin{array}{c}1,\mathrm{idx}<1\\ L_m,\mathrm{idx}>=L_m\\ \mathrm{idx},L_m>\mathrm{idx}\&GreaterEqual;1\end{array}\right.$

$D_c^j=\sqrt{{(x_c^j-x_m^{{\mathrm{\&Gamma;}}_c^j})}^2+{(y_c^j-y_m^{{\mathrm{\&Gamma;}}_c^j})}^2}$

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