葛大會, 尤云祥,3, 馮愛春,3

(1.上海交通大學(xué) 海洋工程國家重點實驗室,上海 200240;2.上海交通大學(xué) 船舶海洋與建筑工程學(xué)院,上海 200240;3.上海交通大學(xué)三亞崖州灣深?？萍佳芯吭?海南 三亞 572000)

0 引 言

在陸地上不可再生資源日漸減少的今天,對蘊藏豐富資源和能源的海洋空間的探索和開發(fā)十分必要且緊迫,在執(zhí)行海底勘測、水面打撈等各種各樣的作業(yè)任務(wù)時,水下機械臂是非常重要和必不可少的裝備[1]．不同于陸用機械臂的工作環(huán)境,水下機械臂在水下會受到水動力因素的影響以及多種難以預(yù)測的外部干擾[2]．目前大多對機械臂動力學(xué)建模采用Lagrange法[3-5]或Newton-Euler法[6-8],針對陸地機械臂和水下機械臂的控制方案越來越成熟,但對水下機械臂的控制或限于靜水情況,或?qū)⒑Ａ鬏d荷作為一種隨機性質(zhì)的擾動來考慮．不同傳統(tǒng)的AUV,對于用于船舶應(yīng)急維修的水下機械臂來說,機械臂一般固結(jié)于船舶底部基座上,受海流載荷的影響較大．董方方等[9]將Lagrange法與Udwadia-Kalaba (U-K)方法結(jié)合,建立了機械臂的動力學(xué)模型,減少了建立動力學(xué)模型所需要的計算量,然后通過數(shù)值仿真驗證了該方法所得到的動力學(xué)模型的準確性．韓江等[10]提出了一種基于Udwadia-Kalaba方程的魯棒伺服控制方法,使2自由度冗余驅(qū)動并聯(lián)機器人系統(tǒng)能夠精確跟蹤理想約束軌跡．寧會峰等[11]對大型光伏電站中光伏組件清掃機器人的結(jié)構(gòu)及其運動過程進行具體分析,并采用Lagrange法建立了機器人中清掃機械臂的動力學(xué)模型．Sciavicco等[12]利用Newton-Euler法對柔性機械臂進行了建模．

水下機械臂是一種高度非線性、強耦合、多輸入多輸出的系統(tǒng),具有時變、外部干擾和水動力干擾的特點,所以水下機械臂的精確動力學(xué)建模和控制問題成為一個熱點和難點,因此對水下機械臂的軌跡跟蹤進行研究具有重要的理論意義和實用價值．Zhong等[13]提出了一種改進的自適應(yīng)模糊滑?？刂?解決了多自由度水下機械臂系統(tǒng)中滑模控制的抖振問題和歸一化因子不可用的問題．在軌跡跟蹤方面,雖然實現(xiàn)了其機械臂系統(tǒng)能精確地跟蹤目標理想軌跡,但是其在建模過程中對水下環(huán)境做了靜水的假設(shè),這樣會使機械臂模型與實際存在誤差,進而影響控制精度．趙偉等[14]提出了一種RBF(radial basis function)滑?？刂撇呗?通過仿真對比證明了該算法可以較好地滿足水下機械臂系統(tǒng)的控制要求,相比傳統(tǒng)滑?？刂瓶s短了關(guān)節(jié)響應(yīng)時間并降低了穩(wěn)態(tài)誤差．他們所設(shè)計的控制算法可以滿足其機械臂系統(tǒng)的控制要求,但其同樣對水下環(huán)境做了靜水的假設(shè),會使實際中機械臂控制精度降低．孫國法等[15]設(shè)計了一種基于擾動觀測器的改進型非奇異快速終端滑模控制策略,很好地補償了機械臂系統(tǒng)在軌跡跟蹤中存在的建模誤差,并實現(xiàn)了精確軌跡跟蹤控制,但其將各種未知干擾作為一種隨機性質(zhì)的擾動來考慮會使控制精度降低．滑?？刂剖怯蒃meleyanov等在20世紀60年代提出的一類控制方法,經(jīng)過近60年的發(fā)展,已經(jīng)成為非線性控制理論體系的一個重要分支,也不斷涌現(xiàn)出各種各樣的新方法,如二階滑模、高階滑模、終端滑模等．通過趨近律來確定滑?？刂频目刂坡?不僅可以減少趨近階段的時間,提高系統(tǒng)的魯棒性,而且能夠有效地減弱滑?？刂浦械亩墩駟栴}[16]．Han等[17]提出了一種新的動態(tài)系數(shù)趨近律,加快了滑?？刂葡到y(tǒng)的響應(yīng)速度．Pan等[18]設(shè)計了一種基于改進的雙冪次趨近律積分的滑模控制算法,并將該算法應(yīng)用于開環(huán)不穩(wěn)定磁懸浮系統(tǒng)．Wang等[19]提出了一種基于改進趨近律的PID(proportional integral derivative)滑?？刂品椒?可以很好地應(yīng)用到光伏系統(tǒng)的超級降壓轉(zhuǎn)換器中．Sun等[20]提出了由一種基于混合趨近律的滑模控制和擴展滑模干擾觀測器組成的控制方法．綜上所述,目前水下機械臂控制的研究中大多將水下環(huán)境做了靜水的假設(shè)或?qū)⒑Ａ鲾_動簡化處理,這樣會降低模型精度,需進一步確定海流載荷的大小進而得到精確的機械臂動力學(xué)模型來提高控制精度．同時可以看出通過趨近律來確定滑?？刂频目刂坡煞浅７奖闱腋咝?而且還可以提高魯棒性和減弱抖振．

