并联式选矸机器人路径规划研究

2022-09-16黄金凤张建喜于江涛苗术佶

工矿自动化 2022年8期

黄金凤，张建喜，于江涛，苗术佶

（1. 华北理工大学机械工程学院，河北唐山 063210；2. 河北省工业机器人产业技术研究院，河北唐山 063000；3. 唐山因泰智能科技发展有限公司，河北唐山 063000）

0 引言

传统的选矸方法主要有人工手选和机械分选[1]。人工手选存在劳动强度大、效率低等问题，且长时间选矸作业会对工人健康造成威胁。机械分选包括跳汰选煤、重介选煤、风选等[2]，这些方法有效降低了人工成本，但均存在环保问题，不符合煤矿绿色发展理念[3-4]。煤炭分选智能化系统是煤矿十大智能化系统之一[5-6]，实现可靠、高效、绿色的煤矸石自动分选具有现实意义。为了实现煤矸石自动化分选，应用机械手完成选矸作业已成为发展趋势[7]。

当机械手末端执行器按不同路径进行作业时，由于路径空间维度不同，导致机械手完成1次完整作业所用时间不同。在不超过驱动电动机额定转矩的情况下，选矸机械手完成1次完整作业的时间长短直接决定了其选矸效率。因此，探究机械手最优作业路径十分必要。

目前，在煤矸石自动分选领域已有不少研究成果。文献[8]采用AUBO-i5机器人进行煤矸石分选工作，并基于G-RRT*算法对机器人进行路径规划。文献[9-12]利用龙门式机械手实现煤矸石分选，并设计了多个动态目标下的多臂协同控制策略。文献[13-15]提出了龙门式矸石分选机器人动态拣取及避障路径规划算法，实现了机器人在线路径规划。上述文献中采用的煤矸石分选机械手多为串联式。与串联机械手相比，并联机械手中的电动机和减速机在高速作业过程中无需随机械手做高速运动，大大降低了高速运动部件的质量，使其能够在短时间内达到较高的运动速度，即增强了其高速特性。此外，并联机械手的作业负荷由不止1条运动支链承担，提高了其负载能力，即增强了其高速重载特性。因此，并联机械手更适用于大质量煤矸石分选，本文对并联式选矸机器人作业路径进行研究，对“梯”型路径规划与“V”型路径规划2种动态分选路径规划方法进行对比分析。

1 并联式选矸机器人系统

并联式选矸机器人系统主要由排队机构、视觉系统、控制系统和执行机构（并联机械手）组成，如图1所示。

图 1 并联式选矸机器人系统Fig. 1 Parallel gangue selection robot system

大量煤和矸石的混合物被传输到矸石分选输送带上，随输送带一起运动。排队机构对混合物进行队列修正，使混合物排成2列并处于标准线两侧，煤和矸石之间的最小距离为100 mm。视觉系统将识别到的煤矸石信息传输至控制系统。控制系统输出指令至并联机械手，并联机械手完成矸石拨出任务，从而达到煤矸分选目的。煤矸分选过程如图2所示。

图 2 煤矸分选过程Fig. 2 Coal and gangue selection process

并联机械手结构如图3所示。横向采用基于五杆机构的二自由度平面并联机构，纵向由电动机驱动丝杠并带动并联机构运动[16-17]。

图 3 并联机械手结构Fig. 3 The structure of parallel manipulator

2 并联机械手路径规划

机械手路径规划是指在有障碍物的环境中，根据任务要求，按时间最短、路径最短、耗能最少的评判条件，寻找出一条从起始位姿到目标状态的无碰撞最优路径[18-19]。机械手推煤矸石任务中包括3种运动：① 机械手末端执行器将煤矸石推出输送带的推程运动；② 机械手末端执行器避障回归起点的回程运动；③ 机械手追踪输送带的运动，以保证与煤矸石在输送带运动方向相对静止。由于每块煤矸石的质量与位置不同，为了减少控制系统计算量，加快处理速度，设定每次推程运动前进的距离为400 mm。

