采茶機器人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采摘機械手的運動控制，對比分析多種控制器在不同情況下軌跡跟蹤控制的效果。