水下機械臂運動空間分析與軌跡跟蹤算法優(yōu)化

樊智敏，哈振騫，薛福峰,王明凱

(青島科技大學(xué)機電工程學(xué)院，山東 青島 266061)

0 引言

隨著海洋開發(fā)與研究蓬勃發(fā)展，無人航行器搭載機械手在進行水下打撈、援潛救生、海底設(shè)施維護與裝置回收、海底生物及巖石標本的采樣等方面得到了越來越廣泛的應(yīng)用。目前，發(fā)展趨勢是研制適合多種水下作業(yè)的航行器，而其末端執(zhí)行器(機械臂)或手爪是水下作業(yè)航行器執(zhí)行作業(yè)的關(guān)鍵部分[1-3]。相比陸地上的移動機械臂和空間機械臂，水下機械臂容易受到海浪、波浪和洋流等環(huán)境的干擾，使末端執(zhí)行器不能達到預(yù)期位置。由于水下機械臂運動的不確定性，導(dǎo)致在運動過程中受力方向也各不相同，再加上多種不可預(yù)見的外界干擾，在建立水下機械臂動力學(xué)模型時，需要充分考慮各種水動力的影響以及各力之間的耦合[4]。因此，在水下機械臂的控制器設(shè)計中要考慮到運動非線性、受到的干擾力不確定等特點。

蒙特卡洛法是通過D-H法則建立機械臂的關(guān)節(jié)變量與工作空間的映射關(guān)系，再通過正解運動學(xué)方程得到坐標值，這些坐標值的集合組成了機械臂的工作空間[5-9]。此方法為研究水下機械臂的工作空間提供了理論基礎(chǔ)。

水下機械臂具有高度非線性、強耦合性和多輸入多輸出的時變性等特點，常規(guī)的線性控制方法不能滿足控制要求。趙克楠[10]提出模糊PID控制方法，通過模糊規(guī)則調(diào)節(jié)PID控制器參數(shù)，實現(xiàn)參數(shù)根據(jù)位置誤差和位置誤差變化率的動態(tài)調(diào)節(jié)；Londhe等[11]將水下機械臂位置誤差和位置誤差變化率輸入到模糊PID控制器中，進行參數(shù)的優(yōu)化；Filaretov等[12]采用多鏈路的方法對水下機械臂運動進行補償，實現(xiàn)水下機械臂運動的自動校正；Barbalata等[13]采用低水平位置/力控制結(jié)構(gòu)，結(jié)合流體動力學(xué)提出一種在沒有力-扭矩傳感器情況下計算末端執(zhí)行器之間相互作用力的方法；陳顯婷等[14]采用模糊控制與CMAC控制方法相結(jié)合的復(fù)合控制策略，對水下機械臂進行控制；邱紅祥[15]設(shè)計基于固定增益的PD控制器，將機械手空載作業(yè)下的參數(shù)作為固定增益的作業(yè)參數(shù)，以此滿足系統(tǒng)的穩(wěn)定性；王亞猛[16]用PWM高速電磁開關(guān)閥和具有抑制水液壓缸摩擦力影響的魯棒控制器控制水液壓缸，以驅(qū)動具有3個自由度兩關(guān)節(jié)的機械臂。上述研究都是通過改進誤差進行的優(yōu)化設(shè)計，水下環(huán)境復(fù)雜，對外界干擾無法預(yù)測，可能導(dǎo)致誤差較大。本文提出一種基于RBF神經(jīng)網(wǎng)絡(luò)的魯棒控制算法，針對干擾項的不確定模型進行逼近，提升水下機械臂的控制性能。

1 工作空間分析

1.1 水下機械臂運動學(xué)分析及仿真

本文研究的水下機械臂主要包括6個旋轉(zhuǎn)關(guān)節(jié)和1個末端水下機械手，如圖1所示。

圖1 水下機械臂結(jié)構(gòu)

