基于改进粒子群算法的钻车机械手轨迹规划研究

申树策，史艳楠,3,4，宋建锋，任泽，王毅颖,3，王翰秋,3

（1.河北工程大学 机械与装备工程学院,河北 邯郸 056038；2.冀凯河北机电科技有限公司,河北 石家庄 050000；3.河北省煤炭生态保护开采产业技术研究院,河北 邯郸 056038；4.邯郸市智能车辆重点实验室,河北 邯郸 056038）

0 引言

随着我国煤炭开采强度加大、开采深度加深、地质条件越来越复杂，冲击地压、水害、瓦斯突出、火灾等灾害发生的概率也越来越高[1]。为了保证煤矿绿色安全开采“智能化、无人化”，国家发布了《煤矿机器人重点研发目录》，防突钻孔机器人与防冲钻孔机器人是其中重点研发内容[2]。防突防冲钻车是防突钻孔机器人与防冲钻孔机器人的结合体，可对煤体应力进行有效卸压，且不会破坏原有的巷道支护，有效降低了煤与瓦斯突出、冲击地压等灾害发生的概率。钻车机械手作为防突防冲钻车不可或缺的装置，关系着钻车是否可以正常钻进及真正实现无人化。提高钻车机械手的工作效率，对于提高煤矿安全生产效率具有重要意义。

在机械手的研究中，使机械手在避免奇异点的同时能够快速、准确、平稳地到达工作点对于其在工业应用中至关重要。为了使机械手快速、准确、平稳运行，轨迹规划优化尤为重要。目前，在机械手轨迹规划优化算法中，研究较多的是如何使机械手在满足约束条件下的时间最优，其目的是在满足运动学约束条件下使机械手各个关节的速度最大化，从而使机械臂运行时间最短。时间优化算法大体上可分为2 类：①点到点的最短时间优化算法。② 给定一条已知的连续路径，使机械手在此路径运行时间最短。后者的研究方法是将路径分段处理，再利用点到点的优化算法对其优化[3]。在轨迹规划中，轨迹大多由多项式、样条、贝塞尔曲线等插值函数构造[4]。文献[5]采用基于正弦变化学习因子的粒子群（Particle Swarm Optimization,PSO）算法对4-3-4多项式插值进行轨迹规划，改善了传统PSO 算法收敛速度缓慢的问题。文献[6]采用5-7-5 多项式插值与复合形法相结合的方式对川崎FS20N 机器人进行轨迹规划，提高了工作效率，降低了能耗，但是阶次较高，计算复杂。文献[7]对六自由度机器人采用非均匀B 样条插值对其时间进行优化，得到了较好的效果，但其样条插值为五次和七次，阶次较高，计算复杂。文献[8]利用差分进化算法对机械臂进行了时间最优轨迹规划，但是差分进化算法容易早熟或使算法停滞。文献[9]采用五次插值多项式对分拣机器人进行轨迹规划，提高了轨迹平滑程度，但是相比于分段处理的方式，该方法耗时较多。以上研究虽然取得了一定的成果，但大多存在阶次较高，易发生“龙格”现象且不易计算和优化算法易早熟的问题。

针对上述问题，本文以钻车机械手为研究对象，提出了一种基于改进PSO 算法的钻车机械手时间最优轨迹规划方法。首先，利用标准Denavit-Hartenberg（D-H）构建钻车机械手三维模型，通过蒙特卡洛法得到钻车机械手的工作空间，从工作空间中选取4 个途径点作为插值点。然后，为了使钻车机械手能够快速平稳地到达指定位置，在关节空间中采用3-5-3 分段多项式插值构造其轨迹。最后，通过改进PSO 算法对构造的轨迹进行时间最短优化，得到钻车机械手的时间最优轨迹规划。Matlab 仿真结果证明了该方法的可靠性。

1 钻车机械手运动学模型

1.1 钻车机械手结构

