基于比例导引法的机械臂拣矸过程轨迹规划方法研究

曹现刚，李 宁，王 鹏，薛祯也，费佳浩，吴旭东

(西安科技大学 机械工程学院，陕西 西安 710054)

党的十九大报告中指出：“要立足国家能源发展战略，推动煤电高质量发展转型。”能源利用率的提高，对最大限度的发挥经济和社会效益的作用至关重要。煤炭作为一种重要的能源形式，随着其消费量的不断增长，燃煤造成的环境问题愈加严重。根据资料显示，我国的原煤入选率只有78%，相比于其他一些发达国家的98%，我国处于较低水平[1，2]。而选煤是节约煤炭资源、合理利用煤炭资源的首选途径[3]。原煤中混有大块矸石，大量的煤矸石被当成废物直接处理掉，造成环境污染[4，5]。必须将之破碎成小块进行分拣，实现矸石在井下分离，节约成功，减少污染[6，7]。传统的煤炭洗选方法主要有人工选矸和机械洗选[8]。人工选矸会对工人的身心健康造成极大伤害，而且选矸效率低下。虽然目前世界各国都十分重视选煤技术与装备的研发，但是在实际生产过程中，机械洗选方法普遍存在占地面积大、设备磨损严重、运行费用高、水资源浪费严重等问题[9]。因此，为改变煤炭生产过程中落后的排矸生产方式，推动煤炭生产工业的技术进步和节能减排，煤矸石自动分选是一条有效的途径，研究高效的煤炭干选法技术，也已是迫在眉睫。

目前，工业上应用较多的煤矸石自动分选系统是以微机系统为识别处理和控制核心，相关识别装置对传送带上的煤矸石进行处理，当计算机经过处理判断出是矸石时，控制气阀打开，将传送带上自由下落的矸石吹进石仓[10]。它的优点是大大减少企业的用电负荷。许多学者对干法选煤装备控制技术进行了一系列的研究。中国矿业大学的严德崑，王洋对空气重介流化床干法选煤机的控制系统进行了一些研究，设计了极点配置控制器[11，12]。然而，世界各国对基于机器人实现在线煤矸分拣研究相对较少。本团队提出了一种多机械臂协同的煤矸分拣机器人，其系统组成如图1所示。煤矸分拣机器人主要由煤矸石识别系统、机械抓取系统和控制系统组成，其工作流程如下：首先由煤矸石识别系统实现带式输送机上运动的煤和矸石的高效识别和三维定位，机械抓取系统根据识别结果控制多个分拣机械臂实现矸石的高效、协同分拣。本文针对煤矸分拣机器人的多矸石排序问题及机械臂拣矸过程的运动轨迹规划问题进行深入研究，为进一步进行机械臂拣矸控制关键技术的研究奠定了基础。

图1 多机械臂协同煤矸分拣机器人模型

1 基于模糊逻辑推理的多目标矸石在线分拣优先级排序方法

1.1 语义变量的模糊化处理

为使机械手有序、高效的拣取多块矸石，基于识别区域识别出的待分拣矸石的属性，通过模糊逻辑推理动态确定待抓取优先级，保证优先级高的矸石优先被抓取，提高分拣效率。而待抓取矸石的优先级通常与动态矸石的三个因素有关，即识别时刻矸石与抓取区域在传送带运行方向的距离、矸石尺寸及是否发生重叠现象，具体涵义如下：

距离：指识别模块在对摄像头拍摄的每一帧图片，经过处理得出矸石实时重心坐标，可实时计算出识别时刻多块矸石在传送带运行方向分别与抓取区域线的距离，距离越短，说明矸石运动到待抓取区域的时间越短，应优先被抓取，则优先级越高。

尺寸：指识别模块识别出的各矸石的三维尺寸信息，包括矸石的长、宽、高，尺寸越大的矸石，优先被抓取，其优先级越高。

重叠：指识别模块在一次识别后，图像中各矸石是否与其它矸石之间存在重叠现象，当存在重叠现象时，为尽量使得每一块被识别出来的矸石被分拣出来，其应该被机械臂优先抓取，则优先级越高。

在模糊逻辑推理过程前需要对以上三个语义变量进行模糊化赋值，赋值结果见表1。

表1 模糊化赋值结果

1.2 模糊逻辑系统的建立

