CN115205354B - 基于ransac和icp点云配准的相控阵激光雷达成像方法 - Google Patents

Abstract

本发明涉及相控阵激光雷达快速成像技术，尤其涉及一种基于RANSAC和ICP点云配准的相控阵激光雷达成像方法，用于解决实际采集三维点云数据过程中单一视角下场景范围小、三维目标信息不完整的问题。根据相控阵激光雷达扫描过程中不同视角下的点云存在目标重合区域的情况，先提取点云的几何特征，寻找重合部分，利用RANSAC算法筛除误匹配点信息，从而完成不同视角的粗配准；随后根据粗配准得到的变换矩阵，进一步利用ICP算法通过不断迭代继续优化变换矩阵得到最佳变换矩阵，完成点云配准，最终将不同视角下的点云通过该变换矩阵进行拼接，获得场景更大、信息更完整的三维点云数据。

Description

基于RANSAC和ICP点云配准的相控阵激光雷达成像方法

技术领域

本发明涉及相控阵激光雷达快速成像技术，尤其涉及一种基于随机采样一致性(Random Sample Consensus，RANSAC)和迭代最近点(Iterative Closed Point，ICP)点云配准的相控阵激光雷达成像方法。

背景技术

扫描式激光雷达是目前获取三维点云的主要设备之一，目前可以分为两大类：机械扫描式激光雷达和相控阵扫描式激光雷达。机械扫描式激光雷达利用电机转动带动探测器旋转，从而扫描目标整体，这种方法受电机机械惯性的影响，无法满足雷达成像中的实时性要求，且功耗大。相控阵扫描式激光雷达借鉴了微波雷达成像技术中利用相控阵天线实现快速扫描的方法，成功解决了机械式扫描激光雷达中实时性低的问题，可实现非机械式多角度快速扫描，且体积更小，更易集成化。

在利用相控阵激光雷达采集三维点云数据的过程中，受到雷达视角限制、目标自身遮挡以及其他物体遮挡目标等因素的影响，采集到的三维点云目标一般不完整，不能很好的表征整个三维场景/目标。为了解决这一问题，需要从不同视角下采集三维点云数据，将采集到的数据通过变换矩阵关联到同一坐标系下，该关联过程称为点云配准，然后对配准后的点云进行拼接，从而获得场景更大、信息更完整的三维点云数据，该过程称为点云视场拼接，点云视场拼接中最为关键的环节为点云配准。

发明内容

针对上述现有技术中存在的问题，本发明提出一种基于RANSAC和ICP点云配准的相控阵激光雷达成像方法，用于解决实际场景中相控阵激光雷达在采集三维点云数据过程中单一视角下场景范围小、三维目标信息不完整的问题。根据相控阵激光雷达扫描过程中不同视角下的点云存在目标重合区域的情况，先提取点云的几何特征，寻找重合部分，利用RANSAC算法筛除误匹配点信息，从而完成不同视角的粗配准；随后根据粗配准得到的变换矩阵，进一步利用ICP算法通过不断迭代继续优化变换矩阵得到最佳变换矩阵，完成点云配准，最终将不同视角下的点云通过该变换矩阵进行拼接和融合，获得场景更大、信息更完整的三维点云数据。

本发明采用的技术方案为：一种基于RANSAC和ICP点云配准的相控阵激光雷达成像方法，该方法包括以下步骤：

S1：相控阵激光雷达获取点云。

在本发明中，通过相控阵激光雷达采集点云。与光学相机不同，相控阵激光雷达通过向目标发射激光光束后收集目标反射的激光信息来获取点云。在单一视角下，相控阵激光雷达采集的数据视场有限、信息量不足，因此需要将在多个视角下采集到的点云数据进行配准拼接，以得到场景更大、信息更丰富的点云数据。在本发明中利用相控阵激光雷达首先获取两帧点云P₁和P₂，P₁、P₂之间的重叠范围大于60％。

S2：点云体素化下采样。

