CN112308961A - 基于分层高斯混合模型的机器人快速鲁棒三维重建方法 - Google Patents

Abstract

本发明公开了基于分层高斯混合模型的机器人快速鲁棒三维重建方法。该方法包含以下步骤：机器人获取测量对象点云数据，GPU加速生成分层高斯混合模型和测试集，构建及更新配准网络，全局优化配准网络，更新重建的高斯混合模型，重复上述步骤直到机器人在所有测量点完成测量，重建测量对象的三维点云模型，分析和评估重建结果。该方法通过GPU并行计算加速点云数据生成分层高斯混合模型，同时能有效处理噪声和测量的不确定性，提高三维重建的速度和效率，通过构建配准网络和更新配准网络，以及全局优化配准网络来减少联合配准误差，保证三维重建的精度。本发明自动化程度高、重建速度快、鲁棒性强，特别适用于工业场景大型测量对象的稠密点云三维重建。

Description

基于分层高斯混合模型的机器人快速鲁棒三维重建方法

技术领域

本发明涉及三维重建技术领域，特别涉及基于分层高斯混合模型的机器人快速鲁棒三维重建方法。

背景技术

三维重建技术就是获取环境中物体的三维信息，从而确定不同物体所在位置并建立相应的空间模型，在自动驾驶、遥感测绘、虚拟博物馆、建筑设计、电子商务、临床医学等高科技领域有着很重要的应用。特别在工业应用中，三维重建是实现对工业产品整体型面尺寸参数测量的关键，对工业产品的加工有着重要指导意义。

三维点云数据的处理目前面临的问题主要包括采样密度不均匀、输入数据非结构化、对不同细节层次的处理速度不够、内存要求高以及传感器噪声和测量不确定性等。此外，这些光学测量设备每秒产生几万甚至数百万个数据点，使得三维点云数据的存储、显示和传输带来了极大的不便，并且大大降低了三维点云数据后续处理的效率，使得在有限计算资源的设备上很难实时有效地利用所有传入数据。目前研究大多通过对数据进行降采样和稠密、稀疏或分层的体素化技术来处理大量点云数据。体素化和降采样点云使空间处理过程变得复杂，而且体素表示的几何模型会产生附加的离散化，没有明确的方法来处理噪声和数据不确定性。

如何解决上述问题，成为近年来科学研究重点关注的问题，而对三维点云数据高斯混合模型表示的研究为解决这些问题提供了新的途径。

发明内容

本发明要解决的技术问题是，克服现有技术存在的上述缺陷，提供基于分层高斯混合模型的机器人快速鲁棒三维重建方法，精简三维点云数据的数据量，降低数据测量中的噪声，提高三维点云数据的处理效率。

本发明解决其技术问题采用的技术方案是：提供基于分层高斯混合模型的机器人快速鲁棒三维重建方法，包括以下步骤：

S1：规划机器人测量点，机器人在测量点测量目标对象，获取测量对象的点云数据；

S2：将点云数据加速生成分层高斯混合模型，用高斯混合模型表示，并提取平滑后的点云数据和测试集；

S3：构建及更新配准网络；

S4：全局优化配准网络；

S5：更新重建的高斯混合模型；

S6：重复步骤S1～S5直到机器人在所有测量点完成测量，重建测量对象的三维点云模型，分析和评估重建结果。

进一步的，所述步骤S2包括以下步骤：

S21：初始化参数：L、J、λ_s、λ_d、λ_c、λ_t、λ_s，其中L为分层高斯混合模型的层数，J为父节点拥有的子节点数量，λ_s为收敛阈值，λ_d为置零阈值，λ_c为自适应阈值，λ_t为测试集控制因子，λ_s为平滑控制因子，以及累加变量

聚类层数l＝0，分层高斯混合模型参数Θ＝Θ_init，分类索引

其中N_r为第r次测量获得的点云数据S_r中点的数量，当前分类索引currIdx＝parentIdx，

S22：并行计算EM算法的E_step和M_step，将点云数据聚类生成分层高斯混合模型，其中，EM算法为期望最大算法，E_step为计算联合分布的条件概率期望，M_step为极大化联合分布的条件概率期望估计模型参数；

S23：通过收敛阈值λ_s判断EM算法是否收敛，若收敛，则更新索引parentIdx＝currIdx，否则返回步骤S22；

S24：更新l←l+1，判断分层高斯混合模型的聚类层数l是否为L，若是则执行下一步骤S25，否则返回步骤S22；

