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

摘要

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

权利要求

1.一种三维激光雷达点云匹配的快速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的平面直角坐标表示Pm其中

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

其中，

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

步骤4：计算 中的点在Pm中的最近点及其距离 其中 表示 中第(i,j)点在Pm中的最近点编号，其距离为 具体过程如下：(4.1)对于 中的每一个点 计算其方向(4.2)计算 方向在Pm中最近点编号 及距离；

(4.3)根据距离阈值选择用于配准的点集步骤5：利用奇异值分解方法计算配准及误差：步骤6：应用配准，

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

说明书

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

技术领域

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

背景技术

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

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

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

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

发明内容

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

[0007] 其具体技术方案为：

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

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

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

[0011] 其中, 表示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列射线的方向,
分别表示行、列方向角度偏移, 分别表示行、列方向角度分辨率；

[0012] 输出：点云数据Rc相对于点云数据Rm的齐次变换T；

[0013] 步骤1：计算模型点集Rm的平面直角坐标表示Pm

[0014]

[0015] 其中

[0016] 步骤2:计算点集Rc的平面直角坐标表示Pc；

[0017]

[0018] 其中，

[0019] 步骤3：初始化迭代次数k＝0, T0＝I4×4，误差阈值τ；

[0020] 步骤4：计算 中的点在Pm中的最近点及其距离 i∈[1,Kc],j∈[1,Lc]；其中 表示 中第(i,j)点在Pm中的最近点编号，其距离为

[0021] 步骤5：利用奇异值分解(SVD,Singular Value Decomposition)方法计算配准及误差：

[0022] 步骤6：应用配准，

[0023] 步骤7：如果dk-1-dk＜τ，则停止迭代，输出Tk，否则，k＝k+1，转步骤4。

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

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

[0026] (4.2)计算 方向在Pm中最近点编号 及距离；

[0027]

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

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

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

附图说明

[0031] 图1是当前点集与模型点集的最近点匹配示意图；

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

具体实施方式

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

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

[0035] Γt相对于Γt-1的位姿表示为xt＝(xt,yt,zt,αt,βt,γt)T，其中xt，yt，zt分别表示Γt坐标系的原点在坐标系Γt-1中的坐标，γt，βt，αt分别表示Γt相对于Γt-1在X,Y,Z轴的旋转角度。

[0036] 则平移向量为，

[0037] pt＝(xt,yt,zt)T

[0038] 旋转矩阵为，

[0039]

[0040] 从Γt-1到Γt的齐次变换T

[0041]

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

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

[0044] 其中, 表示Rt-1的第i行的第j列射线, 表示Rt的第i行的第j列射线,Kt-1为Rt-1的射线行数,Kt为Rt的射线行数,Lt-1为Rt-1的每行射线数,Lt为Rt的每行射线数, 表示Rt-1的第i行第j列射线测量距离, 表示Rt的第i行第j列射线测量距离, 表示Rt-1的第i行射线的方向， 表示Rt的第i行射线的方向，表示Rt-1的第j列射线的方向, 表示Rt的第j列射线的方向,
分别表示行、列方向角度偏移, 分别表示行、列方向角
度分辨率。

[0045] 输出：点云数据Rt相对于点云数据Rt-1的齐次变换T。

[0046] 步骤1：计算模型点集Rt-1的平面直角坐标表示Pt-1

[0047]

[0048] 其中

[0049] 步骤2:计算点集Rt的平面直角坐标表示Pt。

[0050]

[0051] 其中，

[0052] 步骤3：初始化迭代次数k＝0, T0＝I4×4，误差阈值τ。

[0053] 步骤4：计算 中的点在Pt-1中的最近点及其距离 i∈[1,Kt],j∈[1,Lt]。其中 表示 中第(i,j)点在Pt-1中的最近点编号，其距离为 具体过程如下：

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

[0055]

[0056] (4.2)计算 方向在Pt-1中最近点编号 及距离。

[0057]

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

[0059] 步骤5：利用奇异值分解(SVD,Singular Value Decomposition)方法计算配准及误差：

[0060] 步骤6：应用配准，

[0061] 步骤7：如果dk-1-dk＜τ，则停止迭代，输出Tk，否则，k＝k+1，转第4步。

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