海流環境下二關節機械臂動力學建模及其跟蹤控制

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

(1.上海交通大學 海洋工程國家重點實驗室,上海 200240;2.上海交通大學 船舶海洋與建筑工程學院,上海 200240;3.上海交通大學三亞崖州灣深海科技研究院,海南 三亞 572000)

0 引 言

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

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

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

1 海流環境下二關節機械臂動力學建模

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

圖1 二關節機械臂結構Fig.1 Schematic of the 2-joint manipulator

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

f=fd+fm+ff+fl,

(1)

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

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

(2)

1.1 海流載荷

1.1.1 水阻力矩

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

(3)

(4)

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

1.1.2 附加質量力矩

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

(5)

(6)

式中,τm1和τm2是兩個關節的附加質量力矩．

1.1.3 浮力力矩

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

(7)

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

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

1.2 機械臂動力學建模

在應用Lagrange法或Newton-Euler法對二關節機械臂進行建模之前,需求出機械臂的位置、速度坐標、動能和勢能．

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

(8)

對位置坐標求導得

(9)

機械臂兩個連桿的轉動慣量分別為

(10)

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

(11)

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

(12)

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

(13)

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

(14)

1.2.1 Lagrange法

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

(15)

系統的動力學方程式,即Lagrange方程定義如下:

(16)

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

(17)

式(17)中的后兩式對時間求導數:

(18)

將式(17)、(18)中相應各導數和偏導數代入式(16),即可分別求得機械臂兩個連桿關節處的力矩為

(19)

(20)

1.2.2 Newton-Euler法

用Newton-Euler法求同一個二關節機械臂系統的動力學方程,其一般形式為

(21)

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

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

1.2.3 二連桿機械臂系統動力學模型

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

(22)

(23)

(24)

(25)

(26)

(27)

(28)

式中

2 跟蹤控制方法

2.1 問題表述

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

2.2 PD控制

PD控制是按偏差的比例和微分進行控制的方法,其調節的實質是根據輸入的偏差值,按比例和微分的函數關系進行運算,其運算結果用于輸出控制．設機械臂的理想軌跡為θd,實際軌跡為θ,PD控制律為

(29)

2.3 滑?？刂?/h3>

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

(30)

(31)

(32)

(33)

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

(34)

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

(35)

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

(36)

選取Lyapunov函數為

(37)

① 可達性

(38)

因此控制系統滿足滑?？蛇_性條件．

② 存在性

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

(39)

因此控制系統能夠在有限時間內到達切換面．

3 機械臂跟蹤仿真

3.1 仿真概況

本文仿真時選用的二關節機械臂系統的相關參數為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．二關節機械臂的理想軌跡指令分別是θd1=cos(πt),θd2=sin(πt),二關節機械臂的初始位置和初始速度設置為

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

3.2.1 PD控制

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

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

3.2.2 滑?？刂?/p>

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

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

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

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

圖4 改進指數趨近律在不同控制參數下的二關節機械臂滑?？刂栖壽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所示給出了兩個關節的情況,在控制參數為ξ=1,k=10的改進指數趨近律的滑模控制下,靜水與不同海流流速情況下均可以快速地實現精確軌跡跟蹤．對于輸入力矩,在均勻海流情況下,當速度較小時,略大于靜水工況,速度為零時的均勻海流也就變成了靜水;當速度比較大時,比靜水工況大很多,速度越大,輸入力矩越大．

圖5 靜水、不同流速均勻海流下二關節機械臂滑模控制軌跡跟蹤與輸入力矩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

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

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

如圖6所示,各個頻率下機械臂關節1的軌跡跟蹤誤差基本都在2%～3%之間,關節2的誤差更小．研究還發現當理想軌跡的頻率為π時,均勻海流速度取許多不同的值時如0.5 m/s,1 m/s,2 m/s等,機械臂關節1的軌跡跟蹤誤差都為3%左右,關節2的誤差同樣很小．因此,無論是不同流速的均勻海流還是不同頻率的機械臂理想軌跡,機械臂兩個關節的軌跡跟蹤誤差都很小,可以達到較為理想的控制效果．

4 結 論

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

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