煤矸石分拣机器人动态目标稳定抓取轨迹规划

马宏伟，孙那新，张烨，王鹏，曹现刚，夏晶

（1.西安科技大学 机械工程学院，陕西 西安 710054；2.陕西省矿山机电装备智能监测重点实验室，陕西 西安 710054）

0 引言

煤和矸石分离是煤炭清洁能源生产和利用必不可少的工序[1]。依靠人工完成煤矸石分拣工作存在效率低、劳动强度大等问题[2-3]。机器人、人工智能、计算机等技术为煤矿智能化提供了新的契机[4-5]。利用机器替人分拣煤矸石可降低劳动成本，提高分拣效率。目前，利用机器人进行动态目标跟踪抓取以点到点（Point to Point，PTP）[6]、比例导引[7]、金字塔寻优[8]等方法为主，这些方法能够快速跟踪运动目标并对其进行抓取，在机械臂末端达到抓取点时只需确保位置精度，无需速度同步。但是对于机器人分拣煤矸石来讲，矸石随带式输送机运动具有高速度特性，且矸石平均质量大，如果机械臂末端在抓取时只要求位置同步而对速度不做要求，则会因目标和机械臂末端之间速度差产生载荷冲击，严重时会造成机械臂末端损坏，降低机器人安全性和可靠性。同时，带式输送机在运输过程中存在打滑和跑偏问题，这对矸石定位提出了更高要求[9-11]。因此，研究一种适应大质量、高速度的精确跟踪方法是目前机械臂末端精确稳定抓取矸石的关键所在。

目标定位和轨迹规划对机器人的工作效率、运动稳定性和能量消耗具有重大影响，是机器人运动控制的基础[12]。许多学者对分拣机器人在高速高频作业时跟踪动态目标的运动轨迹和运动规律进行了研究。王铮等[13]通过控制传送带速度，预测目标物体位置，实现了一种基于位置预测的拦截抓取，在保证效率的同时降低了漏抓率，但未考虑垂直胶带运动方向的目标位置偏差及抓取时刻的冲击问题。曹现刚等[14]采用比例导引法实现在矸石匀速状态下的动态目标轨迹规划，进一步采用增量比例导引法实现矸石非匀速运动下动态跟踪过程的机械臂运动轨迹规划，但是煤矸石运动过程属于大质量高速运动，此方法在抓取时刻由于速度及运动方向不同，易导致机械臂末端与矸石发生冲击。王鹏等[15]通过余弦定理快速到达目标矸石理论抓取点，再利用PID 进行跟踪，能够实现煤矸石分拣机器人以最优时间进行大质量、动态目标的稳准抓取，但在抓取精度上未能进行误差分析。

针对上述问题，本文结合煤矸石分拣机器人实际工况，提出了一种基于机器视觉的煤矸石分拣机器人动态目标稳定抓取轨迹规划方法。首先，结合机器人系统，利用基于HU 不变矩的目标匹配算法实现视觉识别系统与视觉伺服系统信息传递，完成视觉伺服跟踪目标捕获；其次，通过建立相机-机器人运动学模型，实现运动目标精确定位；最后，通过位置-速度-加速度三环PID 控制算法实现机械臂末端稳准抓取矸石。

1 煤矸石分拣机器人系统

煤矸石分拣机器人系统由煤矸石识别系统、机械臂分拣系统、上位PC 机、机器人控制器等组成，如图1 所示。当目标矸石进入机械臂工作空间后，通过煤矸石识别系统对目标矸石进行匹配识别并获取目标矸石位姿，将获取的目标矸石信息发送给上位PC 机。上位PC 机通过跟踪算法进行各轴机械臂运动增量轨迹计算，将计算得到的轨迹坐标点数据通过以太网通信方式发送至机器人控制器，控制机器人完成对目标轨迹的跟踪和抓取。

图1 煤矸石分拣机器人系统组成Fig.1 Composition of coal gangue sorting robot system

采用双目立体相机获取目标矸石的像素坐标，并结合坐标系转换关系得到机器人坐标系下的目标矸石坐标，实现矸石实时定位；通过三环PID 控制算法结合机器人的位姿转换关系实现机械臂对目标矸石的同步跟踪抓取。具体流程如图2 所示。

图2 动态目标稳定抓取轨迹控制流程Fig.2 Dynamic target stable grasping track control process

2 基于HU 不变矩的目标匹配