固定在船底部的機械臂有著各種各樣的用途,例如船舶維修[21]、水下抓取物件[22]、水下切割[23]、水下標本采集[24]等,這類機械臂作業(yè)時需要固結(jié)于船底的基座上,機械臂依賴穩(wěn)定的基座,需按照規(guī)劃的路線精準作業(yè)．實際水下環(huán)境中的機械臂在作業(yè)時常常遭受穩(wěn)定的海流影響,但是現(xiàn)今大多數(shù)研究中對水下環(huán)境做了靜水的假設(shè)并沒有考慮海流的影響,或只是把海流的干擾當(dāng)作一種簡單的隨機擾動來處理,這樣會使動力學(xué)模型精度不夠,進而會影響控制精度．故本文推導(dǎo)出與實際環(huán)境更接近的機械臂動力學(xué)模型,為提高軌跡跟蹤的精度打下基礎(chǔ)．現(xiàn)今對二關(guān)節(jié)機械臂的研究成果較多,但針對二關(guān)節(jié)水下機械臂的軌跡跟蹤控制的研究大多都著眼于控制算法的創(chuàng)新,而對機械臂本身的水下模型精度方面的考慮較少,故考慮海流載荷影響的二關(guān)節(jié)機械臂模型具有一定的創(chuàng)新意義．本文針對基座靜止的二關(guān)節(jié)機械臂,分別應(yīng)用Lagrange法和Newton-Euler法推導(dǎo)出其在均勻海流環(huán)境下的動力學(xué)模型,對比驗證了該模型的正確性．針對此模型分別進行了PD控制和滑?？刂蒲芯?發(fā)現(xiàn)滑?？刂颇艿玫綕M意的軌跡跟蹤控制效果．

1 海流環(huán)境下二關(guān)節(jié)機械臂動力學(xué)建模

圖1為海流環(huán)境下的二關(guān)節(jié)串聯(lián)機械臂結(jié)構(gòu),圖中m1和m2是機械臂連桿的質(zhì)量;l1和l2是連桿的長度;θ1和θ2是機械臂關(guān)節(jié)的轉(zhuǎn)動角度;τ1和τ2是關(guān)節(jié)的驅(qū)動力矩;x1和x2是沿連桿軸線方向的位移;(X1,Z1)和(X2,Z2)是連桿的質(zhì)心;g是重力加速度,取g=9.8 m/s2．假設(shè)機械臂連桿的質(zhì)量為均勻分布．本文水下機械臂的基座是固定的,同時簡化為在坐標系XOZ下考慮機械臂的運動．