2.1 “梯”型路径规划

当矸石运动到机械手工作空间后，机械手末端执行器将矸石推出输送带。推出矸石之后，机械手末端执行器位于输送带边缘，为了进行后续推矸石的运动，机械手末端执行器需要回到初始位置。如果机械手末端执行器回程路径与推出路径相同，其碰到输送带上煤块的概率很大，从而可能破坏队列，使视觉系统给出的目标物体信息发生变化。因此，机械手末端执行器回程运动时要杜绝与煤块发生接触。在这种情况下，通常的处理方式是将机械手末端执行器向上抬起一定距离，再完成回程运动，即按“梯”型路径运动。“梯”型路径如图4所示，（x,y）为以基座初始位置为原点的坐标。

图 4 “梯”型路径Fig. 4 "Ladder" path

机械手末端执行器从A点出发，经过B，C，D点后回到A点。其中AB段沿x轴的位移为400 mm，沿z轴（输送带运动方向）的位移与输送带位移相同；BC段和DA段沿x轴的位移为50 mm，沿y轴的位移为300 mm，沿z轴的位移为0；CD段沿x轴的位移为300 mm，沿z轴的位移与AB段沿z轴的位移相等。AB段为将煤或矸石推出输送带的工作路径，在AB段，通过机械手的驱动电动机驱动丝杆，保证机械手末端执行器与煤矸石在输送带运动方向相对静止。

为了避免机械手内部产生惯性力冲击，当机械手进行高速运动时，其在起点与终点时的加速度应为0，整体加速度曲线应呈连续变化。

在AB，BC，CD，DA路径上，机械手均按照统一的规律运动，位移函数为

式中：s为位移；b0-b5为多项式系数；t为时间。

对式（1）进行求导，得到速度v与加速度a：

由式（2）可知，加速度计算公式为三次多项式，加速度不存在突变。因此，机械手的惯性力也是连续变化的，不存在机械手内部惯性力冲击。

为简便计算，对变量作如下定义：以AB段为例，设位移为sab，沿x轴的位移为sxab，沿y轴的位移为syab，运动时间为tab。其他路段类似变量定义依此类推。

机械手在初始位置（A点）时，有

机械手在末端位置（B点）时，有

将式（3）、式（4）代入式（1）、式（2），可得机械手末端执行器在AB段沿x轴的位移：

式中amax为机械手末端执行器的最大加速度。

同理可得机械手末端执行器在BC段(tab≤t＜tbc)的位移：

式中axmax，aymax分别为机械手末端执行器在x,y轴方向的最大加速度。

机械手末端执行器在CD段沿x轴的位移为

机械手末端执行器在BC段的位移为

2.2 “V”型路径规划

选矸机器人利用输送带速度推断煤矸石到达机械手工作空间的时刻，控制机械手末端执行器按编号依次完成推矸石任务。“V”型路径如图5所示，机械手末端执行器的位置变化为。其中EF段沿x轴的位移为400 mm，沿z轴的位移与输送带位移相同；FG段沿x轴的位移为350 mm，沿z轴的位移与输送带位移相同；GH段沿x轴的位移为50 mm，沿z轴的位移由下一块待处理矸石的位置决定。

图 5 “V”型路径Fig. 5 "V" path

设1号矸石进入机械手工作空间的时刻为t0，则末端执行器自t0时刻开始运动，其在z轴方向的运动速度与输送带速度一致，在x轴方向进行运动，直至将矸石推出输送带，在图5中表示为E→F路线。在回程运动中，考虑到直接退回可能会破坏队列顺序，设定机械手在z轴方向的运动速度仍与输送带速度一致，直至末端执行器回到标准线内，在图5中表示为F→G路线。标准线内无障碍物，因此无需考虑避障问题，为了能够快速到达推2号矸石的位置，当末端执行器进入标准线内后，快速运动到与2号矸石平行的位置，在图5中表示为G→H路线。以末端执行器开始运动到与下一目标矸石平行为1个运动周期，因其路径似字母“V”，称为“V”型路径。

