履帶式移動采摘機(jī)械手控制算法及仿真研究

張國華，汪木蘭，賈茜，b，魏浩然

(南京工程學(xué)院 a. 江蘇省先進(jìn)數(shù)控技術(shù)重點(diǎn)實(shí)驗(yàn)室； b. 工業(yè)中心，江蘇 南京 211167)

0 引言

在現(xiàn)代化農(nóng)業(yè)生產(chǎn)中，農(nóng)業(yè)機(jī)械化作為中心環(huán)節(jié)，它凝聚著現(xiàn)代科學(xué)技術(shù)的最新成果，與農(nóng)業(yè)生物技術(shù)等農(nóng)業(yè)技術(shù)相結(jié)合，可改變生產(chǎn)模式，提高勞動生產(chǎn)率。

農(nóng)業(yè)機(jī)器人作為農(nóng)業(yè)機(jī)械化的產(chǎn)物，在農(nóng)業(yè)生產(chǎn)中起著關(guān)鍵性的作用。在當(dāng)今老齡化加劇的社會背景下，單純的手工水果采摘已不能滿足需求，為解決農(nóng)業(yè)采摘中的實(shí)際問題，采摘機(jī)器人的研究與應(yīng)用已成為一種迫切需要。

為更好地實(shí)現(xiàn)農(nóng)業(yè)采摘工作，研制了一種多用途履帶式移動平臺采摘機(jī)械手，將履帶車與機(jī)械手結(jié)合起來。機(jī)械手進(jìn)行一定的操作，履帶車輛用于搬運(yùn)和支撐機(jī)械手，這種組合極大地擴(kuò)展了整個機(jī)器人的工作空間和應(yīng)用范圍[1-2]。但是由于履帶車輛與機(jī)械手之間的相互作用，建立這種集成結(jié)構(gòu)的運(yùn)動學(xué)模型是一項(xiàng)具有挑戰(zhàn)性的任務(wù)[3]。輪式移動機(jī)械手的建模、仿真和控制方面已有大量的研究報道，但對履帶式移動機(jī)械手的研究較少[4]。PENG J Z等人提出了一種自適應(yīng)滑模跟蹤控制器來處理位置跟蹤的問題[5]，YAMAMOTO Y等人將移動機(jī)械手的跟蹤任務(wù)進(jìn)行分解，分別完成移動平臺的跟蹤任務(wù)和機(jī)械手的跟蹤任務(wù)，設(shè)計解耦控制器對移動機(jī)械手分別進(jìn)行控制[6]。

本文主要介紹了履帶式移動機(jī)械手的硬件結(jié)構(gòu)組成，對履帶式移動機(jī)械手模型進(jìn)行了簡化，對履帶式移動機(jī)械手的運(yùn)動控制進(jìn)行設(shè)計與驗(yàn)證，實(shí)現(xiàn)履帶式移動機(jī)械手的精確控制。

1 履帶式移動平臺采摘機(jī)械手結(jié)構(gòu)及其建模

1.1 履帶式移動平臺采摘機(jī)械手系統(tǒng)結(jié)構(gòu)

本文分析的履帶式移動平臺采摘機(jī)械手的車體為輕型履帶車輛，由底盤、兩條履帶、兩個驅(qū)動輪、兩個支承輪和兩個行星輪組成，如圖1(a)所示。兩個驅(qū)動輪可以獨(dú)立控制實(shí)現(xiàn)轉(zhuǎn)向，兩個行星輪由同一個電機(jī)驅(qū)動，保證兩條軌道在配置變化時同步通過障礙物。本文研究的是履帶式移動機(jī)械手在剛性水平面上的運(yùn)動，因此行星輪被延長和鎖定，它們可以被模擬成支撐輪。由于未研究移動平臺的卸載問題，因此可以假定左右履帶的橫向滑移保持一致。此外，假設(shè)履帶不能伸展，驅(qū)動輪和履帶之間沒有滑動。

為了方便將履帶式移動平臺采摘機(jī)械手的模型建立運(yùn)動學(xué)模型，故將圖1(a)中埃夫特履帶移動機(jī)械手模型簡化如圖1(b)所示。

圖1 履帶式移動機(jī)械手及簡化模型

1.2 運(yùn)動學(xué)建模