对于分别包含有N和M个点的三维点云P₁和P₂，由于原始点云P₁、P₂中点数较多，如果直接在原始点云上进行配准计算的话计算量繁重。针对这一问题，将P₁、P₂进行体素化下采样以减少后续配准算法的计算量。以P₁体素化下采样为例，具体步骤如下：

S2.1为P₁创建大小为l×w×h、体积为v的三维包围框，设置体素的边长为L，所创建的三维包围框根据体素的边长L被划分为Q个体素：

S2.2体素的大小和数量是体素化下采样的重点，同一点云数据若体素边长小、数量多，则后续数据处理耗时长，但能够表征的点云信息丰富；若体素边长大、数量少，则后续数据处理耗时短，但丢失的点云信息多。为了得到最佳的体素边长和数量，根据点云密度γ和点云下采样率ρ估计体素的边长L′：

S2.3根据S2.2估计计算得到的体素边长L′将P₁划分为Q′个体素，统计体素内的点云个数，计算边长为L′时含有点云的体素的点云平均密度

其中v′表示含有点云的体素的体积。当

L′＝L+λ；当

L′＝L-λ。其中λ为增长因子，

当

时，L′＝L。

S2.4用每个体素中所有点云的重心点(x_m,y_m,z_m)表示当前体素：

其中n表示体素中点云的个数，i表示第i个点，(x_i,y_i,z_i)表示第i个点的坐标值。

S3：计算点云法向量。

点云体素化下采样后，可以根据点的法向量之间的差异找到两个点云中相似区域，这就需要提取点云的法向量特征。以P₁为例，点云法向量的计算过程具体如下：

S3.1采用KD-Tree搜索得到点云P₁中某一点p_1,i的K邻域点集p_1,j，对其构建协方差矩阵C：

其中K表示邻域内包含点的数量，p_{1,j＝1,2...,k}表示p_1,i的第k个近邻点。

为p_K的三维质心，

的求解公式为：

计算得出协方差矩阵C的特征值e和特征向量λ，协方差矩阵的最小特征值e_min对应的特征向量λ_i即为法向量f。

S4：计算点云的快速点特征直方图描述子(Fast Point Feature Histograms，FPFH)特征。

为了更加准确的寻找两个点云的相似区域，需要进一步提取点云的局部几何特征：快速点特征直方图描述子(Fast Point Feature Histograms，FPFH)。点云的FPFH特征是在简化点特征直方图描述子(Simplified Point Feature Histogram，SPFH)特征上进一步优化的一种特征描述符，用于描述点云的局部几何特征。以P₁为例，点云的FPFH特征计算步骤具体如下：

S4.1利用KD-Tree搜索点云P₁中某一点p_1,i的K邻域，得到K近邻点集O，通过参数化p_1,i与点集O中任意一点在空间上的差别和用一个计算得到的33维的向量直方图来描述K近邻点的空间几何特征。

假设p_1,i与其K近邻点集O中的一点O₁及对应的法向量f₁、f₂在局部坐标系vuw的空间位置和空间关系为：

U＝f₁   (7)

V＝(p_1,i-O₁)·U   (8)

W＝U·V   (9)

α＝V·f₂   (10)

θ＝arctan(W·f₁,U·f₂)   (12)

V、U、W表示局部坐标系的坐标轴方向上的单位向量，α表示f₂与局部坐标系V轴的夹角，β表示O₁向p_1,i方向的单位向量与U轴作内积的夹角，θ表示f₂在WU平面投影与U轴的夹角。将点p_1,i和其K近邻点集O中的点组成的所有点对求出(α,β,θ)，将(α,β,θ)按照11个区间划分，得到3×11＝33维的SPFH，根据p_1,i及其K近邻点的SPFH特征计算FPFH：

其中SPFH(p_1,i)表示点云P₁中的一点p_1,i的SPFH特征，SPFH(O_k)表示p_1,i的K近邻点O_k的SPFH特征，ω_k表示p_1,i与K近邻点O_k之间的距离。公式(13)最终得到33维的FPFH特征。

