农业机械的发展已逐步走向智能化、精细化。机器人技术在农业生产领域的应用将极大地改变农业生产作业方式，实现高效生产^[1]。水果、蔬菜采收作业环境极其复杂、劳动强度高、季节性强，常因缺乏足够的劳动力而错失最佳采收时机。实现机器人采摘是果蔬实时采收和低成本收获的有效方案，并且可有效降低劳动强度、提高作业效率^[2-4]。

在复杂农业作业环境中，未知干扰易造成农业机器人采摘机械臂末端轨迹的跟踪误差。针对机械臂末端轨迹跟踪控制精度问题，研究提出了PID控制、ADRC控制、滑模控制、鲁棒控制和神经网络控制等多种算法^[5-9]，但实际应用中往往是多种控制算法互取优势结合使用^[10-11]。其中，滑模变结构控制算法^[12-13]含有切换控制项函数，本质上是一种特殊的非线性控制算法，适用于属于多输入多输出的非线性系统的机械臂。张旭辉等^[14]针对传统滑模控制算法在悬臂式掘进机轨迹跟踪控制中存在的缺陷问题，改进传统指数趋近律函数，并引入位姿偏差、增加幂次函数项，同时采用反步法设计新的切换函数代替符号切换函数，改进后的滑模控制器实现了系统的快速收敛并削弱了系统抖振。Baek等^[15]在机械臂的控制研究中，采用延时估计技术提出一种自适应滑模控制器，在自适应律函数中添加有关速度的可调正增益和正设计参数，实现了良好的跟踪性能和较小的抖振效应。张蕾等^[16]针对复合扰动对机械臂轨迹跟踪精度的影响，设计了基于非奇异积分终端滑模面的轨迹跟踪控制器，积分项提高了系统的鲁棒性，同时采用分数指数项来加快收敛速度。贾华等^[17]针对机械臂模型未知且存在外界干扰时的跟踪控制问题，提出基于动力学模块分块逼近的神经网络终端滑模的控制方案，同时采用多组神经网络分块逼近动力学模型参数，实现模型重构和权值在线调整，与整体逼近算法相比，分块逼近使机械臂关节最大稳态误差减少83.7%，能够抵消末端负载变化情况。上述研究大多基于机械臂关节的控制角度进行，在实际工程应用中，机械臂的应用多关注于末端轨迹的跟踪控制。

在实际工程应用上，机械臂不仅要考虑外部干扰和不确定性问题，还需要考虑平滑、能耗及避障等方面。Lin等^[18]在研究气动人工肌肉驱动的机械臂末端圆形轨迹跟踪时，在控制算法中采用遗传算法进行参数识别，设计出高阶滑模反馈控制算法以实现圆形轨迹跟踪，仿真结果表明控制器在抵抗外界干扰和建模不确定性方面具有更强的鲁棒性。Wang等^[19]针对机械臂轨迹跟踪问题提出无模型自适应滑模控制策略，采用延时估计技术对未知动力学模型进行估计，同时设计自适应律来处理延时估计误差，在控制算法中引入改进的漏斗函数来控制跟踪误差，集成一种基于新型漏斗函数的非奇异快速终端滑模控制器以增强系统的跟踪控制性能。Zhao等^[20]研究了在没有速度测量情况下的机械臂轨迹跟踪问题，提出一种由工作空间期望轨迹估计器、自适应律及速度观测器组成的协作跟踪控制方案，估计器用来产生期望轨迹，观测器可获得工作空间速度，并基于观测器和估计器设计出自适应控制律。倪元相等^[21]研究了基于输出反馈式神经网络的三自由度机械臂轨迹跟踪控制问题，提出以B样条函数为基函数的人工神经网络，采用基于粒子群优化算法的离线训练方法确定控制器增益，并在关节空间和笛卡儿空间中验证了方案的可行性。

机械臂是一个复杂的非线性系统，而农业作业环境又极其复杂且作业对象物理特性复杂，诸如作业目标自然生长无序可循、每次作业目标位姿各不相同等，均会影响机械臂采摘作业的灵活性、反应速度和作业精度，易造成严重的动态扰动^[22-23]。实现采摘机械臂灵活、高效、精准作业是研究机械臂跟踪控制的着眼点，本文针对机械臂在农业领域应用中的末端轨迹跟踪控制问题，提出基于双曲正切函数的控制策略。在MATLAB/Simulink仿真环境中搭建机械臂控制系统，验证所设计的控制器对轨迹跟踪的有效性。