S25：加速搜索分层高斯混合模型的叶子节点，并生成具有不同细节层次的高斯混合模型；

S26：提取平滑后的点云和测试集。

进一步的，所述步骤S22具体步骤为：

S221：输入机器人测量获得的点云数据S_r和初始化参数；

S222：并行计算E_step，其中S_r中的每个点均由一个线程处理：

首先，若点z_i所属节点G_s的权重π_s＞0，则计算：

h_i＝π_s(1-Σ_jπ_j)/η (2)

α_ij＝γ_ij/(Σ_jγ_ij+h_i) (3)

其中，η为噪声簇的体积(噪声在S_r分布空间的凸壳体积)，γ_ij为S_r中的点z_i属于高斯分量G_j的概率，α_ij为归一化的概率，

为原子操作，N_r为点云的数量，Children(i)＝[(i+1)J...(i+2)J-1]为父子节点索引函数，s＝parentIdx(i)，i＝1,...,N_r，j＝(s+1)J,...,(s+2)J-1，π_s ^*＝(1-Σ_jπ_j)为节点包含的子节点中噪声分布的权重；

然后，采用硬分配策略，将点z_i分配到期望γ_ij最大值对应的高斯分量G_k中，即更新currIdx[i]＝k，其中j∈Children(s)，s＝parentIdx(i)；

S223：完成点云数据S_r中所有点的处理后，并行计算M_step，其中每一个高斯分量G_j均由一个线程处理：

Θ_j＝{π_j,μ_j,Σ_j} (8)

其中，μ_j、Σ_j分别为高斯分布的均值和协方差矩阵，N_r为S_r中点的数量，j∈Level(l)，

为第l层节点索引函数；

S224：若

则高斯分量G_j中聚类的点很少，则该节点可以删除，即设置π_j＝0；若Complexity(Θ_j)<λ_c，其中Complexity(Θ_j)为模型复杂度，则高斯分量G_j对应的聚类点集分布变得十分平坦，不需要进一步细化，若G_j有子节点，则π_k＝0，k＝(j+1)J,...,(j+2)J-1；

S225：完成第l层所有高斯分量的处理后，更新分层高斯混合模型参数Θ；

进一步的，所述步骤S3包含以下具体步骤：

S31：初次测量时构建配准网络Graph＝{(GMMs₁,^*T₁)}，其中

GMMs₁为机器人在第一个规划测量点测量目标对象获取的点云数据生成的高斯混合模型，{*}为配准网络的参考坐标系；

S32：初始化K个变量s₁,…,s_K，分别记录与当前机器人末端姿态ξ_r邻近的K个测量点；

S33：将第r个测量点(n_r,t_r)添加到机器人已经经过的测量点的集合V中，其中，机器人末端坐标系Z轴朝向测量对象，t_r、n_r分别为第r个测量点在机器人基座坐标系中的位置和方向，t_r＝(x_r,y_r,z_r)，n_r＝R_r(α_r,β_r,γ_r)e_z，x_r、y_r、z_r分别为机器人在第r个测量点时末端的坐标，α_r、β_r、γ_r分别为机器人在第r个测量点时末端绕X、Y、Z轴的旋转角度，e_z＝[0 0 1]^T，旋转矩阵

S34：通过模拟退火粒子群并行算法实现高斯混合模型

与GMMs_r的快速配准，其中

为机器人在规划的第r个测量点附近的K个测量点测量目标对象获取的点云数据生成的高斯混合模型，k＝1,...,K，GMMs_r为机器人在规划的第r个测量点测量目标对象获取的点云数据生成的高斯混合模型；

S35：选择距离最近的第k个邻近测量点作为对齐参考点，计算GMMs_r变换到{*}坐标系的变换矩阵为

其中，

为

与GMMs_r之间的变换矩阵，其中R_k为旋转矩阵，t_k为平移向量，

为

变换到{*}坐标系的变换矩阵；

S36：将GMMs_r中高斯分量G_j的参数变换到{*}坐标系，记为Θ_j ^*，更新重建的概率模型GMMs；

S37：将{GMMs_r,^*T_r}添加的配准网络Graph中。

进一步的，所述步骤S35中距离最近为分布之间的L2距离度量f_k最大，通过计算f_k来评估

与GMMs_r的分布差异，定义

其中，L2距离为欧氏距离，y_ij＝μ_i-R_kμ_j-t_k，Σ_ij＝Σ_i+R_kΣ_jR_k ^T，μ_i、μ_j为高斯分量的均值，Σ_i、Σ_j为高斯分量的协方差矩阵，π_i、π_j为高斯分量的权重，R_k、t_k分别为R_k为

