[发明专利]基于RANSAC和ICP点云配准的相控阵激光雷达成像方法

权利要求书

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，当LTH时得到最优刚体变换矩阵，此时得到最佳配准效果，迭代停止；

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的关系，若dd_th，说明这两个相邻点重复，去除掉其中的一点；若d≥d_th，说明两个相邻点没有重复。