王小龙，洪东炳，贾志军,2，朱鹏坤

1安徽理工大学机械工程学院 安徽淮南 232001 2安徽省智能矿山装备与技术工程实验室 安徽淮南 232001

随 着矿山提、运、采、掘四大装备自动化、智 能化水平日益提升，辅助安装设备的机械化、自动化和智能化水平的提升就显得尤为重要。井下架管机是一种重要的辅助安装设备，通过核心部件机械臂来实现运管、架管、接管和固管等功能。为了提高架管效率，国内外学者对机械臂进行了大量研究，王君等人[1]通过 MATLAB Robotics Toolbox 对 UR5 机器人建立模型，再对其进行轨迹规划与仿真的研究，验证了机器人运动模型和 3 次多项式插值算法的正确性和合理性；李威等人[2]对三节臂挖掘装置建立运动学模型，采用遗传算法对优化模型求解，并对实例进行分析，验证了此优化方法有较高的应用价值；于瑞等人[3]基于杂交算法对机器人的运动时间实现了最优轨迹规划。以上述相关研究为基础，结合煤矿巷道的实际工作要求，笔者采用 3-5-3 多项式插值算法对三节臂进行轨迹规划，结合粒子群算法，实现了速度约束下三节臂轨迹规划时间的优化。

1 综掘架管机的结构设计

目前大部分矿区仍然采用人工架管的方式，尤其在两淮地区，架管效率较低。煤矿巷道断面通常选择直墙半圆拱形，净断面宽度为 4.2 m，高度为 3.4 m，管道安装高度在 1.7～2.6 m，在没有适合的辅助安装设备下，工人劳动强度大，急需改善。

1.1 架管机的工作原理

针对上述工况，笔者设计了一种煤矿辅助设备架管机，其结构如图 1 所示。架管机主要由牵引机构、行走机构、架管机构、钻孔机构、载人作业机构和运管机构等部分组成。架管机通过牵引机构到达指定工作位置，由钻孔机构对工作区域进行打孔作业，之后架管机构夹取管道，将其提升到一定高度，再协同载人作业机构将管道固定。其中，核心部件架管机构三节臂的运动轨迹决定了架管工作的效率，因此笔者对架管机构三节臂进行了轨迹优化分析。

图1 架管机结构Fig.1 Structure of pipe erector

1.2 架管机三节臂结构

架管机三节臂结构如图 2 所示，主要由回转装置、第一节臂、第二节臂、第三节臂、夹管器和相应的液压缸组成。与两节臂结构相比，三节臂增加了架管的高度，通过辅助液压缸可以调节夹管器的角度。

图2 架管机三节臂结构Fig.2 Structure of three-section arm of pipe erector

2 架管机三节臂的轨迹规划

为了清楚描述三节臂工作时的路径状态，判断末端位置是否满足巷道架管要求，笔者采用粒子群优化算法，以 3-5-3 多项式插值算法[4]对其运动过程中的关节位置、速度、加速度进行规划，优化三节臂的工作时间。

2.1 多项式插值函数构造

3-5-3 多项式规划是指在关节规划中，关节在时间t1采用 3 次多项式进行规划，在时间t2采用 5 次多项式进行规划，在时间t3采用 3 次多项式进行规划。在工作空间内，对三节臂进行轨迹规划，设置其工作路径，利用 3-5-3 次多项式对三节臂进行插值，设置 4 个插值点，用θij表示关节i插值的角度，其中i＝1，2，…，n，n表示关节数，j＝1，2，3，4，表示插值的 4 个序号。

第i个关节 3-5-3 样条多项式的通式为

式中：li1(t)、li2(t)、li3(t) 分别代表第i段关节在 3 次多项式的运动轨迹、5 次多项式的运动轨迹、3 次多项式的运动轨迹；其中，未知系数a可根据约束条件求出，并根据约束条件和约束边界求出矩阵A[5]，用θ=[θi0，θi1，θi2，θi3] 代表第i个关节的 4 个插值点 (弧度值)。已知条件为第i个关节的初始点θi0、路径点θi1和θi2、末端点θi3以及起止速度 0，则

根据已知条件，推导出求解未知数a与插值点的关系式为

2.2 基于粒子群算法的轨迹优化

粒子群算法[6]是对轨迹时间优化的一种方法，是在鸟群捕食过程中受到的启发，将每个个体看作 D 维空间内一个微粒，在搜索空间内以一定速度运动，通过个体之间相互协作和竞争来寻找最优解。在粒子群算法中，将每个需要优化的变数视为一个粒子，每个粒子都有一个适应值，由被优选函数所确定，并利用这种适应值的大小来确定粒子的最优值。在 D 维搜索空间内，每个粒子都有一个位置xi和一个速度vi，粒子经过的地方都会出现 2 个最优位置，即粒子所经历最优适应值的位置pi和所有微粒群体目前所经历的最优位置pg。当 2 个位置都不存在最优解时，第i个粒子就会通过式 (4)、(5) 继续k+1 次迭代来更新自己的位置和速度，直到找到最优的适应值。

式中：vid∈[-Vmax，Vmax]；w为惯性权重；c1、c2为学习因子；r1、r2为 [0，1] 之间随机任意值；xid∈ [-xmax，xmax]。