与GMMs_r之间变换的旋转矩阵和平移向量，

为

GMMs_r为

若f_k越小，则G_i与G_j分布差异越大，f_k越大，则G_i与G_j分布差异越小。

进一步的，所述步骤S4，包括如下步骤：

S41：初始化迭代次数q、最大迭代次数Q和参数矩阵M＝[μ₁,...,μ_K]；

S42：构造添加均匀分布噪声的高斯混合模型GMMs^*＝GMMs+π_K+1u(h)，即

其中u(h)为噪声的均匀分布，h为噪声在空间分布的凸壳体积，

σ_k1,σ_k2,σ_k3分别为Σ_k的特征值，σ_k1≥σ_k2≥σ_k3，π_K+1为噪声分量的权重，离群点所占比率

S43：将GMMs^*、Test_r和γ作为并行JRMPC算法的输入数据，其中，r＝1,...,N_V，N_V为测量点的数量；

S44：并行计算JRMPC算法的E_step和M_step，减少概率模型GMMs的重建误差；

S45：判断是否达到最大迭代次数或JRMPC算法已经收敛，若是则输出优化的概率模型GMMs和各测量点的点云数据变换到{*}坐标系的变换矩阵Trans_r，否则返回步骤S44。

进一步的，所述机器人为拍照式三维扫描仪与机器人组成的机器人测量系统，其中拍照式三维扫描仪通过连接件固定在机器人末端法兰上。

本发明产生的有益效果是：本发明提供基于分层高斯混合模型的机器人快速鲁棒三维重建方法，通过并行EM计算将稠密的三维点云数据生成包含不同细节的高斯混合模型表示，精简三维点云数据的数据量，有效降低数据测量中的噪声；通过增量式构建和全局优化配准网络策略及基于模拟退火粒子群算法的两个高斯混合模型之间的快速配准方法，有效提高三维点云数据的处理效率；同时通过光学扫描仪和机器人的结合，提高了测量效率及测量系统的自动化程度。

附图说明

图1是基于分层高斯混合模型的机器人快速鲁棒三维重建方法的总体流程图；

图2是分层高斯混合模型的结构示意图；

图3是GPU并行计算粒子适应值的线程模型示意图；

图4是GPU加速更新粒子位置和速度的线程模型示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。

参见图1，提供基于分层高斯混合模型的机器人快速鲁棒三维重建方法，该方法对每次测量获得的大量稠密点云通过GPU加速生成分层高斯混合模型HGMMs_r，并搜索分层高斯混合模型HGMMs_r的叶子节点，生成高斯混合模型GMMs_r，将大量稠密点云通过少量且具有不同细节层次的高斯混合模型GMMs_r来描述；通过构建及更新配准网络和全局优化配准网络，更新重建的概率模型，最终实现三维重建。具体步骤包含：

S1：规划机器人测量点，机器人在测量点测量目标对象，获取测量对象的点云数据S_r；

S2：将点云数据S_r聚类生成分层高斯混合模型HGMMs_r和高斯混合模型GMMs_r，并提取平滑后的点云数据Smoothed_r和测试集Test_r，其中r＝1,...,N_V，N_V为测量点的数量；

S3：构建并更新配准网络Graph，添加高斯混合模型GMMs_r(顶点)和^*T_r(边)，其中{*}表示配准网络的参考坐标系；

S4：全局优化配准网络；

S5：更新重建的高斯混合模型GMMs；

S6：重复步骤S1～S5直到机器人在所有测量点完成测量，重建测量对象的三维点云模型Model，分析和评估重建的结果。

进一步的，步骤S2所述HGMMs_r通过GPU加速EM算法生成，GMMs_r通过加速搜索生成，并提取平滑后的点云Smoothed_r和测试集Test_r，用于重建点云模型和优化配准网络。

S21：初始化参数：L、J、λ_s、λ_d、λ_c、λ_t、λ_s，其中L为分层高斯混合模型的层数，J为父节点拥有的子节点数量，λ_s为收敛阈值，λ_d为置零阈值，λ_c为自适应阈值，λ_t为测试集控制因子，λ_s为平滑控制因子，以及累加变量

聚类层数l＝0，分层高斯混合模型参数Θ＝Θ_init，分类索引

其中N_r为第r次测量获得的点云S_r中点的数量，当前分类索引currIdx＝parentIdx，