圖1 二關(guān)節(jié)機械臂結(jié)構(gòu)Fig.1 Schematic of the 2-joint manipulator

水中作業(yè)的機械臂除受到重力等保守力作用外,還要受到海流載荷等非保守力的影響．在應(yīng)用Lagrange法或Newton-Euler法對二關(guān)節(jié)機械臂進行建模之前,需計算均勻海流對機械臂的作用力．機械臂在水下運動時所受到的作用力可以表示為

f=fd+fm+ff+fl,

(1)

式中,fd是水阻力;fm是附加質(zhì)量力;ff是浮力;fl是升力．升力是由于水下運動物體有異形沖角時產(chǎn)生的,本文研究的機械臂各連桿均為規(guī)則圓柱體,故升力可忽略不計．

Morison方程[25]給出了水阻力和附加質(zhì)量力的計算公式:

(2)

1.1 海流載荷

1.1.1 水阻力矩

當(dāng)機械臂在水中運動時,水由于其黏滯特性會引起水阻力．如圖1所示,根據(jù)式(2)右邊第一項可得二關(guān)節(jié)機械臂的水阻力矩分別為

(3)

(4)

式中,τd1和τd2是兩個關(guān)節(jié)的水阻力矩;D1和D2是機械臂連桿的直徑．

1.1.2 附加質(zhì)量力矩

當(dāng)機械臂在水下做加速運動時,由于機械臂和水之間的相對加速度產(chǎn)生的作用于機械臂的反作用力,稱為附加質(zhì)量力．如圖1所示,根據(jù)式(2)右邊第二項和第三項可得二關(guān)節(jié)機械臂的附加質(zhì)量力矩分別為

(5)

(6)

式中,τm1和τm2是兩個關(guān)節(jié)的附加質(zhì)量力矩．

1.1.3 浮力力矩

浮力是物體受到液體對其產(chǎn)生的豎直向上的力,假設(shè)機械臂各連桿的重心和浮心位置重合,又由于重力和浮力的方向相反,所以可以在原有的重力項上加一個修正因子來等效替代重力和浮力[27],可根據(jù)式(7)求得機械臂各連桿的等效重力為

(7)

式中,G是等效重力;ρi是機械臂連桿的密度;mi是機械臂連桿的質(zhì)量;Vi是機械臂連桿的體積．

綜上所述,海流對兩個機械臂的作用力矩分別為τ′1=τd1+τm1和τ′2=τd2+τm2．

1.2 機械臂動力學(xué)建模

在應(yīng)用Lagrange法或Newton-Euler法對二關(guān)節(jié)機械臂進行建模之前,需求出機械臂的位置、速度坐標、動能和勢能．

機械臂兩個連桿的位置坐標為

(8)

對位置坐標求導(dǎo)得

(9)

機械臂兩個連桿的轉(zhuǎn)動慣量分別為

(10)

機械臂兩個連桿的線速度的平方分別為

(11)

機械臂兩個連桿的動能分別為

(12)

機械臂兩個連桿的勢能分別為

(13)

這樣,二連桿機械臂系統(tǒng)的總動能和總勢能分別為

(14)

1.2.1 Lagrange法

Lagrange函數(shù)L被定義為系統(tǒng)的動能K和勢能P之差,根據(jù)式(14)可以求得此二連桿機械臂系統(tǒng)的Lagrange函數(shù)L,即

(15)

系統(tǒng)的動力學(xué)方程式,即Lagrange方程定義如下:

(16)

-(τd2+τm2),“-”表示海流作用力對機械臂的運動起阻力作用．

(17)

式(17)中的后兩式對時間求導(dǎo)數(shù):

(18)

將式(17)、(18)中相應(yīng)各導(dǎo)數(shù)和偏導(dǎo)數(shù)代入式(16),即可分別求得機械臂兩個連桿關(guān)節(jié)處的力矩為