当xid和vid符合约束条件时，就用边界值代替，其约束条件为式中：t1、t2、t3为适应值函数。

为了方便计算，将t作为自变量，搜索维度降到三维，求解位置矩阵。

粒子群算法对三节臂第i关节进行时间最优规划的步骤为：

(1) 选取初始化种群规模，确定迭代次数，在三维空间内随机初始化粒子的位置xi和速度vi；

(2) 给出惯性权重w和学习因子c1、c2；

(3) 计算粒子的适应值及粒子群体适应值；

(4) 调用适应度函数 fitness Trac，计算系数矩阵A，求出关节极值速度Vmax，若满足约束条件，则说明t1、t2、t3有效，否则说明当前值无效，flag 返回 false；

(5) 根据式 (4)、(5) 优化粒子的速度vi和位置xi；

(6) 判断粒子的速度vi和位置xi是否满足边界条件，若超出边界条件，则用边界值代替；

(7) 获取粒子个体最优pi和粒子群体pg，判断是否满足结束条件，若不满足，则回到步骤 2，反之则终止迭代。

3 仿真与分析

3.1 三节臂的运动学模型

笔者采用改进 D-H 法坐标系[7]建立架管机三节臂的运动学模型，忽略三节臂的外形和尺寸，将其结构进行简化，得到机械臂各个关节的坐标系，如图 3 所示。确定架管机三节臂的参数，得到 D-H 参数，如表 1 所列。

图3 三节臂连杆坐标系简图Fig.3 Sketch of coordinate system of three-section arm linkage

表1 架管机三节臂的 D-H 参数Tab.1 D-H parameters of three-section arm of pipe erector

以基座坐标系为基准，依次完成各个关节坐标系的转换，坐标系 {i=1} 到 {i} 的变换矩阵ii-1T由 4 个基础变换矩阵相乘得到，

其中

式中：ai-1表示从zi-1到zi沿xi-1测量的距离；αi-1表示从zi-1到zi沿xi-1旋转的角度；θi表示从xi-1到xi沿zi旋转的角度；di表示从xi-1到xi沿zi测量的距离。

由表 1 可知，每个相邻连杆的位置变换矩阵相乘，即可得到三节臂的末端矩阵

3.2 仿真结果分析

给出三节臂的 4 个位置，即起始点θi0、抓取点θi1、路径θi2和终止点θi3，利用 3-5-3 多项式插值进行仿真，关节空间角度插值点如表 2 所列。在架管机三节臂工作时，为了架管过程中管道平稳性和安全性，关节 1 回转盘需要处于静止状态，因此，关节 1 的 4 个位置始终为 0。

表2 关节空间角度插值点Tab.2 Interpolation points of joint space angle

根据粒子群算法最优规划步骤，利用基于粒子群的 3-5-3 多项式插值算法，确定每段插值的最优时间。确定插值的粒子种群数为 50，迭代次数为 100，粒子的惯性权重w＝0.8，学习因子c1＝c2＝2，允许的最大速度Vmax＝3 rad/s，起止点的速度和加速度都为 0，得到关节位置进化图，如图 4 所示。由图 4 可以看出，在速度约束下，4 个关节最优粒子pg位置进化曲线中，关节 2 经过 56 次迭代趋于收敛；关节 5 经过 18 次迭代后快速收敛；其余收敛均在 20～40 次之间。所以关节 5 相对平稳，说明在架管过程中末端夹管器处于稳定运动状态。

图4 关节位置进化图Fig.4 Evolution of joint position

3-5-3 多项式插值仿真轨迹曲线如图 5 所示。在给定各段时间具体插值准则的情况下。初始和终点的速度和加速度都为 0，满足运动学约束要求，其中t1=1，t2=1，t3=1；三节臂关节位置在每个时间段运动时间跨度大，速度未接近最大值，加速度在 2 s 左右出现跳变。

图5 运动仿真轨迹曲线Fig.5 Motion simulation trajectory curve

经过粒子群算法的 100 次迭代，得到每个关节的运动轨迹，与之前相比，时间整体缩短。优化后末端到达指定位置时间为 0.89 s，比优化前缩短了 2.11 s，速度在 0.21 s 达到峰值 2.97 rad/s，更接近设置的最大速度 3 rad/s。相比之前加速度的跳变，优化后的加速度曲线连续，跳变幅度更小。

基于粒子群优化算法，以 3-5-3 多项式插值算法对三节臂进行规划，其末端位置的轨迹如图 6 所示。由图 6 可以看出，曲线经过设定的 4 个位置，验证了该方法的有效性。架管机三节臂的工作空间高度可达 2.2 m，满足巷道架管的要求。

图6 规划后的轨迹Fig.6 Planned trajectory

4 结语

通过设计三节机械臂架管机，并对其进行运动学分析，运用粒子群算法对三节臂进行运动时间优化，得到了如下结论：通过轨迹规划得到三节臂末端高度为 2.2 m，满足巷道架管高度 1.7～2.6 m 的要求；通过 MATLAB 仿真得到三节臂的速度、加速度曲线，均满足插值时间点的平滑过渡，能够满足三节臂各关节在速度约束下的运动平稳性，具有较好的实用性。