煤矸石分拣机械臂的轨迹跟踪控制研究

Research on Trajectory Tracking Control for Coal Gangue Sorting Robot Arm

摘要: 煤矸石分拣机械臂在捡取过程中，带式输送机上煤矸石抖振等外部因素对机械臂末端形成冲击，导致机械臂控制系统出现不确定性问题，造成机械臂抓取煤矸石时控制性能下降、轨迹跟踪误差偏大；为此，在传统滑模控制的基础上，改进出一种RBF神经网络的切换增益调节滑模控制方法。根据拉格朗日理论推导出煤矸石分拣机械臂动力学数学模型，针对模型中不确定部分，结合滑模控制算法与RBF神经网络，利用RBF神经网络来调节滑模控制中的切换增益，使得切换增益能够随着不确定部分改变而改变，同时减弱传统滑膜控制中的抖振问题。采用MATLAB软件，进行所述控制器仿真实验，分析机械臂关节角度跟踪效果及切换增益调节响应时间。仿真结果表明：本文所述控制方法可以更快地贴近期望轨迹值，同时跟踪误差也得到降低。搭建实验平台，在煤矸石分拣机械臂上展开实验，模拟分拣煤矸石几字形路径。根据激光标定仪反馈数据得出：该控制方法可以保证机械臂运动的合理性和安全性，能够实现较短时间内收敛达到煤矸石分拣过程中的期望轨迹，符合煤矸石分拣的实际工况需求。

Abstract: During the picking process of the coal gangue sorting robotic arm, external factors such as buffeting of coal gangue on the belt conveyor impact the end of the manipulator, which leads to the uncertainty of the control system of the manipulator, and causes the control system when the manipulator grabs the coal gangue. The performance is degraded and the trajectory tracking error is too large; for this reason, based on the traditional sliding mode control, an RBF neural network switching gain adjustment sliding mode control method is improved. According to the Lagrangian theory, the dynamic mathematical model of the coal gangue sorting manipulator is deduced. For the uncertain part of the model, the sliding mode control algorithm and the RBF neural network are combined, and the RBF neural network is used to adjust the switching gain in the sliding mode control. The switching gain can be changed with the change of the uncertain part, while reducing the chattering problem in the traditional synovial control. Using MATLAB software, the controller simulation experiment was carried out, and the tracking effect of the joint angle of the manipulator and the response time of the switching gain adjustment were analyzed. The simulation results show that the control method described in this paper can approach the desired trajectory value faster, and the tracking error is also reduced. Build an experimental platform, carry out experiments on the coal gangue sorting robotic arm, and simulate the zigzag path of sorting coal gangue. According to the feedback data of the laser calibrator, it is concluded that the control method can ensure the rationality and safety of the movement of the manipulator, and can achieve the desired trajectory in the coal gangue sorting process by convergence in a short time, which is in line with condition needs of the actual work of coal gangue sorting.

1. 引言

传统的煤矸石分拣正面临工人工伤风险系数高、矸石分拣效率低、劳动强度大等问题 [1]。《关于加快煤矿智能化发展的指导意见》 [2] 明确提出对具备条件的煤矿要加快智能化改造，推进危险岗位的机器人作业；到2035年各类煤矿基本实现智能化，建成智能感知、智能决策、自动执行的煤矿智能化体系。煤矸石分拣朝着智能机器人化方向发展符合现代工业发展趋势 [3]。但是，煤矸石分拣机器人在实际生产应用中，其控制系统会受到外界诸多因素影响，如煤矸石掉落、撞击障碍物等诸多不可预知的影响因素造成的问题，导致机械臂工作性能下降。因此，对机械臂轨迹跟踪控制技术的研究具有深远的意义。

多关节机械臂在动力学建模时，其中不确定性因素就造就了控制系统是一个非线性时变控制系统，所以，一种鲁棒性强、实施性好的控制算法就显得尤为重要了。目前工业机器人领域常用控制算法有自适应控制 [4]、滑模控制 [5]、神经网络控制 [6] 等。这些方法中，滑模控制因具有结构简单、对动力学建模要求不高、对动力学模型参数不敏感等点，在众多算法脱颖而出 [7] [8]。滑模控制可以按照需要设计好固定滑模面，且在控制运动过程中受控制系统中不确定参数影响小，因此其表现出很高的鲁棒性。但是，滑模控制算法在接近设计滑模面时，会出现在设定滑模面周边振动，从而影响系统的稳定性 [9]。