其中，分层高斯混合模型结构如图2所示，每一个节点代表一个高斯分量，置零节点表示没有足够点云支持的高斯分量(由λ_d判断)，叶子节点表示聚类最深层数的非置零节点，或对应的聚类点集分布十分平坦(由λ_c判断)，不需要进一步细分的节点；

初始化参数Θ_init可以通过以下步骤设置：

S211：采用如图3所示的线程模型并行计算S_r的中心

以P_-1为中心将S_r划分为J个子区域，分别统计每个子区域的点数比重、均值和方差作为子节点的权重、均值和方差；

S212：采用上述划分方式，进一步对子区域划分和统计，直到初始化L层所有节点的参数；

S22：初始化完成后，并行计算EM算法的E_step，并采用硬分配策略，将点z_i分配到期望值γ_ij最大时对应的高斯分量G_j中，再并行计算EM算法的M_step，并精简分层高斯混合模型HGMMs节点，具体包含以下步骤：

S221：输入机器人测量获得的点云数据S_r和初始化参数；

S222：并行计算E_step，其中S_r中的每个点均由一个线程处理：

首先，若点z_i所属节点G_s的权重π_s＞0，则计算：

h_i＝π_s(1-Σ_jπ_j)/η (2)

α_ij＝γ_ij/(Σ_jγ_ij+h_i) (3)

其中，η为噪声簇的体积(噪声在S_r分布空间的凸壳体积)，γ_ij为S_r中的点z_i属于高斯分量G_j的概率，α_ij为归一化的概率，

为原子操作，N_r为点云的数量，Children(i)＝[(i+1)J...(i+2)J-1]为父子节点索引函数，s＝parentIdx(i)，i＝1,...,N_r，j＝(s+1)J,...,(s+2)J-1，π_s ^*＝(1-Σ_jπ_j)为节点包含的子节点中噪声分布的权重；

S223：完成点云数据S_r中所有点的处理后，并行计算M_step，其中每一个高斯分量G_j均由一个线程处理：

Θ_j＝{π_j,μ_j,Σ_j} (8)

其中，μ_j、Σ_j分别为高斯分布的均值和协方差矩阵，N_r为S_r中点的数量，j∈Level(l)，

为第l层节点索引函数；

S224：若

则高斯分量G_j中聚类的点很少，则该节点可以删除，即设置π_j＝0；若Complexity(Θ_j)<λ_c，其中Complexity(Θ_j)为模型复杂度，则高斯分量G_j对应的聚类点集分布变得十分平坦，不需要进一步细化，若G_j有子节点，则π_k＝0，k＝(j+1)J,...,(j+2)J-1，定义函数

σ₁≥σ₂≥σ₃，其中，σ₁,σ₂,σ₃分别为Σ_j的特征值，λ_c可取0.01；

S225：完成第l层所有高斯分量的处理后，更新分层高斯混合模型参数Θ；

S23：通过收敛阈值判断EM算法是否收敛，即定义函数Converged(λ_s)判断分层高斯混合模型参数Θ连续多次的变化是否均小于λ_s，若是则EM算法已经收敛；若收敛则更新索引parentIdx＝currIdx，否则返回步骤S22；

S24：更新l←l+1，判断分层高斯混合模型的聚类层数l是否为L，若是则执行下一步骤S25，否则返回步骤S22，进行更深层次的聚类；

S25：当聚类层数l达到初始化值L后，加速搜索分层高斯混合模型的叶子节点，生成具有不同细节层次的高斯混合模型GMMs_r：

S251：初始化l＝0，GMMs_r＝{}，搜索节点集合Seq＝{-1}，集合中元素个数N＝1；

S252：集合Seq中的每个节点Seq[j]均由一个线程处理，其中j＝1,...,N：

搜索节点的子节点，若该节点没有子节点且P_j＞0，则G_j为叶子节点，添加到高斯混合模型GMMs_r中，否则计算

其中，k＝(Seq[j]+1)J,...,(Seq[j]+2)J-1，如果P_j＜ε，ε为极小的正值，则G_j为叶子节点，添加到高斯混合模型GMMs_r中；若不满足上述两种情况，则从搜索节点集合Seq中删除该节点，并将该节点的子节点添加到Seq中；

S253：更新l←l+1，重复步骤S252直到l等于L，最后更新高斯混合模型GMMs_r＝Seq。

S26：最后提取平滑后的点云Smoothed_r和测试集Test_r：

由于噪声点z_i的期望

