基于高斯混合模型和深度-彩色图像的空间障碍判别方法

本发明涉及一种基于高斯混合模型和深度-彩色图像的空间障碍判别方法，属于航空航天。

背景技术：

1、随着航天技术的快速发展，机械臂在空间碎片清除、航天器在轨维护及在轨组装等复杂空间任务中将发挥重要的作用。它能够协助或代替宇航员完成耗时长、危险性高的太空作业，极大地降低在轨操作的成本和风险，提高空间探索开发的效率。

2、目前，多元化和复杂化的在轨服务任务对机械臂的操作能力提出了更高的技术要求。机械臂在完成如维修、装配等高精度在轨操作时，涉及到对工具的定位抓取与归置收纳，需要获取空间环境中的障碍物分布信息，避免与障碍物发生碰撞，并寻找合适的机械臂操作区域。目前，在轨服务机械臂大都使用基于帧的传统图像相机进行测量，过程中需要处理的数据量很大，不利于对空间障碍物进行高效及时的识别。因此，有必要研究一种快速准确的空间障碍判别方法来解决上述难题。

技术实现思路

1、本发明公开的基于高斯混合模型和深度-彩色图像的空间障碍判别方法要解决的技术问题是：根据相机测量得到的空间环境的深度图像与彩色图像，利用所述深度图像与彩色图像生成空间环境的三维点云模型，采用统计滤波对点云模型进行噪声去除，并采用体素栅格滤波对去噪后的点云模型进行数量精简，以便提高后续构建点云高斯概率密度分布函数的效率；使用高斯混合模型对空间环境的点云三维坐标进行聚类分析，得到用于判别是否为空间障碍的空间环境点云高斯概率密度分布函数；根据所述空间环境点云高斯概率密度分布函数使用迭代方法选取设定空间障碍判别阈值；将机械臂运动过程中待判定的路径空间三维点坐标输入点云高斯概率密度分布函数中，得到对应的高斯概率密度值，根据待判定的路径空间三维点对应的高斯概率密度值和空间障碍判别阈值对比，对待判定的路径空间点判别后实现空间障碍区域与机械臂可操作区域的估计与划分。

2、本发明的目的是通过下述技术方案实现的。

3、本发明公开的基于高斯混合模型和深度-彩色图像的空间障碍判别方法，包括如下步骤：

4、步骤一：根据相机测量得到的空间环境的深度图像与彩色图像，利用所述深度图像与彩色图像生成空间环境的三维点云模型。

5、步骤二：采用统计滤波对步骤一构建的空间环境三维点云模型进行噪声去除，并采用体素栅格滤波对噪声去除后的点云模型进行数量精简，以便提高后续构建点云高斯概率密度分布函数的效率。

6、统计滤波：记步骤一构建的空间环境三维点云模型中的第n个数据点为qn，n＝1,2,3,...,s，记qn到任意点的距离为di，定义qn到它所有k个邻近点的平均距离davr-n服从均值为μ和标准差为σ的高斯分布，

7、

8、设定标准范围spa-n，用于判断模型中的数据点是否为噪声点，

9、spa-n＝μ±g·σ(g＝0,1,2,...)       (2)

10、根据平均距离davr-n和标准范围spa-n选择相应的点。即，当点qn对应的平均距离davr-n大于标准范围spa-n时，删除该点；当点qn对应的平均距离davr-n小于等于标准范围spa-n时，则保留该点。

11、体素栅格滤波：获取去噪后的三维点云模型在x轴、y轴和z轴3个方向上的最小和最大坐标值，记作xmin、xmax、ymin、ymax、zmin、zmax。计算去噪后的三维点云模型的长方体包围盒边长分别为，

12、

13、其中，lx、ly、lz为去噪后的三维点云模型的长方体包围盒在x轴、y轴和z轴3个方向上的边长。

14、将去噪后的三维点云模型的长方体包围盒划分为若干个边长为l的体素栅格，作为优选，

15、l＝(2％～10％)lmin       (4)

16、其中，lmin为lx、ly、lz中的最小值。遍历去噪后的三维点云模型，记每个体素栅格中的点云数据个数为m，计算每个体素栅格的重心，

17、

18、以体素栅格的重心(xc,yc,zc)代表所述体素栅格内的所有数据点，实现点云数量精简。