S5：RANSAC筛除误匹配信息。

在S4中得到的FPFH特征可以很好的描述点云之间的局部几何关系，但是由于噪声和算法自身性能的影响，计算得到的FPFH特征中包含一些错误的特征点会影响后续配准算法的性能，所以需要利用RANSAC算法筛除这些错误的特征点。

假设原始点云P₁中有一点p_1,i，待配准点云P₂中有一对应点p_2,i，在经过S4步骤得到FPFH特征后，通过利用RANSAC算法筛除误匹配的FPFH特征得到最佳的粗配准变换矩阵。利用RANSAC算法筛除FPFH特征中误匹配点的流程为：

S5.1在原始点云P₁中随机选择不共线的4个点p_1,i，其中i＝0,1,2,3，在待配准点云P₂中寻找与这4个点的FPFH特征最相似的点p_2,i，将其作为这四个点的对应点，对应点对的两个点p_1,i、p_2,i满足以下关系：

p_1,i＝R_p2,i+T   (14)

S5.2一般来说，正确的对应点应使下式L为0：

由于数据中含有噪声、对应点对之间的空间关系{R|T}变换矩阵求解不准确及计算误差等因素的影响，L通常情况下不为0，为了得到最佳的{R|T}变换矩阵，一般通过优化算法循环迭代公式(15)，当公式(15)的L为最小值时，此时{R|T}为最佳变换矩阵。

{R|T}变换矩阵通过单元四元数法求解。单元四元数由四矢量a₀,a₁,a₂,a₃构成：

a＝a₀+a₁I+a₂J+a₃L＝[a₀,a₁,a₂,a₃]^T   (16)

其中a₀≥0且a₀ ²+a₁ ²+a₂ ²+a₃ ²＝1，I、J、L为虚部单位。通过单元四元数求解(Closed-form solution of absolute orientation using unit quaternions，BKP Horn；AMethod for Registration of 3-D Shapes，PJ Besl)的旋转矩阵R和平移矩阵T分别为公式(15)、(16)：

T＝[a₄,a₅,a₆]^t   (18)

S5.2.1在求解旋转矩阵R和平移矩阵T之前，需要先求出原始点云P₁的质心

和待配准点云P₂的质心

其中

分别表示P₁、P₂中点的数量。

S5.2.2利用质心来计算P₁和P₂的互协方差矩阵

然后利用反对称矩阵

构成列向量Δ＝[G₂₃ G₃₁ G₁₂]^T，随后使用该矢量组成4×4的对称矩阵

其中

表示

矩阵中对角线上所有元素的和；I₃表示3×3单位矩阵。求出矩阵

中最大特征向量所对应的单位特征向量

即可求出最佳旋转矩阵R，确定最佳旋转矩阵R后即可求出最佳平移矩阵T：

S6：ICP点云精细配准。

ICP点云精细配准是在S5的基础上进一步精细化变换矩阵{R|T}，其基本流程为：

S6.1搜索最近点以确定对应点对：在原始点云P₁中选取一个点p_1,i，计算待配准点云P₂中所有点与p_1,i的距离，得到与p_1,i距离最近的点p_2,i，这一组距离最近的点构成一组对应点对

对集合P₁中所有的点都在集合Y中搜索距离最近的对应点。

S6.2求解配准变换：求解配准变换即计算点云在不同坐标系中产生的变换矩阵，本文所说的点云数据相对原始数据不会发生形变，只存在平移和旋转。变换矩阵即采用单位四元素法求解的3×3旋转矩阵和3×1平移矩阵。

S6.3应用变换：对三维点云Q根据刚体变换矩阵按照公式(14)进行变换。

S6.4重复迭代：重复迭代以上三个步骤，设置ICP收敛阈值TH，通常，TH＝0.02。当L<TH时得到最优刚体变换矩阵，此时得到最佳配准效果，迭代停止。

S7：点云融合。

点云融合是在点云配准完成后将两个点云融合为一个点云，融合具体步骤为：

