采茶机器人SCARA机械手的轨迹规划与控制

汪 琳，关胜晓

(中国科学技术大学 信息科学技术学院，安徽 合肥 230026)

0 引言

近年来，茶叶产业不断发展，对茶叶的采摘要求也越来越高，目前茶园主要通过采茶工人和使用采茶机两种方式来采摘茶叶[1-2]。其中，人工采摘对劳动力的需求量较大，在采茶旺季往往会面临劳动力短缺的现象，随着城市化的发展和人口老龄化的加剧，越来越多的年轻人不愿意从事采茶工作，使得采茶工人中大多都是妇女和老年人，采摘效率低下，但茶叶的采摘具有很强的时令限制，人工短缺会造成很多茶园的季节性茶叶无法得到及时的采摘，成为茶叶产业经济发展的主要障碍；使用采茶机采摘虽然提高了茶叶的采摘效率，但目前的采茶机在工作时仍需要人员参与其中，无法实现完全自动化，且无法满足名优茶有选择性的高质量采摘要求。为了缓解这种现状，提高茶叶的采摘质量和效率，越来越多的高校与学者开始研究智能采茶机器人，通过电机驱动和算法设计实现机器人的自动行走功能，从而解放人类双手；通过视觉处理系统自主完成茶叶的识别定位与新叶老叶的判别，再控制机械手完成指定茶叶的自动化采摘，市场应用前景广阔。

本文基于智能采茶机器人整体性能的考虑，采用SCARA机械手作为机器人的采摘机构，并给出了采茶机器人的系统组成。针对该SCARA机械手，首先通过D-H法进行了运动学分析，得到机械手的正逆向运动学模型；其次，为了使机械手从起点连续、快速、平稳地运动到达目标点，基于MATLAB利用五次多项式插值算法对机械手的大臂关节和小臂关节进行了关节空间的轨迹规划，并通过三角函数拟合得到了这两个关节的运动轨迹方程，用作后续轨迹跟踪控制时的期望轨迹；最后，针对SCARA机械手在其动力学建模过程中可能存在的建模误差和各种外界扰动以及自身非线性关节耦合等因素的影响，在传统的PD控制方法的基础上，利用自适应鲁棒算法补偿的方法设计了一种自适应鲁棒PD控制器，在有外部扰动的情况下实现了对大臂关节和小臂关节的轨迹跟踪控制，结果表明该控制器可以实现较小的位置跟踪误差，运行过程稳定。

1 采茶机器人的系统组成

目前国内外大多数的采茶机在采摘茶叶时会对茶叶叶芽造成不同程度的损伤，且采摘得到的茶叶叶芽形状各异，一致性较差，这不仅影响了茶叶的质量，也对茶树上下一季新茶叶芽的生长形成了阻碍[3]。为了实现名优茶的自动化采摘，需要满足两点要求：其一是在采摘时要对茶叶有一定的选择性，保证采摘的茶叶都是符合名优茶的形状要求的；其二是保证采摘的茶叶叶芽是完好无缺的。基于这两点因素，本文采用摄像机来采集茶叶图像，在移动框架上固定安装摄像机，通过视觉伺服的方法识别和定位需要采摘的茶叶，在这过程中辨识的因素主要包括茶叶的颜色和形状等；采摘的末端执行器采用小型的滑块-切刀式结构，通过对指定茶叶叶芽的叶梗实行截断操作来保证叶芽的完整度。

大多数茶园中每一垄上的茶树顶面都接近于平面，茶叶大致生长在一定垂直高度的弧面上[4]，因此采茶机器人中的采摘机械手需要能够在XYZ三个方向上产生平动。采摘时末端执行器的进刀方向与茶叶的生长方向应保持垂直，因此为了适应不同方位上的茶叶采摘，末端执行器还应该具有一个旋转自由度。由于SCARA机械手具有三个旋转关节和一个移动关节共四个自由度，相比于其他串联机械手，这类机械手在水平方向上的运动柔性较大，而在垂直方向上的运动刚性较强[5]，其中它的前两个旋转关节可以实现末端执行器在水平面内的定位，移动关节可以实现末端执行器在垂直平面内的运动，腕部旋转关节可以进一步调节末端执行器的运动方向，因此可以满足采茶机器人的采摘需求，使得每个自由度都可以得到充分的功能运用，故本文选择SCARA机械手作为采茶机器人的采摘机构。采用多个SCARA机械手执行采摘动作，并根据茶垄宽度和单个机械手的作业空间，合理布局多个采摘机械手的安装位置，交错排列，前后相邻机械手的作业空间有一定的重叠空间，以保证较高的茶叶采摘率，多个机械手的使用也可以使得采茶机器人的采摘效率得到更大幅度的提高。