履帶移動平臺簡化模型如圖2所示，相關(guān)參數(shù)定義見表1。

圖2 移動平臺簡化模型

表1 履帶移動平臺參數(shù)定義表

根據(jù)表1中定義的參數(shù)得到履帶式移動平臺的速度為：

(1)

(2)

(3)

由式(3)可得履帶式移動平臺的運(yùn)動模型為

(4)

履帶移動平臺的姿態(tài)由xp、yp、θp控制。

機(jī)械手簡化坐標(biāo)圖如圖3所示，根據(jù)機(jī)械手坐標(biāo)系建立機(jī)械手矩陣參數(shù)表如表2所示。

圖3 機(jī)械手簡化坐標(biāo)圖

表2 機(jī)械手矩陣參數(shù)表

將矩陣參數(shù)表中的參數(shù)代入得到每兩個關(guān)節(jié)之間的變換矩陣Ri，將各個Ri依次相乘，得到移動機(jī)械手的固定端和末端執(zhí)行器的總變換齊次矩陣RA。各個變換矩陣和齊次矩陣如下：

其中：

l1=0.01 m,l2=0.53 m,l3=0.5 m,l4=0.07 m

Uz=sin(θ5+θ4+θ3+θ2)

Vz=cos(θ5+θ4+θ3+θ2)

Wx=sinθ1,Wy=-cosθ1,Wz=0

Px=0.09×sinθ1+0.25×cos(-θ4-θ3-θ2+θ1)+

0.25×cos(θ4+θ3+θ2+θ1)+0.25×

cos(-θ3-θ2+θ1)+0.25×cos(θ3+θ2+θ1)+0.261 5×

cos(θ1-θ2)+0.261 5×cos(θ2+θ1)

Py=-0.09×cosθ1+0.25×sin(θ4+θ3+θ2+θ1)+

0.25×sin(-θ4-θ3-θ2+θ1)+0.25×

sin(θ3+θ2+θ1)+0.25×sin(-θ3-θ2+θ1)+0.261 5×

sin(θ1+θ2)+0.261 5×sin(θ1-θ2)

Pz=0.5sin(θ4+θ3+θ2)+0.5×sin(θ3+θ2)+0.523×sinθ2

由于關(guān)節(jié)5中θ5的運(yùn)動只影響機(jī)械手的姿態(tài),不影響其位置，故此處將其省略。該關(guān)節(jié)的長度加至關(guān)節(jié)4中，由上述公式推算可得機(jī)械手的空間位置由θ1、θ2、θ3、θ4控制。

2 履帶式移動平臺的自適應(yīng)模糊運(yùn)動控制

2.1 模糊控制基本介紹

首先根據(jù)經(jīng)驗(yàn)編寫模糊控制規(guī)則，將測量控制對象的狀態(tài)模糊化，得到的模糊值完成模糊推理后，將推理后的值進(jìn)行非模糊化處理轉(zhuǎn)化為非模糊值控制輸出?；谀：碚摵腿斯そ?jīng)驗(yàn)，不需要數(shù)學(xué)模型，易于理解，魯棒性好[7-8]。

2.2 履帶式移動平臺的自適應(yīng)模糊運(yùn)動控制策略

在平臺控制中，分解為平臺位置和姿態(tài)變量的控制。

履帶平臺運(yùn)動控制系統(tǒng)設(shè)計如圖4所示。

圖4 履帶移動平臺模糊控制示意圖

圖5 直接自適應(yīng)模糊控制

在控制器1中，ex的論域是[-1,1]，ecx的論域是[-0.1,0.1]，Ux的論域?yàn)閇-3,3]。

在控制器2中，ey的論域是[-1,1]，ecy的論域是[-0.1,0.1]，Uy的論域?yàn)閇-3,3]。

對于控制器3來說：eθp的論域是[-0.1,0.1]，ecθp的論域是[-0.01,0.01]，Uθp的論域?yàn)閇-1,1]。

考慮到系統(tǒng)的精度要求，各語言變量的量化水平設(shè)置為11。每個語言變量域的模糊子集數(shù)為7，分別為NB、NM、NS、ZE、PS、PM、PB。

所選變量的具體隸屬度函數(shù)如圖6所示。