为解决动力学模型中的不确定性因素影响与滑模控制抖振问题，研究人员教青睐神经网络与滑模控制相结合的方法。在滑模控制实时控制的基础上，有学者提出引用GA-RBF算法 [10] [11] [12]，用于逼近并且补偿外界扰动带来对控制系统的影响，实现高精度轨迹跟踪控制；以及改进RBF神经网络算法 [13]，自适应的更新神经网络达到更新滑模控制补偿目的，缩短根基跟踪时间。也有学者提出引用模糊滑模控制法 [14]、自适应滑模控制法 [15] 以此来消除滑模控制抖振。文献 [16] 提出了一种自适应固定滑模控制器尽可能消除滑模趋近面抖振，并设计一种改进干扰观测器，针对外部不确定性扰动进行实时观测和检查，实现了对控制系统的在线跟踪补偿。文献 [17] 设计了一种模糊滑模控制器，利用模糊规则将切换增益项模糊化并替代原滑模控制器中的切换项，针对三关节取得了较好的跟踪控制效果。文献 [18] 在固定面滑模控制器基础上，提出一种切换增益调节的RBF神经网络滑模控制器，将RBF神经网络应用在滑模控制切换增益调节中，训练神经网络跟随关节位置变化而变化的调节滑模控制器。文献 [19] 在采用RBF神经网络的基础上，提出一种非奇异快速终端滑模自适应轨迹跟踪控制方法，对控制系统的不确定部分和外部扰动非线性干扰因素进行实时估计和补偿，对切换控制项设计RBF非奇异快速终端滑模控制器，实现对系统的无模型高精度控制。

在前人研究基础上，针对煤矸石分拣机械臂控制系统中不确定部分的非线性扰动问题，采用RBF神经网络与滑模控制相结合，补偿系统不确定扰动来提高系统控制精度，得到了广泛的应用 [20] [21]。因此，本文将滑膜控制与RBF神经网络有效的结合起来，设计了一种基于RBF神经网络切换增益调节的滑膜控制器，应用于煤矸石分拣机械臂的控制。分析分拣机械臂控制系统中不确定性部分，在传统滑模控制基础上引入神经网络算法，采用切换增益抑制机械臂运动的振动幅度，并引用李雅普诺夫理论证明了该控制系统的稳定性；且以三关节机械臂为对象，建立其结构简图，通过仿真实验理论验证该控制方法的可行性以及可以更短的时间里达到消除误差的稳定性。此外，搭建实验平台，以煤矸石分拣机械臂为对象，采用激光跟踪仪记录机械臂末端轨迹路径，实验证明，该控制方法可以实现机械臂稳定的运动以及较高的跟踪速度、跟踪精度，为深入研究机械臂运动轨迹的定位控制提供了基础。

2. 机械臂动力学建模

煤矸石分拣机械臂如图1所示；该臂由六个主驱动电机、两根连杆与末端执行装置组成，电机1和电机2为主要驱动关节，装在机械臂基座上，大大减轻了关节处的惯量比，电机3装在肘关节处，提高了机械臂的整机灵活性。

Figure 1. Coal gangue sorting robotic arm structure

图1. 自煤矸分拣机械臂结构

基于拉格朗日方程的N关节机械臂的动力学建模 [1]，可得出N关节机械臂动力学方程式为：

$M\left(q\right)\ddot{q}+C\left(q,\stackrel{\dot{}}{q}\right)\stackrel{\dot{}}{q}+F\left(\stackrel{\dot{}}{q}\right)+G\left(q\right)=\tau -{\tau }_d$ (1)