整个机器人采用四轮驱动行走机构，每个轮子都具有转向功能，使得机器人能够一直沿着茶垄的走向前进或后退，利用大容量的移动电源作为行走机构的动力能源，方便更换，且不会对茶叶带来品质隐患和对环境造成污染；在采茶机器人框架四周及顶部添加遮光罩和统一辅助光源，使得整个采茶过程不受环境光和天气的影响，在采茶旺季可以通过延长工作时间使得茶叶叶芽得到及时的采摘；通过软管传送茶叶，软管一端连接到SCARA机械手的末端执行器上，另一端连接收集装置袋，利用负压将已摘下的茶叶叶芽吸附到收集装置袋中。综合上述，将采茶机器人的系统组成如图1所示。

2 SCARA机械手的运动学建模

SCARA机械手的运动学建模包括其正向运动学建模和逆向运动学建模。其中，正向运动学建模即已知各个关节位置，求解末端执行器在笛卡尔空间的位置和姿态[6]。本文采用D-H法对SCARA机械手进行运动学分析，在机械手的各个关节上建立空间直角坐标系，需要确保在第i个关节处所建立的坐标系{Oi-XiYiZi}中Zi轴的方向与该关节的运动轴线相重合[7]。所建立的D-H坐标系简化图(没有表示出Yi轴)如图2所示，模型参数参见表1。其中，d1表示基座的高度。

根据表1中的模型参数，可以得到SCARA机械手中各个关节相对于固定基座坐标系{O0-X0Y0Z0}的转换矩阵分别如下：

图1 采茶机器人的系统组成

图2 SCARA机械手各关节D-H坐标系简化图

连杆编号扭转角/rad连杆长度/mm连杆夹角/rad连杆距离/mm关节变量100θ1d1θ120L1θ20θ23πL20d3d3400θ40θ4

注：扭转角表示Zi-1轴与Zi轴在Xi-1轴方向的夹角；连杆夹角表示Xi-1轴与Xi轴在Zi-1轴方向的夹角。

(2)

(3)

(4)

最后将上述四个关节的坐标变换矩阵连乘即可得到SCARA机械手腕部坐标系{O4-X4Y4Z4}相对于固定基座坐标系{O0-X0Y0Z0}的齐次坐标变换矩阵：

(5)

(6)

其中，R表示SCARA机械手腕关节在坐标系{O0-X0Y0Z0}中的方向矩阵，P表示SCARA机械手腕关节在坐标系{O0-X0Y0Z0}中的位置矩阵，θ124表示θ1+θ2+θ4，θ12表示θ1+θ2。T即为SCARA机械手的正向运动学模型。

SCARA机械手的逆向运动学建模是已知腕部坐标系{O4-X4Y4Z4}相对于固定基座坐标系{O0-X0Y0Z0}的期望位置和姿态，求解满足期望要求的关节变量[6-8]。即已知式(5)中矩阵T的各个元素值，求解式(6)中的所有未知变量(θ1,θ2,d3,θ4)。联立式(5)和式(6)可以推导出：

(7)

(8)

结合式(7)和式(8)可以求出SCARA机械手的逆运动学解如下，即其逆向运动学模型：

(9)

实际茶园中茶垄宽度基本在1 m左右，茶叶所生长的弧面高度一般在150 mm左右[4]。为了提高采茶机器人对茶叶叶芽的采摘效率，在左右两端各安装一个机械手，同时采用多排机械手交错排列，前后排机械手的采摘空间有一定的重叠区。假设单个机械手的采摘作业空间为500 mm，并以此设计SCARA机械手连杆1和连杆2的长度，以及连杆3的最大移动距离如下：L1= 250 mm，L2= 250 mm，d3max= 200 mm。利用MATLAB Robotic Toolbox建立SCARA机械手的空间仿真模型如图3所示。

