孫佳杰，楊柯，葛彤

（1.杭州電子科技大學機械工程學院，杭州310018；2.上海交通大學船舶海洋與建筑工程學院，上海200240）

基于凱恩動力學的AUV建模

孫佳杰1，楊柯1，葛彤2

（1.杭州電子科技大學機械工程學院，杭州310018；2.上海交通大學船舶海洋與建筑工程學院，上海200240）

推導出基于凱恩動力學的AUV運動學和動力學方程。采用廣義速度取代速度和角速度，給出了廣義慣性力和廣義主動力的計算方法。推導出作用在AUV上的水動力的計算方法，并通過仿真實驗給出了水動力系數(shù)。為了驗證該模型的有效性，對AUV的深度控制、前進距離控制進行了仿真。仿真結(jié)果表明，該模型是有效的。

凱恩動力學；AUV；建模；廣義速度

0 引言

計算機建模和仿真可以為機器人的研究提供巨大的幫助。動力學模型可以通過調(diào)整設計參數(shù)和水動力系數(shù)優(yōu)化機器人的性能。AUV的動力學模型是研究AUV運動控制以及機械設計的基礎。機器人的動力學可以采用理論力學和分析力學的動力學方程（例如，Newton-Euler（N-E）動力學方程、Lagrange-Euler（L-E）方程、Routh方程、Appell方程、Kane方程）來分析。采用不同的建模方法對同一機器人進行建模，最終結(jié)果是相同的。但是，運算速度和質(zhì)量有很大差別。而運算速度和質(zhì)量恰恰是實時控制的關鍵[1-2]。

N-E方程包含機器人模塊間的相互作用力，很難消除該作用力以便獲得機器人運動與受力之間的清晰表達式[3]。因此，N-E方程主要用于機器人設計階段的受力分析。L-E方程、Routh方程和Appell方程通常為非線性差分方程，求解該方程時耗時長、運算速度慢[4-6]。Kane方程是1970年發(fā)展起來的分析力學的分支，方程的形式非常簡單（只需加、減、乘、除運算），因此，非常適合創(chuàng)建復雜系統(tǒng)的動力學方程用于實時控制。例如，夏丹[7]提出了基于Kane動力學的機器魚的波狀運動模型；李新友[8]創(chuàng)建了基于Kane動力學的3UPS/S關聯(lián)機構(gòu)動力學模型；沈飛[9]提出基于Kane動力學的機器海豚動力學模型；Cheng[10]分析了四自由度平行髖關節(jié)模擬器的運動學；Liu[11]提出采用Kane動力學方程創(chuàng)建多體系統(tǒng)的動力學方程。Yang[12-13]推導出基于Kane動力學方程的水下蛇形機器人和水下四足行走機器人的動力學模型。

盡管Kane動力學已經(jīng)出現(xiàn)在一些機器人的建模中，基于Kane動力學的AUV動力學模型仍然沒有出現(xiàn)。本文提出了基于Kane動力學的AUV建模方法，該方法消除了N-E方程中的相互作用力和L-E方程中的勢能或Gibbs方程。Kane方程采用廣義速度，可以為非正交約束系統(tǒng)選取一組相互獨立的運動變量，以便獲得一階差分方程形式的動力學模型。Kane方法可以將外部環(huán)境的作用力直接添加到AUV動力學方程，該方程可以方便地轉(zhuǎn)化為適用于控制的閉環(huán)形式。AUV動力學方程可以為“哪些力真正影響動力學模型”提供物理解釋。

1 水動力

1.1 水動力計算公式

AUV在運動過程中受到的水動力主要包括：浮力、附加質(zhì)量力、附加質(zhì)量矩、柯氏力、柯氏力矩、拖曳力和拖曳力矩。浮力可表示為

式中：ρ表示流體的密度；V表示AUV排開水的體積；g表示重力加速度。

附加質(zhì)量力和附加質(zhì)量矩取決于AUV的形狀?？紤]到AUV的對稱性和載體坐標系原點的位置，附加質(zhì)量力和附加質(zhì)量矩可以表示為：

圖1 AUV慣性坐標系與載體坐標系Fig.1 The inertia frame and body frame of AUV