圖6 ex、ey、eθp、ecx、ecy、ecθp、Ux、Uy、Uθp隸屬度函數(shù)圖

為了提高系統(tǒng)穩(wěn)態(tài)精度，增加模糊控制的分辨率，根據(jù)模糊控制的規(guī)則，通過對履帶移動平臺全部輸入變量量化后的全部組合，根據(jù)特定的一些模糊推理進(jìn)行具體計算，得出各個狀態(tài)的輸出[9]。模糊控制規(guī)則表如表3所示。

表3 履帶移動平臺自適應(yīng)模糊控制規(guī)則表

3 機(jī)械手的RBF神經(jīng)網(wǎng)絡(luò)自適應(yīng)運(yùn)動控制

3.1 RBF神經(jīng)網(wǎng)絡(luò)

RBF神經(jīng)網(wǎng)絡(luò)是一種性能好、全局逼近且不受局部極小問題的前向網(wǎng)絡(luò)模型。它分為三層：輸入層、具有非線性PBF激活函數(shù)的隱層和線性輸出層[10]。

利用RBF作為隱單元的“基”，形成隱層空間使輸入向量直接映射到隱空間是RBF網(wǎng)絡(luò)的基本思想。在確定RBF中心的同時也能夠確定映射關(guān)系。神經(jīng)網(wǎng)絡(luò)的線性輸出加權(quán)和是網(wǎng)絡(luò)可調(diào)參數(shù)?？梢钥闯?，一般情況下，對于可調(diào)參數(shù)而言，網(wǎng)絡(luò)的輸出是線性的，但是網(wǎng)絡(luò)從輸入到輸出的映射又是非線性的。利用線性方程組直接求解網(wǎng)絡(luò)的權(quán)值，同時提升了學(xué)習(xí)的效率和速度，并且避免出現(xiàn)局部極小化問題。

3.2 履帶式移動平臺的機(jī)械手RBF神經(jīng)網(wǎng)絡(luò)運(yùn)動控制策略

機(jī)械手的控制器的設(shè)計如圖7所示，其中θ1d、θ2d、θ3d、θ4d分別為關(guān)節(jié)1、2、3、4的轉(zhuǎn)角期望值；e1、e2、e3、e4為各個關(guān)節(jié)的轉(zhuǎn)角誤差；u1、u2、u3、u4分別為神經(jīng)網(wǎng)絡(luò)控制器的輸出；θ1、θ2、θ3、θ4為4個關(guān)節(jié)的實(shí)際轉(zhuǎn)角。

圖7 機(jī)械手臂的控制器

RBF神經(jīng)網(wǎng)絡(luò)的學(xué)習(xí)過程分為兩個部分，一部分為無教師學(xué)習(xí)過程，另一部分為有教師學(xué)習(xí)過程。其過程如下：

a)無教師學(xué)習(xí)：k-均值聚類算法在這一階段，用于適當(dāng)調(diào)整隱層的中心向量，即在訓(xùn)練過程中，對所有的向量進(jìn)行分組，找到最佳的中心向量，使每個樣本與中心的距離最小化。

1) 機(jī)械手關(guān)節(jié)角度跟蹤誤差：

ei=θid-θi,i=1,2,3,4

(5)

2) 控制器的目標(biāo)函數(shù)：

(6)

3) 控制律設(shè)計：

(7)

4)在隱藏層設(shè)置初始值和學(xué)習(xí)率η(0)，(0<η(0)<1)和每個節(jié)點(diǎn)的閾值ε，中心向量ci(0)(i=1,2,3,4)。

5)歐氏距離確定，求出最小距離節(jié)點(diǎn)

(8)

其中k是樣本序數(shù)。

6)調(diào)整中心

(9)

7)聚類質(zhì)心判定

b)有教師學(xué)習(xí)：確定中心向量ci后，對訓(xùn)練后的兩層之間進(jìn)行加權(quán)。權(quán)值wki的學(xué)習(xí)算法:

wki(k+1)=wki(k)+η(tk-yk)hi(x)/HTH

其中H=[h1(x),h2(x),h3(x),h4(x)]T，hi(x)為高斯基函數(shù)。

確定隱層參數(shù)后，對照樣本，確定隱層和輸出層的網(wǎng)絡(luò)權(quán)值。