本文研究的钻车机械手为2 自由度机械手，由1 个转动副和1 个移动副组成，其整体结构如图1 所示。主要工作任务为从回转托板抓取钻杆装入主机夹持器内，具体上杆步骤如下：①判断机械手与托板状态，保证顺利抓杆；通过判断仰角检测接近开关和俯角检测接近开关状态，调整机械手和托板位置。② 机械手摆动油缸逆时针旋转至憋压状态，机械手到达抓杆角度位置。③机械手伸缩油缸伸出至憋压状态，机械手到达抓杆位。④ 机械手爪夹紧，抓住钻杆。⑤ 机械手摆动油缸顺时针旋转至憋压状态，机械手到达上杆位置。⑥ 夹持器（副夹持器）夹紧新钻杆，夹紧前端已钻入钻杆，回转器正转进给，连接新加钻杆。

图1 钻车机械手整体结构Fig.1 Overall structure of drill rig manipulator

1.2 钻车机械手运动学模型

1.2.1 D-H 参数确定

根据标准D-H 参数法以X0Y0Z0为基坐标建立连杆坐标系[10]，如图2 所示。其中，XfYfZf（f=1,2）为第f个连杆的坐标系，d1为基底与连杆1 的偏移距，d2为连杆1 和连杆2 的偏移距，且d1=267 mm，d2=702 mm。

图2 标准D-H 坐标系Fig.2 Standard D-H coordinate system

结合图2 的坐标系及其参数可求得机械手的D-H 参数，见表1。表1 中，θf为关节角，df为偏移距，af为杆件长度，αf扭转角。

1.2.2 变换矩阵计算

根据图2 坐标系和表1 中的各个参数，可求得连杆之间的齐次变换矩阵。其中，相邻2 个连杆之间的齐次变换矩阵为[11]

表1 机械手D-H 参数Table 1 D-H parameters of manipulator

将表1 中的参数值代入式（1）可计算出2 个变换矩阵分别为

通过式（2）、式（3）可得到钻车机械手相对于直角坐标系x，y，z轴的末端位姿矩阵[12]：

式中：n，o和q为位姿矩阵的姿态分量；p为位姿矩阵的位置分量。

2 钻车机械手关节空间轨迹规划

轨迹规划通常在笛卡尔空间或关节空间中进行，且两者可互相转换[13]。在笛卡尔空间中，可自然地描述末端执行器的轨迹，用逆运动学求解每个关节的插补点，计算相应的关节角度，计算量非常大，且容易导致运动奇异点和机械手冗余。而在关节空间中进行轨迹规划，可实时调整各个关节的角度插值点，从而使机械手达到期望的状态。结合实际情况，本文在关节空间中对钻车机械手进行轨迹规划。

2.1 分段多项式轨迹规划

对于多项式样条插值函数，可以保证钻车机械手关节准确地经过各个插值点。速度的变化规律可通过对时间求1 阶导来观察。同理，对时间求2 阶导可获得加速度的变化规律。为了保证钻车机械手的速度和加速度的连续，需要最少保证函数C3连续，同时为了保证计算量不太复杂，根据实际工况需要，将钻车机械手的运行轨迹分为3 段，即3-5-3 多项式插值。其中，3-5-3 样条多项式的通式为

式中：hj1(t)，hj2(t)，hj3(t) 分别为3 段多项式的轨迹，j为当前关节，t为总时间；mj为第j个关节轨迹的系数；t1，t2，t3分别为每个关节初始段、中间段和终止段的运动时间。

已知各个关节在通过抓杆点β0、路径点β1和β2、放杆点β3时，各个端点连接处的速度和加速度都平稳连续，且在抓杆点和放杆点时速度和加速度都为0[14]，得多项式系数与插值点关系为

式中：m为关节系数矩阵；A为经过4 个插值点时3-5-3 插值多项式的系数矩阵；b为对应的插值点矩阵。

2.2 基于改进PSO 算法的轨迹优化