图3 SCARA机械手的空间仿真模型

本节的运动学建模为SCARA机械手三维模型的设计开发以及优化设计提供了重要的理论依据。通过运动学建模及实际应用场景可以初步确定单个SCARA机械手各个连杆的尺寸参数和工作空间大小，这对采茶机器人中多个机械手的空间布局安排具有重要的指导意义。同时，后续章节中SCARA机械手的轨迹规划与控制也都要以其运动学分析为基础。

3 SCARA机械手的轨迹规划

SCARA机械手因在运动精度、速度和稳定性等方面具有一定的优势，且工作空间大，因而被广泛应用于各种工业场合中[8]，研究其在这些场合中的运动轨迹具有很好的研究价值，对后续的运动控制具有重要意义，直接影响控制精度和灵敏度[9]。在采茶过程中，为了保证采摘手能够快速平稳地到达指定的茶叶叶芽位置，在控制器对机械手的运动指令下发之前需要对其进行轨迹规划，使得SCARA机械手的各个关节在运动过程中不产生震荡和突变现象[8-9]。本节在关节空间下对SCARA机械手的大臂关节和小臂关节(即前两个旋转关节)的运动轨迹进行研究。

假设采茶机器人在工作过程中对不同高度上的茶叶叶芽进行分层采摘。对于同一层上的茶叶叶芽，机械手在采摘时高度保持不变，这就将复杂的茶叶采摘过程简化成了XOY水平面上的茶叶识别和定向问题。通过视觉伺服的方法识别和定位需要采摘的茶叶叶芽在XOY平面内的位置，即末端执行器的X、Y坐标，再通过第2节中推导出的逆运动学公式(9)求解得到大臂关节1和小臂关节2的旋转角位移。假设采摘手末端执行器在XOY平面内从初始位置(500，0)在2 s内运动到(200，-350)位置处，经逆解计算可得到这两个关节的初始角度和目标点角度分别为：

θ10=0；θ1f=1.456 9

θ20=0；θ2f=1.266 1

利用MATLAB Robotic Toolbox中的关节轨迹规划函数jtraj()，即五次多项式插值函数，每计算一次插值的时间间隔为0.02 s，仿真得到大臂关节1和小臂关节2的运动轨迹曲线分别如图4和图5所示。

图4 SCARA机械手关节1的轨迹规划曲线

图5 SCARA机械手关节2的轨迹规划曲线

由图4和图5可以看出，SCARA机械手的前两个旋转关节在从起点到目标点的运动过程中，所产生的角位移、角速度和角加速度曲线都是光滑连续的，全程都没有发生突变，从而证明在关节空间采用五次多项式插值算法进行轨迹规划是合理的，可以满足轨迹光滑连续无冲击的要求。

对上述通过五次多项式插值算法获得的大臂关节1和小臂关节2的运动点坐标，在MATLAB中使用Curve Fitting工具进行正弦曲线逼近，可以近似得到这两个关节的期望轨迹分别为：

q1d=1.311sin(0.758 7t-0.005 35)+0.275 sin(2.275t+2.619)+0.205 5sin(2.973t+3.831)

(10)

q2d=1.085sin(0.775t+0.142 8)+0.310 2sin(1.736t+3.585)+0.12sin(3.182t+3.322)

(11)

基于这样的期望轨迹，下一节将通过设计控制器进行SCARA机械手的轨迹跟踪控制仿真，使得位置跟踪误差尽可能的小。

4 SCARA机械手的轨迹跟踪控制

采茶机器人在执行采摘作业的过程中，SCARA机械手末端采摘器的定位精度主要受到其大臂旋转关节和小臂旋转关节控制精度的影响[10]，移动关节和腕部旋转关节对末端采摘器在固定基座坐标系X0O0Y0平面内的投影没有影响[11]。因此可以将空间三维SCARA机械手简化为固定基座坐标系中的平面二连杆模型，如图6所示。对该简化的机械手模型进行X0O0Y0平面内的轨迹跟踪控制。

图6 SCARA机械手简化的平面二连杆模型

