基于Kane 動力學的十字舵型欠驅動AUV 建模與仿真

楊 柯

(中國計量大學 機電工程學院,浙江 杭州,310018)

0 引言

隨著國家海洋戰略的實施,對海洋裝備的需求日益增多。水下機器人作為海洋觀測和采樣的重要工具,得到了蓬勃發展。自主水下航行器(autonomous undersea vehicle,AUV)由于其自主性強、運動靈活等特點,在海洋觀測[1-3]、水下組網[4-5]和水下目標跟蹤[6-7]等領域發揮了重要作用。控制系統作為AUV 的重要組成部分,其性能直接影響了AUV 的避障和路徑跟蹤能力。建立精度高、運算速度快的AUV 動力學模型,對設計和驗證控制算法有著重要作用。

機器人動力學建模常用的方法主要有Newton-Euler(N-E)方程、Kane 方程和Lagrange-Euler(L-E)方程。采用N-E 方程建模需要考慮剛體之間的相互作用力,該力屬于內力,在建模過程中難以消除。因此,N-E 方程主要用于設計階段的受力分析或者可以近似為單剛體的系統建模,但解決多剛體建模問題非常困難。例如,尹欣繁等[8]構建了基于N-E 方程的小型旋翼無人機的模型。王林濤等[9]通過N-E 方程研究了四旋翼無人機特種彈藥懸停發射時的動力學問題。在建模過程中,將旋翼作為質點,僅提供升力而忽略旋翼自身轉動以及旋翼與本體之間的相互作用。L-E 方程從能量的角度解決動力學建模問題。例如,Do 等[10]采用L-E 方程解決了靈活機械手的振動模型線性化難題。Huang 等[11]推導出基于L-E 方程的水下機械手動力學模型。孫志偉等[12]采用L-E 方程對Delta機器人的動力學進行了分析。在建模時,需要計算動能和勢能,通過求解2 階微分方程獲得最終的動力學模型,計算機求解效率較低。Kane 方程屬于分析力學的分支,引入了廣義坐標、廣義主動力及廣義慣性力等概念,可以分別計算各個模塊的廣義主動力和廣義慣性力,將所有模塊的計算結果相加,即可獲得最終的動力學模型。因此,該方法非常適合處理多剛體建模問題,并且最終的模型結構簡單、運算效率高,滿足實時控制的需求。Zhang 等[13]推導出基于Kane 方程的無人自行車動力學模型。Cai 等[14]構建了基于Kane 方程船載Stewart 平臺的動力學模型。Caruso 等[15]采用Kane方程的相關知識,分析了太空探測車的動力學問題。Cao 等[16]通過Kane 方程完成了F 直升機吊裝系統的動力學建模。Cibicik 等[17]利用Kane 動力學解決了機械手的運動學和動力學建模。

AUV 動力學建模是將舵的作用換算到AUV重心,將整個AUV 近似為1 個剛體處理。這種處理方法無法獲得舵與本體之間的相互作用力,不利于舵的設計,在處理更為復雜的AUV 結構時會非常困難。文中基于Kane 方程對十字舵型欠驅動AUV 進行建模,既不需要考慮舵與AUV 本體之間的相互作用力,也不需要計算動能和勢能。該方法可以清晰地展示哪些力對AUV 動力學模型有貢獻,也可以非常方便地將外界作用力添加到動力學方程中。

1 運動學分析

文中分析了不同坐標系之間的轉換問題和廣義速度的計算方法,并推導出偏速度和偏角速度的計算公式。

1.1 坐標系

文中采用的坐標系如圖1 所示。坐標系之間的轉換矩陣為

圖1 慣性坐標系與載體坐標系Fig.1 The inertia coordinate and body coordinate systems

1.2 廣義坐標與廣義速度

十字舵型欠驅動AUV 的本體有6 個自由度,十字舵有2 個自由度,因此,廣義坐標可以寫成

式中:xg,yg,zg為AUV 本體重心相對于慣性坐標系的位置;δs為 水平舵的舵角;δr為垂直舵的舵角。

廣義速度為

1.3 角速度與偏角速度

AUV 本體的角速度在載體坐標系 {b}下的矢量形式為

式中:p,q,r為AUV 本體的角速度在載體坐標系{b}下 的表示形式;nx,ny,nz為 與載體坐標系 {b}的3 個坐標軸對應的單位矢量。

同理,水平舵和垂直舵的偏角速度為

1.4 速度與偏速度

AUV 本體重心處的速度在載體坐標系 {b}下的矢量形式為

式中,u,v,w為AUV 本體重心處的速度在載體坐標系 {b}下的表示形式。

AUV 本體浮心處的速度在載體坐標系 {b}下的矢量形式為

式 中:rgc為重心到浮心的位置矢量;xgc,ygc,zgc為rgc在 載體坐標系 {b}下的坐標。

水平舵重心處的偏速度為

式中:xg,s_tail,yg,s_tail,zg,s_tail為AUV 本體重心到水平舵與AUV 本體連接處的矢量在載體坐標系 {b}下的表示形式;xs_tail,sg,ys_tail,sg,zs_tail,sg為水平舵與AUV 本體連接處到水平舵重心處的矢量在載體坐標系{b}下的表示形式。

垂直舵重心處的偏速度為