对目标矸石质心进行识别是实现精准跟踪抓取的基础。机械臂末端在接近目标矸石过程中，通过相机对输送带上的目标矸石进行图像采集，图像采集过程中会产生摄像头所在的视角及物体本身的移动[16]，导致图像变化（如旋转和尺度的变化），引起图像像素变化。HU 不变矩具有平移、旋转和尺度不变性，以及高稳定性和速度快等优点，因此，选择HU 不变矩作为特征向量来实现目标矸石识别。基于HU 不变矩的目标匹配流程如图3 所示。利用相机获取煤矸石分拣机器人当前位姿下所对应的煤矸石图像，对模板图像（已知）和采集到的图像进行图像预处理后，获取最小封闭轮廓。以最小封闭轮廓为输入，分别计算HU 不变矩特征向量，以欧氏距离作为目标矸石与模板图像的相似性测量函数，计算采样图像轮廓与模板图像的相似性测量函数值。如相似性测量函数值不小于阈值，则继续循环采样，否则判断为目标矸石轮廓。采用最小外接矩形对得到的目标轮廓进行处理，得到目标矸石位置，实现目标矸石位姿确定。

图3 基于HU 不变矩的动态目标匹配流程Fig.3 Flow chart of dynamic target matching based on HU moment invariants

为获取目标矸石在机器人坐标系下质心坐标，本文在煤矸石分拣机器人实验平台上进行煤矸石图像的动态采集，以所采集图像为例，进行目标矸石匹配识别及质心获取。采集的图像分辨率为1 920×1 080，图像尺寸为960 mm×540 mm。安装相机时，相机坐标系与机械臂末端坐标系之间相对固定，通过机器人控制器获取机械臂末端位姿信息。

2.1 目标矸石边缘特征提取

为更好地提取图像边缘特征，需先对图像进行预处理，再进行边缘提取[17-18]。

（1）对采集到的RGB 图像进行灰度转换，减少计算量，提升图像识别速度。

（2）对灰度图像进行中值滤波去噪操作，降低图像干扰，提高图像识别准确度。

（3）对经过去噪处理后的图像进行二值化处理，将感兴趣的目标和背景分离。

（4）进行形态学运算子处理，填充空洞，去除孤立杂点，平滑边界。

（5）对滤波后图像使用Canny 算子提取边缘，跟踪边缘得到外轮廓图像。

2.2 目标匹配

提取目标矸石轮廓后，采用HU 不变矩特征进行图像的匹配识别。物体可通过HU 不变矩特征对图像进行分类等操作。像素尺寸为G×B的二维离散图像g（x，y）的p+q（p，q=0，1，…）阶几何矩Hpq和中心矩ηpq可表示为

利用中心矩的构造即可实现平移不变性。为抵消尺度变化对中心矩的影响，对中心矩进行归一化处理，得到归一化中心矩：

式中 η00为零阶中心距。

利用二阶归一化中心距 μ20，μ02，μ11和三阶归一化中心矩 μ30，μ21，μ12，μ03推导对平移、缩放、镜像和旋转都不敏感的7 个二维不变矩I1-I7，则[I1I2···I7]为HU 不变矩的特征向量。

在目标匹配过程中，以欧氏距离作为搜索窗内图像特征与目标矸石图像匹配程度的客观表征量，可表示为

式中：Dn为第n个搜索窗与模板特征向量的欧氏距离，n为搜索匹配窗的标号；F(e)为模板图像HU 不变矩特征向量的第e个不变矩；Fn(e)为第n个搜索窗的HU 不变矩特征向量的第e个不变矩；Dmin为最小欧氏距离，对应当前图像中相似度最大的匹配窗。

2.3 动态目标质心获取

在完成目标匹配后，采用最小外接矩形对得到的目标轮廓进行红框标志，得到目标矸石位置。目标矸石形心像素坐标（X，Y）为

式中：umax，umin分别为最小外接矩形在像素坐标系（ouv）u轴方向的最大、最小像素值；vmax，vmin分别为最小外接矩形在像素坐标系v轴方向的最大、最小像素值。

目标矸石匹配结果如图4 所示。以左上角为像素坐标系ouv原点o，以水平向右为u轴正向，以竖直向下为v轴正向。左相机目标矸石形心像素坐标为（1 098 mm，355 mm），右相机目标矸石形心像素坐标为（869 mm，355 mm）。为获取相机坐标系下目标矸石位置，对左右两侧相机获取的目标矸石形心像素坐标进行像素坐标到图像坐标的转换，可得目标矸石形心在左右两侧图像坐标系下的坐标分别为（224.96 mm，-389.58 mm），（-222.75 mm，-389.58 mm）。根据图像坐标进行双目测距原理计算，可得目标矸石到相机坐标系的垂直距离为555.3 mm。以右侧像素坐标为基础，结合目标矸石到相机坐标系的垂直距离，根据图像坐标系与相机坐标系之间转换关系，可得目标矸石质心在右侧相机坐标系下的坐标为（-57.64 mm，-100.84 mm，555.30 mm）。机械臂与相机之间的转换关系由相机安装方式决定，将该点坐标信息发送至被控机械臂，完成机械臂末端对目标矸石的精确跟踪。

