[发明专利]一种三维激光雷达点云匹配的快速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_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

其中

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

其中，

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