式中:xg,r_tail,yg,r_tail,zg,r_tail為AUV 本體重心到垂直舵與AUV 本體連接處的矢量在載體坐標系 {b}下的表示形式;xr_tail,rg,yr_tail,rg,zr_tail,rg為垂直舵與AUV 本體連接處到垂直舵重心處的矢量在載體坐標系{b}下的表示形式。

1.5 加速度與角加速度

AUV 本體重心處的加速度的矢量形式為

AUV 本體角加速度的矢量形式為

同理,可以寫出水平舵片的加速度asg、角加速度 αs、垂直舵片的加速度arg以 及角加速度 αr在載體坐標系 {b}下的矢量形式。

2 動力學分析

十字舵型欠驅動AUV 由AUV 本體、水平舵和垂直舵組成,可以分別計算每個組成部分的廣義主動力和廣義慣性力,將各個部分的廣義主動力和廣義慣性力相加即可獲得最終的動力學方程。由于舵和本體相對轉動,作用力和反作用力相互抵消。力與第n個偏速度點積即可得到該力對第n個廣義主動力的貢獻;力矩與第n個偏角速度點積即可得到該力矩對第n個廣義主動力的貢獻;慣性力和慣性矩分別與對應的偏速度和偏角速度求點積即可得到廣義慣性力。

2.1 AUV 本體動力學分析

AUV 本體的質量為mM,慣性張量為IM,受到慣性力、慣性矩、重力G、浮力B、水動力Fh(包括附加質量、科里奧利向心力、粘性類水動力)、水動力矩Th[18]、推進器推力T以及舵對AUV本體的反作用力矩-Tδs(方向為ny)和-Tδr(方向為nz)的影響。

AUV 本體的慣性力和慣性矩公式為

施加在AUV 本體上的Fh、Th、G、B、T、-Tδs和 -Tδr對廣義主動力的貢獻為

2.2 水平舵動力學分析

水平舵的質量為ms,慣性張量為Is,施加在水平舵上的力主要有慣性力、慣性力矩、重力Gs、浮力Bs、水動力Fs、水動力矩Ts[18]以及舵機施加在水平舵上的扭矩Tδs(方向為ny)。通常情況下,舵片的重心和浮心重合,重力和浮力相等,因此,重力和浮力相互抵消。

水平舵的Fs、Ts和Tδs對廣義主動力的貢獻為

2.3 垂直舵動力學分析

垂直舵的質量為mr,慣性張量為Ir,施加在垂直舵上的力主要有慣性力、慣性力矩、重力Gr、浮力Br、水動力Fr、水動力矩Tr[18]以及舵機施加在垂直舵上的扭矩Tδr(方向為nz)。通常情況下,垂直舵片的重心和浮心重合,重力和浮力相等,因此,重力和浮力相互抵消,即

垂直舵對廣義慣性力的貢獻為

垂直舵的Fr、Tr和Tδr對廣義主動力的貢獻為

2.4 動力學方程

十字舵型欠驅動AUV 的動力學方程為

3 仿真驗證

為了驗證動力學模型的有效型,在Matlab/Simulink 環境下構建仿真驗證平臺。以中國海洋大學自主研制的210 型AUV 為仿真對象,其質量63 kg,長度2.1 m,慣性張量Iyy=Izz=17.362 kg·m2,水動力系數分別為:Xu˙=0.000 486 4,Zw˙=0.054,=0.002 443,Xu=0.000 303 4,Zw=0.052 75,Mq=0.004 988,Xu|u|=0.002 408,Zw|w|=0.103,Mq|q|=0.033 5。

動力學模型在S 函數中求解,采用滑模控制器,解決深度控制和水平面的路徑跟蹤問題。由圖2 可知,0～15 s 為AUV 的下潛過程;10～40 s 為AUV 的過渡過程,通過調整水平舵角實現;40 s 后,AUV 穩定在10 m 水深。圖3 為下潛過程中縱傾角的變化曲線。圖4 描述了AUV 在水平面內的圓周運動,開始階段AUV 的位置坐標為(20,20),該點并不在期望軌跡上,通過控制算法調整垂直舵角,AUV 最終可以跟蹤圓形軌跡。圖5 給出了路徑跟蹤過程中的速度變化曲線,圖6 為推力和扭矩的變化曲線。仿真結果符合AUV 的運動規律,進一步驗證了文中模型的正確性。

圖3 AUV 縱傾角隨時間變化曲線Fig.3 Curve of pitch angle of AUV versus time

圖4 AUV 路徑跟蹤結果Fig.4 Results of AUV path tracking

圖5 路徑跟蹤速度隨時間變化曲線Fig.5 Curves of the speed of path tracking versus time

圖6 推力和扭矩隨時間變化曲線Fig.6 Curves of thrust and torque versus time

4 結束語

文中詳細介紹了基于Kane 動力學的十字舵型欠驅動AUV 的建模方法。該方法可以簡化相互作用力或力矩的處理過程,便于施加外界約束,從而非常清晰地展現哪些力對廣義主動力或廣義慣性力有貢獻。最終的動力學模型形式簡單,計算機求解效率高,滿足實時控制的需求。通過水平面路徑跟蹤問題和垂直面定深問題的仿真實驗,驗證了該模型的有效性。