式中：u、v、w表示AUV速度在載體坐標系三個坐標軸的分量；p、q、r表示AUV角速度在載體坐標系三個坐標軸的分量，如圖1所示；Xu˙、Yv˙、Zw˙表示與加速度相關的水動力系數(shù)；Kp˙、Mq˙、Nr˙表示與角加速度相關的水動力系數(shù)。

柯氏力和柯氏力矩可以表示為

式中：m表示AUV本體的質(zhì)量；Ix、Iy、Iz表示AUV本體相對于載體坐標系三個坐標軸的中心轉(zhuǎn)動慣量，如表1所示；α1、α2、α3、β1、β2和β3可以表示為

拖曳力和拖曳力矩分別為

表1 AUV本體的參數(shù)Tab.1 Parameters of AUV body

式中：Xu、Yv、Zw表示與速度相關的水動力系數(shù)；Kp、Mq、Nr表示與角速度相關的水動力系數(shù)；分別表示與相關的水動力系數(shù)。

有水流存在的情況下，AUV相對于水流的速度可以表示為

式中：uf、vf、wf為水流速度在載體坐標系三個坐標軸方向的分量。u′、v′、w′代替上述中的u、v、w即可獲得水流作用下的水動力計算公式。

1.2 水動力系數(shù)

我們采用CFD軟件設計AUV外形，通過Gambit導入AUV外形并對流體區(qū)域進行網(wǎng)格劃分，最后將網(wǎng)格劃分后的結(jié)果導入Fluent軟件，設置邊界條件，計算水動力系數(shù)。計算獲得的水動力系數(shù)如表2～5所示，剩余水動力系數(shù)的取值為0。

表2 與速度相關的水動力系數(shù)Tab.2 Hydrodynamic coefficients associated with the velocity

表3 與速度的二次方相關的水動力系數(shù)Tab.3 Hydrodynamic coefficients associated with the square of the velocity

表4 與加速度相關的主要水動力系數(shù)Tab.4 The main hydrodynamic coefficients associated with the acceleration velocity

表5 與加速度相關的耦合水動力系數(shù)Tab.5 The coupling hydrodynamic coefficients associated with the acceleration velocity

2 運動學分析

本小節(jié)推導出AUV的速度、角速度、加速度、角加速度、偏速度和偏角速度的計算公式。速度對第r個廣義速率求偏導數(shù)即可得到第r個偏速度，角速度對第r個廣義速率求偏導數(shù)即可得到第r個偏角速度。為了便于描述我們引入Z函數(shù)。

2.1 廣義坐標與廣義速度

AUV可以等效為6自由度的剛體，因此，廣義坐標可以定義為

式中：x0、y0、z0表示AUV質(zhì)心在慣性坐標系中的位置；φ、θ、ψ表示AUV相對于慣性坐標系的姿態(tài)角。

廣義速度可以定義為

本文用到兩個坐標系：慣性坐標系和載體坐標系，如圖1所示。兩個坐標系之間可以通過坐標變換矩陣相互關聯(lián)。從慣性坐標系到載體坐標系的變換矩陣可以表示為

式中：Z1=sinθ，Z2=cosφ，Z3=sinφ，Z4=cosθ，Z7=sinψ，Z9=cosψ。

2.2 運動學相關變量的計算

角速度可以表示為

式中：nx、ny、nz為x、y、z軸方向的單位矢量；Z5=Z3Z4，Z6=Z2Z4。

AUV質(zhì)心的速度可以表示為

式中：Z8＝Z4Z7，Z10＝Z4Z9，Z11＝Z2Z9+Z1Z3Z7，Z12＝Z1Z3Z9-Z2Z7，Z13＝Z3Z7+Z1Z2Z9，Z14＝Z1Z2Z7-Z3Z9。

AUV上任意點的速度可以寫成

式中：r為質(zhì)心（載體坐標系原點）到任意點的矢徑；（xa,ya,za）表示任意點在載體坐標系下的坐標。

角加速度為

式中：Z15=Z4u5u6，Z16=Z6u4u6-Z3u4u5-Z1Z3u5u6，Z17=-Z2u4u5-Z1Z2u5u6-Z5u4u6。