很小，通过设定合适的λ_t和λ_s可以有效分离噪声数据并控制Test_r和Smoothed_r中点云的规模，即如果γ_ij＞λ_t，则将z_i添加到Test_r中，如果γ_ij＞λ_s，则将z_i添加到Smoothed_r中，其中λ_t、λ_s∈(c,1)，c为较小的正值，可以设置为常数或是按一定规律变化的随机数。

进一步的，步骤S3所述构建配准网络为通过模拟退火粒子群并行算法实现两个高斯混合模型之间的快速配准，并通过变换矩阵构建星型配准网络，该方法包含：

S31：初次测量时构建配准网络Graph＝{(GMMs₁,^*T₁)}，其中

GMMs₁为机器人在第一个规划测量点测量目标对象获取的点云数据生成的高斯混合模型，{*}为配准网络的参考坐标系；

S32：初始化K个变量s₁,…,s_K，分别记录与当前机器人末端姿态ξ_r邻近的K个测量点：

S321：初始化s₁＝C,s₂＝C,...,s_K＝C，i＝1，其中，C为足够大的正数；

S322：如果n_i ^Tn_r＞0，则计算d_ir＝(t_i-t_r)^T(t_i-t_r)，如果d_ir＜s_K，则更新s₁＝s₂,s₂＝s₃,...,s_K＝i，其中，(n_i,t_i)、(n_r,t_r)分别为机器人经过的第i、r个测量点；

S323：更新i←i+1，重复步骤S322直到i＞N_V，其中N_V为机器人经过的测量点的数量；

S33：将第r个测量点(n_r,t_r)添加到机器人已经经过的测量点的集合V中，其中，机器人末端坐标系Z轴朝向测量对象，t_r、n_r分别为第r个测量点在机器人基座坐标系中的位置和方向，t_r＝(x_r,y_r,z_r)，n_r＝R(α_r,β_r,γ_r)e_z，x_r、y_r、z_r分别为机器人在第r个测量点(末端姿态ξ_r)时末端的坐标，α_r、β_r、γ_r分别为机器人在第r个测量点(末端姿态ξ_r)时末端绕X、Y、Z轴的旋转角度，e_z＝[0 0 1]^T，旋转矩阵为

S34：通过模拟退火粒子并行算法实现GMMs_s*与GMMs_r的快速配准，其中s*＝s₁,…,s_K：

S341：初始化退火温度T，冷却因子α，最大迭代次数N_m，迭代次数N＝0，从x₁∈[α_rmin,α_rmax]、x₂∈[β_rmin,β_rmax]、x₃∈[γ_rmin,γ_rmax]、x₄∈[x_rmin,x_rmax]、x₅∈[y_rmin,y_rmax]、x₆∈[z_rmin,z_rmax]搜索空间中初始化一个规模为m的粒子群，设第k个粒子的初始位置x_k＝(x_k1,x_k2,x_k3,x_k4,x_k5,x_k6)和速度v_k＝(v_k1,v_k2,v_k3,v_k4,v_k5,v_k6)，并初始化适应值s_k＝0、s_g＝0，k＝1,...,m；搜索空间由公式

确定，x∈(X-ΔX_min,X+ΔX_max)，其中，

ΔX_min和ΔX_max限制搜索空间的范围；

S342：并行搜索配准问题的解，每个粒子均由一个线程块处理：

S3421：计算平移向量t_k＝[x₄,x₅,x₆]^T和旋转矩阵R_k＝R(x₁,x₂,x₃)；

S3422：计算GMMs_s*与GMMs_r之间的距离分量，其中每个分量f_ij均由一个线程处理：

y_ij＝μ_i-R_kμ_j-t_k (9)

Σ_ij＝Σ_i+R_kΣ_jR_k ^T (10)

其中，GMMs_s*为

GMMs_r为

S3423：采用如图3所示的线程模型，计算粒子的适应值

S3424：如果f_k＞s_k，则更新当前的粒子经历过最好位置X_k＝x_k，s_k＝f_k；如果f_k＞s_g，则更新粒子全局经历过的最好位置X_g＝x_k，s_g＝f_k；

S3425：采用如图4所示的线程模型，更新粒子的速度v_k和位置x_k：

x_ks(t+1)＝x_k(t)+v_ks(t+1) (13)

其中k＝1,...,m，s＝1,...,6，v_ks∈[-v_smax,v_smax]，v_smax为第s维的最大搜索速度，