4 履帶式移動平臺采摘機(jī)械手的運(yùn)動控制仿真分析

4.1 履帶式移動平臺的運(yùn)動控制仿真

選擇通用性履帶移動平臺的車體尺寸，長度為680 mm，寬度405 mm，高度270 m。規(guī)定移動平臺只在水平面上運(yùn)動，即ZM(0)=0。以移動平臺和機(jī)械臂連接點(diǎn)的軌跡作為參考點(diǎn)進(jìn)行跟蹤。事先設(shè)置一定的初始條件，才能有效地進(jìn)行實(shí)驗(yàn)驗(yàn)證，此處的初始條件設(shè)置為：

xp(0)=0,yp(0)=0,θp(0)=0.014。

設(shè)置期望軌跡為：

實(shí)際跟蹤控制軌跡運(yùn)行結(jié)果如圖8所示。圖9為x方向和y方向的位置跟蹤情況。其中，實(shí)線為x軸位置跟蹤情況，虛線為y軸位置跟蹤情況。

圖8 移動平臺圓形軌跡的控制跟蹤效果圖

圖9 x、y方向的位置跟蹤情況

4.2 機(jī)械手運(yùn)動控制仿真

預(yù)先設(shè)定各關(guān)節(jié)角初始條件：qeo=[1,1]T，

θ1=0.263 8，θ2=0.257 2，θ3=0.764 5，θ4=0.512 2。

末端執(zhí)行器初始位置，設(shè)置期望軌跡：xe=cost,ye=sint,ze=1。機(jī)械手的RBF神經(jīng)網(wǎng)絡(luò)仿真結(jié)果如圖10所示。圖11為仿真過程中機(jī)械手x方向運(yùn)動的位置誤差，y、z方向的誤差與x方向近乎相同。

圖10 機(jī)械手的RBF神經(jīng)網(wǎng)絡(luò)仿真結(jié)果

圖11 手臂x方向的位置誤差

4.3 履帶移動平臺及機(jī)械手的仿真數(shù)據(jù)分析

在x方向預(yù)期的理想情況為標(biāo)準(zhǔn)的余弦函數(shù)，在y方向預(yù)期的理想情況為標(biāo)準(zhǔn)的正弦函數(shù)。由圖8看出，平臺的實(shí)際運(yùn)行路徑基本上可以遵循預(yù)先設(shè)定的路徑，過了起始位置，正常運(yùn)行后運(yùn)動誤差在可控的波動范圍內(nèi)，滿足系統(tǒng)的可控性。由此可見，基于自適應(yīng)模糊控制器對履帶移動平臺的設(shè)計能夠滿足預(yù)定的控制要求，并完成相應(yīng)的控制功能，是一種合理的設(shè)計方法。

由圖10可以看到在初始位置之后機(jī)械手基本按照設(shè)定的預(yù)期軌跡進(jìn)行運(yùn)動。圖11中，在A點(diǎn)之后，機(jī)械手的運(yùn)動趨于穩(wěn)定的預(yù)定軌跡中，且誤差范圍較小，基本可以滿足預(yù)設(shè)的運(yùn)動操作需求。由此可見，該算法是合理可行的，經(jīng)過驗(yàn)證是有效的。

5 結(jié)語

使用MATLAB軟件對履帶式移動平臺采摘機(jī)械手的運(yùn)動控制問題進(jìn)行了模擬驗(yàn)證分析。研究發(fā)現(xiàn)：自適應(yīng)模糊控制和RBF神經(jīng)網(wǎng)絡(luò)控制算法可以對履帶式移動平臺和機(jī)械手進(jìn)行精確控制，對履帶式移動機(jī)械手模型進(jìn)行了簡化后，先計算履帶式移動平臺采摘機(jī)械手的運(yùn)動學(xué)方程，再對履帶式移動平臺和機(jī)械手分別提出模糊控制和RBF神經(jīng)網(wǎng)絡(luò)控制，運(yùn)用MATLAB對其進(jìn)行了實(shí)驗(yàn)數(shù)據(jù)模擬仿真，最后對實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析，分別驗(yàn)證了所提算法的可行性。該方法為農(nóng)業(yè)機(jī)械工程化提供依據(jù)，也可為坦克等軍用車輛的控制提供幫助。