S7.1将待配准点云P₂与点云配准计算得到的变换矩阵{R|T}相乘，然后将其和原始点云P₁拼接得到配准后的点云P₃：

P₃＝[P₁,P₂·{R|T}]   (23)

S7.2计算点云P₃中两个相邻点之间的距离d，设置距离阈值d_th，通常，d_th＝0.02；

S7.3判断d和d_th的关系，若d<d_th，说明这两个相邻点重复，去除掉其中的一点；若d≥d_th，说明两个相邻点没有重复。

本发明具有以下有益效果：通过本发明，根据相控阵激光雷达扫描成像过程中在单一视场视角小、扫描得到的三维场景信息有限的问题，通过将激光雷达扫描过程中各个视角下的小视场配准拼接成一个大的点云场景，通过RANSAC算法进行点云粗配准，通过ICP算法在粗配准的前提下进一步进行点云精细配准。本发明对促进大视场下相控阵激光雷达快速成像向工程化、使用化方向发展有重要作用。

附图说明

图1是本发明的处理流程图；

图2基于RANSAC的点云粗配准流程图；

图3是基于ICP的点云精细配准流程图；

图4是最小二乘法和RANSAC算法拟合数据效果；

图5是点云Q中某一点的K近邻点O集合中的两点O₁、O₂的局部坐标系；

图6是点云配准Simu Data数据集示例；

图7在Simu Data上RANSAC算法的实验结果；

图8在Simu Data上ICP算法的实验结果；

图9在Actu Data上RANSAC算法的实验结果；

图10在Actu Data上ICP算法的实验结果；

具体实施方式

下面结合附图对本发明进行进一步说明：

图1为本发明总处理流程。

本发明所述一种基于RANSAC和ICP点云配准的相控阵激光雷达成像方法，该方法包括以下步骤：

S1：相控阵激光雷达获取点云；

S2：点云体素化下采样；

S3：计算点云法向量；

S4：计算点云的快速点特征直方图描述子(Fast Point Feature Histograms，FPFH)特征；

S5：RANSAC筛除误匹配信息；

S6：ICP点云精细配准；

S7：点云融合。

下面以仿真数据来检验本发明的有益效果。

本发明在两个数据集上测试算法效果，数据集中包含的目标以及各个目标信息如表1所示。第一个数据集是通过仿真生成大场景点云，在本发明中称之为Simu Data，通过将Simu Data中的一个大场景点云切分为若干个小场景点云来模拟相控阵激光雷达的扫描过程；第二个数据集是通过Realsense L515在实际场景中获取军事目标模型的深度图像，将深度图转换为点云数据，在本发明中称之为Actu Data。对于Simu Data，本发明将切分后小场景点云绕Z轴旋转

度，使之模拟相控阵激光雷达的扫描过程，然后将小场景点云进行配准拼接；对于Actu Data，本发明将整个点云场景绕Z轴旋转

度，然后对其进行配准。旋转角度

服从高斯分布(5.73,0.29)。图6展示了切分后的用于视场拼接的两个仿真场景。

表1 Simu Data场景信息

表2 Actu Data场景信息

在本发明中，RANSAC算法的超参数设定为：点云搜索半径为10cm；对应近邻点数为30；点云之间的距离阈值为2cm；随机采样点数为4。ICP算法超参数设定为：点云之间的距离阈值为0.2cm。为验证算法的抗噪声性能，本文向p_1,...,i、p′_1,...i、G和G‘中分别添加不同的高斯噪声(Gauss Noise，GN)，GN满足正态分布(μ,σ)，其中μ＝0，在[0,0.01]中等间隔选取15个值作为标准差σ。