为收缩因子，有效控制与约束粒子的飞行速度，同时增强算法局部搜索能力，

C＝c₁+c₂且C＞4，t为当前迭代次数，学习因子c₁、c₂为非负常数，r₁、r₂为相互独立的伪随机数，服从[0,1]上的均匀分布；

S3426：重复步骤S3422和S3422，计算粒子适应值f_k；

S3427：按Metropolis准则选择更新X_k，如果

则更新X_k＝x_k，s_k＝f_k；如果f_k＞s_g，则更新X_g＝x_k，s_g＝f_k；

S3428：重复步骤S3425，更新粒子的速度v_k和位置x_k；

S343：更新退火温度T←αT，迭代次数N←N+1，若N＞N_m或X_g停滞不再变化，则输出配准结果X_g、s_g，否则返回步骤S342。

S35：完成K个邻近测量点的GMMs_s*与GMMs_r之间的快速配准后，其中s*＝s₁,…,s_K，选择s_g最大值对应的第k个邻近测量点作为对齐参考点，计算高斯混合模型GMMs_r变换到{*}坐标系的变换矩阵

其中T_g为

与GMMs_r之间的变换矩阵，t_g＝[x_g4,x_g5,x_g6]^T，R_g＝R(x_g1,x_g2,x_g3)，

X_g＝(x_g1,x_g2,x_g3,x_g4,x_g5,x_g6)为s_g最大值对应的配准结果；

S36：将GMMs_r中高斯分量G_j的参数变换到{*}坐标系，记为Θ_j ^*，其中每个分量均由一个线程处理：

μ_j ^*＝R_kμ_j+t_k (14)

Σ_j ^*＝R_kΣ_jR_k ^T (15)

Θ_j ^*＝{π_j,μ_j ^*,Σ_j ^*} (16)

其中，

更新概率模型GMMs，采用一个线程块处理：

y_ij＝μ_i-μ_j ^* (17)

Σ_ij＝Σ_i+Σ_j ^* (18)

其中，

计算

如果f_j<η_G，则GMMs_r中的高斯分量G_j具有新的信息，将Θ_j ^*添加到GMMs中，其中η_G为设定的较小的正值；

S37：将{GMMs_r,^*T_r}添加到配准网络Graph中。

进一步的，步骤S4所述全局优化配准网络为利用配准网络Graph包含的全局信息和测试集Test_r包含的局部信息对配准网络进行全局优化，减少联合配准误差，该算法包含：

S41：初始化迭代次数q、最大迭代次数Q和参数矩阵M＝[μ₁,...,μ_K]；

S42：构造添加均匀分布噪声的高斯混合模型GMMs^*＝GMMs+π_K+1u(h)，即

其中u(h)为噪声的均匀分布，h为噪声在空间分布的凸壳体积，

σ_k1,σ_k2,σ_k3分别为Σ_k的特征值，σ_k1≥σ_k2≥σ_k3，π_K+1为噪声分量的权重，离群点所占比率

S43：将GMMs^*、Test_r和γ作为并行JRMPC算法的输入数据，其中r＝1,...,N_V，N_V为测量点的数量；

S44：并行计算JRMPC算法(多点云联合配准算法)的E_step和M_step，减少概率模型GMMs的重建误差；

S45：判断是否达到最大迭代次数或JRMPC算法已经收敛，若是则输出优化的概率模型GMMs和各测量点的点云数据变换到{*}坐标系的变换矩阵Trans_r，否则返回步骤S44。

进一步的，步骤S6根据配准网络的优化结果，通过GPU加速重建测量对象的点云模型Model并进行点云精简，输出精简后的点云Trimmed：

首先，输入平滑后的点云Smoothed_r和变换矩阵Trans_r，其中r＝1,...,N_V；

然后，并行计算*z_ir＝R_rz_ir+t_r，将点*z_ir添加到点云模型Model中，其中，z_ir∈Smoothed_r，i＝1,...,N_r，N_r为Smoothed_r中点的数量，Trans_r＝(R_r,t_r)，R_r、t_r分别为点云Smoothed_r变换到{*}坐标系的旋转矩阵和平移向量；完成平滑后的点云中所有点的计算后，对点云模型Model进行点云精简，输出精简后的点云Trimmed。

进一步的，所述机器人为拍照式三维扫描仪与机器人集成的机器人测量系统，其中拍照式三维扫描仪通过连接件固定在机器人末端法兰上，可以实现大范围的高效测量。