19、步骤三：使用高斯混合模型对步骤二去噪精简后的空间环境的点云三维坐标进行聚类分析，得到用于判别是否为空间障碍的空间环境点云高斯概率密度分布函数。

20、使用高斯混合模型对去噪精简后的空间环境的点云三维坐标进行聚类分析。高斯混合模型表示为，

21、

22、式中，r为去噪精简后的空间环境的点云三维坐标，k为高斯混合模型中高斯分布的个数，πk为第k个高斯分布所占的权重，满足如下约束

23、

24、n(r；μk,σk)是第k个高斯分布概率密度函数，μk和σk分别代表均值向量和协方差矩阵。πk,μk和σk即为高斯混合模型中的待求参数。

25、作为优选，首先采用k-means算法对待求参数πk,μk和σk进行初始化，初始化后采用期望最大算法(expectation-maximization,em)进行迭代求解，分为期望步(e-setp)和最大化步(m-step)两步，迭代过程如下：

26、e-step：利用当前的高斯混合模型参数πk,μk和σk求解对数似然期望值。

27、引入隐变量zik，其取值情况为

28、

29、将点云三维坐标数据展开成完全数据

30、(ri,zi1,zi2,...,zik),i＝1,2,...,n       (9)

31、其中n为点云三维坐标数据的数量，完全数据的对数似然函数为

32、

33、计算对数似然函数的期望

34、

35、记e(zik)＝γik，表示在当前高斯混合模型参数{π,μ，σ}下第i个点云三维坐标ri来自第k个高斯分布的概率，计算过程为

36、

37、m-step：通过最大化e-step中的对数似然函数期望值对高斯混合模型参数进行优化。考虑到πk需满足约束条件式(7)，设定带拉格朗日算子λ的优化目标函数j为

38、

39、分别求出目标函数j对πk,μk,σk的偏导数并令其为0，即可得到高斯混合模型参数新的估计值：

40、

41、重复e-step和m-step，直至前后两次迭代得到高斯混合模型参数的估计值几乎不发生变化，即可终止迭代，输出模型参数πk,μk和σk。

42、基于求解得到的参数已知的高斯混合模型，得到如式(6)所示的用于判别是否为空间障碍的空间环境点云高斯概率密度分布函数。

43、步骤四：根据步骤三得到的空间环境点云高斯概率密度分布函数使用迭代方法选取设定空间障碍判别阈值。

44、迭代确定空间障碍判别阈值ε的过程如下：

45、步骤4.1：选择空间环境点云高斯概率密度的中值作为初始空间障碍判别阈值ε0，

46、步骤4.2：根据阈值ε0把空间环境点云划分为两个区域，若空间环境点云对应的高斯概率密度小于等于判别阈值ε0，将空间环境点云划分到非障碍区域r1中，反之则划分到障碍区域r2中，分别计算非障碍区域r1和障碍区域r2的空间环境点云高斯概率密度的中值m1和m2，

47、步骤4.3：更新空间障碍判别阈值εi+1，

48、

49、步骤4.4：重复步骤4.2和步骤4.3，直到空间障碍判别阈值εi+1和εi的差小于预设值，则停止迭代过程，得到最终的空间障碍判别阈值ε。

50、步骤五：将机械臂运动过程中待判定的路径空间三维点坐标输入步骤三得到的点云高斯概率密度分布函数中，得到对应的高斯概率密度值，根据待判定的路径空间三维点对应的高斯概率密度值和步骤四得到的空间障碍判别阈值对比，对待判定的路径空间点判别后实现空间障碍区域与机械臂可操作区域的估计与划分。

51、将机械臂运动过程中待判定的路径空间三维点坐标x输入步骤三得到的点云高斯概率密度分布函数中，得到对应的高斯概率密度值

52、

53、对比待判定的路径空间三维点对应的高斯概率密度值p(x)和步骤四得到的空间障碍判别阈值ε，若p(x)＞ε，则待判定的路径空间三维点属于空间障碍，若p(x)≤ε，则待判定的路径空间三维点不属于空间障碍；完成对待判定的路径空间点是否为空间障碍的判别后，实现空间障碍区域与机械臂可操作区域的估计与划分。