图7和图9分别展示了基于RANSAC算法的点云粗配准技术在Simu Data和ActuData上的配准效果，从图中可以看出，随着高斯噪声标准差σ的增大，点云配准的角度均方误差逐渐增大，点云配准效果逐渐变差。在高斯噪声的标准差小于0.005时，噪声对RANSAC算法的影响很小，可以得到较好的变换矩阵。图8和图10分别展示了基于ICP算法的点云精细配准技术在Simu Data和ActuData上的配准效果，从图中可以看出，随着高斯噪声标准差σ的增大，点云配准的角度均方误差逐渐增大，点云配准效果逐渐变差。从图7至图10可知，点云粗配准的角度均方误差大于点云精细配准的均方误差，说明点云精细配准可以在点云粗配准的前提下进一步优化变化矩阵，在点云精细配准配准后，角度均方误差保持了一个较小的值，证明了本发明方法的配准拼接能力。

Claims (3)

1.一种基于RANSAC和ICP点云配准的相控阵激光雷达成像方法，其特征在于，该方法包括以下步骤：

S1：相控阵激光雷达获取点云；

S2：点云体素化下采样

对于分别包含有N和M个点的三维点云P₁和P₂，P₁体素化下采样具体步骤如下：

S2.1为P₁创建大小为l×w×h、体积为v的三维包围框，设置体素的边长为L，所创建的三维包围框根据体素的边长L被划分为Q个体素：

S2.2为了得到最佳的体素边长和数量，根据点云密度γ和点云下采样率ρ估计体素的边长L′：

S2.3根据S2.2估计计算得到的体素边长L′将P₁划分为Q′个体素，统计体素内的点云个数，计算边长为L′时含有点云的体素的点云平均密度

其中v′表示含有点云的体素的体积；当

L′＝L+λ；当

L′＝L-λ，其中λ为增长因子，

当

时，L′＝L；

S2.4用每个体素中所有点云的重心点(x_m,y_m,z_m)表示当前体素：

其中n表示体素中点云的个数，i表示第i个点，(x_i,y_i,z_i)表示第i个点的坐标值；

S3：计算点云法向量

P₁点云法向量的计算过程具体如下：

S3.1采用KD-Tree搜索得到点云P₁中某一点p_1,i的K邻域点集p_1,j，对其构建协方差矩阵C：

其中K表示邻域内包含点的数量，p_{1,j＝1,2...,k}表示p_1,i的第k个近邻点；

为p_K的三维质心，

的求解公式为：

计算得出协方差矩阵C的特征值e和特征向量λ，协方差矩阵的最小特征值e_min对应的特征向量λ_i即为法向量f；

S4：计算点云的快速点特征直方图描述子特征

P₁点云的快速点特征直方图描述子特征计算步骤具体如下：

S4.1利用KD-Tree搜索点云P₁中某一点p_1,i的K邻域，得到K近邻点集O，通过参数化p_1,i与点集O中任意一点在空间上的差别和用一个计算得到的33维的向量直方图来描述K近邻点的空间几何特征；

假设p_1,i与其K近邻点集O中的一点O₁及对应的法向量f₁、f₂在局部坐标系vuw的空间位置和空间关系为：

U＝f₁                        (7)

V＝(p_1,i-O₁)·U                  (8)

W＝U·V                       (9)

α＝V·f₂                     (10)

θ＝arctan(W·f₁,U·f₂)                   (12)

V、U、W表示局部坐标系的坐标轴方向上的单位向量，α表示f₂与局部坐标系V轴的夹角，β表示O₁向p_1,i方向的单位向量与U轴作内积的夹角，θ表示f₂在WU平面投影与U轴的夹角；将点p_1,i和其K近邻点集O中的点组成的所有点对求出(α,β,θ)，将(α,β,θ)按照11个区间划分，得到3×11＝33维的SPFH，根据p_1,i及其K近邻点的SPFH特征计算FPFH：

其中SPFH(p_1,i)表示点云P₁中的一点p_1,i的SPFH特征，SPFH(O_k)表示p_1,i的K近邻点O_k的SPFH特征，ω_k表示p_1,i与K近邻点O_k之间的距离；公式(13)最终得到33维的FPFH特征；

S5：RANSAC筛除误匹配信息