本发明提供基于分层高斯混合模型的机器人快速鲁棒三维重建方法，通过并行EM算将稠密的三维点云数据生成包含不同细节的高斯混合模型表示，精简三维点云数据的数据量，有效降低数据测量中的噪声；通过增量式构建和全局优化配准网络策略及基于模拟退火粒子群算法的两个高斯混合模型之间的快速配准方法，有效提高三维点云数据的处理效率；同时通过光学扫描仪和机器人的结合，提高了测量效率及测量系统的自动化程度。

上面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，不能理解为对本发明保护范围的限制。

总之，本发明虽然列举了上述优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和替换，这些改进和替换也应视为本发明的保护范围。

Claims (7)

1.基于分层高斯混合模型的机器人快速鲁棒三维重建方法，其特征在于，包括以下步骤：

S1：规划机器人测量点，机器人在测量点测量目标对象，获取测量对象的点云数据；

S2：将点云数据加速生成分层高斯混合模型，使用高斯混合模型表示，并提取平滑后的点云数据和测试集；

S3：构建及更新配准网络；

S4：全局优化配准网络；

S5：更新重建的高斯混合模型；

S6：重复步骤S1～S5直到机器人在所有测量点完成测量，重建测量对象的三维点云模型，分析和评估重建结果。

2.根据权利要求1所述的基于分层高斯混合模型的机器人快速鲁棒三维重建方法，其特征在于，所述步骤S2包括以下步骤：

S21：初始化参数：L、J、λ_s、λ_d、λ_c、λ_t、λ_s，其中L为分层高斯混合模型的层数，J为父节点拥有的子节点数量，λ_s为收敛阈值，λ_d为置零阈值，λ_c为自适应阈值，λ_t为测试集控制因子，λ_s为平滑控制因子，以及累加变量

聚类层数l＝0，分层高斯混合模型参数Θ＝Θ_init，分类索引

其中N_r为第r次测量获得的点云数据S_r中点的数量，当前分类索引currIdx＝parentIdx，

S22：并行计算EM算法的E_step和M_step，将点云数据聚类生成分层高斯混合模型，其中，EM算法为期望最大化算法，E_step为计算联合分布的条件概率期望，M_step为极大化联合分布的条件概率期望估计模型参数；

S23：通过收敛阈值λ_s判断EM算法是否收敛，若收敛，则更新索引parentIdx＝currIdx，否则返回步骤S22；

S24：更新l←l+1，判断分层高斯混合模型的聚类层数l是否为L，若是则执行下一步骤S25，否则返回步骤S22；

S25：加速搜索分层高斯混合模型的叶子节点，并生成具有不同细节层次的高斯混合模型；

S26：提取平滑后的点云和测试集。

3.根据权利要求2所述的基于分层高斯混合模型的机器人快速鲁棒三维重建方法，其特征在于，所述步骤S22具体步骤为：

S221：输入机器人测量获得的点云数据S_r和初始化参数；

S222：并行计算E_step，其中S_r中的每个点均由一个线程处理：

首先，若点z_i所属节点G_s的权重π_s＞0，则计算：

h_i＝π_s(1-Σ_jπ_j)/η (2)

α_ij＝γ_ij/(Σ_jγ_ij+h_i) (3)

其中，η为噪声簇的体积即噪声在S_r分布空间的凸壳体积，γ_ij为S_r中的点z_i属于高斯分量G_j的概率，α_ij为归一化的概率，

为原子操作，N_r为S_r中云的数量，Children(i)＝[(i+1)J...(i+2)J-1]为父子节点索引函数，s＝parentIdx(i)，i＝1,...,N_r，j＝(s+1)J,...,(s+2)J-1，π_s ^*＝(1-Σ_jπ_j)为节点包含的子节点中噪声分布的权重；

然后，采用硬分配策略，将点z_i分配到期望γ_ij最大值对应的高斯分量G_k中，即更新currIdx[i]＝k，其中j∈Children(s)，s＝parentIdx(i)；

S223：完成点云数据S_r中所有点的处理后，并行计算M_step，其中每一个高斯分量G_j均由一个线程处理：

Θ_j＝{π_j,μ_j,Σ_j} (8)

其中，μ_j、Σ_j分别为高斯分布的均值和协方差矩阵，N_r为S_r中点的数量，j∈Level(l)，

为第l层节点索引函数；

S224：若

则高斯分量G_j中聚类的点很少，则该节点可以删除，即设置π_j＝0；若Complexity(Θ_j)<λ_c，其中Complexity(Θ_j)为模型复杂度，则高斯分量G_j对应的聚类点集分布变得十分平坦，不需要进一步细化，若G_j有子节点，则π_k＝0，k＝(j+1)J,...,(j+2)J-1；