54、步骤六：根据步骤五判别得到的属于空间障碍区域的点云三维坐标，进一步使用高斯聚类椭球模型对属于空间障碍区域的点云进行包络，得到相应的用于避障路径规划的高斯聚类椭球外包络表面方程；根据所述空间障碍高斯聚类椭球外包络表面方程解析表达式，得到障碍物外包络的扩展切平面的法向量，根据所述法向量进行机械臂实时避障路径的规划，在保证路径规划精度的基础上提高避障路径规划效率。

55、作为优选，步骤六实现方法如下：

56、记步骤五判别得到的属于空间障碍区域的三维点云集合为b，建立点云集合b的高斯混合模型{πm,μm,σm}m＝1:m，其中m为高斯混合模型中高斯分布的个数，将服从第m个高斯分布的点云集合视作一个障碍物，用高斯聚类椭球模型对服从第m个高斯分布的点云集合进行包络，得到第m个障碍物的高斯聚类椭球外包络表面方程为

57、

58、式中ξ是空间环境三维点坐标，r是使服从第m个高斯分布的所有点云包含在同一个椭球内的最小半径；μm是第m个高斯聚类椭球的中心点；σm决定了椭球的大小与方向，是3×3维正定矩阵，其特征值矩阵λ和特征向量矩阵v如式(18)所示：

59、

60、式中r1,r2,r3代表椭球半轴长，α1,α2,α3为r1,r2,r3对应的椭球半轴朝向。

61、将γm(ξ；μm,σm)简写为γm(ξ)。针对位于第m个障碍物的高斯聚类椭球外包络上的点使用法向量n(ξb)定义在点ξb处的切平面：

62、

63、通过扩展障碍物，空间环境中的任何三维点ξ都可以位于扩展障碍物的表面上，扩展障碍物的切平面的法向量为

64、

65、根据如式(20)所示的障碍物外包络的扩展切平面的法向量，进行机械臂实时避障路径的规划，在保证路径规划精度的基础上提高避障路径规划效率。

66、有益效果：

67、1、本发明公开的基于高斯混合模型和深度-彩色图像的空间障碍判别方法，根据相机测量得到的空间环境的深度图像与彩色图像，利用所述深度图像与彩色图像生成空间环境的三维点云模型，使用高斯混合模型对空间环境的点云三维坐标进行聚类分析，得到用于判别是否为空间障碍的空间环境点云高斯概率密度分布函数；将机械臂运动过程中待判定的路径空间三维点坐标输入点云高斯概率密度分布函数中，得到对应的高斯概率密度值，根据待判定的路径空间三维点对应的高斯概率密度值和空间障碍判别阈值对比，对待判定的路径空间点判别后实现空间障碍区域与机械臂可操作区域的估计与划分，能够在保证空间障碍判别精度的基础上提高判别效率。

68、2、本发明公开的基于高斯混合模型和深度-彩色图像的空间障碍判别方法，根据所述空间环境点云高斯概率密度分布函数使用迭代法选取设定空间障碍判别阈值，进一步提高空间障碍判别精度和效率。

69、3、本发明公开的基于高斯混合模型和深度-彩色图像的空间障碍判别方法，采用统计滤波对点云模型进行噪声去除，并采用体素栅格滤波对去噪后的点云模型进行数量精简，以便提高构建点云高斯概率密度分布函数的效率，进而提高空间障碍判别效率。

70、4、本发明公开的基于高斯混合模型和深度-彩色图像的空间障碍判别方法，在实现上述有益效果1、2、3的基础上，根据本发明判别得到的属于空间障碍区域的点云三维坐标，进一步使用高斯聚类椭球模型对属于空间障碍区域的点云进行包络，得到相应的用于避障路径规划的高斯聚类椭球外包络表面方程；根据所述空间障碍高斯聚类椭球外包络表面方程解析表达式，得到障碍物外包络的扩展切平面的法向量，根据所述法向量进行机械臂实时避障路径的规划，在保证路径规划精度基础上提高避障路径规划效率。

71、5、本发明公开的基于高斯混合模型和深度-彩色图像的空间障碍判别方法，根据有益效果4的分析结果，能够解决航空航天领域相关技术问题，所述航空航天领域相关技术问题包括识别空间环境障碍和支撑机械臂在轨操作。