(19)

(20)

1.2.2 Newton-Euler法

用Newton-Euler法求同一個二關(guān)節(jié)機械臂系統(tǒng)的動力學(xué)方程,其一般形式為

(21)

式中,W為保守力所做的功,因為還受到海流作用力Q1(τ′1)和Q2(τ′2)等非保守力的作用,故W=(T1+Q1)θ1+(T2+Q2)θ2,其中T1和T2為機械臂兩個連桿關(guān)節(jié)處的力矩;D為系統(tǒng)的能耗,因為在這里不考慮摩擦力,所以有D=0;其余K和P等與Lagrange法中含義一樣．

把式(21)中各項導(dǎo)數(shù)與偏導(dǎo)數(shù)計算后代入可發(fā)現(xiàn)τ1=T1和τ2=T2,故用Lagrange法和Newton-Euler法推導(dǎo)出的機械臂兩個連桿關(guān)節(jié)處的力矩相等,故用這兩種方法對水下機械臂建模,對比驗證了模型推導(dǎo)的正確性,為后續(xù)控制器的設(shè)計奠定了堅實可靠的基礎(chǔ)．

1.2.3 二連桿機械臂系統(tǒng)動力學(xué)模型

把海流載荷τd1,τm1,τd2和τm2計算后代入式(19)、(20)中并整理成矩陣方程的形式,可以總結(jié)得到均勻海流環(huán)境下二連桿機械臂系統(tǒng)的動力學(xué)模型為

(22)

(23)

(24)

(25)

(26)

(27)

(28)

式中

2 跟蹤控制方法

2.1 問題表述

本文旨在為固定在船舶底部的水下機械臂建立更精確的動力學(xué)模型,因其在水下作業(yè)時要受到海流載荷的影響,所以不能單純地認為機械臂在靜水中工作,還要考慮海流的速度和擾動才能使機械臂的動力學(xué)模型有更高的控制精度和更接近真實的海洋狀況．建立好機械臂的動力學(xué)模型,需要選用合適的控制方法來使它能達到理想的軌跡跟蹤,即在很短的時間內(nèi)能使機械臂運動的實際軌跡能跟蹤上設(shè)定好的理想軌跡,這樣才能使機械臂能快速地到達預(yù)期位置．所以控制目標就是在很短的時間內(nèi)能使機械臂的實際軌跡與理想軌跡重合,即θ-θd→0,在這里分別嘗試了PD控制與滑?？刂疲?/p>

2.2 PD控制

PD控制是按偏差的比例和微分進行控制的方法,其調(diào)節(jié)的實質(zhì)是根據(jù)輸入的偏差值,按比例和微分的函數(shù)關(guān)系進行運算,其運算結(jié)果用于輸出控制．設(shè)機械臂的理想軌跡為θd,實際軌跡為θ,PD控制律為

(29)

2.3 滑?？刂?/h3>

滑模控制系統(tǒng)的運動可分為兩個階段,分別為趨近運動階段和滑模運動階段．根據(jù)滑模變結(jié)構(gòu)原理,滑?？蛇_性條件僅能保證狀態(tài)空間任意位置運動點在有限時間內(nèi)到達切換面的要求,而對趨近運動的具體軌跡沒有任何限制．因此為了改善趨近運動的動態(tài)品質(zhì),提出了各種各樣的趨近律公式,式(30)—(33)是3種經(jīng)典趨近律(等速、冪次和指數(shù)趨近律)和一種改進指數(shù)趨近律:

(30)

(31)

(32)

(33)

式中,ξ,α和k均為控制參數(shù),定義滑模函數(shù)為

(34)

式中,取λ1=λ2=5．對式(34)求導(dǎo)得

(35)

將式(22)、(35)與趨近律方程結(jié)合就可以得到控制律,如與指數(shù)趨近律式(32)結(jié)合可以得到基于指數(shù)趨近律的滑?？刂坡?

(36)

選取Lyapunov函數(shù)為

(37)

① 可達性