以时间最优为优化目标，利用改进的PSO 算法对钻车机械手进行优化。将每个关节的3 段时间t1，t2，t3组成一个三维的粒子群[15]，其中粒子群适应度函数为

初始化之后，计算每个粒子初始化后的适应度，并将其赋值给pbest（个体最优值）。利用改进PSO 算法以式（11）为适应度函数进行迭代更新，迭代完成后，计算当前全部粒子的适应度，其中值最小的粒子作为gbest（全局最优值）。PSO 算法速度和位置更新公式为

式中：vidk+1为第i个粒子在d维时，迭代k+1 次的速度；ω为惯性权重；c1，c2为学习因子，决定粒子获取外界信息的能力；r1，r2为[0,1]的随机数；xid为第i个粒子在d维时的位置。

固定不变的ω在PSO 算法整体求解的过程中可能会存在易陷入局部最优解的缺陷[16]，为此本文采用一种随迭代次数增加ω线性下降的方法来提高搜寻解的能力，其计算公式为

式中：ωmax为最大惯性权重；ωmin为最小惯性权重；k为当前迭代次数；kmax为最大迭代次数。

式（12）和式（13）在本文中指时间变化量和时间的更新公式。以时间t为寻优量在粒子群里寻优，具体优化步骤如下：

（1）参数初始化。首先明确有多少个粒子，在三维搜索空间中随机产生M个粒子（共有3M个）构成种群，使用rand 函数随机给定时间变化量及时间。

（2）初始化完成后，将时间变量tj1，tj2，tj3代入式（8）-式（10），可求解出系数矩阵m中的元素。

（3）式（5）-式（7）中的三段多项式轨迹可根据步骤（2）所得到的系数矩阵m得到。判断3 段轨迹的速度函数是否全部小于各个关节允许的最大速度。如果求得的3 段轨迹的速度函数都在速度约束范围内，则将式（11）作为其适应度函数，计算适应度，以时间最短为目标进行迭代；否则，将赋值一个极大的常数充当其适应度，在接下来迭代寻优时，会通过比较适应度的大小来淘汰该粒子。

（4）对随机产生的M个粒子，将其自身经过迭代k次后，所产生的n个适应度值作比较，适应度值最小的位置作为pbest，再通过比较每个粒子的pbest，选取其中最小的适应度值作为gbest。

（5）根据式（12）和式（13）来更新种群的速度和位置，并将其重新构成具有新的位置和速度的M个粒子的新种群。

（6）如果达到最大迭代次数或找到局部最优解，则算法结束，否则返回步骤（2）。

3 仿真实验与结果分析

3.1 钻车机械手的Matlab 建模

根据式（4）可得到坐标系{2}相对于坐标系{0}的变换矩阵，由此可计算出机械手末端执行器的位姿。利用Matlab 仿真软件内的Robotics Toolbox工具箱对钻车机械手进行建模,并用蒙特卡洛法绘制出钻车机械手的工作空间，分别如图3、图4 所示。

图3 钻车机械手三维模型Fig.3 3D model of drill rig manipulator

图4 钻车机械手工作空间Fig.4 Workspace of drill rig manipulator

3.2 钻车机械手的Matlab 仿真

根据钻车机械手的工作空间及抓杆、放杆的工作特点，给定其在笛卡尔空间中的抓杆点、2 个路径点及放杆点的位置，见表2。

根据逆运动学，可以将表2 给定的各点转换到关节空间中，从而获得各个关节在各点的角度插值点，见表3。其中，关节2 为伸缩关节，只改变伸缩量，所以它的角度插值不变，为0。

表2 笛卡尔空间路径Table 2 Cartesian space path mm

表3 关节空间的角度插值点Table 3 Angle interpolation points in joint space

3-5-3 分段插值多项式中的3 段插值时间都是根据实际情况提前给定的。在满足抓杆点和放杆点的速度和加速度都是0 的前提下，给定总时间t=3.168 5 s，t1=0.866 5 s，t2=0.667 s，t3=1.635 s。在给定每段关节时间的插值效果如图5-图7 所示。