本其中： $q,\stackrel{\dot{}}{q},\ddot{q}\in R^n$ 分别为机械臂关节角位移、角速度和角加速度； $M\left(q\right)\in R^{n\times n}$ 为正定惯性矩阵； $C\left(q,\stackrel{\dot{}}{q}\right)\in R^n$ 为哥氏力和离心力矩阵； $F\left(\stackrel{\dot{}}{q}\right)\in R^n$ 为黏性摩擦力矩； $G\left(q\right)\in R^n$ 为关节重力项； ${\tau }_d\in R^n$ 为环境因素干扰项； $\tau \in R^n$ 为驱动力矩。

机械臂动力学方程具有如下性质 [16]：

性质1：惯性矩阵 $M\left(q\right)$ 是正定对称矩阵，那么存在正数 $m_1$， $m_2$ 满足如下不等式：

$m_1{\Vert x\Vert }^2\le x^{\text{T}}D\left(q\right)x\le m_2{\Vert x\Vert }^2$ (2)

性质2： $M\left(q\right)-2C\left(q,\stackrel{\dot{}}{q}\right)$ 是一个斜对称矩阵。

由于机械臂实际运行过程中，黏性摩擦力对运动的影响很小，故这里设置名义的控制模型为：

$M\left(q\right)\ddot{q}+C\left(q,\stackrel{\dot{}}{q}\right)+G\left(q\right)=u\left(t\right)$ (3)

考虑到系统的建模误差，为获得精确的动力学模型，将其分离为确定部分与误差部分分别进行处理：