(38)

因此控制系統(tǒng)滿足滑?？蛇_性條件．

② 存在性

對式(32)應(yīng)用Bernoulli方程求解可得

(39)

因此控制系統(tǒng)能夠在有限時間內(nèi)到達切換面．

3 機械臂跟蹤仿真

3.1 仿真概況

本文仿真時選用的二關(guān)節(jié)機械臂系統(tǒng)的相關(guān)參數(shù)為m1=5 kg,m2=3 kg,l1=0.6 m,l2=0.4 m,D1=0.1 m,D2=0.09 m,ρ1=1 061 kg/m3,ρ2=1 179 kg/m3[28],海水密度ρ=1 025 kg/m3．二關(guān)節(jié)機械臂的理想軌跡指令分別是θd1=cos(πt),θd2=sin(πt),二關(guān)節(jié)機械臂的初始位置和初始速度設(shè)置為

3.2 PD控制及滑?？刂品抡?/h3>

3.2.1 PD控制

圖2 二關(guān)節(jié)機械臂PD控制不同控制參數(shù)下軌跡跟蹤與輸入力矩Fig.2 Trajectory trackings and input torques of the 2-joint manipulator under different control parameters of the PD control注 為了解釋圖中的顏色,讀者可以參考本文的電子網(wǎng)頁版本,后同．

由于機械臂的兩個關(guān)節(jié)中第1個關(guān)節(jié)離基座更近,受力也更大更復(fù)雜,因此在這里僅給出更具代表性和說服力的關(guān)節(jié)1的軌跡跟蹤和輸入力矩圖．由圖2可以看出,該二關(guān)節(jié)機械臂模型在PD控制方法下基本無法達到精確的軌跡跟蹤．當(dāng)控制參數(shù)很大時,軌跡跟蹤的誤差相對較小,但是此時輸入控制力矩的初始啟動力矩已經(jīng)非常大,已經(jīng)失去了實際意義和可行性,所以PD控制方法不適用于此模型．

3.2.2 滑?？刂?/p>

選用式(30)—(33)所示的4種趨近律進行滑模控制對比,控制參數(shù)取ξ=0.5,α=0.5,k=5,均勻海流速度仍取為u=0.5 m/s,仿真結(jié)果如圖3所示．

圖3 4種趨近律下二關(guān)節(jié)機械臂滑?？刂栖壽E跟蹤與輸入力矩Fig.3 Trajectory trackings and input torques of the 2-joint manipulator under 4 reaching laws of the sliding mode control

如圖3,還是僅給出關(guān)節(jié)1的情況．可以看出,指數(shù)趨近律和改進指數(shù)趨近律情況下,系統(tǒng)可以在很短的時間內(nèi)精確跟蹤理想軌跡;等速趨近律和冪次趨近律情況下,需要較長時間才可以精確跟蹤理想軌跡,冪次趨近律略快于等速趨近律．指數(shù)趨近律和改進指數(shù)趨近律初始階段的輸入力矩比較小,小于等速趨近律和冪次趨近律,改進指數(shù)趨近律在控制抖振方面略優(yōu)于指數(shù)趨近律．因此,選取總體控制效果最佳的改進指數(shù)趨近律在不同的控制參數(shù)下繼續(xù)進行實驗．

仍然僅給出關(guān)節(jié)1的情況,由圖4可以看出,隨著控制參數(shù)的增大,達到精確軌跡跟蹤的時間基本一致,初始輸入力矩逐漸增大,達到穩(wěn)定后幾種情況相差不大．選用初始輸入力矩較小情況下的參數(shù)ξ=1,k=10,在靜水與不同流速u的均勻海流情況下進行仿真對比．

圖4 改進指數(shù)趨近律在不同控制參數(shù)下的二關(guān)節(jié)機械臂滑?？刂栖壽E跟蹤與輸入力矩Fig.4 Trajectory trackings and input torques of the 2-joint manipulator under different control parameters of the improved exponential reaching law of the sliding mode control