机械手末端执行器在EF，FG，GH段沿x轴的位移分别为

3 并联机械手动力学模型

考虑到机械手的使用寿命，要求将驱动电动机输出转矩控制在电动机额定转矩以下。由于机械手末端执行器路径有“梯”型路径与“V”型路径2种，且2种路径规划的空间维度存在差异，导致驱动电动机转矩变化情况不同。因此，通过建立并联机械手动力学模型，探究机械手在不同路径运动时驱动电动机转矩的变化情况，计算在转矩满足要求的前提下，沿2种路径完成1个运动周期所用的时间，进而对比机械手沿2种路径的作业效率。驱动电动机选用HG-RR353伺服电动机，额定转矩为11.1 N·m。

3.1 机械手运动学逆解

在进行机构动力学分析时，机构简图是重要的理论基础。机械手机构简图如图6所示。

图 6 机械手机构简图Fig. 6 Schematic diagram of manipulator mechanism

由图6可得机械手逆向运动学方程：

式中：r为机械手末端执行器运动点O'的位置矢量，为起点O到两边转动副旋转中心的距离；l1，l2分别为主从臂的臂长；ui，wi分别为主从臂方向的单位矢量；θ1i和 θ2i分别为主从臂的旋转角度。

对式（12）等号两边分别取模的平方，得到三角方程：

根据装配模式求解可得

用式（12）对时间求导可得

式中：V为动平台相对静平台的速度；与分别为第i个驱动支链中主从臂的角速度；Q为定常矩阵，

将式（21）两端左乘，结合得

将式（22）写为矩阵形式，得

同理，将式（21）两端左乘，结合得

将式（27）写为矩阵形式，得

用式（21）对时间求导，并将两端左乘，得

3.2 机械手动力学分析

根据机械手作业时的实际情况，分析机械手运动时的受力状态，将系统做功分为动平台与矸石对系统做功（W1）、主动臂对系统做功（W2）、从动臂对系统做功（W3）3个部分，建立虚功方程。式中：md为动平台质量；g为重力加速度；f0为目标物体作用在机械手上的力；δR为动平台的虚位移。

式中：T为主动臂的转动力矩；Ia为主动臂关于其主动铰接点的转动惯量，包含减速器和伺服电动机折算到主动臂的转动惯量；为主动臂的角加速度，为主动臂关于其主动铰接点的重力矩为2个主动臂的角度虚位移；m1为主动臂质量；c1为主动臂总长度的一半。

式中：m2为从动臂质量；a21为1号主从臂铰接点的加速度；mp为主从臂铰接点质量；δR21为1号主从臂铰接点的虚位移；a22为2号主从臂铰接点的加速度；δR22为2号主从臂铰接点的虚位移；Ib，Ip为从动臂关于主从臂铰接点的转动惯量；为从动臂角加速度为从动臂的角度虚位移。

根据虚功原理，有

计算可得

4 仿真分析及现场验证

4.1 仿真分析

通过Matlab进行仿真模拟，获得机械手驱动电动机转矩变化曲线。采用“梯”型路径、“V”型路径2种情况下的电动机转矩变化如图7所示。

图 7 机械手驱动电动机转矩变化曲线Fig. 7 Torque curves of drive motor of manipulator

由图7（a）可知，采用“梯”型路径时，在满足电动机转矩要求的条件下，完成1个运动周期所用时间为1.2 s。针对每一块煤矸石，机械手末端执行器都要执行推出→抬升→回程→下降运动，在一定程度上造成了时间浪费，从而影响了煤矸分选效率。由图7（b）可知，采用“V”型路径时，在满足电动机转矩要求的条件下，完成1个运动周期所用时间为0.65 s，比采用“梯”型路径时缩短了将近一半时间。

在机械手运行过程中，驱动电动机转矩和电流成正比，在转矩需求相同的前提下，采用“V”型路径所用时间较短，故在功耗相同的情况下，选矸效率较高。