基于凱恩動(dòng)力學(xué)的AUV建模

孫佳杰，楊柯，葛彤

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

基于凱恩動(dòng)力學(xué)的AUV建模

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

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

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

凱恩動(dòng)力學(xué)；AUV；建模；廣義速度

0 引言

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

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

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

1 水動(dòng)力

1.1 水動(dòng)力計(jì)算公式

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

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

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

圖1 AUV慣性坐標(biāo)系與載體坐標(biāo)系Fig.1 The inertia frame and body frame of AUV

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

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

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

拖曳力和拖曳力矩分別為

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

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

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

式中：uf、vf、wf為水流速度在載體坐標(biāo)系三個(gè)坐標(biāo)軸方向的分量。u′、v′、w′代替上述中的u、v、w即可獲得水流作用下的水動(dòng)力計(jì)算公式。

1.2 水動(dòng)力系數(shù)

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

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

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

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

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

2 運(yùn)動(dòng)學(xué)分析

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

2.1 廣義坐標(biāo)與廣義速度

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

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

廣義速度可以定義為

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

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

2.2 運(yùn)動(dòng)學(xué)相關(guān)變量的計(jì)算

角速度可以表示為

式中：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上任意點(diǎn)的速度可以寫成

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

角加速度為

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

加速度為

AUV上任意點(diǎn)的偏速度為

偏角速度為

3 動(dòng)力學(xué)分析

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

3.1 廣義主動(dòng)力和廣義慣性力

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

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

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

重力對(duì)廣義主動(dòng)力的貢獻(xiàn)為：

浮力對(duì)廣義主動(dòng)力的貢獻(xiàn)為：

拖曳力和拖曳力矩對(duì)廣義主動(dòng)力的貢獻(xiàn)為：

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

附加質(zhì)量力和附加質(zhì)量矩對(duì)廣義主動(dòng)力的貢獻(xiàn)為：

柯氏力和柯氏力矩對(duì)廣義主動(dòng)力的貢獻(xiàn)為：

推力對(duì)廣義主動(dòng)力的貢獻(xiàn)為：

3.2 AUV凱恩動(dòng)力學(xué)模型

AUV的凱恩動(dòng)力學(xué)模型可以表示為

式中：

4 仿真驗(yàn)證

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

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

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

圖3 AUV前進(jìn)控制Fig.3 The forward control of AUV

5 結(jié)論

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

[1]管欣,張素民,詹軍.基于凱恩方法的汽車懸架實(shí)時(shí)仿真模型[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方法的柔性機(jī)械臂系統(tǒng)動(dòng)力學(xué)建模及其模態(tài)截取研究[J].天津工業(yè)大學(xué)學(xué)報(bào),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ī)器人波狀游動(dòng)的動(dòng)力學(xué)建模[J].機(jī)械工程學(xué)報(bào),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)機(jī)構(gòu)動(dòng)力學(xué)研究[J].機(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]沈飛,曹志強(qiáng),徐德,等.基于Kane方法的機(jī)器海豚動(dòng)力學(xué)建模及速度優(yōu)化方法[J].自動(dòng)化學(xué)報(bào),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

國家自然科學(xué)基金項(xiàng)目（51309133）；杭州電子科技大學(xué)科研啟動(dòng)基金項(xiàng)目資助（ZX150204301002/006）

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