利用RANSAC算法筛除FPFH特征中误匹配点的流程为：

S5.1在原始点云P₁中随机选择不共线的4个点p_1,i，其中i＝0,1,2,3，在待配准点云P₂中寻找与这4个点的FPFH特征最相似的点p_2,i，将其作为这四个点的对应点，对应点对的两个点p_1,i、p_2,i满足以下关系：

p_1,i＝Rp_2,i+T                (14)

S5.2正确的对应点应使下式L为0：

由于数据中含有噪声、对应点对之间的空间关系{R|T}变换矩阵求解不准确及计算误差等因素的影响，L通常情况下不为0，为了得到最佳的{R|T}变换矩阵，通过优化算法循环迭代公式(15)，当公式(15)的L为最小值时，此时{R|T}为最佳变换矩阵；

{R|T}变换矩阵通过单元四元数法求解，单元四元数由四矢量a₀,a₁,a₂,a₃构成：

a＝a₀+a₁I+a₂J+a₃L＝[a₀，a₁，a₂，a₃]^T             (16)

其中a₀≥0且a₀ ²+a₁ ²+a₂ ²+a₃ ²＝1，I、J、L为虚部单位；通过单元四元数求解的旋转矩阵R和平移矩阵T分别为公式(15)、(16)：

T＝[a₄，a₅，a₆]^t                      (18)

S5.2.1在求解旋转矩阵R和平移矩阵T之前，先求出原始点云P₁的质心

和待配准点云P₂的质心

其中

分别表示P₁、P₂中点的数量；

S5.2.2利用质心计算P₁和P₂的互协方差矩阵

S5.2.3利用反对称矩阵

构成列向量Δ＝[G₂₃ G₃₁ G₁₂]^T，随后使用该矢量组成4×4的对称矩阵

其中

表示

矩阵中对角线上所有元素的和；I₃表示3×3单位矩阵；

S5.2.4求出矩阵

中最大特征向量所对应的单位特征向量

即可求出最佳旋转矩阵R，确定最佳旋转矩阵R后即可求出最佳平移矩阵T：

S6：ICP点云精细配准

ICP点云精细配准是在S5的基础上进一步精细化变换矩阵{R|T}，其基本流程为：

S6.1搜索最近点以确定对应点对：在原始点云P₁中选取一个点p_1,i，计算待配准点云P₂中所有点与p_1,i的距离，得到与p_1,i距离最近的点p_2,i，这一组距离最近的点构成一组对应点对

对集合P₁中所有的点都在集合Y中搜索距离最近的对应点；

S6.2求解配准变换：求解配准变换即计算点云在不同坐标系中产生的变换矩阵，变换矩阵即采用单位四元素法求解的3×3旋转矩阵和3×1平移矩阵；

S6.3应用变换：对三维点云Q根据刚体变换矩阵按照公式(14)进行变换；

S6.4重复迭代：重复迭代以上三个步骤，设置ICP收敛阈值TH，当L<TH时得到最优刚体变换矩阵，此时得到最佳配准效果，迭代停止；

S7：点云融合

点云融合是在点云配准完成后将两个点云融合为一个点云，融合具体步骤为：

S7.1将待配准点云P₂与点云配准计算得到的变换矩阵{R|T}相乘，然后将其和原始点云P₁拼接得到配准后的点云P₃：

P₃＝[P₁,P₂·{R|T}]                        (23)

S7.2计算点云P₃中两个相邻点之间的距离d，设置距离阈值d_th；

S7.3判断d和d_th的关系，若d<d_th，说明这两个相邻点重复，去除掉其中的一点；若d≥d_th，说明两个相邻点没有重复。

2.一种根据权利要求1所述基于RANSAC和ICP点云配准的相控阵激光雷达成像方法，其特征在于：S6.4中，ICP收敛阈值TH＝0.02。

3.一种根据权利要求1所述基于RANSAC和ICP点云配准的相控阵激光雷达成像方法，其特征在于：S7.2中，距离阈值d_th＝0.02。

Images