為了描述相鄰桿件之間的關(guān)系，基于D-H法則對水下機械臂進行建模。D-H法則描述相鄰桿件之間坐標系之間的關(guān)系如圖2所示。其中，i(i=1,2,3,4,5,6)為關(guān)節(jié)數(shù)；ai為沿著Xi軸從Zi移動到Zi+1的距離；αi為繞著Xi軸從Zi轉(zhuǎn)到Zi+1的角度；di為沿著Zi軸從Xi-1到Xi的距離；θi為繞著Zi軸從Xi-1到Xi的角度。表1是基于D-H法則各連桿的參數(shù)表。其中，假設(shè)第4節(jié)和第5節(jié)質(zhì)心重合，兩者坐標系原點重合。

圖2 D-H法則相鄰連桿坐標系關(guān)系

表1 水下機械臂各連桿參數(shù)

θidi/mmαi-1/(°)ai-1/mmθimin/(°)θimax/(°)θ112600-6060θ29390930120θ37930793-6060θ4633-900-180180θ5900-9090θ6457-9000360

機械臂的運動學(xué)分析是機械臂研究的前提，是機械臂工作空間分析和運動控制的基礎(chǔ)。根據(jù)連桿參數(shù)和D-H法則建立的坐標系，機械臂相鄰關(guān)節(jié)坐標系i-1和i之間位姿變換矩陣為

(1)

式中，cθi=cosθi，sθi=sinθi，cαi-1=cosαi-1，sαi-1=sinαi-1。

將各個關(guān)節(jié)位姿矩陣依次相乘，便可得到機械臂末端的位姿矩陣為

(2)

[pxpypz]T為末端執(zhí)行器相對基座坐標系的位置向量；[r11r21r31]T，[r12r22r32]T和[r13r23r33]T為末端執(zhí)行器相對基座坐標系的位姿向量。

1.2 水下機械臂工作空間分析

描述水下機械臂的工作空間，可以不用考慮末端執(zhí)行器位姿的關(guān)系，所以根據(jù)水下機械臂的運動學(xué)正解，可以求出水下機械臂的位置向量[pxpypz]T。根據(jù)各個關(guān)節(jié)限位在MATLAB中利用Rand函數(shù)得到隨機值，將隨機值代入到運動學(xué)正解方程中，得到相應(yīng)點的位置向量，最后得到水下機械臂的工作空間云圖，如圖3～圖6所示。

由圖4～圖6可知，水下機械臂的工作范圍為x∈[-1 250,1 500]，y∈[-1 500,1 500]，z∈[-1 000,1 250]；水下機械臂的工作空間近似一個橢球體，空間符合各個關(guān)節(jié)實際運動空間，模擬出的工作空間結(jié)構(gòu)緊湊，能夠真實地描述水下機械臂的工作空間。

圖3 水下機械臂運動空間云圖

圖4 XOY面運動空間云圖

圖5 XOZ面運動空間云圖

圖6 YOZ面運動空間云圖

2 水下機械臂的軌跡跟蹤控制

關(guān)節(jié)空間軌跡跟蹤是給定機械手各個關(guān)節(jié)的驅(qū)動力和力矩，嚴格地控制各個關(guān)節(jié)的運動軌跡，使各個關(guān)節(jié)的速度和位移等狀態(tài)量能夠跟隨理想的運動軌跡。水下機械臂是一個復(fù)雜的多輸入多輸出的非線性系統(tǒng)，建立的動力學(xué)模型必然存在著不確定性，傳統(tǒng)的控制算法難以得到理想控制效果。本文采用基于HJI理論的RBF神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制算法，并以雙關(guān)節(jié)水下機械臂為例驗證該控制算法的有效性。

2.1 水下機械臂軌跡跟蹤控制

水下機械臂的動力學(xué)模型為

(3)

定義理想跟蹤軌跡為qd，跟蹤誤差為e=q-qd，反饋控制律為u，則設(shè)計前饋控制律為

(4)

將式(4)代入式(3)中，可得閉環(huán)系統(tǒng)式為

(5)

(6)

采用RBF神經(jīng)網(wǎng)絡(luò)逼近Δf，其表達式為

(7)

將式(6)和式(7)聯(lián)立可得

(8)

(9)

利用HJI不等式將式(9)改寫為

(10)

d=εf

對系統(tǒng)式(9)設(shè)計如下自適應(yīng)律：

(11)

設(shè)計反饋控制律為

(12)

最后可以得出閉環(huán)系統(tǒng)式(5)滿足J≤γ。可以利用Lyapunov函數(shù)驗證分析該控制方法的穩(wěn)定性，在這里不過多贅述。