SCARA机械手是一个具有非线性和关节耦合性的复杂时变系统，运行时易受外界干扰的影响，而在工业机器人领域中，传统的轨迹跟踪控制方法经常采用线性PD控制，没有考虑机构关节中存在的耦合性，因此控制效果不理想。本节采用一种将自适应控制与鲁棒控制相结合的控制策略对图6中简化的机械手模型在X0O0Y0平面内进行轨迹跟踪控制。这种控制策略可以根据位置跟踪过程中同一时刻下实际轨迹与期望轨迹之间的差值大小，让PD反馈控制和自适应控制分别在控制的不同阶段发挥主要作用，从而使得被控对象快速地追踪到达期望位置，位置跟踪误差收敛到零附近，动态性能良好[12]。

4.1 自适应鲁棒PD控制律的推导

SCARA机械手简化模型(平面二关节机械手)动力学方程的一般形式可以表示成：

(12)

(13)

(14)

当外界扰动信号τd的上界已知时，设计控制器和自适应律如下：

(15)

Κp=Κp1+Κp2Bp(e)

(16)

(17)

其中，

Κp1=diag(kp11,kp12,…,kp1n)

(18)

Κp2=diag(kp21,kp22,…,kp2n)

(19)

Κd1=diag(kd11,kd12,…,kd1n)

(20)

Κd2=diag(kd21,kd22,…,kd2n)

(21)

(22)

(23)

其中，kp1i,kp2i,kd1i,kd2i,αi,βi(i=1,2,…,n)均大于零。

(24)

则对式(24)中的Lyapunov函数求导可得：

(24)

(26)

(27)

将Kp、Kd、Bp(e)、Bd(e)的具体表达式代入式(27)中，可得：

(28)

当γ满足一定的取值范围时，可以使得：

(29)

若要使SCARA机械手系统全局渐近稳定，则取kp1i、kd1i、αi、βi需要满足以下条件：

(30)

此时：

(31)

即系统全局渐近稳定。

4.2 实验结果分析

观察图7～图9可以发现，在有外部扰动且外部扰动上确界已知的情况下，基于自适应鲁棒PD控制的控制策略能够有效地消除有界外部扰动和SCARA机械手本身非线性的影响，关节的位置跟踪误差较小。其中，关节1的最大位置跟踪误差率大约为8.11%，而关节2的最大位置跟踪误差率仅为0.95%左右，两个关节均可以较好地跟踪期望轨迹，保证了SCARA机械手系统的快速收敛及稳定，从而验证了自适应鲁棒PD控制策略是有效可行的。

图7 SCARA机械手关节1的轨迹跟踪曲线

图8 SCARA机械手关节2的轨迹跟踪曲线

图9 SCARA机械手关节1和关节2的位置跟踪误差曲线

5 结论

针对人工采茶劳动力短缺和机械采茶质量低的现状，本文基于智能采茶机器人整体性能的考虑，采用多个SCARA机械手交错排列作为采摘机器人的采摘机构。首先采用D-H法建立了SCARA机械手的正逆向运动学模型，并通过MATLAB Robotic Toolbox工具箱建立了机械手的空间仿真模型；其次采用五次多项式插值算法对机械手的大臂旋转关节和小臂旋转关节进行了关节轨迹规划，轨迹规划的结果表明采用五次多项式插值算法是合理的，可以使得机械手关节在运动过程中平滑连续无冲击；最后针对SCARA机械手自身的非线性和耦合性以及外部扰动的影响，设计了一种自适应鲁棒PD控制器，在X0O0Y0平面内对机械手的大臂旋转关节和小臂旋转关节的运动轨迹进行了位置跟踪控制，仿真结果表明自适应鲁棒PD控制策略可以很好地消除动力学模型中不确定因素的影响，使得系统各个关节对期望轨迹的追踪误差较小，能够稳定运行。根据由运动学分析确定的机械手尺寸，目前正在进行采茶机器人各个零部件的三维结构设计。因名优茶采摘具有相对复杂性和综合性等特点，未来将研究采茶机器人的避障算法和视觉图像处理算法，进一步提升茶叶采摘的准确率和效率；并将进一步研究利用RBF神经网络补偿的有限时间内SCARA采摘机械手的运动控制，对比分析多种控制器在不同情况下轨迹跟踪控制的效果。