(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210986644.7 (22)申请日 2022.08.17 (71)申请人 沈阳工业大 学 地址 110870 辽宁省沈阳市铁西区经济技 术开发区沈辽西路1 11号 (72)发明人 徐靖　魏喆　王雷　赵铁军　张明　 (74)专利代理 机构 沈阳智龙专利事务所(普通 合伙) 21115 专利代理师 王聪耀　宋铁军 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于高斯过程的串联机械臂路径规划 方法 (57)摘要 本发明涉及一种基于高斯过程的串联机械 臂路径规划方法， 该方法包括以下步骤： 在构型 空间中分别采样机械臂的初始位姿节点和目标 位姿节点； 基于高斯过程模型在初始位姿节点和 目标位姿节点间采样np条平滑随机路径； 计算np 条平滑随机路径的代价值； 以代 价值从小到大的 顺序排序， 得到排序后的随机采样路径集合； 采 用惰性碰撞检测方法， 从随机采样路径集合中筛 选出代价值最低的随机 路径为无碰撞路径， 若无 法筛选出无碰撞路径， 则重复上述步骤， 直到筛 选出机械臂的高质量运动路径 为止。 本发明具有 机械臂路径规划 效率高、 规划质量高、 稳定性强 等特点。 权利要求书2页 说明书6页 附图3页 CN 115229797 A 2022.10.25 CN 115229797 A 1.一种基于高斯过程的串联机 械臂路径规划方法， 其特 征在于： 包括以下步骤： 步骤1： 在构型空间中分别采样机 械臂的初始位姿节点xinit和目标位姿节点xgoal； 步骤2： 基于高斯过程模型在初始位姿节点xinit和目标位姿节点xgoal间采样np条平滑随 机路径； 步骤3： 计算 步骤2中np条平滑随机路径的代价 值； 步骤4： 以代价值从小到大的顺序， 将步骤2 中np条平滑随机路径进行排序， 得到排序后 的随机采样路径集 合 步骤5： 采用惰性碰撞检测方法， 从步骤4随机采样路径集合 中筛选出代价值最低 的随机路径为无碰撞路径， 所述无碰撞路径则为机 械臂的高质量 运动路径； 步骤6： 若步骤5无法筛选出无碰撞路径， 则重复步骤2至步骤5， 直到筛选出机械臂的高 质量运动路径为止 。 2.根据权利要求1所述的一种基于 高斯过程的串联机械臂路径规划方法， 其特征在于： 步骤2中采样np条平滑随机路径的方法步骤如下： 步骤2.1： 确定高斯过程模型的数学特征， 高斯过程模型的数学特征包括均 值函数 μ(s) 和核函数k(s,s ′)； 步骤2.2： 根据步骤2.1中高斯过程模型的数学特征得到机械臂随机路径高斯过程模 型， 将机械臂构型空间中某一维度的随机采样路径δ＝{x(s),s∈S}视为高斯过程， x(s)表 示机械臂系统在s索引下的位姿， S为系统索引集； 步骤2.3： 基于步骤2.2的机械臂随机路径高斯过程模型可推导出随机采样路径的概率 后验分布； 步骤2.4： 对概率后验分布中的协方差矩阵 进行矩阵奇异值分解， 基于矩阵奇 异值分解得到高斯随机路径采样模型， 并在机械臂构型空间的某一 维度内进 行高斯随机路 径采样； 步骤2.5： 重 复步骤2.2至步骤2.4， 在机械臂构型空间 的每一个维度内都进行一次高斯 随机路径采样， 并将所有采样的随机路径合并为 一条完整的机 械臂随机采样路径； 步骤2.6： 重复步骤2.5， 在机械臂构型空间内随机采样np条完整的机械臂随机采样路 径。 3.根据权利要求1所述的一种基于 高斯过程的串联机械臂路径规划方法， 其特征在于： 步骤2.1中均值 函数 μ(s)设为0； 高斯过程模型中的核函数k(s,s ′)为： 式中， s和s ′为系统状态索引， 参数σf反映了s＝s ′时路径状态点的概率分布特性， 参数 σl反映不同路径点间的相关性， 参数σf和σl共同决定基于高斯过程的连续时间轨 迹特性。 4.根据权利要求1所述的一种基于 高斯过程的串联机械臂路径规划方法， 其特征在于： 步骤2.2中机 械臂随机路径高斯过程模型为： 式中， δ为随机采样 路径集合， 为高斯过程模型表示符号， μ(s)为均 值函数， k(s,s ′) 为二次指数核函数， s和s ’均为系统状态索引。权　利　要　求　书 1/2 页 2 CN 115229797 A 25.根据权利要求1所述的一种基于 高斯过程的串联机械臂路径规划方法， 其特征在于： 步骤2.3中随机采样路径的概 率后验分布为： 式中， μp和Σp分别是均值向量和协方差矩阵； δt＝{xinit,xgoal}和St＝{s1,s2}分别是机 械臂初始状态、 目标状态集合和对应的状态索引集合； δp＝{xi|i＝1,2, …,N}和Sp＝{si|i ＝1,2,…,N}分别是随机路径状态点集合和对 应的状态索引集合； 噪声方差由 表示， 采样 中设为0； 是关于Sp和St的核矩阵； 是关于Sp 自身的核矩阵； 是关于St自身的核矩阵； I为单位矩阵； xinit和xgoal为 机械臂初始状态和目标状态， s1,s2为对应的状态索引； δp＝{xi|i＝1,2, …,N}中1,2, …,N 为随机路径状态点序号， xi为随机路径中的状态点。 6.根据权利要求1所述的一种基于 高斯过程的串联机械臂路径规划方法， 其特征在于： 步骤2.4中协方差矩阵 进行矩阵奇异值分解： 其中， U和V为酉矩阵， ∑为对角阵。 7.根据权利要求1所述的一种基于 高斯过程的串联机械臂路径规划方法， 其特征在于： 步骤2.4中， 高斯随机路径采样模型为： 其中， 为高斯随机分量。 8.根据权利要求1所述的一种基于 高斯过程的串联机械臂路径规划方法， 其特征在于： 步骤5中惰性碰撞检测方法为： 步骤5.1： 选随机采样路径集 合 中第一个代价 值最小的随机路径， 进行稀疏路径点 的碰撞检测 和边界有效性检测； 步骤5.2： 若出现路径点与周围环境出现碰撞或超出构型空间边界， 则在随机采样路径 集合 中删除该随机路径， 并重复步骤5.1； 步骤5.3： 若路径点碰撞检测和边界有效性检测通过， 则在稀疏路径点间进行高斯密集 插值， 并对密集插值 点进行碰撞检测； 步骤5.4： 若密集插值点出现碰撞， 则在随机采样路径集合 中删除该 随机路径， 并 重复步骤5.1至步骤5.3； 步骤5.5： 若密集插值 点的碰撞检测通过， 则该 条路径为随机采样路径集 合 中代价 值最小的无碰撞路径， 该最小的无碰撞路径为机 械臂的高质量 运动路径。权　利　要　求　书 2/2 页 3 CN 115229797 A 3