2.2 仿真與分析

根據(jù)文獻[18]中對于水下二自由度水下機械臂動力學(xué)模型和實驗研究，可以得到水動力摩擦系數(shù)CD和附加質(zhì)量系數(shù)CA一般取常數(shù)，通常情況下取值為CD=1.1和CA=1?？紤]到實際控制中可靠性和簡單性原則，一般的水下機械臂連桿長度大多小于2 m，此時水下機械臂水動力分析主要對重要組成部分進行補償。這里對二自由度水下機械臂水動力項取近似表達式為

(13)

基于HJI理論的RBF神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制器的有關(guān)參數(shù)為：α=20，γ=0.005，ci=[-1.5,-1,-0.5,0,0.5,1,1.5] ，bi=10，其中涉及到自適應(yīng)律的η=1 500。

取機械臂參考值：m1=15 kg，m2=15 kg，l1=1 m，l2=1 m，r1=0.5 m，r2=0.5 m，g=9.8 m/s2，其中r1，r2為機械臂中點長度值。根據(jù)這些參數(shù),對二自由度水下機械臂在不同作業(yè)情況下的運動情況進行分析。

2.2.1 基于水動力作用下軌跡跟蹤控制

通過水下機械臂相關(guān)參數(shù)設(shè)定，結(jié)合該控制器控制效果的仿真結(jié)果進行分析，如圖7所示，水下機械臂兩關(guān)節(jié)通過初始階段的調(diào)整后，均能較好地跟蹤既定角度，且有較高的控制精度和較快的響應(yīng)時間?？刂破黜憫?yīng)時間較快，0.1 s內(nèi)即完成了調(diào)整準備工作，其中關(guān)節(jié)1能夠以±0.002 rad的精度跟蹤角度，具有較好控制效果。相比關(guān)節(jié)1而言，關(guān)節(jié)2的耦合項較少，其控制效果更好，關(guān)節(jié)2的控制精度近乎為0。兩關(guān)節(jié)均表現(xiàn)出了較好的跟蹤性能。

圖7 水下機械臂角度跟蹤曲線(無干擾)

機械臂兩關(guān)節(jié)角速度跟蹤效果如圖8所示，各關(guān)節(jié)的起始速度為0，速度跟蹤效果較好，能夠依照設(shè)定速度完成跟蹤，未出現(xiàn)速度突變情況，控制效果良好。跟蹤角速度誤差較小，可以忽略角速度變化，因此能夠較為平順地完成作業(yè)任務(wù)。

圖8 水下機械臂角速度跟蹤曲線(無干擾)

水下機械臂運動時會受到水動力力矩影響，本文采用近似表達式模擬水動力作用對水下機械臂的影響，機械臂運動時各關(guān)節(jié)均會受到水流影響，其影響也與機械臂的運動速度和尺寸相關(guān)聯(lián)。由于只考慮水動力的作用，并未將系統(tǒng)模型的不確定性和外界干擾考慮在內(nèi)，此時所涉及的RBF神經(jīng)網(wǎng)絡(luò)控制器逼近對象為機械臂關(guān)節(jié)受到的水動力力矩。仿真結(jié)果如圖9所示。不考慮初始調(diào)整階段，關(guān)節(jié)1逼近的水動力力矩效果較差，究其原因是由于關(guān)節(jié)1耦合項較多影響較大，但根據(jù)角度跟蹤效果和速度跟蹤效果來看，也實現(xiàn)了對關(guān)節(jié)1的補償控制；關(guān)節(jié)2逼近的水動力力矩效果較好，可以實現(xiàn)對關(guān)節(jié)2的補償控制。總體來說，在只考慮水動力作用因素影響下，所設(shè)計的控制器能夠較好實現(xiàn)補償任務(wù)，控制表現(xiàn)良好。

圖9 控制器逼近水動力曲線

2.2.2 基于模型不確定性和外界未知干擾下軌跡跟蹤控制

針對系統(tǒng)模型不確定性作如下假設(shè)：假設(shè)模型不確定性部分作為外界干擾力矩輸入到系統(tǒng)中，與水動力共同作用分別施加到關(guān)節(jié)1和關(guān)節(jié)2上。設(shè)關(guān)節(jié)1和關(guān)節(jié)2的模型不確定性和外界干擾的總干擾假設(shè)為