1.   试验平台与设计

1.1   试验平台简介

果蔬采摘机器人主要由无人移动平台、视觉识别系统、机械臂操作系统和末端执行机构组成。基于此搭建实验室环境下的采摘平台，如图1所示。控制机械臂末端按期望轨迹到达采摘目标是农业采摘机器人的关键技术之一，本文针对其轨迹跟踪控制问题展开研究，以二自由度机械臂为模型来设计控制算法，并进行仿真验证。

图  1  采摘机器人

Figure  1.  Picking robot

下载: 全尺寸图片 幻灯片

1.2   坐标转换

数学分析中，机械臂的动力学模型通常是基于关节角度构建的，进而通过设计关节扭矩实现关节角度和角速度的跟踪。但在实际应用中，机械臂的控制重点是末端轨迹的稳定跟踪控制，因此需要建立工作空间中机械臂末端节点直角坐标的动力学模型。

如图2所示，根据几何关系计算出机械臂末端在工作空间中的位置为：

图  2  二自由度机械臂简化图

Figure  2.  Simplified diagram of 2-DOF manipulator

下载: 全尺寸图片 幻灯片

式中：$ x、y $分别为机械臂末端端点横坐标、纵坐标；$ {{l}}_{1}、{{l}}_{2} $分别为二自由度机械臂连杆OA、AB的长度； $ {q}_{1}、{q}_{2} $分别为连杆OA、AB的关节角度。

依据直角三角形定理得：

化简式(2)得：

根据图2中几何关系可得：

或

在$ \Delta OAB $中，根据三角形余弦定理得：

由式(6)可求得：

从而有：

定义$ {\boldsymbol{z}}=\left[x\;y\right],{\boldsymbol{q}}=\left[{q}_{1}\;{q}_{2}\right] $，

则

定义

则

其中，$ {\boldsymbol{J}}=\left[\begin{array}{cc}\dfrac{\partial x}{\partial {q}_{1}}& \dfrac{\partial x}{\partial {q}_{2}}\\ \dfrac{\partial y}{\partial {q}_{1}}& \dfrac{\partial y}{\partial {q}_{2}}\end{array}\right] $，$ {\boldsymbol{J}} $为机械臂末端端点速度与机械臂关节速度之间关系的雅可比矩阵。

由式(1)可得：

雅可比矩阵$ {\boldsymbol{J}}\left({\boldsymbol{q}}\right) $则有如下表示：

雅可比矩阵求导可得：

根据${\boldsymbol{ J}}\left({\boldsymbol{q}}\right) $和$ \dot{{\boldsymbol{J}}}\left({\boldsymbol{q}}\right) $可知，$ {\boldsymbol{J}}\left({\boldsymbol{q}}\right) $是由结构决定的，在此假设$ {\boldsymbol{J}}\left({\boldsymbol{q}}\right) $在有界的工作空间中是非奇异的。

1.3   机械臂建模

基于关节角度的机械臂模型为：

式中：$ {\boldsymbol{q}}\in {R}^{n} $，为关节变量向量；$ {\boldsymbol{\tau}} \in {R}^{n} $，为执行机构施加的关节扭矩向量；$ {\boldsymbol{D}}\left({\boldsymbol{q}}\right)\in {R}^{n\times n} $，为对称正定惯性矩阵；$ {\boldsymbol{C}}\left({\boldsymbol{q}},\dot{{\boldsymbol{q}}}\right)\in {R}^{n\times n} $，为哥氏力和离心力向量；$ {\boldsymbol{G}}\left({\boldsymbol{q}}\right)\in {R}^{n} $，为重力向量。

在工程实际应用中，为实现机械臂末端轨迹的跟踪控制目标，需要将基于关节角度的动力学方程转换为基于直角坐标的动力学方程，即在直角坐标系下研究末端力与轨迹跟踪目标的数值关系。