3 相机-机器人运动学分析

3.1 煤矸石分拣机器人运动学分析

煤矸石分拣机器人根据获取的目标矸石绝对位置坐标进行轨迹规划，轨迹规划前需要对煤矸石分拣机器人进行运动学分析。采用D-H 参数法[19]建立运动学模型，运用4×4 的齐次变换矩阵代替复杂的运动学计算，煤矸石分拣机器人运动学坐标系模型如图5 所示，其中机器人坐标系为OWXWYWZW、滑轨坐标系为O1X1Y1Z1、机械臂坐标系为O2X2Y2Z2、机械臂末端坐标系为OEXEYEZE。

煤矸石分拣机器人运动结构参数见表1。其中，为坐标系OWXWYWZW变换至坐标系O1X1Y1Z1的转换矩阵，α1为绕XW轴转动量，α1=0，l1为 沿XW轴平移量，d1为沿ZW轴平移量，θ1为绕ZW轴转动量，θ1=0；为坐标系O1X1Y1Z1变换至坐标系O2X2Y2Z2的转换矩阵，α2为绕X1轴转动量，α2=-90°，l2为沿X1轴平移量，l2=0，d2为沿Z1轴平移量，θ2为绕Z1轴转动量，θ2=90°；为坐标系O2X2Y2Z2变换至坐标系OEXEYEZE的转换矩阵，α3为绕X2轴转动量，α3=-90，l3为沿X2轴平移量，d3为沿Z2轴平移量，θ3为绕Z2轴转动量。

表1 煤矸石分拣机器人运动结构参数Table 1 Motion structure parameters of coal gangue sorting robot

根据D-H 参数法建立机械臂末端坐标系相对机器人坐标系的正运动学模型，并将α1=0，α2=-90°，α3=-90°，l2=0，θ1=0，θ2=90°代入正运动学模型，可得正运动学方程：

通过逆运动学方程可完成机械臂末端到达期望位置的运动结构参数转换，从而驱动煤矸石分拣机器人运动。

3.2 相机-机器人转换关系分析

为实现矸石精确抓取，需通过相机获取目标矸石实时位置，并将得到的像素坐标转换为机器人坐标[20]。机器人坐标系、相机坐标系、图像坐标系及像素坐标系之间的转换关系如图6 所示。其中分别为相机坐标系相对于机械臂末端坐标系的旋转矩阵和平移矩阵，分别为机械臂末端坐标系相对于机器人坐标系的旋转矩阵和平移矩阵，i为相机焦距，即像素坐标系与相机坐标系的原点距。

图6 坐标系转换关系Fig.6 Coordinate system transformation diagram

图6 中，点P在机器人坐标系下的坐标与在像素坐标系下坐标之间的转换矩阵为

式中PC为点P在相机坐标系下的坐标。

4 基于视觉伺服三环PID 动态目标跟踪

为实现机械臂末端快速跟踪和稳定抓取，采用三环PID 控制算法[21]进行目标矸石动态跟踪。三环PID 控制算法原理是以位置环控制器的输入为期望位置，即机械臂上摄像头捕捉到目标矸石的当前位置，位置环控制器的输出作为速度环控制器的输入，速度环控制器的输出作为加速度环控制器的输入，将加速度环控制器的输出叠加到伺服电动机上，共同作用控制机械臂运动[22]，使机械臂末端与目标矸石达到位置、速度同步运动的效果，实现平稳快速抓取。对返回的位置信息进行位置式PID 控制（由比例控制、积分控制、微分控制3 部分合成）。在位置式PID 控制的基础上分别扩展加速度、速度、位置控制表达式。

式中：A（k），V（k），S（k）分别为第k次加速度、速度、位置控制调整量；KP，KI，KD分别为比例、积分、微分控制系数；ΔAk，ΔVk，ΔSk分别为第k次加速度、速度、位置偏差；ΔAsum，ΔVsum，ΔSsum分别为累计末端的加速度、速度、位置偏差；ΔA′，ΔV′，ΔS′分别为末端加速度、速度、位置偏差。

式中：Sk为第k次机械臂末端位置；Slast为上一次机械臂末端的位置；ΔSlast为上一次机械臂末端位置偏差。