加速度為

AUV上任意點的偏速度為

偏角速度為

3 動力學分析

本小結(jié)推導出基于凱恩動力學的AUV動力學方程。在推導過程中，首先計算出作用在AUV上的廣義主動力和廣義慣性力。作用在AUV上的力與作用點處的第r個偏速度點積，即可獲得力對第r個廣義主動力的貢獻，力矩與第r個偏角速度求點積，即可得到力矩對第r個廣義主動力的貢獻。慣性力和慣性矩分別與偏速度和偏角速度求點積，即可得到慣性力和慣性矩對廣義慣性力的貢獻。

3.1 廣義主動力和廣義慣性力

作用在AUV上的力包括：慣性力F*、慣性矩T*、重力G、浮力B、附加質(zhì)量力FA和附加質(zhì)量矩MA、柯氏力FC和柯氏力矩MC、拖曳力FD和拖曳力矩MD、推進器的推力T1,T2,…,T6。慣性力、重力、附加質(zhì)量力、柯氏力和拖曳力的作用點為質(zhì)心，令（15）式中的xa=0，ya=0，za=0即可得到質(zhì)心處的偏速度。同理，令（15）式中的xa、ya、za分別取浮心、推力作用點的坐標，即可得到浮心、推力作用點處的偏速度。

慣性力和慣性矩可以表示為

AUV本體對廣義慣性力的貢獻，可通過慣性力與質(zhì)心處的偏速度點積、慣性矩與偏角速度點積獲得。

重力對廣義主動力的貢獻為：

浮力對廣義主動力的貢獻為：

拖曳力和拖曳力矩對廣義主動力的貢獻為：

式中：FD,x、FD,y和FD,z為拖曳力在載體坐標系下的三個分量；TD,x、TD,y和TD,z為拖曳力矩在載體坐標系下的三個分量。

附加質(zhì)量力和附加質(zhì)量矩對廣義主動力的貢獻為：

柯氏力和柯氏力矩對廣義主動力的貢獻為：

推力對廣義主動力的貢獻為：

3.2 AUV凱恩動力學模型

AUV的凱恩動力學模型可以表示為

式中：

4 仿真驗證

我們采用Matlab/Simulink中得S函數(shù)創(chuàng)建AUV凱恩動力學方程，在Simulink中設計AUV的控制系統(tǒng)，分別對AUV的深度控制（圖2）和前進距離控制（圖3）進行仿真。

由圖2可以看出，0～16 s為AUV下潛的過程，16 s后，AUV達到期望的深度10m，并保持該深度不變。由圖3可以看出，0～20 s為AUV前進的過程，20 s后，AUV達到期望的前進距離。仿真結(jié)果表明，我們創(chuàng)建的AUV模型是正確的。

圖2 AUV深度控制Fig.2 The deep control of AUV

圖3 AUV前進控制Fig.3 The forward control of AUV

5 結(jié)論

通過仿真實驗，獲得了AUV的水動力系數(shù)。詳細介紹了基于凱恩動力學的AUV建模方法。通過該方法可以清晰地看出哪些力對AUV的動力學方程起作用。基于凱恩動力學建立的AUV模型，可以方便地轉(zhuǎn)化成適用于控制的閉環(huán)形式。通過仿真，驗證了基于凱恩動力學建立的AUV模型的有效性。

[1]管欣,張素民,詹軍.基于凱恩方法的汽車懸架實時仿真模型[J].汽車工程,2010,32(8):649-653. Guan X,Zhang S M,Zhan J.A real-time simulation model for automotive suspension based on Kane’s method[J].Automotive Engineering,2010,32(8):649-653.

[2]孫占庚,金國光,常志,等.基于Kane方法的柔性機械臂系統(tǒng)動力學建模及其模態(tài)截取研究[J].天津工業(yè)大學學報,2009,28(4):61-63. Sun Z G,Jin G G,Chang Z,et al.Research of dynamic modeling of flexible manipulator system based on Kane’s method and its mode interception[J].Journal of Tianjin Polytechnic University,2009,28(4):61-63.