S225：完成第l层所有高斯分量的处理后，更新分层高斯混合模型参数Θ。

4.根据权利要求3所述的基于分层高斯混合模型的机器人快速鲁棒三维重建方法，其特征在于，所述步骤S3包含以下具体步骤：

S31：初次测量时构建配准网络Graph＝{(GMMs₁,^*T₁)}，其中

GMMs₁为机器人在第一个规划测量点测量目标对象获取的点云数据生成的高斯混合模型，{*}为配准网络的参考坐标系；

S32：初始化K个变量s₁,…,s_K，分别记录与当前机器人末端姿态ξ_r邻近的K个测量点；

S33：将第r个测量点(n_r,t_r)添加到机器人已经经过的测量点的集合V中，其中，机器人末端坐标系Z轴朝向测量对象，t_r、n_r分别为第r个测量点在机器人基座坐标系中的位置和方向，t_r＝(x_r,y_r,z_r)，n_r＝R_r(α_r,β_r,γ_r)e_z，x_r、y_r、z_r分别为机器人在第r个测量点时末端的坐标，α_r、β_r、γ_r分别为机器人在第r个测量点时末端绕X、Y、Z轴的旋转角度，e_z＝[0 0 1]^T，旋转矩阵

S34：通过模拟退火粒子群并行算法实现高斯混合模型

与GMMs_r的快速配准，其中

为机器人在规划的第r个测量点附近的第k个测量点测量目标对象获取的点云数据生成的高斯混合模型，k＝1,...,K，GMMs_r为机器人在规划的第r个测量点测量目标对象获取的点云数据生成的高斯混合模型；

S35：选择距离最近的第k个邻近测量点作为对齐参考点，计算GMMs_r变换到{*}坐标系的变换矩阵为

其中，

为

与GMMs_r之间的变换矩阵，其中R_k为旋转矩阵，t_k为平移向量，

为

变换到{*}坐标系的变换矩阵；

S36：将GMMs_r中高斯分量G_j的参数变换到{*}坐标系，记为Θ_j ^*，更新重建的概率模型GMMs；

S37：将{GMMs_r,^*T_r}添加的配准网络Graph中。

5.根据权利要求4所述的基于分层高斯混合模型的机器人快速鲁棒三维重建方法，其特征在于，所述步骤S35中距离最近为分布之间的L2距离度量f_k最大，通过计算f_k来评估

与GMMs_r的分布差异，定义

其中，L2距离为欧氏距离，y_ij＝μ_i-R_kμ_j-t_k，Σ_ij＝Σ_i+R_kΣ_jR_k ^T，μ_i、μ_j为高斯分量的均值，Σ_i、Σ_j为高斯分量的协方差矩阵，π_i、π_j为高斯分量的权重，R_k、t_k分别为

与GMMs_r之间变换的旋转矩阵和平移向量，

为

GMMs_r为

若f_k越小，则G_i与G_j分布差异越大，f_k越大，则G_i与G_j分布差异越小。

6.根据权利要求5所述的基于分层高斯混合模型的机器人快速鲁棒三维重建方法，其特征在于，所述步骤S4，包括如下步骤：

S41：初始化迭代次数q、最大迭代次数Q和参数矩阵M＝[μ₁,...,μ_K]；

S42：构造添加均匀分布噪声的高斯混合模型GMMs^*＝GMMs+π_K+1u(h)，

其中u(h)为噪声的均匀分布，h为噪声在空间分布的凸壳体积，π_K+1为噪声分量的权重，离群点所占比率

S43：将GMMs^*、Test_r和γ作为并行JRMPC算法的输入数据，其中r＝1,...,N_V，N_V为测量点的数量；

S44：并行计算JRMPC算法的E_step和M_step，减少概率模型GMMs的重建误差；

S45：判断是否JRMPC算法已经收敛或达到最大迭代次数，若是则输出优化的概率模型GMMs和各测量点的点云数据变换到{*}坐标系的变换矩阵Trans_r，否则返回步骤S44。

7.根据权利要求1-6中任一项所述的基于分层高斯混合模型的机器人快速鲁棒三维重建方法，其特征在于，所述机器人为拍照式三维扫描仪与机器人组成的机器人测量系统，其中拍照式三维扫描仪通过连接件固定在机器人末端法兰上。

Images