式中：Vk为第k次机械臂末端速度；Vlast为上一次机械臂末端的速度；ΔVlast为上一次机械臂末端速度偏差；Δt为时间步长；Vmax为最大速度。

式中：Ak为第k次机械臂末端加速度；Alast为上一次机械臂末端的加速度；ΔAlast为上一次机械臂末端加速度偏差；Amax为最大加速度。

5 仿真及实验验证

5.1 轨迹规划仿真

根据煤矸石分拣机器人实验平台实际情况，设置机器人最大速度为4.2 m/s，最大加速度为10 m/s2。目标矸石运动速度为0.9 m/s。将三环PID 控制算法与比例导引算法、偏置比例导引算法[23]进行对比分析。用Matlab 软件对动态目标跟踪抓取在追随式（机械臂末端起始位置在目标矸石起始位置斜后方）、同步式（机械臂末端起始位置和目标矸石起始位置在X轴重合）、拦截式（机械臂末端起始位置在目标矸石起始位置斜前方）3 种情况下的轨迹规划进行动态仿真分析，3 种算法运动仿真参数见表2，分析结果如图7-图18 所示。①在追随式轨迹下目标矸石随输送带沿X轴正方向运动，目标矸石初始位置为（0.5，0.5，0），机械臂末端初始位置为（0，0，0.4），机械臂末端位于目标矸石运动方向的斜后方。② 在同步式轨迹下目标矸石随输送带沿X轴正方向运动，目标矸石初始位置为（0，0.5，0），机械臂末端初始位置为（0，0，0.4），机械臂末端位于目标矸石运动方向的正后上方。③在拦截式轨迹下目标矸石随输送带沿X轴负方向运动，目标矸石初始位置为（0.5，0.5，0），机械臂末端初始位置为（0，0，0.4），机械臂末端位于目标矸石运动方向的斜前方。

表2 3 种算法运动仿真参数Table 2 3 kinds of algorithm motion simulation parameters

从图7、图11、图15 可看出，三环PID 控制运动轨迹和偏置比例导引运动轨迹在机械臂抓取时刻与目标矸石运动方向相同，而比例导引运动轨迹在机械臂抓取时刻与目标矸石运动方向存在一定角度。仿真结果显示，3 种算法跟踪位置偏差控制在1 mm左右，偏差较小，均满足位置偏差要求。目标矸石沿X轴正方向匀速运动，因此以目标矸石在X轴向速度为0.9 m/s，在Y，Z轴向的速度及X，Y，Z轴向的加速度均为0 作为目标值。从图8-图10、图12-图14、图16-图18 可看出，只有三环PID 控制运动轨迹各轴向速度、加速度在抓取点完全趋近于目标矸石，即实现了同步跟踪，且时间最短；而偏置比例导引运动轨迹速度、加速度只在Y轴和Z轴2 个方向趋近目标值，在X轴向速度和加速度均未趋近目标值，即同向不同速；比例导引运动轨迹各轴向速度、加速度均未与目标矸石达到一致。

图7 追随式3 种算法动态目标轨迹规划曲线Fig.7 Dynamic target track planning curves of three algorithms under following track planning mode

图8 追随式3 种算法X 轴向位置、速度、加速度变化曲线Fig.8 X axial position,velocity and acceleration curves of three algorithms under following track planning mode

图9 追随式3 种算法Y 轴向位置、速度、加速度变化曲线Fig.9 Y axial position,velocity and acceleration curves of three algorithms under following track planning mode

图10 追随式3 种算法Z 轴向位置、速度、加速度变化曲线Fig.10 Z axial position,velocity and acceleration curves of three algorithms under following track planning mode

图11 同步式3 种算法动态目标轨迹规划曲线Fig.11 Dynamic target track planning curves of three algorithms under synchronous track planning mode

图12 同步式3 种算法X 轴向位置、速度、加速度变化曲线Fig.12 X axial position,velocity and acceleration curves of three algorithms under synchronous track planning mode

图13 同步式3 种算法Y 轴向位置、速度、加速度变化曲线Fig.13 Y axial position,velocity and acceleration curves of three algorithms under synchronous track planning mode

图14 同步式3 种算法Z 轴向位置、速度、加速度变化曲线Fig.14 Z axial position,velocity and acceleration curves of three algorithms under synchronous track planning mode

图15 拦截式3 种算法动态目标轨迹规划曲线Fig.15 Dynamic target track planning curves of three algorithms under intercepting track planning mode