如圖5所示給出了兩個關(guān)節(jié)的情況,在控制參數(shù)為ξ=1,k=10的改進指數(shù)趨近律的滑?？刂葡?靜水與不同海流流速情況下均可以快速地實現(xiàn)精確軌跡跟蹤．對于輸入力矩,在均勻海流情況下,當(dāng)速度較小時,略大于靜水工況,速度為零時的均勻海流也就變成了靜水;當(dāng)速度比較大時,比靜水工況大很多,速度越大,輸入力矩越大．

圖5 靜水、不同流速均勻海流下二關(guān)節(jié)機械臂滑?？刂栖壽E跟蹤與輸入力矩Fig.5 Trajectory trackings and input torques of the 2-joint manipulator under still water and uniform ocean currents with different velocities of the sliding mode control

前面仿真時機械臂兩個關(guān)節(jié)的理想軌跡θd1=cos(ωt)和θd2=sin(ωt)中ω取的是π,現(xiàn)將ω取不同的值來使機械臂來跟蹤頻率不同的理想軌跡,圖6分別是機械臂跟蹤不同頻率的理想軌跡時兩個關(guān)節(jié)的跟蹤誤差．

圖6 不同頻率理想軌跡下機械臂兩個關(guān)節(jié)的軌跡跟蹤誤差Fig.6 Trajectory tracking errors of 2 joints of the manipulator under ideal trajectories with different frequencies

如圖6所示,各個頻率下機械臂關(guān)節(jié)1的軌跡跟蹤誤差基本都在2%～3%之間,關(guān)節(jié)2的誤差更?。芯窟€發(fā)現(xiàn)當(dāng)理想軌跡的頻率為π時,均勻海流速度取許多不同的值時如0.5 m/s,1 m/s,2 m/s等,機械臂關(guān)節(jié)1的軌跡跟蹤誤差都為3%左右,關(guān)節(jié)2的誤差同樣很?。虼?無論是不同流速的均勻海流還是不同頻率的機械臂理想軌跡,機械臂兩個關(guān)節(jié)的軌跡跟蹤誤差都很小,可以達到較為理想的控制效果．

4 結(jié) 論

本文針對基座固定的水下機械臂的水動力特性進行了深入研究,考慮了海流載荷對機械臂動力學(xué)建模的影響．基于Morison方程計算海流對機械臂的水阻力和慣性力,然后分別應(yīng)用Lagrange法和Newton-Euler法推導(dǎo)出均勻海流環(huán)境下二關(guān)節(jié)機械臂的動力學(xué)模型,兩種方法對比驗證了該模型的正確性和可靠性．應(yīng)用MATLAB軟件針對此模型分別進行PD控制和滑模控制仿真實驗．從仿真結(jié)果可以看出:PD控制下軌跡跟蹤存在誤差,且控制參數(shù)較大時初始輸入力矩非常大,表明PD控制不適用于此模型的控制．在不同趨近律、不同控制參數(shù)下進行滑?？刂?通過仿真對比選出最優(yōu)趨近律和控制參數(shù),應(yīng)用此控制系統(tǒng)在靜水和不同流速均勻海流環(huán)境下進行仿真實驗,研究發(fā)現(xiàn)此滑?？刂葡到y(tǒng)在各種工況下均可以快速地實現(xiàn)精確軌跡跟蹤,同時輸入力矩也在合理的范圍內(nèi)．在不同頻率的理想軌跡下也都可以快速地實現(xiàn)精確跟蹤,所以無論是不同頻率的理想軌跡還是不同流速的均勻海流,都可以實現(xiàn)誤差很小的精確跟蹤．本文推導(dǎo)的機械臂動力學(xué)模型可將具有隨機性質(zhì)的海流擾動轉(zhuǎn)化為確定的海流載荷而考慮,故應(yīng)用此動力學(xué)模型可使機械臂軌跡跟蹤更精確進而達到預(yù)期的工作目標．

致謝本文作者衷心感謝上海交通大學(xué)雙一流建設(shè)項目(WF220401013)對本文的資助．