(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210659338.2 (22)申请日 2022.06.13 (71)申请人 胜利油田胜 机石油装备有限公司 地址 257000 山东省东营市东营区西四路 906号 (72)发明人 王贺　张端光　沈君芳　汪秋陵　 于吉迪　 (74)专利代理 机构 东营双桥专利代理有限责任 公司 37107 专利代理师 方圆 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种修井装备机 械臂路径规划方法 (57)摘要 本发明属于修井 设备自动控制技术领域， 尤 其涉及一种修井装备机械臂路径 规划方法。 一种 修井装备机械臂路径规划方法， 以路径能耗为目 标函数， 以机械臂末端位置坐标为决策变量， 构 建二层台整机排管系统机械臂路径规划数学模 型， 并采用粒子群算法进行求解， 实现地面、 钻台 面及二层台整机系统机械臂关节空间的最低能 耗路径规划。 该修井装备机械臂路径 规划方法包 括有建立整机排管系统的三维空间模 型； 建立二 层台整机排管系统机械臂运动学模 型； 构建二层 台整机排管系统机械臂路径规划数学模型； 用径 向基函数神经网络进行系统辨识， 建立以机械臂 关节角位置、 角速度为输入， 机械臂路径跟踪能 耗为输出的数 学模型等 步骤。 权利要求书2页 说明书6页 CN 114986508 A 2022.09.02 CN 114986508 A 1.一种修井装备机 械臂路径规划方法， 其特 征在于， 包括有如下步骤： 步骤1： 基于地面、 钻台面及二层台整机排管系统所处空间， 建立整机排管系统的三维 空间模型； 步骤2： 建立 二层台整机排管系统机 械臂运动学模型； 步骤3： 以路径能耗为目标函数， 以机械臂末端位置坐标为决策变量， 以步骤2所述二层 台整机排管系统机械臂路径规划数学模型为约束 条件， 构建二层台整机排管系统机械臂路 径规划数 学模型； 步骤4： 用径向基函数神经网络进行系统辨识， 建立以机械臂关节角位置、 角速度为输 入， 机械臂路径 跟踪能耗 为输出的数 学模型， 并将其作为能耗优化的目标函数。 2.根据权利要求1所述的一种修井装备机 械臂路径规划方法， 其特 征在于， 还 包括有： 步骤5： 对步骤4所使用的神经网络中的控制器输入与输出进行模糊化处 理。 3.根据权利要求2所述的一种修井装备机械臂路径规划方法， 其特征在于， 步骤5可具 体描述为： 依据模糊决策表， 进行控制策略制定； 运用加权平均法对 模糊控制器 变量进行去模糊化， 获得控制器输出变量。 4.根据权利要求1所述的一种修井装备机械臂路径规划方法， 其特征在于， 步骤1可具 体描述为： 运用栅格法， 将地面、 钻台面及二层台整机排管系统所 处空间用大小相等的方块 单元分割表示， 建立整机排管系统的三维空间模型。 5.根据权利要求1所述的一种修井装备机械臂路径规划方法， 其特征在于， 步骤2可具 体描述为： 运用D ‑H法， 建立 二层台整机排管系统机 械臂运动学模型。 6.根据权利要求1所述的一种修井装备机械臂路径规划方法， 其特征在于， 步骤3中构 建形成的二层台整机排管系统机 械臂路径规划数 学模型可具体描述 为： 式 （1） ； 其式（1） 中， 为机械臂路径规划长度， 为机械臂末端坐标， 为环境模型中机械臂末端坐标最小值， 为环境模型中机 械臂末端坐标最大值。 7.根据权利要求1所述的一种修井装备机械臂路径规划方法， 其特征在于， 步骤4中构 建形成的能耗优化目标函数 可具体描述 为： 式 （3） ； 其式 （3） 中， 为机械臂路径跟踪能耗， 为矩阵， 表示机械臂关节俯仰角、权　利　要　求　书 1/2 页 2 CN 114986508 A 2横滚角、 偏航角度值； 为矩阵， 表示机械臂关节俯仰角速度、 横滚角速度、 偏航角速度值；   为机械臂关节能达 到的最小值；   为机械臂关节能达 到的最大值。 8.根据权利要求1所述的一种修井装备机械臂路径规划方法， 其特征在于， 采用粒子群 算法， 分别对步骤3构建形成二层台整机排管系统机械臂路径规划数学模型、 步骤4构建形 成的能耗优化目标函数进行求 解； 其中， 粒子群算法对位置参数进行的更新满足：  式 （2） ； 其式 （2） 中，   ，  ；  为惯性因子；   为加速度常数；   为[0,1]内随机数； 为速度权 重系数；  为粒子i的飞翔速度； 为粒子i的位置 。权　利　要　求　书 2/2 页 3 CN 114986508 A 3