[发明专利]一种激光雷达与视觉点云的初始化自动配准方法

权利要求书

1.一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，包括如下步骤：

步骤一：获得整个真实场景的激光雷达扫描点云P_l；

步骤二：利用相机对真实场景的某一区域进行拍照，采用视觉重建的方式获得该区域的视觉点云P_v；

步骤三：对点云P_l和点云P_v进行预处理；

步骤四：从点云P_l中随机选取M个点作为球心，以每一个球心构建N个半径大小不同的球体，把球体内部的点作为构建候选区域点云的点，从而获得规模大小不同的候选区域点云

步骤五：尺度统一化，以点云的规模大小作为尺度基准，对点云P_v的尺度调整，得到点云

步骤六：分别计算点云和点云的ESF描述子；

步骤七：对点云的ESF描述子和点云的ESF描述子进行粗匹配；

步骤八：选出匹配程度最好的前NN个候选区域点云中的点云和对应点云NN取大于2整数；

步骤九：采用具有尺度估计的JRMPC算法计算变换矩阵，得到点云与点云间的变换矩阵Tⁿ；

步骤十：计算点云与点云间的配准误差；然后返回步骤九，直到达到设定的执行次数，执行步骤十一；

步骤十一：选出配准误差最小的变换矩阵T_best和其对应的点云Y_v-best，完成点云P_l和P_v的配准。

2.如权利要求1所述的一种激光雷达与视觉点云的初始化自动配准方法，其特征在于，所述步骤九具体计算过程如下：

S90、将X_lⁿ表示为X₁，Y_vⁿ表示为X₂；

S91、假设点云X₁和X₂之间存在一个虚拟场景中心点云W，用K个混合高斯模型表示该点云；其中μ_k,σ_k，k＝1,...,K是混合高斯模型的均值和方差，且W＝[μ_k]，W为3*K的矩阵，K为虚拟场景中心点云W的点云数量；

S92、基于此混合高斯模型的参数，构建点云X₁和X₂分别到虚拟场景中心点云W变换后点云T₁(X₁)、T₂(X₂)，点云T₁(X₁)、T₂(X₂)中各点属于混合高斯模型的概率如公式(2)所示:

其中p()表示一个混合高斯模型的概率；x_ji表示第j个点云中的第i个点，且j＝1,2,i＝1,...,N_j，N_j为第j个点云中点的数量；ω是内外点比率，h是点云X_j球体的体积；变换后点云中各点T_j(x_ji)＝s_jR_jx_ji+t_j，s_j、R_j和t_j分别是第j个点云到虚拟场景中心点云W变换矩阵的尺度因子、变换矩阵和平移矢量；

S93、采用EM算法计算混合模型参数和变换矩阵参数，具体为：

S931、初始化混合高斯模型参数和变换矩阵参数；

S932、E步计算点云X_j的后验分布概率，其后验分布概率α_jik表示第j个点云的第i个点属于第k个高斯模型的概率：

其中

S933、M步计算混合高斯模型参数和变换矩阵参数：先利用步骤a计算变换矩阵参数T_j，j＝1,2；然后利用步骤b计算混合高斯模型参数μ_k,σ_k：

步骤a:计算变换矩阵T_j的R_j、t_j、s_j参数：

R_j＝U_j*diag([11det(U_jV_j^T)])*V_j^T (4)

其中A_j＝[A_jk],为3*K的矩阵，Λ_j是K*K的对角矩阵,λ_jk是Λ_j的第k个对角元素的值，且取e为1*k的单位矢量；其中将矩阵D_jΛ_j²C_j^T进行SVD分解，得到矩阵U_j、M_j、V_j^T；

步骤b:计算混合高斯模型参数μ_k,σ_k：

其中公式(8)中的ε²是调整因子；

S934、重复S932和S933直至迭代达到设定次数；

S935、基于上述步骤得到点云和分别到虚拟场景中心点云W的变换矩阵T_l、T_v；

S94、计算点云X_lⁿ到Y_vⁿ的变换矩阵Tⁿ。