在静态平衡状态下，机械臂末端力F与关节力矩$ {\boldsymbol{\tau}} $之间存在线性映射关系，根据虚功原理可知其关系式如下：

变形得：

由公式(10)得：

则有：

根据式(21)可得出关节角加速度$(\ddot{q}) $为：

将式(22)代入式(16)，可得：

即：

整理得：

等式两边同时乘以$ {J}^{-T} $可得：

同时，考虑建模不确定性，得到如下机械臂动力学模型：

其中，$ {{\boldsymbol{D}}}_{{\textit{z}}}={{\boldsymbol{J}}}^{-{\mathrm{T}}}{\boldsymbol{D}}{{\boldsymbol{J}}}^{-1} $，$ {{\boldsymbol{C}}}_{{\textit{z}}}={{\boldsymbol{J}}}^{-{\mathrm{T}}}\left({\boldsymbol{C}}-{\boldsymbol{D}}{{\boldsymbol{J}}}^{-1}\dot{{\boldsymbol{J}}}\right){{\boldsymbol{J}}}^{-1} $，$ {{\boldsymbol{G}}}_{{\textit{z}}}= {{\boldsymbol{J}}}^{-{\mathrm{T}}}{\boldsymbol{G}} $，$ \left\| {\Delta \left(q,\dot{q},\ddot{q}\right)} \right\|\leqslant \eta $。

上述机械臂动态方程具有2个特性：惯性矩阵$ {{\boldsymbol{D}}}_{{{{\boldsymbol{z}}}}} $对称正定；矩阵$ {\dot{{\boldsymbol{D}}}}_{{\boldsymbol{z}}}-2{{\boldsymbol{C}}}_{{\boldsymbol{z}}} $是斜对称的。

1.4   控制目标

在二维直角坐标系中研究二关节机械臂末端轨迹跟踪控制问题，因此，控制器的控制对象是末端端点在横轴和纵轴上的位置和行速度。设横轴方向的位置为 $ x $，纵轴上的位置为 $ y $，记为$ {\boldsymbol{z}}=\left[x\;y\right] $。末端轨迹为时间的函数，记为$ {\boldsymbol{z}}\left(t\right) $。假设$ {{\boldsymbol{z}}}_{d}\left(t\right) $是在工作空间中的理想轨迹，则$ {\dot{{\boldsymbol{z}}}}_{d}\left(t\right) $和$ {\ddot{{\boldsymbol{z}}}}_{d}\left(t\right) $分别是理想的速度和加速度。

末端轨迹跟踪误差的定义如下：

末端速度跟踪误差计算公式为：

上述跟踪误差记为控制目标，设计控制器使控制对象能够快速稳定地跟踪期望目标$ {{\boldsymbol{z}}}_{d}\left(t\right) $和$ {\dot{{\boldsymbol{z}}}}_{d}\left(t\right) $，即跟踪误差$ e\left(t\right)\to 0 $，$ \dot{e}\left(t\right)\to 0 $。

1.5   控制器的设计

在传统的滑模控制器设计中，切换控制项通常采用一般切换函数——符号函数或滑模面函数，但此类切换函数会引起控制系统的抖振。为解决这一问题，采用连续的双曲正切函数代替一般切换函数，双曲正切函数的陡度可调节切换控制的切换程度。

双曲正切函数定义为：

