Research on trajectory planning of drill rig manipulator based on improved particle swarm optimization
-
摘要: 钻车机械手是防突防冲钻车的重要装置，关系着钻车是否可以正常钻进及真正实现无人化。为确保钻车机械手快速、准确、平稳运行，其轨迹规划优化尤为重要。现有钻车机械手轨迹规划存在阶次较高、优化算法易早熟等问题。针对上述问题，提出了一种基于改进粒子群（PSO）算法的钻车机械手时间最优轨迹规划方法。首先，利用标准Denavit−Hartenberg（D−H）构建钻车机械手三维模型，通过蒙特卡洛法得到钻车机械手的工作空间，从工作空间中选取4个途径点作为插值点。然后，为了使钻车机械手能够快速平稳地到达指定位置，在关节空间中采用3−5−3分段多项式插值构造其轨迹。最后，通过改进PSO算法对构造的轨迹进行时间最短优化，得到钻车机械手的时间最优轨迹规划。Matlab仿真结果表明：基于改进PSO算法的钻车机械手时间最优轨迹规划方法可以在保证钻车机械手各关节运行平稳的同时，使运行时间从3.1685 s减少到2.3854 s，整体运行时间较优化前减少约25%，提高了机械手的工作效率。Abstract: The manipulator is an important device of anti-outburst and anti-impact drill rig, which is related to whether the drill rig can drill normally and truly realize unmanned operation. In order to ensure the rapid, accurate and stable operation of the drill rig manipulator, the trajectory planning optimization is particularly important. There are some problems in the existing trajectory planning of drill rig manipulator, such as higher order, prematurity of optimization algorithm and so on. In order to solve the above problems, a time optimal trajectory planning method of drill rig manipulator based on improved particle swarm optimization ( PSO) algorithm is proposed. Firstly, the 3D model of the drill rig manipulator is constructed by using the standard Denavit-Hartenberg ( D-H), and the workspace of the manipulator is obtained by Monte Carlo method, and four path points are selected as interpolation points from the workspace. Secondly, in order to make the manipulator reach the specified position quickly and smoothly, the trajectory of the manipulator is constructed by using 3-5-3 piecewise polynomial interpolation in the joint space. Finally, by the improved PSO algorithm, the constructed trajectory is optimized in the shortest time, and the optimal trajectory planning of the drill rig manipulator is obtained. The Matlab simulation results show that the time optimal trajectory planning method of the drill rig manipulator based on improved PSO algorithm can not only ensure the smooth operation of each joint of the drill rig manipulator, but also reduce the running time from 3.168 5 s to 2.385 4 s, reduce the overall running time by about 25% compared with that before optimization, and improve the efficiency of the manipulator.
-
0. 引言
锚杆钻车是矿井巷道作业主力装备之一，在矿井巷道掘进和锚固支护方面发挥着重要作用。我国钻机整体技术水平比较落后，存在定位精度低、定位速度慢等问题，严重制约巷道施工效率。研究锚杆钻车钻臂的自动精准定位问题，对实现矿用生产装备智能化、提高巷道施工效率有重要意义。
锚杆钻车钻臂为冗余多自由度结构，可保障作业的灵活性，但自由度的增加导致逆运动学求解复杂，降低了求解效率和精度。目前常用代数法和几何法求解机械臂运动学问题。代数法根据逆矩阵变换构建钻臂各关节变量与目标位姿之间的函数，但效率低，且存在无解或多解情况。几何法效率高，但对于不同的对象需构造不同的求解模型，通用性差。针对上述问题，许多学者将智能算法用于机械臂定位控制研究，如：李国江等[1]使用多群协同进化方法补偿绳索牵引并联机器人末端定位偏差；吉阳珍等[2]将改进的鲸鱼优化算法用于机械臂逆运动学求解，提高了求解精度和稳定性。在各种智能算法中，粒子群优化（Particle Swarm Optimization，PSO）算法用于求解机械臂运动学问题时具有编程简单、易于计算机实现、搜索性能强、容错性优等优点，有利于钻臂定位控制的稳定性。但PSO算法易陷入局部最优解，求解性能欠佳。对此，学者对PSO算法进行了改进，如：史也等[3]提出了一种基于量子PSO（Quantum-behaved PSO，QPSO）算法的路径规划方法，通过规划机械臂关节角的运动，使基座姿态和机械臂末端姿态同时达到期望状态；刘洋[4]通过多目标PSO（Multiple Objective PSO，MOPSO）算法实现了机器人位姿精准控制。上述算法应用于机械臂运动学求解时未考虑粒子初始位置状态，导致粒子初始状态不佳，且粒子间信息交流不充分，个体和全局最优粒子易陷入局部最优而无法跳出，使得算法整体寻优效率较低，寻优时间过长。
本文在精英反向PSO（Elite Opposition-based PSO，EOPSO）算法基础上进行改进，提出混沌交叉精英变异反向PSO(Chaotic Crossover Elite Mutation Opposition-based PSO，CEMOPSO)算法，并将其用于锚杆钻车钻臂定位控制，提高了钻臂逆向运动学求解的速度和精度，实现了锚杆钻车钻臂精准定位。
1. 锚杆钻车钻臂运动模型
锚杆钻车钻臂为八自由度机构，含6个回转关节（大臂摇摆关节、大臂俯仰关节、推进梁俯仰关节、推进梁摆动关节、推进梁回转关节、锚杆关节）和2个移动关节（大臂伸缩关节、推进梁伸缩关节），如图1所示。
![]()
图 1 锚杆钻车钻臂结构1−大臂摇摆关节；2−大臂俯仰关节；3−大臂伸缩关节；4−推进梁俯仰关节；5−推进梁摆动关节；6−推进梁回转关节；7−锚杆关节；8−推进梁伸缩关节。Figure 1. Drilling arm structure of bolt drilling rig为简化钻臂坐标系的建立，分析钻臂基座到钻臂末端变换关系，利用D−H建立钻臂正向运动学模型[5-9]。钻臂坐标系如图2所示，o0x0y0z0为基坐标系，oixiyizi(i=1，2，…，8)分别为大臂摇摆关节坐标系、大臂俯仰关节坐标系、大臂伸缩关节坐标系、推进梁俯仰关节坐标系、推进梁摆动关节坐标系、推进梁回转关节坐标系、锚杆关节坐标系和推进梁伸缩关节坐标系。
根据D−H法，相邻2个关节坐标系之间的变换矩阵为
$$ \begin{split} {\boldsymbol{T}}_j^{j - 1} = \left[ {\begin{array}{*{20}{c}} {\cos \ {\theta _{ j}}}& { - \cos \ {\alpha _{ j}}\sin \ {\theta _{ j}}}& {\sin \ {\alpha _{ j}}\sin \ {\theta _{ j}}}& {{a_j}\cos \ {\theta _{ j}}} \\ {\sin \ {\theta _{ j}}}& {\cos \ {\theta _{ j}}\sin \ {\alpha _{ j}}}& { - \cos \ {\theta _{ j}}\sin \ {\alpha _{ j}}}& {{a_j}\sin \ {\theta _{ j}}} \\ 0& {\sin \ {\alpha _{ j}}}& {\cos \ {\alpha _{ j}}}& {{d_j}} \\ 0& 0& 0& 1 \end{array}} \right] \end{split} $$ (1) 式中：θj为关节j的关节角；αj为关节j所在杆件的扭转角；aj为关节j所在杆件长度；dj为关节j横距。
锚杆钻车钻臂的D−H参数见表1。
表 1 锚杆钻车钻臂D−H参数Table 1. D-H parameters of drilling arm of bolt drilling rig关节 $ {\theta _j}/(^\circ ) $ $ {\alpha _j}/(^\circ ) $ $ {a_j}/{\rm{m}} $ $ {d_j}/{\rm{m}} $ 1 [45，135] 90 0.30 0 2 [−150，−60] −90 0 0 3 180 −90 0 [0，1.8] 4 [−120，−30] −90 0.35 0 5 [−135，−45] 90 0 0 6 [−270，90] −90 0.60 0.4 7 [−90，0] 90 0 0.8 8 90 −90 0 [0，2.5] 将表1数据代入式（1），可得相邻2个关节坐标系之间的变换矩阵。由左乘法则联立各变换矩阵，得到钻臂末端（推进梁伸缩关节）坐标系相对基坐标系的位姿矩阵：
$$ {\boldsymbol{T}}_8^0 = {\boldsymbol{T}}_1^0{\boldsymbol{T}}_2^1{\boldsymbol{T}}_3^2{\boldsymbol{T}}_4^3{\boldsymbol{T}}_5^4{\boldsymbol{T}}_6^5{\boldsymbol{T}}_7^6{\boldsymbol{T}}_8^7 = \left[ {\begin{array}{*{20}{c}} {{N_x}}&{{O_x}}&{{A_x}}&{{L_x}} \\ {{N_y}}&{{O_y}}&{{A_y}}&{{L_y}} \\ {{N_{\textit{z}}}}&{{O_{\textit{z}}}}&{{A_{\textit{z}}}}&{{L_{\textit{z}}}} \\ 0&0&0&1 \end{array}} \right] $$ (2) 式中：
$ {[{N_x},{N_y},{N_{\textit{z}}}]^{\rm{T}}} $ ，$ {[{O_x},{O_y},{O_{\textit{z}}}]^{\rm{T}}} $ ，$ {[{A_x},{A_y},{A_{\textit{z}}}]^{\rm{T}}} $ 分别为钻臂末端的法向向量、滑动向量和接近向量；$ [{L_x}, {L_y},{L_{\textit{z}}}]^{\rm{T}} $ 为钻臂末端相对于基坐标系的位置向量。2. CEMOPSO算法
2.1 EOPSO算法改进
采用PSO算法对锚杆钻车钻臂进行逆运动求解时，适应度函数为钻臂末端位姿与目标位姿的误差分析函数，即
$$ h({{\boldsymbol{X}}_i}) = \left\| {{\boldsymbol{P}}({{\boldsymbol{X}}_{\text{i}}}) - {{\boldsymbol{P}}_{{\rm{obj}}}}} \right\| $$ (3) 式中：
$ h( \cdot ) $ 为适应度函数；Xi为粒子i位置；P(Xi)为粒子i在位置Xi时的钻臂末端位姿；Pobj为钻臂末端目标位姿。迭代时，设粒子i当前最优位置Xibest=（ei1，ei2，…，eiD），eiD为粒子i在D维空间的当前最优位置，全局最优位置Xgbest=（eg1，eg2，…，egD），egD为D维空间的全局最优位置。粒子i的速度和位置更新公式为
$$ \begin{split} {{\boldsymbol{V}}_i}(t + 1) = & \omega {{\boldsymbol{V}}_i}(t) + {c_1}{r_1}({{\boldsymbol{X}}_{i{\rm{best}}}} - {{\boldsymbol{X}}_i}(t)) + {c_2}{r_2}\left( { {{\boldsymbol{X}}{\boldsymbol{}}_{{\rm{gbest}}}} - {{\boldsymbol{X}}_i}(t) } \right) \end{split} $$ (4) $$ {{\boldsymbol{X}}_i}(t + 1) = {{\boldsymbol{X}}_i}(t) + {{\boldsymbol{V}}_i}(t + 1) $$ (5) 式中：ω为惯性权重；Vi（t）为第t次迭代时粒子i的速度；c1，c2为学习因子；r1，r2为[0，1]的随机数；Xi（t）为第t次迭代时粒子i的位置。
EOPSO算法在PSO算法基础上，对群体内的最优粒子（即精英个体）进行反向学习，增加搜索的目的性，尽可能避免搜索的盲目性，从而以最快速度得到最优解。但该算法存在种群内粒子间信息交流不充分、易局部最优、收敛性差等问题[5-7]。因此，将混沌初始化、交叉操作、变异操作和极值扰动引入EOPSO算法，提出CEMOPSO算法。
1） 混沌初始化。通过混沌映射空间对粒子群位置信息进行初始化，在不改变初始种群随机性的条件下，使得种群初始位置均匀分布在可行域内，提高种群初期多样性。
采用Logistic和Sinusoidal的复合混沌模型[10-12]（式（6）），使粒子在设定空间内呈现混沌状态。
$$ {\varphi _{n + 1}} = 4\sin \; ({\text{π}} {\varphi _n})(1 - \sin \; ({\text{π}} {\varphi _n})) \;\;\;\;\; n \in {{{\bf{N}}}} $$ (6) 设粒子i的位置
$ {{\boldsymbol{X}}_i} = ({{{e}}_1},{{{e}}_2}, \cdots ,{{{e}}_D}) $ ，eD为粒子i在D维空间的位置，粒子i经过混沌映射变换后的初始位置$ {\boldsymbol{X}}_i^0 = ({{e}}_{i1}^0,{{e}}_{i2}^0, \cdots ,{{e}}_D^0) $ ，则$$ {\boldsymbol{X}}_i^0 = {{\boldsymbol{X}}_{\min }} + {\varphi _{n + 1}}({{\boldsymbol{X}}_{\max }} - {{\boldsymbol{X}}_{\min }}) $$ (7) 式中Xmin，Xmax分别为搜索空间内粒子位置最小值和最大值。
2） 交叉和高斯变异。第t次迭代时对粒子i的位置
$ {{\boldsymbol{X}}_i}(t) $ 与其历史最优位置$ {{\boldsymbol{X}}_{i{\rm{best}}}}(t - 1) $ 进行离散相交[13-15]，则交叉后粒子i位置${{\boldsymbol{X}}'_i}(t){{ =({{e}}}}_{i1}'(t),{{{{e}}}}_{i2}'(t), \cdots , {{{{e}}}}_{iD}'(t))$ 。交叉算法公式为$$ e_{iJ}'(t) = \left\{ \begin{gathered} be_{iJ}(t) + (1 - b)e_{iJ{\rm{best}}}(t - 1)\;\; {\text{ }} {k_{\rm{c}}} ＞{\rm{rand}}(0,1) \\ be_{iJ{\rm{best}}}(t - 1) + (1 - b)e_{iJ}(t) \;\;{\text{ }} {k_{\rm{c}}} \leqslant {\rm{rand}}(0,1) \end{gathered} \right. $$ (8) 式中：b为交叉系数，取值为0~1；eiJ（t）为第t次迭代时粒子i在J维空间的位置，J=1，2，…，D；eiJbest（t）为第t次迭代时粒子i在J维空间的当前最优位置；kc为交叉概率。
则更新后的粒子i最优位置为
$$ {{\boldsymbol{X}}_{i{\rm{best}}}}(t) = \left\{ \begin{gathered} {{{\boldsymbol{X}}}_{i}^{}}(t) \qquad {\text{ }}h({{{\boldsymbol{X}}}_{i}^{}}(t)) ＜ h({{\boldsymbol{X}}_{i{\rm{best}}}}(t)) \\ {{\boldsymbol{X}}_{i{\rm{best}}}}(t - 1) \qquad {\text{ }}h({{{\boldsymbol{X}}}_{i}^{}}(t)) \leqslant h({{\boldsymbol{X}}_{i{\rm{best}}}}(t)) \\ \end{gathered} \right. $$ (9) 为保证算法的求解精度，对交叉后的个体进行高斯变异，得
$$ {\boldsymbol{X}}_{i{\rm{best}}}^{*}(t) = {{\boldsymbol{X}}_{i{\rm{best}}}(t)} + ({{\boldsymbol{W}}_{\max }} - {{\boldsymbol{W}}_{\min }}) G $$ (10) 式中：
$ {\boldsymbol{X}}_{i{\rm{best}}}^ {*}(t) $ 为粒子i变异后的最优位置；Wmax，Wmin分别为搜索空间的最大值和最小值；G为标准高斯分布。更新后的粒子i最优位置为
$${\boldsymbol{ X}}_{i{\rm{best}}}^{ * * }(t) = \left\{ \begin{gathered} {\boldsymbol{X}}_{i{\rm{best}}}^ *(t) \qquad h({\boldsymbol{X}}_{i{\rm{best}}}^ * (t)) ＜ h({{\boldsymbol{X}}_{i{\rm{best}}}} (t)) \\ {{\boldsymbol{X}}_{i{\rm{best}}}}(t) \qquad h({\boldsymbol{X}}_{i{\rm{best}}}^ * (t)) \geqslant h({{\boldsymbol{X}}_{i{\rm{best}}}} (t) ) \\ \end{gathered} \right. $$ (11) 3） 柯西变异。全局最优位置Xgbest引导群体朝向最优解。当Xgbest陷入局部最优时，群体搜索停滞，导致算法失效。采用柯西变异策略，以协助精英粒子跳出局部最优。全局最优位置变异值为
$$ {\boldsymbol{X}}_{{\rm{gbest}}}^ * = {{\boldsymbol{X}}_{{\rm{gbest}}}} + ({{\boldsymbol{W}}_{\max }} - {{\boldsymbol{W}}_{\min }}) {\rm{cauchy}}(0,s (t)) $$ (12) 式中：cauchy(0，s（t）)为柯西分布表达式；s(t)为随迭代次数线性递减的柯西分布比例参数[16-18]。
$$ s(t + 1) = s(t) - \sin \; ({1 \mathord{\left/ {\vphantom {1 {{t_{\max }})}}} \right. } {{t_{\max }})}} $$ (13) 式中tmax为最大迭代次数。
更新后的全局最优位置为
$$ {\boldsymbol{X}}_{{\rm{gbest}}}^{ * * } = \left\{ \begin{gathered} {\boldsymbol{X}}_{{\rm{gbest}}}^ * \qquad h({\boldsymbol{X}}_{{\rm{gbest}}}^ * ) ＜ h({{\boldsymbol{X}}_{{\rm{gbest}}}}) \\ {{\boldsymbol{X}}_{{\rm{gbest}}}} \qquad h({\boldsymbol{X}}_{{\rm{gbest}}}^ * ) \geqslant h({{\boldsymbol{X}}_{{\rm{gbest}}}}) \\ \end{gathered} \right. $$ (14) 4） 极值扰动。粒子种群具有趋同性，因此在算法后期，粒子飞行速度难以更新，导致难以发现更优位置。引入极值扰动避免粒子陷入停滞，速度更新公式为
$$\begin{split} {{\boldsymbol{V}}_i}(t + 1) = & \omega {{\boldsymbol{V}}_i}(t) + {c_1}{r_1}\left[ { \left( { \frac{1}{2} + \frac{{{r_3}}}{2} } \right){{\boldsymbol{X}}_{i{\rm{best}}}} - {{\boldsymbol{X}}_i}(t) } \right] +\\& {c_2}{r_2}\left[ { \left( { \frac{1}{2} + \frac{{{r_4}}}{2} } \right){{{{\boldsymbol{X}}}}_{{\rm{gbest}}}} - {{\boldsymbol{X}}_i}(t) } \right] \end{split} $$ (15) 式中r3，r4为[0，1]上均匀分布的随机数。
2.2 自适应参数控制
1） 交叉概率。在PSO算法中，交叉概率kc过大会消除部分优秀个体，过小则影响算法收敛速度。为提高算法性能，采用自适应交叉概率：
$$ {k_{\rm{c}}} = \left\{ \begin{gathered} \frac{{{k_{{\rm{c}}1}} + {k_{{\rm{c}}2}}}}{2} + \frac{{{k_{{\rm{c}}1}} - {k_{{\rm{c}}2}}}}{2} \sin \left( { \frac{{ h_{{\rm{max}}}' - {h_{{\rm{avg}}}}}}{{{h_{\max }} - {h_{{\rm{avg}}}}}} \frac{{\text{π}} }{2} } \right) \;\;\;\; h_{{\rm{max}}}' \geqslant {h_{{\rm{avg}}}} \\ {k_{{\rm{c}}1}} \;\;\;\; h_{{\rm{max}}}' ＜ {h_{{\rm{avg}}}} \\ \end{gathered} \right. $$ (16) 式中：kc1，kc2分别为初始交叉概率最大值和最小值；
$ h_{{\rm{max}}}' $ 为2个粒子进行交叉操作时的适应度最大值；havg为适应度平均值；hmax为适应度最大值。2） 正态分布衰减惯性权重。惯性权重的取值直接影响算法性能。惯性权重应随迭代次数的增加而动态变化，即在迭代过程中由初期的较大值逐步线性减小。本文采取正态分布衰减的惯性权重：
$$ \omega = {\omega _{\min }} + ({\omega _{\max }} - {\omega _{\min }}) \dfrac{1}{{\sqrt {2{\text{π}} } \sigma }}{{\rm{exp}}\left( { - \frac{{{t^2}}}{{2{\sigma ^2}t_{\max }^2}}} \right)} $$ (17) 式中：ωmin，ωmax分别为惯性权重最小值和最大值；σ为趋势参数，根据文献[19-20]可知最佳值为0.443 3。
3. 锚杆钻车钻臂定位控制流程
基于CEMOPSO算法的锚杆钻车钻臂定位控制流程如图3所示。
![]()
图 3 基于CEMOPSO算法的锚杆钻车钻臂定位控制流程Figure 3. Positioning control flow of drilling arm of bolt drilling rig based on chaotic crossover elite mutation opposition-based particle swarm optimization(CEMOPSO) algorithm1） 对种群进行混沌初始化，确定种群规模、自适应参数，并给定钻臂末端目标位姿。
2） 计算粒子个体适应度，确定个体最优位置和全局最优位置，其大小为钻臂当前位姿和目标位姿的误差。
3） 根据精英反向规则，计算精英个体的反向解。根据柯西变异公式，对精英个体实施柯西变异操作。
4） 根据交叉规则，对粒子个体实施交叉操作，并对交叉后的个体最优位置实施高斯变异操作。
5） 更新粒子个体最优位置和全局最优位置。
6） 根据式（5）和式（15）对粒子位置和速度进行更新。
7） 确定目标函数值是否达到收敛要求，若是则结束迭代，输出结果，否则重复步骤2），直至符合收敛要求。
4. CEMOPSO算法性能测试
为检验CEMOPSO算法性能，分别从稳定性、精度、收敛速度3个方面，将其与PSO算法、EOPSO算法和交叉精英反向粒子群优化（Crossover Elite Opposition-based PSO，CEOPSO）算法进行对比。算法参数设置：种群规模为90，惯性权重最大值、最小值分别为0.9，0.6，限制速度为0.5，初始变异概率最小值、最大值分别为0.2，0.3，柯西分布比例参数初值为1。4种算法分别对4个标准测试函数（表2）执行20次，根据测试结果计算标准差和最优解，以此反映算法稳定性和求解精度，结果见表3。可看出CEMOPSO算法的稳定性和精度最优。
表 2 标准测试函数Table 2. Standard test functions函数 维度 搜索范围 最优解 ${f}_{1}(g)\text{=}{\displaystyle \sum _{r=1}^{n}{g}_{r}^{2} }$ 30 [−100，100] 0 ${f_2}(g) =\displaystyle \sum\limits_{r = 1}^n {\left| { {g_r} } \right|} + \prod\limits_{r = 1}^n {\left| { {g_r} } \right|}$ 30 [−10，10] 0 ${f_3}(g) = \displaystyle \sum\limits_{r = 1}^n {(\sum\limits_{q = 1}^n { {g_q}{)^2} } }$ 30 [−100，100] 0 $\mathop f\nolimits_4 (g) = \max \{ \left| {\mathop g\nolimits_r } \right|,1 \leqslant r \leqslant n\}$ 30 [−100，100] 0 为便于直观分析各算法性能，随机选择其中1组测试结果进行曲线可视化比较，如图4所示。可看出在算法迭代初期，CEMOPSO算法与其他算法没有显著差别，但随着迭代次数增加，其收敛速度迅速增大，明显优于其他3种算法。
表 3 标准测试函数计算结果Table 3. Calculation results of standard test functions函数 PSO算法 EOPSO算法 CEOPSO算法 CEMOPSO算法 $ {f_1}(g) $ 标准差：$3.223\; 2 \times {10^{ { { - } }2} }$ 标准差：$ 6.193\;9 \times {10^{{{ - }}2}} $ 标准差：$2.925\;9 \times {10^{{{ - 6}}}}$ 标准差：$2.870\;6 \times {10^{{{ - 18}}}}$ 最优解：$ 2.807\;2 \times {10^{{{ - }}2}} $ 最优解：$ 2.979\;5 \times {10^{{{ - }}2}} $ 最优解：$1.393\;2 \times {10^{{{ - 6}}}}$ 最优解：$4.794\;3 \times {10^{{{ - 19}}}}$ $ {f_2}(g) $ 标准差：$ 1.001\;8 \times {10^0} $ 标准差：$ 1.255\;4 \times {10^0} $ 标准差：$ 7.436\;1 \times {10^{{{ - }}2}} $ 标准差：$5.045\;2 \times {10^{{{ - 13}}}}$ 最优解：$ 8.349\;6 \times {10^{{{ - }}1}} $ 最优解：$ 8.012\;2 \times {10^{{{ - }}1}} $ 最优解：$ 6.558\;2 \times {10^{{{ - }}2}} $ 最优解：$1.479\;4 \times {10^{{{ - 13}}}}$ $ {f_3}(g) $ 标准差：$ 39.100\;3 \times {10^0} $ 标准差：$ 36.417\;4 \times {10^0} $ 标准差：$ 34.092\;9 \times {10^0} $ 标准差：$9.092\;9 \times {10^{{{ - }}2}}$ 最优解：$ 32.092\;9 \times {10^0} $ 最优解：$ 31.565\;9 \times {10^0} $ 最优解：$ 32.073\;7 \times {10^0} $ 最优解：$7.686\;5 \times {10^{{{ - }}2}}$ $ {f_4}(g) $ 标准差：$ 1.268\;5 \times {10^0} $ 标准差：$ 1.820\;8 \times {10^0} $ 标准差：$ 5.433\;3 \times {10^{{{ - }}1}} $ 标准差：$1.683\;6 \times {10^{{{ - 3}}}}$ 最优解：$ 1.167\;1 \times {10^0} $ 最优解：$ 1.035\;9 \times {10^0} $ 最优解：$ 5.398\;9 \times {10^{{{ - }}1}} $ 最优解：$1.327\;9 \times {10^{{{ - 3}}}}$ 5. 钻臂定位控制建模与仿真分析
为验证基于CEMOPSO算法的锚杆钻车钻臂定位控制方法的可行性及效果，采用Matlab2020a软件进行仿真验证。
5.1 锚杆钻车钻臂模型
根据钻臂D−H参数在Matlab2020a中建立锚杆钻车钻臂模型，如图5所示。
为便于观察锚杆钻车钻臂末端在空间中的运动范围，通过调节钻臂各关节变量来控制钻臂末端位姿。利用蒙特卡罗法绘制钻臂末端三维工作空间及其平面投影，如图6所示。
5.2 仿真分析
根据锚杆钻车钻臂末端位姿矩阵，采用欧拉角形式定义钻臂末端位姿：
$$ {\boldsymbol{P}}({{\boldsymbol{X}}_i}) = [{p_x},{p_y},{p_{\textit{z}}},\beta ,\gamma ,\eta ] $$ (18) $$ \beta = \arctan \ 2({O_{\textit{z}}},{A_{\textit{z}}}) $$ (19) $$ \gamma = \arctan \ 2\left( - {N_{\textit{z}}},\sqrt {{O_{\textit{z}}}^2 + {A_{\textit{z}}}^2} \right) $$ (20) $$ \eta = \arctan\ 2({N_x},{N_y}) $$ (21) 式中：
$ ({p_x},{p_y},{p_z}) $ 为钻臂末端目标位置坐标；$ (\beta ,\gamma ,\eta ) $ 为钻臂末端目标位置坐标系与基坐标系对应坐标轴的夹角。针对4种算法，选取相同的初始参数进行仿真。针对多冗余自由度钻臂结构特点，为提高钻臂定位控制精度，取种群规模为300[14-16]；惯性权重最大值、最小值分别为0.8，0.5。CEMOPSO算法采用正态分布衰减的惯性权重，限制速度为0.2。c1，c2均取1.5[17]。初始交叉概率最大值、最小值分别为0.9，0.7；柯西分布比例参数初值为1，迭代次数为500。
4种算法的位置误差和姿态误差收敛曲线如图7所示。可看出在相同的迭代次数和误差精度约束条件下，无论是位置误差还是姿态误差，CEMOPSO算法从迭代初期即具有极快的收敛速度，收敛性能优于其他算法。
![]()
图 7 4种算法对钻臂定位控制的位置误差和姿态误差收敛曲线Figure 7. Convergence curves of position errors and posture errors of drilling arm positioning control by use of four algorithms在相同的约束条件下，采用4种算法重复进行多次钻车钻臂定位控制，结果如图8所示。可看出CEMOPSO算法的位置误差和姿态误差均小于其他算法，误差曲线较平稳，最大位置误差为0.005 m，最大姿态误差为0.005 rad，验证了该算法用于锚杆钻车钻臂定位控制时具有较好的性能。
![]()
图 8 4种算法对钻臂定位控制的位置误差和姿态误差曲线Figure 8. Position error and posture error curves of drilling arm positioning control by use of four algorithms实际工程应用中一般要求控制算法能够在指定精度下快速收敛[21-22]。在相同定位精度下，4种算法的迭代次数如图9所示。可看出当设定位置误差为1 mm、姿态误差为0.01 rad时，PSO算法、EOPSO算法、CEOPSO算法、CEMOPSO算法的平均迭代次数分别为651，607，543，343 ；当设定位置误差为0.1 mm、姿态误差为0.001 rad时，PSO算法、EOPSO算法、CEOPSO算法、CEMOPSO算法的平均迭代次数分别为1 090，949，784，473，CEMOPSO算法的收敛速度最快，稳定性最佳，且求解精度越高，其优越性越突出。
![]()
图 9 4种算法在不同精度条件下的迭代次数Figure 9. Iteration times of four algorithms under different precision conditions6. 结论
1） CEMOPSO算法将混沌初始化、变异操作和交叉操作引入EOPSO算法，在保持初始种群多样性的基础上，增强了粒子个体之间的信息交流，平衡了算法局部搜索力和全局搜索力，使粒子个体能够更快地到达最优解，提高了算法的收敛速度和精度。
2） 将CEMOPSO算法应用于锚杆钻车钻臂定位控制可避免产生无解状态，且能够保证在满足定位精度要求下，改善求解速度和稳定性，提高钻臂定位效率，具有良好的工程实用价值。
-
![]()
图 1 钻车机械手整体结构
1−回转器；2−机械手摆动油缸；3−托板；4−机械手伸缩油缸；5−机械手爪；6−仰角检测接近开关；7−俯角检测接近开关；8−机械手竖直回转减速器；9−导轨竖直回转减速器；10−夹持器。
Figure 1. Overall structure of drill rig manipulator
![]()
图 5 3−5−3样条插值机械手关节位置曲线
Figure 5. Joint position curves of manipulator based on 3-5-3 spline interpolation
![]()
图 7 3−5−3样条插值机械手关节加速度曲线
Figure 7. Joint acceleration curves of manipulator based on 3-5-3 spline interpolation
![]()
图 6 3−5−3样条插值机械手关节速度曲线
Figure 6. Joint velocity curves of manipulator based on 3-5-3 spline interpolation
表 1 机械手D-H参数
Table 1 D−H parameters of manipulator
关节 θf /(°) df /mm af /mm αf /(°) 关节变量范围/（°） 1 θ1 267 0 0 [−127.5,127.5] 2 θ2 702 0 −90 [0,60] 
下载: 导出CSV
表 2 笛卡尔空间路径
Table 2 Cartesian space path
mm 抓杆点 路径点1 路径点2 放杆点 (0,704,267) (−277,653,267) (−460,548,267) (−657,−328,267)
下载: 导出CSV
表 3 关节空间的角度插值点
Table 3 Angle interpolation points in joint space
关节位置 关节1 关节2 β0 0 0 β1 0.4012 0 β2 0.6978 0 β3 2.0410 0
下载: 导出CSV
表 4 各关节优化结果
Table 4 Optimization results of each joint
s 关节 运行时间 tj1 tj2 tj3 关节1 0.6487 0.3919 1.3448 关节2 0 0 0
下载: 导出CSV
-
[1] 卢新明,阚淑婷. 煤矿动力灾害本源预警方法关键技术与展望[J]. 煤炭学报,2020,45(增刊1):128-139. LU Xinming,KAN Shuting. Key technology and prospect of the original source early warning method for coal mine dynamic disaster[J]. Journal of China Coal Society,2020,45(S1):128-139.
[2] 国家煤矿安全监察局. 煤矿机器人重点研发目录[EB/OL]. [2021-08-13]. https://www.chinamine-safety.gov.cn/zfxxgk/fdzdgknr/tzgg/201901/t20190109_349156.shtml. National Coal Mine Safety Supervision Bureau. Catalogue of key research and development of coal mine robots [EB/OL]. [2021-08-13]. https://www.chinamine-safety.gov.cn/zfxxgk/fdzdgknr/tzgg/201901/t20190109_ 349156.shtml.
[3] 付荣,居鹤华. 基于粒子群优化的时间最优机械臂轨迹规划算法[J]. 信息与控制,2011,40(6):802-808. FU Rong,JU Hehua. Time-optimal trajectory planning algorithm for manipulator based on PSO[J]. Information and Control,2011,40(6):802-808.
[4] HUANG Junsen,HU Pengfei,WU Kaiyuan,et al. Optimal time-jerk trajectory planning for industrial robots[J]. Mechanism and Machine Theory,2018,121:530-544. DOI: 10.1016/j.mechmachtheory.2017.11.006
[5] 韩顺杰,单新超,于爱君,等. 基于改进粒子群算法的工业机器人轨迹规划[J]. 制造技术与机床,2021(4):9-14. HAN Shunjie,SHAN Xinchao,YU Aijun,et al. Industrial robot trajectory planning based on improved PSO algorithm[J]. Manufacturing Technology & Machine Tools,2021(4):9-14.
[6] 陈晗,李林升. 基于复合形法的时间最优机械臂轨迹规划[J]. 机械传动,2019,43(3):72-75. CHEN Han,LI Linsheng. Trajectory planning of time optimal manipulator based on complex method[J]. Mechanical Transmission,2019,43(3):72-75.
[7] 乐英,岳艳波. 六自由度机器人运动学仿真及轨迹规划[J]. 组合机床与自动化加工技术,2016(4):89-92. YUE Ying,YUE Yanbo. The kinematics simulation and trajectory planning of six-DOF robot[J]. Modular Machine Tool & Automatic Manufacturing Technique,2016(4):89-92.
[8] 王学琨,李刚,周东凯,等. 基于DE的时间最优6−DOF机械臂轨迹规划算法[J]. 计算机仿真,2015,32(8):332-337. DOI: 10.3969/j.issn.1006-9348.2015.08.072 WANG Xuekun,LI Gang,ZHOU Dongkai,et al. Time-optimal trajectory planning algorithm based on DE for 6-DOF manipulator[J]. Computer Simulation,2015,32(8):332-337. DOI: 10.3969/j.issn.1006-9348.2015.08.072
[9] 赵明辉. 双臂并联煤矸石分拣机器人及其轨迹规划研究[J]. 工矿自动化,2020,46(9):57-63. ZHAO Minghui. Research on dual-arm parallel coal gangue sorting robot and its trajectory planning[J]. Industry and Mine Automation,2020,46(9):57-63.
[10] 郭锐,石月,李永涛,等. 液压凿岩机器人机械臂轨迹规划研究[J]. 中国工程机械学报,2021,19(4):289-294. GUO Rui,SHI Yue,LI Yongtao,et al. Research on trajectory planning of hydraulic rock drilling robot manipulator[J]. Chinese Journal of Construction Machinery,2021,19(4):289-294.
[11] 徐尤南,刘志强,陈洁. 基于粒子群算法的码垛机器人时间轨迹优化研究[J]. 华东交通大学学报,2021,38(3):75-81. XU Younan,LIU Zhiqiang,CHEN Jie. Time trajectory optimization of palletizing robot based on particle swarm optimization[J]. Journal of East China Jiaotong University,2021,38(3):75-81.
[12] 赵丽娟,张海宁,岳海涛,等. 滚筒螺旋叶片的激光熔覆中机械臂路径轨迹研究[J]. 煤炭学报,2020,45(增刊2):1041-1051. ZHAO Lijuan,ZHANG Haining,YUE Haitao,et al. Path and trajectory of manipulator in laser cladding of shearer blade[J]. Journal of China Coal Society,2020,45(S2):1041-1051.
[13] 王晓丽,侯媛彬,王涛. 基于VC++的工业机器人轨迹规划研究[J]. 工矿自动化,2009,35( 5):34-37. WANG Xiaoli,HOU Yuanbin,WANG Tao. Research on trajectory planning of industrial robot based on VC++[J]. Industry and Mine Automation,2009,35( 5):34-37.
[14] 李小为,胡立坤,王琥. 速度约束下PSO的六自由度机械臂时间最优轨迹规划[J]. 智能系统学报,2015,10(3):393-398. LI Xiaowei,HU Likun,WANG Hu. PSO-based time optimal trajectory planning for six degrees of freedom robot manipulators with speed constraints[J]. CAAI Transactions on Intelligent Systems,2015,10(3):393-398.
[15] 黄超,茅健,马丽,等. 基于改进粒子群算法的时间最优机械臂轨迹规划[J]. 上海工程技术大学学报,2020,34(3):238-246. HUANG Chao,MAO Jian,MA Li,et al. Time-optimal trajectory planning for manipulator based on improved particle swarm optimization algorithm[J]. Journal of Shanghai University of Engineering Science,2020,34(3):238-246.
[16] SHI Yannan,QI Penglei,LIU Yang,et al. Channel modeling and optimization of leaky coaxial cable network in coal mine based on state transition method and particle swarm optimization algorithm[J]. IEEE Access,2021,9:86889-86898. DOI: 10.1109/ACCESS.2021.3088842
计量
- 文章访问数: 180
- HTML全文浏览量: 101
- PDF下载量: 20
