(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210585094.8 (22)申请日 2022.05.27 (71)申请人 太原理工大 学 地址 030024 山西省太原市万柏林区迎泽 西大街79号 (72)发明人 宋建成　李力恒　 (74)专利代理 机构 北京睿智保诚专利代理事务 所(普通合伙) 11732 专利代理师 杜娟 (51)Int.Cl. G06F 30/17(2020.01) G06F 30/25(2020.01) G06N 3/00(2006.01) G06N 7/08(2006.01) E21B 7/02(2006.01)E21B 15/00(2006.01) E21B 44/00(2006.01) G06F 111/08(2020.01) (54)发明名称 基于改进粒子群算法的锚杆钻车钻臂定位 控制方法及系统 (57)摘要 本发明公开了基于改进粒子群算法的锚杆 钻车钻臂定位控制方法及系统， 涉及工业机械臂 控制技术领域， 包括以下步骤： 关节角关系建立 步骤、 适应度值计算步骤、 粒子个体处理步骤、 粒 子更新步骤、 判断步骤、 输 出结果步骤、 定位控制 步骤。 本发明使得钻臂末端快速逼近目标孔位， 有效提高定位效率和定位精度。 权利要求书5页 说明书11页 附图2页 CN 114896736 A 2022.08.12 CN 114896736 A 1.基于混沌交叉精英变异反向粒子群算法的锚杆钻车钻臂定位控制方法， 其特征在 于， 包括以下步骤： S101.关节角关系建立步骤： 基于八自由度钻臂模型， 利用DH法建模建立钻臂末端位置 和钻臂各关节角之间的关系表达式； S201.适应度值计算步骤： 复合混沌初始化种群， 使用目标函数计算每个粒子个体的适 应度函数值得到全局最优粒子个 体， 定义为精英粒子； S301.粒子个体处理步骤： 根据反向学习规则， 对精英粒子进行反向求解以及柯西变 异； 将粒子个 体和个体最优粒子进行交叉操作以及高斯变异； S401.粒子更新步骤： 引入极值扰动， 根据粒子群算法的更新规则更新粒子个体的速度 和位置； S501.判断步骤： 判断当前更新的粒子个体的速度和位置是否使位置误差和姿态误差 小于预设值， 若为是， 则进入S6 01， 反之则返回S201； S601.输出结果步骤： 停止迭代， 当前更新的粒子个体的速度和位置则为全局最优解， 并输出该全局最优解； S701.定位控制步骤： 基于得到的全局最优解， 通过正向运动学将钻臂送达目标孔位， 从而实现钻臂的定位控制。 2.根据权利要求1所述的基于混沌交叉精英变异反 向粒子群算法的锚杆钻车钻臂定位 控制方法， 其特 征在于， S101的具体内容 为： 八自由度钻臂模型为具有六个旋转关节和两个移动关节的钻臂， 钻臂末端位置和钻臂 各关节角之间的关系表达式为： 其中， (Px,Py,Pz)为钻臂末端目标位置坐标， ( θ1, θ2, θ4, θ5, θ6)为钻臂五个关节角， a1、 a4、 a6、 d3、 d6、 d7、 d8为钻臂DH模型参数。 3.根据权利要求1所述的基于混沌交叉精英变异反 向粒子群算法的锚杆钻车钻臂定位 控制方法， 其特 征在于， S201中利用混沌映射序列初始化种群， 采用Logistic和Sinusoidal的复合混沌模型使 粒子在设定空间里 呈现混沌状态： 设任意粒子为Xi＝(x1,x2,…,xJ)， 复合混沌模型为： 粒子经过混沌映射变换后的初始位置为 粒子关权　利　要　求　书 1/5 页 2 CN 114896736 A 2系为： 式中， ai和bi分别为搜索空间中粒子所在位置的最小值和最大值。 4.根据权利要求3所述的基于混沌交叉精英变异反 向粒子群算法的锚杆钻车钻臂定位 控制方法， 其特 征在于， S201中的目标函数 F(Xi)： F(Xi)＝||P(Xi)‑Po||          (4) 式中， F(Xi)为粒子个体在Xi时的适应度； P(Xi)为粒子在Xi时的钻臂末端位姿； Po为机械 臂末端目标位姿。 5.根据权利要求4所述的基于混沌交叉精英变异反 向粒子群算法的锚杆钻车钻臂定位 控制方法， 其特 征在于， 目标函数 F(Xi)具体包括 位置误差函数Ep和姿态误差函数Eo， 表达式如下： 其中， Ep为位置误差函数， Eo为姿态误差函数， l和m分别为位置误差函数和姿态误差函 数的权重系数， 满足l+m＝1， (p ′x,p′y,p′z)为钻臂实际运动过程中末端位置坐标， (px,py, pz)为钻臂末端目标位置坐标， ( α ′, β′,γ′)为钻臂实际运动过程中末端位置坐标系中三坐 标轴与基准 坐标系中三 坐标轴各自对应的夹角。 6.根据权利要求1所述的基于混沌交叉精英变异反 向粒子群算法的锚杆钻车钻臂定位 控制方法， 其特 征在于， S301中根据反向学习规则， 对精英粒子进行反向求 解以及柯西变异， 其具体内容 为： 在J维搜索区域中， 设Xi＝(X1,X2,…,XJ)为任意普通粒子， 当其自身极值为精英粒子 则精英反向解 为 定义如下： 其中， k为一动态系数， ai和bi分别为搜索空间中粒子所在位置的最小值和最大值； 当精英反向解超出自己搜索边界成为一个不可行解， 则采用基于随机生成法重置反 解， 计算公式如下： 进行柯西变异操作为： 其中， xmax和xmin分别为搜索空间的最大值和最小值， 为最优粒子变异值， cauch y(0, s)是柯西分布， s是随迭代次数线性 递减的柯西分布比例参数， 公式如下： s(k+1)＝s(k) ‑sin(1/kmax)             (9) 式中， kmax为最大迭代次数；权　利　要　求　书 2/5 页 3 CN 114896736 A 3