模糊逻辑系统的基本结构包括知识库、模糊推理、输入量模糊化和输出量精确化[16]。在对输入量进行模糊化处理后，本文的输入输出隶属度函数选择三角形隶属度函数。根据实际工况，设间距的变化范围为0～900mm，尺寸变化范围为80～250mm，重叠变化范围为0～1，发生重叠为1，否则为0，优先级变化范围是0～1。则优先级的隶属度函数如图2所示。

根据矸石的三个属性，本文的推理规则有18条IF—THEN规则，根据专家先验知识制定规则，如：

if(间距is近)and(尺寸is大)and(重叠is是)then(优先级is极高)。

if(间距is中)and(尺寸is中)and(重叠is是)then(优先级is中)。

if(间距is远)and(尺寸is小)and(重叠is否)then(优先级is极低)。

(a)距离隶属度函数

(b)尺寸隶属度函数

(c)重叠隶属度函数

(d)优先级隶属度函数图2 隶属度函数

1.3 仿真

为了检验基于模糊逻辑推理建立多矸石的优先级的有效性，通过matlab软件进行仿真。

矸石优先级曲面图如图3所示。当间距越大、矸石尺寸越大时，优先级越高，符合实际情况。某一矸石与抓取区域线的距离为211mm，矸石尺寸为138mm，有重叠现象发生时，模糊推理出的抓取优先级为0.658。可见，优先级的数值大小较为精确，机械臂分拣顺序一目了然。

图3 优先级曲面图

针对待分拣多矸石的排序问题，基于模糊逻辑推理的方法建立了待分拣多矸石的优先级，仿真结果表明此方法符合实际应用，具有很好的合理性和可行性。

2 比例导引追踪抓取

当采用机械手实时抓取矸石时，机械手的运动路径与分拣效率成正比。本文尝试研究将运用在导弹制导方法之一的比例导引法用在机械手跟踪抓取目标矸石上，实时规划机械手的运动路径，实现机械手的平滑运动，最终实现煤与矸石的在线分拣。

2.1 机械手与目标的相对运动方程

假设煤矸石传送带运行方向为水平向右，矸石在传送带上做匀速直线运动，如图4所示。图4中：R为机械手末端；T为运动目标。

图4 机械手与目标的相对运动

将机械手末端和运动目标的速度向量分别在视线方向上和其垂直方向上分解。得到相对距离的变化率和目标视线角的变化率方程：

θ=εr+φr(3)

θ=εt(4)

式中，dr/dt为相对距离的变化率；dθ/dt为目标视线角的变化率；Vr为机械手末端运动速度，m/s；Vt为目标运动速度，m/s；εr为机械手末端运动速度前置角，(°)；εt为目标运动速度前置角，(°)；r为机械手末端与目标之间的距离，m；φr为机械手末端运动速度方向角，(°)；θ为目标视线角，(°)。

结合比例导引方程：

由式(1)、式(2)、式(3)、式(4)以及式(5)组合便可得到机械手与目标的相对运动方程组。

2.2 机械手比例导引跟踪目标的实现步骤

实际运用比例导引法引导机械手在线跟踪抓取目标矸石时，为进一步提高抓取效率，尽量缩短机械手的动态跟踪时间，使得机械手以最短的直线路径运动，此时，视线角的变化(视线角速度)为零，即：

由此得出：

Vrsinεr-Vtsinεt=0(7)

εr=arc sin((Vtsinεt)/Vr)(8)

由式(8)知，目标的速度Vt已知，εt可求出，机械手的速度Vr已知，可求前置角εr。根据实际情况，机械手的运动是由电机驱动的，电机的启动必须有加速的过程，才能防止由于启动速度过大引起堵转。因此，机械手在加速阶段，其每一时刻的速度都在变化，即前置角εr也在不断变化，当机械手加速完匀速运动时，εr为一定值。

机械手以最短路径跟踪动态目标可按以下步骤进行：①机械手完成对上一目标抓取、放置后，控制器根据机械手关节角得到机械手当前位置，根据目标运动方程得出下一待抓取目标实时位置，求出视线角θ；②根据设定的机械手加速度方程，求出εr的变化方程；③根据εr，求出φr；④根据机械手逆运动学方程，求出各时刻关节角(位移)及加速度；⑤控制机械手按期望轨迹运动至抓取点。

2.3 仿真

为验证上述方法规划机械臂动态跟踪矸石的运动轨迹的有效性，通过matlab软件进行仿真。

