[发明专利]一种基于动态系统和高斯聚类椭球的机械臂避障规划方法

权利要求书

1.一种基于动态系统和高斯聚类椭球的机械臂避障规划方法，其特征在于：包括如下步骤，

步骤一：根据相机测量得到的空间环境的深度图像与彩色图像，利用所述深度图像与彩色图像生成空间环境的三维点云模型，并进行噪声去除和数量精简；使用高斯混合模型对空间环境的点云三维坐标进行聚类分析，构建用于判别是否为空间障碍的空间环境点云高斯概率密度分布函数；根据所述空间环境点云高斯概率密度分布函数使用迭代方法选取设定空间障碍判别阈值；将机械臂运动过程中待判定的路径空间三维点坐标输入点云高斯概率密度分布函数中，得到对应的高斯概率密度值，根据待判定的路径空间三维点对应的高斯概率密度值和空间障碍判别阈值对比判定所述路径空间是否为障碍；

步骤二：对步骤一判定为障碍的点云三维坐标，进一步使用高斯聚类椭球模型对属于空间障碍区域的点云进行包络，得到相应的用于避障路径规划的高斯聚类椭球外包络表面方程，能够根据相机视觉测量结果得到用于描述空间障碍的解析表达式；根据所述空间障碍高斯聚类椭球外包络表面方程解析表达式，得到障碍物外包络的扩展切平面的法向量；

步骤三：通过步骤二得到的扩展障碍物的切平面法向量构造用于指导机械臂避碰的扩展切平面正交坐标系，通过所述扩展切平面正交坐标系构建用于调整机械臂靠近障碍物运动方向的动态调制矩阵，通过所述动态调制矩阵对机械臂末端速度进行修正，使机械臂末端沿着障碍物表面运动而不穿透障碍物；

步骤四：根据步骤三得到的修正后的机械臂末端运动速度，更新机械臂的末端三维位置坐标，用于该时刻机械臂的运动控制；

步骤五：重复执行步骤二至步骤四，对各时刻下的机械臂进行避障规划与控制，即通过步骤二构建的空间障碍高斯聚类椭球外包络表面方程解析表达式和步骤三构建的动态调制矩阵使机械臂末端沿着障碍物表面运动而不穿透障碍物，在保证路径规划精度基础上提高避障路径规划效率，实现机械臂实时避障路径的规划，直到机械臂的末端到达目标位置。

2.如权利要求1所述的一种基于动态系统和高斯聚类椭球的机械臂避障规划方法，其特征在于：步骤一实现方法为，

步骤1.1：根据相机测量得到的空间环境的深度图像与彩色图像，利用所述深度图像与彩色图像生成空间环境的三维点云模型；作为优选，采用统计滤波对构建的空间环境三维点云模型进行噪声去除，并采用体素栅格滤波对去噪后的点云模型进行数量精简，以便提高后续构建点云高斯概率密度分布函数的效率；

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

高斯混合模型表示为，

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

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

采用K-means算法对待求参数π_k,μ_k和Σ_k进行初始化，初始化后采用期望最大算法(Expectation-Maximization,EM)进行迭代求解，分为期望步(E-setp)和最大化步(M-step)两步，迭代过程如下：

E-step：利用当前的高斯混合模型参数π_k,μ_k和Σ_k求解对数似然期望值；

引入隐变量z_ik，其取值情况为

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

(r_i,z_i1,z_i2,...,z_iK),i＝1,2,...,N (4)

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

记E(z_ik)＝γ_ik，表示在当前高斯混合模型参数{π,μ，Σ}下第i个点云三维坐标r_i来自第k个高斯分布的概率，计算过程为

M-step：通过最大化E-step中的对数似然函数期望值对高斯模型参数进行优化；考虑到π_k需满足约束条件式(2)，设定带拉格朗日算子λ的优化目标函数J为

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

重复E-step和M-Step，直至前后两次迭代得到高斯混合模型参数的估计值几乎不发生变化，即终止迭代，输出模型参数π_k,μ_k和Σ_k；

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

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

步骤1.4：将机械臂运动过程中待判定的路径空间三维点坐标x输入步骤1.2得到的点云高斯概率密度分布函数中，得到对应的高斯概率密度值p(x)，根据待判定的路径空间三维点对应的高斯概率密度值p(x)和步骤1.3得到的空间障碍判别阈值ε，若p(x)＞ε，则待判定的路径空间三维点属于空间障碍，若p(x)≤ε，则待判定的路径空间三维点不属于空间障碍。