图16 拦截式3 种算法X 轴向位置、速度、加速度变化曲线Fig.16 X axial position,velocity and acceleration curves of three algorithms under intercepting track planning mode

图17 拦截式3 种算法Y 轴向位置、速度、加速度变化曲线Fig.17 Y axial position,velocity and acceleration curves of three algorithms under intercepting track planning mode

图18 拦截式3 种算法Z 轴向位置、速度、加速度变化曲线Fig.18 Z axial position,velocity and acceleration curves of three algorithms under intercepting track planning mode

在整个过程中，三环PID 控制运动轨迹在各轴速度、加速度上无突变现象，且没有出现超限现象，而偏置比例导引运动轨迹和比例导引运动轨迹在各轴加速度曲线上均出现突变现象，且加速度存在超限现象，这样会导致机械臂在运动过程中发生振荡。

综上，三环PID 控制算法在追随式、同步式和拦截式3 种情况下响应时间、跟踪抓取时间均较比例导引算法及偏置比例导引算法短，且三环PID 控制算法在整个过程中各轴速度、加速度连续、平滑，没有出现突变情况，状态平稳，没有大幅度振荡，适应性强，可实现同步跟踪和精准抓取。

5.2 实验验证

煤矸石分拣机器人实验平台如图19 所示。本实验煤矸石分拣机器人为桁架式结构，相机安装在机械臂上，机器人可实现机械臂在空间X，Y，Z方向的移动及机械臂末端绕Z轴的转动。

图19 煤矸石分拣机器人实验平台Fig.19 Experimental platform of coal gangue sorting robot

为验证三环PID 控制算法的适应性、高效性，结合双目相机所获得的目标矸石位置信息，在已有的双机械臂煤矸石分拣系统平台上应用三环PID 控制算法、比例导引算法和偏置比例导引算法进行适应性实验，结果见表3。其中，ΔX，ΔY，ΔZ为目标矸石与机械臂末端三轴向起始位置差，v为输送带速度，tPID，tP，tB分别为三环PID 控制算法、比例导引算法和偏置比例导引算法的平均抓取时间，ESPID，ESB，ESP分别为三环PID 控制算法、比例导引算法和偏置比例导引算法的抓取时刻平均位置偏差，EVPID，EVB，EVP分别为三环PID 控制算法、比例导引算法和偏置比例导引算法的抓取时刻平均速度偏差。

表3 3 种算法实验结果Table 3 Experimental results of three algorithms

3 种算法在机器人运行时各个关节均未超限，同时由表3 可看出，三环PID 控制算法完成抓取的平均时间比比例导引算法和偏置比例导引算法短；三环PID 控制算法的位置偏差与其他2 种算法相差不大，都控制在1 mm 左右；输送带速度的变化对3 种算法的跟踪抓取时间影响不大。比例导引算法和偏置比例导引算法在抓取点的平均速度偏差在2 m/s 左右，并未实现同速，而三环PID 控制算法在抓取点的平均速度偏差在1 mm/s 左右，跟踪速度偏差较小，可满足对高速度运动目标的同步跟踪、精准抓取要求。

6 结论

针对机器人稳定分拣煤矸石问题，提出了一种基于机器视觉的煤矸石分拣机器人动态目标稳定抓取轨迹规划方法。该方法通过视觉获取煤矸石实时位置，并通过三环PID 控制算法进行机器人轨迹规划，实现了机器人分拣系统的全闭环控制。结论如下：

（1）通过HU 不变矩对目标矸石进行目标匹配和精确定位，有效解决了煤矸石在传送过程中因输送带打滑、左右摆动而造成的目标定位不准确问题，实现快速目标定位，提高目标位置的精确性。

（2）用Matlab 软件对实际工况中的追随式、同步式、拦截式3 种情况轨迹规划进行动态仿真分析，结果表明：三环PID 控制算法在追随式、同步式和拦截式3 种情况下响应时间、跟踪抓取时间均较比例导引算法及偏置比例导引算法的时间短，且三环PID 控制算法在整个过程中各轴速度、加速度连续、平滑，没有出现突变情况，状态平稳，没有大幅度的振荡，适应性强，可实现同步跟踪、精准抓取。

（3）对三环PID 控制算法、比例导引法和偏置比例导引算法进行了实验验证，结果表明：3 种算法在规划控制机器人运行过程中，均未出现机器人关节超限现象；三环PID 控制算法完成抓取的平均时间比比例导引算法和偏置比例导引算法短；三环PID 控制算法在抓取点的平均速度偏差在1 mm/s 左右，跟踪速度偏差较小，可满足对高速度目标的同步跟踪、精准抓取要求。