[3]Zhao D X.Dynamic analysis on a feeding and unloading manipulator using Kane’s method combined with screw theory[C]// Proceedings of the 2012 International Conference on Industrial Control and Electronics Engineering.Xi’an,China:IEEE, 2012:999-1002.

[4]Huang Y H,Liao Q Z,Wei S M,et al.Dynamics modeling and analysis of a front-wheel drive bicycle robot moving on a slope[C]//Proceedings of the 2010 IEEE International Conference on Automation and Logistics.Hong Kong,China:IEEE, 2010:43-48.

[5]Yang C F,Huang Q T,Ye Z M,et al.Dynamic modeling of a spatial 6-DOF parallel robots using Kane method for control purposes[C]//Proceedings of the 2010 2nd International Conference on Intelligent Human-Machine Systems and Cybernetics.Nanjing,China:IEEE,2010:180-183.

[6]Ge X F,Jin J T.Dynamics analyze of a dual-arm space robot system based on Kane’s method[C]//Proceedings of the 2010 2nd International Conference on Industrial Mechatronics and Automation.Wuhan,China:IEEE,2010:646-649.

[7]夏丹,陳維山,劉軍考,等.基于Kane方法的仿魚機器人波狀游動的動力學建模[J].機械工程學報,2009,45(6): 41-49. Xia D,Chen W S,Liu J K,et al.Dynamic modeling of a fishlike robot with undulatory motion based on Kane’s method [J].Journal of Mechanical Engineering,2009,45(6):41-49.

[8]李新友,陳五一,韓先國.基于Kane方程的3UPS/S并聯(lián)機構(gòu)動力學研究[J].機床與液壓,2011,39(13):1-5. Li X Y,Chen W Y,Han X G.Dynamic analysis of a 3UPS/S parallel mechanism based on Kane equations[J].Machine Tool and Hydraulics,2011,39(13):1-5.

[9]沈飛,曹志強,徐德,等.基于Kane方法的機器海豚動力學建模及速度優(yōu)化方法[J].自動化學報,2012,38(8): 1247-1256. Shen F,Cao Z Q,Xü D,et al.A dynamic model of robotic dolphin based on Kane method and its speed optimization[J]. Acta Automatica Sinica,2012,38(8):1247-1256.

[10]Cheng G,Shan X L.Dynamics analysis of a parallel hip joint simulator with four degree of freedoms(3R1T)[J].Nonlinear Dynamics,2012,70(4):2475-2486.

[11]Liu X B.A Lie group formulation of Kane’s equations for multibody systems[J].Multibody System Dynamics,2008,20 (1):29-49.

[12]Yang K,Wang X Y,Ge T,et al.Dynamic model of underwater snake-like robot using Kane’s method[J].Journal of Shanghai Jiaotong University,2014,19(2):146-154.

[13]Yang K,Wang X Y,Ge T,et al.A dynamic model of an underwater quadruped walking robot using Kane’s method[J]. Journal of Shanghai Jiaotong University,2014,19(2):160-168.

Modeling of AUV based on Kane dynamics

SUN Jia-jie1,YANG Ke1,GE Tong2
(1.School of Mechanical Engineering,Hangzhou Dianzi University,Hangzhou 310018,China;2.School of Naval Architecture,Ocean and Civil Engineering,Shanghai Jiaotong University,Shanghai 200240,China)

Kinematics and dynamics for AUV are developed.The generalized active force and the generalized inertia force are deduced by means of replacing velocity and angular velocity by generalized velocity. The equations of hydrodynamic forces of AUV are deduced.Hydrodynamic coefficients are determined through simulation.The simulations of depth control and forward distance control are implemented to verify the dynamic model of AUV.Simulation results show that the dynamic model is effective.

Kane dynamics;AUV;model;generalized velocity

TP242.6

A

10.3969/j.issn.1007-7294.2017.08.004

1007-7294（2017）08-0960-08

2017-03-11

國家自然科學基金項目（51309133）；杭州電子科技大學科研啟動基金項目資助（ZX150204301002/006）

孫佳杰（1998-），男，本科；楊柯（1983-），男，博士，講師，E-mail:yjs2yangke@163.com。