仿真时，机械臂的速度均为先匀加速后匀速运动。设机械臂匀加速阶段，由速度0匀加速到2m/s，加速度为4m/s2，加速时间为0.5s。机械臂加速阶段，机械臂速度方向角在不断变化，当机械臂匀速运动时，速度方向角不变，机械臂直线运动，直到跟踪到矸石。设传送带的速度为0.7m/s，根据实际情况下机械臂与目标矸石的相对位置关系，设置以下三种场景下的机械臂动态跟踪抓取仿真。

1)追踪式抓取：指机械手的位置在传送带方向上位于目标矸石位置的后方。设机械手初始位置为(0，0.5，0)，目标初始位置为(0.5，1.5，0.4)，仿真轨迹如图5所示，抓取到时用时1.070s。

图5 追踪式抓取仿真轨迹

2)平行式抓取：指机械手的位置在传送带方向上与目标矸石位置平行。设机械手初始位置为(0，0.5，0)，目标初始位置为(0，1.5，0.4)，仿真轨迹如图6所示，抓取到时用时0.873s。

图6 平行式抓取仿真轨迹

3)拦截式抓取：指机械手的位置在传送带方向上位于目标矸石位置的前方。设机械手初始位置为(0.5，0.5，0)，目标初始位置为(0，1.5，0.4)，仿真轨迹如图7所示，抓取到时用时0.791s。

图7 拦截式抓取仿真轨迹

由图5—7可知，三种位置下的机械手运动曲线，在机械手匀加速阶段，有一小段的曲线，曲线曲率较大，弯曲程度较低，轨迹较为光滑。机械手匀速阶段，运动轨迹基本为一直线，机械手的运动时间基本最短，消耗能量最小，机械手运动稳定性高。此外，由图5—7可以看出，比例导引法用于机械手动态跟踪矸石，无论机械手与矸石的相对位置关系如何，机械手均能以最短路径跟踪到矸石，即此方法的通用性强。

根据实际工况，传送带在运动过程中，可能会发生速度的变化，致使传送带上的矸石运动速度也会跟着发生改变，即矸石的运动速度不是匀速运动，为此，以追踪式抓取为例，采用增量比例导引法，仿真六种场景下矸石速度变化时动态跟踪抓取效果。仿真时，均设置矸石加速度为1m/s2，在0.6s时，矸石速度开始变化。情形1：矸石从0.7m/s匀加速到0.8m/s，接着从0.8m/s匀减速至0.7m/s，其它时间段矸石速度不变；情形2：矸石从0.7m/s匀加速到0.8m/s，接着以0.8m/s匀速运动；情形3：矸石从0.7m/s匀减速到0.6m/s，接着以0.6m/s匀速运动；情形4：矸石速度从0.7m/s直接变为0.8m/s，之后以0.8m/s速度匀速运动；情形5：矸石速度从0.7m/s直接变为0.9m/s，之后以0.9m/s匀速运动；情形6：矸石速度从0.7m/s直接变为0.6m/s，之后以0.6m/s匀速运动。结果见表2。

表2 传送带速度变化时仿真结果

由表2可知，当目标矸石的速度变化情况为匀变速，即前三种情形时，机械手的运动轨迹无明显变化，运动时间的差别大小在0.02s内。当目标矸石的速度变化情况为突变，即后三种情形时，机械手的运动轨迹跟随目标矸石速度变化有微小变化，变化段曲线的曲率较大，弯曲不明显，几乎为直线，运动时间与矸石速度无变化时相比，差别大小在0.03s内。因此，由以上结果可得出，比例导引法用于机械手动态跟踪抓取目标矸石，当目标矸石速度变化时，机械手运动轨迹无明显变化，几乎不受运动目标速度变化的影响，运动轨迹平滑，抓取时间变化较小，动态跟踪效果良好。

3 结 语

1)提出了一种基于模糊逻辑推理方法的在线多目标矸石优先级排序模型，保证了机械臂有序、高效、合理的分拣多块矸石。结果表明此方法具有很好的可行性。

2)建立了基于比例导引方法的机械臂运动轨迹规划模型，给出了机械臂实现跟踪抓取的轨迹规划步骤。仿真结果表明无论机械臂与矸石位置如何，机械臂均能够平滑、快速跟踪到矸石，此方法通用性较强。

3)采用增量比例导引法代替比例导引法规划机械臂运动轨迹。实验结果表明机械臂跟踪效果好，基本不受目标矸石速度变化影响。