图5 3-5-3 样条插值机械手关节位置曲线Fig.5 Joint position curves of manipulator based on 3-5-3 spline interpolation

图6 3-5-3 样条插值机械手关节速度曲线Fig.6 Joint velocity curves of manipulator based on 3-5-3 spline interpolation

图7 3-5-3 样条插值机械手关节加速度曲线Fig.7 Joint acceleration curves of manipulator based on 3-5-3 spline interpolation

从图5-图7 可看出，机械手各关节位置、速度和加速度都是连续的，表明了该分段插值多项式可以很好地用于构造机械手轨迹，但是图6 中关节的速度并未达到峰值。

根据改进的PSO 算法对防突防冲钻车机械手进行优化，设定初始粒子数M=50，初始位置为0.1～4 s之间的任意随机位置，粒子迭代速度在[-3，3]之间，迭代次数为50，惯性权重根据式（14）变化，速度和加速度在起始和终止时为0。优化后，适应度值和迭代次数之间的变换规律如图8 所示。可看出粒子在迭代23 次以后几乎趋于稳定，证明改进PSO 算法可快速收敛。

图8 改进PSO 算法的适应度曲线Fig.8 Fitness curve of improved PSO algorithm

采用改进的PSO 算法对钻车机械手进行时间最优轨迹规划，以机械手关节1 为例，得到其3 段插值时间的迭代过程如图9 所示。

图9 关节1 最优粒子迭代过程Fig.9 Optimal particle iteration process of joint 1

从图9 可看出，关节1 的3 段插值时间最后平稳趋于定值，即可得到关节1 优化后的时间。

由于关节2 为伸缩油缸，在进行抓杆、放杆的轨迹规划时不改变关节位置，所以关节2 各段运行时间为0，各关节优化结果见表4。

表4 各关节优化结果Table 4 Optimization results of each joint s

为了保证关节1 和关节2 在同一时间到达放杆点的位置，同时确保每个关节运行平稳，应选取优化后各关节时间的最大值，由于关节2 各段插值时间为0，所以只采用关节1 优化后的插值时间，即0.648 7,0.391 9,1.344 8 s，总时间为2.385 4 s，与优化前相比缩短了0.783 1 s，缩短约25%，提高了机械手的工作效率。

优化后机械手各关节的位置、速度和加速度曲线如图10-图12 所示。

图10 优化后的机械手关节位置曲线Fig.10 Joint position curves of optimized manipulator

图11 优化后的机械手关节速度曲线Fig.11 Joint velocity curves of optimized manipulator

图12 优化后的机械手关节加速度曲线Fig.12 Joint acceleration curves of optimized manipulator

从图10 可看出，优化后的关节位置相较之前更加平滑。从图11 可看出机械手的关节最大速度较图6 有很大提高。从图12 可看出，优化后的加速度虽然较之前有起伏，但其位置、速度和加速度都连续且在运动学约束范围内。从图11 和图12 还可看出，起始点和终点数值都为0，符合预先假设。仿真结果显示，通过改进的PSO 算法对钻车机械手进行轨迹规划，可以减少作业时间，提高其工作效率。

4 结论

（1）基于改进PSO 算法的钻车机械手时间最优轨迹规划方法通过构建钻车机械手运动学模型得到其末端位姿矩阵，并利用蒙特卡洛法得到了钻车机械手的工作空间。从工作空间中选取4 个途径点作为插值点，采用3-5-3 分段插值多项式进行轨迹规划，通过改进的PSO 算法对得到的轨迹进行优化，得到了钻车机械手的时间最优轨迹规划。

（2）基于改进PSO 算法的钻车机械手时间最优轨迹规划方法可在满足钻车机械手运动学约束的前提下，使其运行时间从3.168 5 s 减少到2.385 4 s，整体时间较优化前可缩短约25%，提高了关节运行速度。