(14)

基于模型不確定性和外界干擾下的水下機械臂角度跟蹤效果如圖10所示，關(guān)節(jié)1和關(guān)節(jié)2有不俗的跟蹤效果。

圖10 水下機械臂角度跟蹤曲線(有干擾)

結(jié)合跟蹤誤差圖11所示可知，關(guān)節(jié)1的跟蹤誤差有微小波動可忽略，關(guān)節(jié)2表現(xiàn)出更好的控制效果，表現(xiàn)出控制器良好的控制跟蹤特性。角速度跟蹤效果如圖12所示，關(guān)節(jié)1和關(guān)節(jié)2角度的跟蹤均表現(xiàn)良好跟蹤效果，未出現(xiàn)明顯尖角由此證明能夠平穩(wěn)完成跟蹤任務(wù)。通過神經(jīng)網(wǎng)絡(luò)控制器的逼近能力，對水下機械臂系統(tǒng)模型不確定性和未知外界干擾進行逼近，逼近效果如圖13所示。由圖13可知，關(guān)節(jié)2的逼近效果明顯要優(yōu)于關(guān)節(jié)1的逼近效果，這是因為在關(guān)節(jié)1處參數(shù)的耦合效應(yīng)更明顯；根據(jù)角度跟蹤效果和角速度跟蹤效果來看，關(guān)節(jié)1處也能實現(xiàn)逼近效果并完成補償任務(wù)，關(guān)節(jié)2處也有不錯的逼近效果。由此證明該控制器能夠逼近系統(tǒng)模型不確定性和外界不確定干擾，并且能夠完成對水下機械臂系統(tǒng)的補償任務(wù)，有較好的控制性能。

圖11 水下機械臂角度跟蹤誤差(有干擾)

圖12 水下機械臂角速度跟蹤曲線(有干擾)

圖13 控制器逼近外界總干擾曲線

水下機械臂關(guān)節(jié)1和關(guān)節(jié)2的運動規(guī)律關(guān)系到末端執(zhí)行器的運動軌跡，由水下機械臂系統(tǒng)的坐標可以得到機械臂在垂直面坐標表達式為

(15)

根據(jù)設(shè)定的初始值與機械臂運動規(guī)律可知，水下機械臂末端執(zhí)行器初始坐標為(x(0),z(0))=(0.99,0)。將仿真結(jié)果轉(zhuǎn)換到末端執(zhí)行器的運動軌跡如圖14所示。由圖14可知，在開始階段運動軌跡是有較大的偏差，這是因為控制器處在調(diào)整期，屬于系統(tǒng)正常誤差范圍，進入穩(wěn)定期后跟蹤軌跡基本與理想軌跡重合，跟蹤效果良好，在交匯點也未出現(xiàn)跟蹤錯誤，能夠正確跟蹤軌跡，印證該控制器良好的控制效果。

圖14 機械臂末端軌跡跟蹤效果

無論是水動力的影響，還是模型不確定性或外界未知干擾的影響，該控制器均表現(xiàn)出較好的控制效果，通過末端執(zhí)行器的跟蹤軌跡驗證，證明本文所設(shè)計的基于HJI理論的RBF神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制器具有良好的魯棒性。

3 結(jié)束語

通過對水下機械臂的運動學(xué)分析，設(shè)計基于HJI理論的RBF神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制器，并進行仿真驗證。

以六自由度水下機械臂為例，基于D-H法則分析了機械臂的運動學(xué)，通過仿真驗證了其正確性，通過MATLAB構(gòu)建了水下六自由度機械臂的運動空間云圖，真實體現(xiàn)水下機械臂的運動空間。

針對水下機械臂模型不確定性和外界未知干擾的問題，提出基于HJI理論的RBF神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制方法，以水下二自由度機械臂為例進行驗證分析。在無外界干擾情況下控制器能夠很好逼近水動力力矩曲線，對機械臂施加控制補償。

為進一步確認控制器的控制效果，在水動力作用和系統(tǒng)模型不確定性及未知外界干擾共同作用情況下驗證該控制器，控制器表現(xiàn)出優(yōu)良的控制性能，具有較強的控制魯棒性。