式中，$ \varepsilon$为双曲正切函数的陡度且$ \varepsilon > 0 $。引理1：针对任意给定的实数$ {x'} $，存在下面的不等式：

式中，$ \mu=0.278\;5 $。

引理 2：假设$ \mathrm{f}，V:[0,\mathrm{\infty })\in \mathrm{R} $，如果存在$ \dot{V}\leqslant -\alpha V+\mathrm{f} $，$ \forall \mathrm{t}\geqslant {t}_{0}\geqslant 0 $，则：

式中，$ \alpha $为任意实数。

由轨迹跟踪误差公式得：

求导得：

定义

设计滑模函数为：

式中，$ {\boldsymbol{\varLambda}} $为正定矩阵。

根据机械臂动力学模型公式(27)，设计一种具有光滑双曲正切切换的滑模控制器：

式中，$ K $表示控制器的增益且$ K > 0 $。

综上分析，机械臂末端轨迹跟踪控制框图见图3。

图  3  机械臂跟踪控制框图

Figure  3.  Block diagram of manipulator tracking control

下载: 全尺寸图片 幻灯片

2.   试验结果与分析

2.1   稳定性验证

通过构造Lyapunov函数来证明所设计的滑模控制器满足系统稳定性的要求。

将控制律式(38)代入式 (27)中，得：

将$ \dot{{\boldsymbol{z}}}\left(t\right)={\dot{{\boldsymbol{z}}}}_{r}\left(t\right)-s\left(t\right) $，$ \ddot{{\boldsymbol{z}}}\left(t\right)={\ddot{{\boldsymbol{z}}}}_{r}\left(t\right)-\dot{s}\left(t\right) $代入公式(39)得：

由于$ {{\boldsymbol{D}}}_{{\boldsymbol{z}}}\left({\boldsymbol{q}}\right) $为对称正定，则可定义Lyapunov函数为：

则有：

由于矩阵$ {\dot{{\boldsymbol{D}}}}_{{\boldsymbol{z}}}\left({\boldsymbol{q}}\right)-2{{\boldsymbol{C}}}_{{\boldsymbol{z}}}\left({\boldsymbol{q}},\dot{{\boldsymbol{q}}}\right) $是斜对称的，则$ {s}^{{\mathrm{T}}}{\dot{{\boldsymbol{D}}}}_{{\boldsymbol{z}}}- 2{{\boldsymbol{C}}}_{{\boldsymbol{z}}}s=0 $，即$ \dfrac{1}{2}{s}^{{\mathrm{T}}}{\dot{{\boldsymbol{D}}}}_{{\boldsymbol{z}}}s={s}^{{\mathrm{T}}}{{\boldsymbol{C}}}_{{\boldsymbol{z}}}s $，代入式(41)得：

根据引理1，可得：

于是：

式中，$ \lambda_{\mathrm{m}\mathrm{a}\mathrm{x}}\left({D}_{x}\right) $为$ {D}_{x} $的最大特征值；$ \lambda_{\mathrm{m}\mathrm{i}\mathrm{n}}\left(K\right) $为$ K $的最小特征值。

根据$ \dot{V}\leqslant -2{\lambda }V+b $，采用引理2，可得：

则：

根据式(48)可知，跟踪误差和误差导数渐进收敛，收敛精度取决于$ \varepsilon $、$ \lambda $和$ \eta $，即$ \varepsilon$越小，$ K $越大，$ \Delta $越小，收敛效果就越好。

2.2   仿真分析

为验证文中所设计的控制器的有效性和可靠性，以上述二自由度机械臂为控制对象，其动力学方程为式(13)，经过计算可得出相关矩阵表达式如下：

式中，$ {{p}}_{1}=\left({{m}}_{1}+{{m}}_{2}\right){{l}}_{1}^{2} $，$ {{p}}_{2}={{m}}_{2}{{l}}_{2}^{2} $，$ {{p}}_{3}={{m}}_{2}{{l}}_{1}{{l}}_{2} $，$ {{p}}_{4}= ({{m}}_{1}+ {{m}}_{2}){{l}}_{1} $，$ {{p}}_{5}={{m}}_{2}{{l}}_{2} $，$ {{m}}_{1} $、$ {{m}}_{2} $为连杆质量。

在直角坐标系中，假设理想跟踪轨迹为$ {x}_{{{d}}}= \mathrm{c}\mathrm{o}\mathrm{s}t $，$ {y}_{{{d}}}=\mathrm{s}\mathrm{i}\mathrm{n}t $，根据合成运动知识可知，该轨迹是一个半径为1.0，圆心为$ \left(\mathrm{0.0,0.0}\right) $的圆。设置机械臂末端轨迹的初始条件为$ {{\boldsymbol{z}}}=\left[1.0 \;1.0\right] $，$ \dot{{{\boldsymbol{z}}}}=\left[0.0 \;0.0\right] $，末端实际轨迹按逆时针方向进行跟踪。按式(3)和(8)将工作空间中的关节末端直角坐标$ \left(x,y\right) $转为关节角度坐标$ \left({q}_{1},{q}_{2}\right) $。

采用工作空间模式公式(27)实现被控对象的描述，针对建模不确定性，取$ \Delta \left(q,\dot{q},\ddot{q}\right)=10\mathrm{s}\mathrm{i}\mathrm{n}t $，滑模控制器取式(35)，并通过虚功原理式(17)转化为控制关节角度的实际控制器。控制器的增益选为$ {\boldsymbol{K}}=\left[\begin{array}{cc}30& 0\\ 0& 30\end{array}\right] $，$ \boldsymbol{\varLambda }=\left[\begin{array}{cc}15& 0\\ 0& 15\end{array}\right] $，$ \eta=12 $，$ \varepsilon=0.10 $。

为体现本文所用控制算法的有效性和优越性，再设计2种不同的控制算法与之进行对比试验。设计在工作空间中机械臂末端轨迹PD控制器和基于传统切换函数的滑模控制器。

PD控制器的控制律表达式为：

式中，$ {{\boldsymbol{K}}}_{{\mathrm{p}}} $、$ {{\boldsymbol{K}}}_{{\mathrm{d}}} $分别表示PD控制算法中的比例系数和微分系数矩阵，

取$ {{\boldsymbol{K}}}_{{\mathrm{p}}}=\left[\begin{array}{cc}30& 0\\ 0& 30\end{array}\right] $，$ {{\boldsymbol{K}}}_{{\mathrm{d}}}=\left[\begin{array}{cc}30& 0\\ 0& 30\end{array}\right] $。

基于传统切换函数的滑模控制器的控制律表达式为：

其中，$ {K} > 0 $。

基于以上3种控制器，在MATLAB/Simulink环境中搭建仿真系统，仿真结果如图4~6所示。

图  4  机械臂末端轨迹跟踪

Figure  4.  Trajectory tracking of the end of the manipulator

下载: 全尺寸图片 幻灯片

图  5  x轴和y轴轨迹跟踪(A、B)及跟踪误差(C、D)

Figure  5.  Trajectory tracking (A, B) and tracking errors (C, D) of x-axis and y-axis

下载: 全尺寸图片 幻灯片

从图4机械臂末端轨迹跟踪图来看，机械臂末端在3种不同算法控制下均能在一定的误差范围内实现圆形轨迹跟踪。跟踪曲线明显表明本文所设计的基于双曲正切的滑模控制器使系统的收敛速度增大。

结合机械臂末端分运动轨迹跟踪图5A、5B和误差图5C、5D来分析，滑模控制算法在系统收敛时间方面明显优于PD控制算法。由图5B机械臂末端y轴跟踪结果可以看出，切换函数滑模控制器的跟踪开始时间为1.5 s，而双曲正切滑模控制器的跟踪开始时间在1.0 s，根据y轴的速度跟踪误差曲线(图6C)也可看出，双曲正切控制比切换函数控制在收敛速度方面提高了50%。

在机械臂末端速度轨迹跟踪曲线图6A、6B中，双曲正切滑模控制器相比另2个控制器来说，尽管其在初始时刻存在较大的误差，但其总体跟踪效果强于切换函数控制器和PD控制器，并且速度跟踪的收敛时间降低了50%，收敛速度相应提高了50%。观察图6C、6D中的跟踪误差曲线可知，本文所提的控制算法在收敛速度和稳定性方面体现了良好的控制性能，但在响应初始时刻，该算法有急剧突变，需要对控制算法进行优化以缩小初始变化的误差。

图  6  x轴和y轴的速度跟踪(A、B)及跟踪误差(C、D)

Figure  6.  Velocity tracking (A, B) and tracking errors (C, D) of x-axis and y-axis

下载: 全尺寸图片 幻灯片

3.   结论

为实现采摘机器人中机械臂末端轨迹高精度跟踪控制，本文提出了一种基于双曲正切函数的滑模控制器。在Simulink环境中搭建机械臂控制系统并进行仿真验证，分析比较了3种控制器对轨迹跟踪的收敛性能和效果。结果表明：本文设计的滑模控制器能够实现高精度的轨迹跟踪和稳定控制，并使系统收敛时间减少50%，有效提高了系统收敛速度。研究为后续采摘机械臂设计奠定了基础。