$\{\begin{array}{l}M\left(q\right)=M_0\left(q\right)+\Delta M\left(q\right)\\ C\left(q,\stackrel{\dot{}}{q}\right)=C_0\left(q,\stackrel{\dot{}}{q}\right)+\Delta C\left(q,\stackrel{\dot{}}{q}\right)\\ G\left(q\right)=G_0\left(q\right)+\Delta G\left(q\right)\end{array}$ (4)

故建模误差表示为：

$\Delta h=\Delta M\left(q\right)\ddot{q}+\Delta C\left(q,\stackrel{\dot{}}{q}\right)\stackrel{\dot{}}{q}+\Delta G\left(q\right)$ (5)

系统确定部分控制率为：

$M_0\left(q\right)\ddot{q}+C_0\left(q,\stackrel{\dot{}}{q}\right)\stackrel{\dot{}}{q}+G_0\left(q\right)={\tau }_{eq}$ (6)

结合式(1)总控制器为：

$M_0\left(q\right)\ddot{q}+C_0\left(q,\stackrel{\dot{}}{q}\right)\stackrel{\dot{}}{q}+G_0\left(q\right)+\Delta h+{\tau }_d={\tau }_{eq}+{\tau }_s=\tau$ (7)

其中： ${\tau }_{eq}$ 为确定部分驱动力矩； ${\tau }_s$ 为滑模控制器，以滑模控制器为主控制器对误差进行处理。

3. 基于RBF网络增益调节的滑膜控制

3.1. 增益滑膜控制器的设计

滑模控制具有快速响应、鲁棒性强等优点，但会出现抖振现象，对机械臂的末端位置控制会产生较大的误差，而采用神经网络对切换项的增益进行调节，可降低滑模控制的抖振，进而起到减小误差的作用 [14]。

定义跟踪误差为：

$e=q-q_d$ (8)

设计滑模面为：

$s\left(x,t\right)=ce+\stackrel{\dot{}}{e}$ (9)

其中： $e={\left[e_1,e_2,e_3\right]}^{\text{T}}$ 为关节角误差； $\stackrel{\dot{}}{e}={\left[{\stackrel{\dot{}}{e}}_1,{\stackrel{\dot{}}{e}}_2,{\stackrel{\dot{}}{e}}_3\right]}^{\text{T}}$ 为关节角速度误差。

进一步可得确定部分控制率：

${\tau }_{eq}=M_0\left(q\right)\left({\ddot{q}}_d+\stackrel{\dot{}}{e}+e\right)+C_0\left(q,\stackrel{\dot{}}{q}\right)\stackrel{\dot{}}{q}+G_0\left(q\right)$ (10)

带入式(7)可得：

$\ddot{e}=M_0^{-1}\left(q\right)\left(\Delta h+{\tau }_d-{\tau }_s\right)-\stackrel{\dot{}}{e}-e$ (11)

设计切换函数为：

$s=\stackrel{\dot{}}{e}+Ce$ (12)

其中： $C=diag\left(C_1,C_2,C_3\right)$。

结合式(11)，可得：

$\stackrel{\dot{}}{s}=\ddot{e}+C\stackrel{\dot{}}{e}=M_0^{-1}\left(q\right)\left(\Delta h+{\tau }_d\right)-M_0^{-1}\left(q\right){\tau }_s-\stackrel{\dot{}}{e}-e$ (13)

采用指数趋近律：

$\stackrel{\dot{}}{s}=-\xi \mathrm{sgn}\left(s\right)-ks$ (14)

结合式(13)和式(14)得滑模控制器为：

${\tau }_s=M_0\left(q\right)\left(\xi \mathrm{sgn}\left(s\right)+ks+C\stackrel{\dot{}}{e}-\stackrel{\dot{}}{e}-e\right)$ (15)

3.2. 采用RBF网络增益调节

煤矸石分拣过程中是一个复杂的过程，环境因素以及分拣物品的重量随时都会发生变化，分拣过程中机械臂的磕磕碰碰以及一直变换的煤矸石质量，都将对机械臂的运行轨迹跟踪控制产生影响 [5] [6]。因此，本提出一种采用神经网络优化的增益滑模控制跟踪控制方法，确保机械臂控制系统能在短时间内获得稳定的高精度轨迹跟踪。滑膜控制的抖振大小是由其控制器切换的增益决定的，采用神经网络对建模误差 $\Delta h$ 进行自适应逼近，可降低滑膜控制的抖振。其控制系统流程如图2所示：

Figure 2. Neural network gain adjustment control system

图2. 神经网络的切换增益调节控制系统图

采用RBF神经网络 [12] 来调节切换项得增益K。设RBF网络得输入为：

${\hat{h}}_e={\hat{W}}^{\text{T}}\phi \left(x\right)$ (16)

进一步的有：

$\Delta h-{\hat{h}}_e=W^{\text{T}}\phi \left(x\right)-{\hat{W}}^{\text{T}}\phi \left(x\right)+\xi$ (17)

其中， $\xi$ 为神经网络估计误差。

输出得绝对值为切换项得增益K。取：

$K=\left|w^{\text{T}}h\left(x\right)\right|$ (18)

其中： $w^{\text{T}}$ 为RBF神经网络得权值， $h\left(x\right)$ 为高斯函数。

$h_i\left(x\right)=\exp \left(-\frac{{\Vert x-m_i\Vert }^2}{{\sigma }_i^2}\right)$

其中： $m_i$ 是第i个神经元的中心位置， ${\sigma }_i$ 为第i个神经元的宽度； $i=1,2,3$ ； $j=1,2$。

神经网络权值调整的指标为：

$E=\frac{1}{2}{e}^2$ (19)

其中 $e=y-r$，r为阶跃响应信号。

设计神经网络自适应律以实现自适应逼近：

$\hat{W}=Fh{s}^{\text{T}}{M}_0^{-1}$ (20)

故结合式(7)得：

$M_0\left(q\right)\ddot{q}+C_0\left(q,\stackrel{\dot{}}{q}\right)\stackrel{\dot{}}{q}+G_0\left(q\right)+{\hat{W}}^{\text{T}}\phi =\tau$ (21)

综合式(14)和式(15)，可知滑模控制器趋近律为：

$\stackrel{\dot{}}{s}=-\xi \mathrm{sgn}\left(s\right)-ks+M_0^{-1}\left({\hat{W}}^{\text{T}}\phi \left(x\right)+\xi \right)$ (22)

进一步的，结合式(21)和式(22)，得机械臂的总控制器为：

$\tau ={\tau }_{eq}+{\tau }_s=M_0\left(q\right)\left({\ddot{q}}_d+\xi \mathrm{sgn}\left(s\right)+ks+C\stackrel{\dot{}}{e}\right)+C_0\left(q,\stackrel{\dot{}}{q}\right)+G_0\left(q\right)+{\hat{W}}^{\text{T}}\phi \left(x\right)$ (23)

定义李雅普诺夫函数为：

$V=\frac{1}{2}{s}^{\text{T}}s+\frac{1}{2}tr\left({\tilde{W}}^{\text{T}}{F}^{-1}\tilde{W}\right)$ (24)

则：

$\stackrel{\dot{}}{V}=s^{\text{T}}\left(-\xi \mathrm{sgn}\left(s\right)-ks\right)+s^{\text{T}}\left(M_0^{-1}\xi \right)\le s^{\text{T}}\left(-\xi \mathrm{sgn}\left(s\right)-ks\right)\le 0$ (25)

4. 仿真分析

4.1. 工况场景描述

为了验证本文所提算法的有效性与可行性，采用如图3所示工况。该分拣系统又带式输送机、六关节机械臂以及控制系统等组成。分拣过程中，由视觉检测系统优先检测需要捡取的煤矸石，上传需要分拣煤矸石信息到控制系统，再由带式输送机继续载着矸石向后方运行，控制系统控制机械臂抓取煤矸石，将需要分拣煤矸石从带式输送机上抓取拿下。期间，其末端运行轨迹如图4示意所示。

Figure 3. Schematic diagram of coal gangue mechanical arm sorting system

图3. 煤矸石机械臂分拣系统示意图

Figure 4. Schematic diagram of sorting track

图4. 分拣轨迹示意图

4.2. MATLAB仿真分析

结合仿真工况所述，控制六关节机械臂按照理想运行轨迹运行。假定6个关节初始位置为 $q_0$ 均为0，初始角度也均设为0，末端分拣质量20 kg煤矸石；本次仿真神经元宽度设为：

$w_i={\left[\begin{array}{cccccc}20& 20& 20& 20& 20& 20\end{array}\right]}^{\text{T}}$，其中 $i=1~6$。

并且网络权值系数初始值设为：

${\delta }_1\left(t\right){\left[\begin{array}{cccccc}20& 20& 20& 20& 20& 20\end{array}\right]}^{\text{T}}$，其中 $i=1~6$。

且将煤矸石分拣机器人期望误差值设为0 mm；各个关节动量因子设为0.05，不确定性扰动量设为正弦变化 ${\tau }_d=0.2\sin t$，对期望轨迹跟踪误差仿真和控制力矩输入仿真，并于传统滑模控制做对比，其仿真结果如图5和图6所示。

由图5和图6可知当外部不确定因素按时变正弦规律变化时，末端轨迹跟踪上，传统滑模控制会在一开始出现较大的振动幅度，最大跟踪误差达到1.4 rad，且在4.2 s左右开始出现逼近误差为0，随后在设定值附近出现抖振现象；当采用基于RBF神经网络切换增益调节的滑模控制时，系统出现最大误差为0.6 rad，且在0.5 s左右开始出现逼近误差为0值，随后的仿真数值来看，消除抖振现象较明显，且振动误差控制在0.1 rad以内。控制输入上，当采用传统滑模控制时，关节的控制输入会出现较大的抖振，不能很快的适应外界的环境变化，前2 s内关节抖振较明显；而采用基于RBF神经网络切换增益调节的滑模控制时，抖振幅度明显减小，且很快稳定，随外部扰动呈现周期性变化。

(a) 传统滑模控制 (b) 基于切换增益调节的神经网络滑模控制

Figure 5. Tracking error response curve of sinusoidal trajectory

图5. 正弦变化轨迹跟踪误差响应曲线

(a) 传统滑模控制 (b) 基于切换增益调节的神经网络滑模控制

Figure 6. Sinusoidal variation control input response curve

图6. 正弦变化轨控制输入响应曲线

由图7和图8可知，当外部不确定性因素设置 ${\tau }_d=0.2t$ 斜坡变化，仿真机械臂开始分拣煤矸石出现关节摩擦增大等情况时，轨迹跟踪控制方面，传统滑模控制出现最大1.5 rad的误差，且4 s以后才会稳定在滑模设定值范围附近振动，采用基于RBF神经网络切换增益调节的滑模控制时，受外部环境变化影响较小，明显消除抖振，且响应速度更快，可在前2 s内逼近误差为0值；控制输入上，传统滑模控制受外部环境变化影响较大，出现较大抖振现象，而基于RBF神经网络切换增益调节的滑模控制，相比于传统滑模控制，受影响较小，仍然可以较短时间达到预期稳定。

由图9和图10可知，当外部不确定性因素设置为阶跃响应变化，仿真煤矸石在分拣过程中脱落、撞击障碍物等情况时，传统滑模控制出现2.2 rad误差，在5 s时开始逼近误差为0，对阶跃变化反应较大；基于RBF神经网络切换增益调节的滑模控制，抖振较小，在3 s时出现逼近误差为0，相较于传统滑模控制，面对阶跃变化时具有更好的稳定性和更快的响应速度。控制输入上，传统滑模控制抖振剧烈，不能很好的适应外界环境的突然变化，基于RBF神经网络切换增益调节的滑模控制输入上，受影响很小，系统的误差仍然稳定在允许范围以内。

(a) 传统滑模控制 (b) 基于切换增益调节的神经网络滑模控制

Figure 7. Tracking error response curve of ramp change trajectory

图7. 斜坡变化轨迹跟踪误差响应曲线

(a) 传统滑模控制 (b) 基于切换增益调节的神经网络滑模控制

Figure 8. Ramp change control input response curve

图8. 斜坡变化控制输入响应曲线

(a) 传统滑模控制 (b) 基于切换增益调节的神经网络滑模控制

Figure 9. Step change trajectory tracking error response curve

图9. 阶跃变化轨迹跟踪误差响应曲线

(a) 传统滑模控制 (b) 基于切换增益调节的神经网络滑模控制

Figure 10. Step change control input response curve

图10. 阶跃变化控制输入响应曲线

5. 实验分析

5.1. 实验平台搭建

为验证所设计的机器人轨迹跟踪控制系统的有效性及其准确性，采用一六自由度煤矸石分拣机械臂搭建轨迹跟踪控制实验平台，如图11所示。根据GB/T12642-2013《工业机器人性能规范及其实验方法》 [22]，对机械臂的轨迹跟踪性能进行实验。通过PC端输入理想轨迹进入机械臂控制器内，采用Lecia激光跟踪仪，型号为AT960-LR，通过激光标定仪来反馈机械臂末端实际路径路线。

按照4.1节工况分析路线，重复30次实验计算每次结束位置与理想位置的最大距离。

Figure 11. Coal gangue sorting robotic arm experimental platform

图11. 煤矸分拣机械臂实验平台

5.2. 结果分析

如图12所示，基于激光跟踪仪反馈回的数据，以空间曲线的形式绘制出机械臂末端轨迹图，观察图中曲线可知，机械臂在分拣作业中，多次走出同样几字的轨迹，机械臂的轨迹跟踪误差在±0.03 mm以内，轨迹控制效果较好。故可得出结论，控制器可以很好地控制该机械臂，位置控制效果也较为理想，证明了RBF神经网络增益调节在煤矸石分拣机械臂控制系统应用的可行性与可靠性。

Figure 12. Robot arm end curve

图12. 机械臂末端曲线

6. 结论

1) 研究煤矸石分拣机械臂分拣煤矸石过程中轨迹跟踪控制，针对煤矸石分拣机械臂关节位置，提出一种基于RBF神经网络切换增益调节滑模控制器；实现对控制系统中不确定性部分的实时补偿调节，达到提高控制系统精度、控制响应速度的目的。

2) 设计控制系统，利用MATLAB的Simulink仿真模块，实现分拣机械臂轨迹跟踪控制。仿真结果表明：本文所述方法较好地消除了滑模控制中抖振问题，同时也减小了轨迹跟踪误差且响应速度更快。

3) 搭建实验平台，采用本文所述控制方法。结果表明：据激光标定仪反馈回数据，机械臂末端轨迹光滑，理想值与期望值误差较小。从而实验验证了本文所提算法的可行性、准确性以及稳定性。

参考文献