(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211149409.0 (22)申请日 2022.09.21 (71)申请人 北京理工大 学 地址 100081 北京市海淀区中关村南大街5 号 (72)发明人 方丽萍　胡权　李金健　曹凌云　 贺子轩　 (74)专利代理 机构 北京正阳理工知识产权代理 事务所(普通 合伙) 11639 专利代理师 邬晓楠 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于动态系统和高斯聚类椭球的机械 臂避障规划方法 (57)摘要 本发明公开的一种基于动态系统和高斯聚 类椭球的机械臂避障规划方法， 属于机械臂避障 技术领域。 本发明实现方法为： 根据深度图像与 彩色图像生成空间环境三维点云模 型， 通过聚类 分析得到用于判别是否为空间障碍的高斯概率 密度分布 函数； 将待判定的路径空间三维点坐标 输入点云高斯概率密度分布函数， 对比判定所述 路径空间是否为障碍； 使用高斯聚类椭球模型对 属于空间障碍的点云进行包络， 得到高斯聚类椭 球外包络表 面方程； 根据所述包络表 面方程解析 表达式得到障碍物外包络的扩展切平 面法向量， 并构建用于调整机械臂靠近障碍物运动方向的 动态调制矩阵， 使机械臂末端沿着障碍物表面运 动而不穿透障碍物， 在保证 路径规划精度基础上 提高路径规划效率。 权利要求书5页 说明书9页 附图6页 CN 115533897 A 2022.12.30 CN 115533897 A 1.一种基于动态系统和高斯聚类椭球的机械臂避障规划方法， 其特征在于： 包括如下 步骤， 步骤一： 根据相机测量得到的空间环境的深度图像与彩色图像， 利用所述深度图像与 彩色图像生成空间环境的三维点云模型， 并进行噪声去除和数量精简； 使用高斯混合模型 对空间环境的点云三 维坐标进 行聚类分析， 构建用于判别是否为空间障碍的空间环境点云 高斯概率密度分布函数； 根据所述空间环境点云高斯概率密度分布函数使用迭代方法选取 设定空间障碍判别阈值； 将机械臂 运动过程中待判定的路径空间三 维点坐标输入点云高斯 概率密度分布函数中， 得到对应的高斯概率密度值， 根据待判定的路径空间三维点对应的 高斯概率密度值和空间障碍判别阈值对比判定所述路径空间是否为障碍； 步骤二： 对步骤一判定为障碍的点云三维坐标， 进一步使用高斯聚类椭球模型对属于 空间障碍区域的点云进行包络， 得到相应的用于避障路径规划的高斯聚类椭球外包络表面 方程， 能够根据相 机视觉测 量结果得到用于描述空间障碍的解析表达式； 根据所述空间障 碍高斯聚类椭球外包络表面方程 解析表达式， 得到障碍物外包络的扩展切平面的法向量； 步骤三： 通过步骤二得到的扩展障碍物的切平面法 向量构造用于指导机械臂避碰的扩 展切平面正交坐标系， 通过所述扩展切平面正交坐标系构建用于调整机械臂靠近障碍物运 动方向的动态调制矩阵， 通过所述动态调制矩阵对机械臂末端速度进行修正， 使机械臂末 端沿着障碍物 表面运动而不 穿透障碍物； 步骤四： 根据步骤三得到的修正后的机械臂末端运动速度， 更新机械臂 的末端三维位 置坐标， 用于该时刻机 械臂的运动控制； 步骤五： 重复执行步骤二至步骤四， 对各时刻下的机械臂进行避障规划与控制， 即通过 步骤二构建的空间障碍高斯聚类椭球外包络表面方程解析表达式和 步骤三构建的动态调 制矩阵使机械臂末端沿着障碍物表面运动而不穿透障碍物， 在保证路径规划精度基础上提 高避障路径规划效率， 实现机 械臂实时避障路径的规划， 直到 机械臂的末端到 达目标位置 。 2.如权利要求1所述的一种基于动态系统和高斯聚类椭球的机械臂避障规划方法， 其 特征在于： 步骤一实现方法为， 步骤1.1： 根据相机测量得到的空间环境的深度图像与彩色图像， 利用所述深度图像与 彩色图像生成空间环境的三维点云模型； 作为优选， 采用统计滤波对构建的空间环境三维 点云模型进行噪声去除， 并采用体素栅格滤波对去 噪后的点云模型进行数量精简， 以便提 高后续构建点云高斯 概率密度分布函数的效率； 步骤1.2： 使用高斯混合模型对步骤1.1去噪精简后的空间环境的点云三维坐标进行聚 类分析， 构建用于判别是否为空间障碍的空间环境 点云高斯 概率密度分布函数； 高斯混合模型表示 为， 式中， r为去噪精简后的空间环境的点云三维坐标， K为高斯混合模型中高斯分布的个 数， πk为第k个高斯分布所占的权 重， 满足如下约束 N(r； μk,Σk)是第k个高斯 分布概率密度函数， μk和Σk分别代表均值向量和协方差矩阵；权　利　要　求　书 1/5 页 2 CN 115533897 A 2πk, μk和Σk即为高斯混合模型中的待求 参数； 采用K‑means算法对待求参数πk, μk和Σk进行初始化， 初始化后采用期望最大算法 (Expectation ‑Maximization,EM)进行迭代求解， 分为期望步(E ‑setp)和最大化步(M ‑ step)两步， 迭代过程如下： E‑step： 利用当前的高斯混合模型参数πk, μk和Σk求解对数似然期望值； 引入隐变量zik， 其取值情况为 将点云三维坐标 数据展开成完全数据 (ri,zi1,zi2,...,ziK),i＝1,2,...,N       (4) 其中N为点云三维坐标 数据的数量， 完全数据的对数似然函数为 记E(zik)＝γik， 表示在当前高斯混合模型参数{π, μ， Σ}下第i个点云三维坐标ri来自 第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)≤ ε， 则待判定的路径空间三 维点不属于空间障权　利　要　求　书 2/5 页 3 CN 115533897 A 3