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

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

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

0 引言

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

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

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

1 工作空間分析

1.1 水下機械臂運動學分析及仿真

本文研究的水下機械臂主要包括6個旋轉關節和1個末端水下機械手，如圖1所示。

圖1 水下機械臂結構

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

圖2 D-H法則相鄰連桿坐標系關系

表1 水下機械臂各連桿參數

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

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

(1)

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

將各個關節位姿矩陣依次相乘，便可得到機械臂末端的位姿矩陣為

(2)

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

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

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

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

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

圖4 XOY面運動空間云圖

圖5 XOZ面運動空間云圖

圖6 YOZ面運動空間云圖

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

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

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

水下機械臂的動力學模型為

(3)

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

(4)

將式(4)代入式(3)中，可得閉環系統式為

(5)

(6)

采用RBF神經網絡逼近Δf，其表達式為

(7)

將式(6)和式(7)聯立可得

(8)

(9)

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

(10)

d=εf

對系統式(9)設計如下自適應律：

(11)

設計反饋控制律為

(12)

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

2.2 仿真與分析

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

(13)

基于HJI理論的RBF神經網絡自適應控制器的有關參數為：α=20，γ=0.005，ci=[-1.5,-1,-0.5,0,0.5,1,1.5] ，bi=10，其中涉及到自適應律的η=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為機械臂中點長度值。根據這些參數,對二自由度水下機械臂在不同作業情況下的運動情況進行分析。

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

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

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

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

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

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

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

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

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

(14)

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

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

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

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

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

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

水下機械臂關節1和關節2的運動規律關系到末端執行器的運動軌跡，由水下機械臂系統的坐標可以得到機械臂在垂直面坐標表達式為

(15)

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

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

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

3 結束語

通過對水下機械臂的運動學分析，設計基于HJI理論的RBF神經網絡自適應控制器，并進行仿真驗證。

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

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

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