徐正武，唐國(guó)元，鄧智勇，黃道敏，黃永忠

1華中科技大學(xué)船舶與海洋工程學(xué)院，湖北武漢430074 2武漢第二船舶設(shè)計(jì)研究所，湖北武漢430064 3空軍預(yù)警學(xué)院電子對(duì)抗系，湖北武漢430019

四元數(shù)在水下航行體運(yùn)動(dòng)建模中的應(yīng)用

徐正武1，唐國(guó)元1，鄧智勇2，黃道敏3，黃永忠1

1華中科技大學(xué)船舶與海洋工程學(xué)院，湖北武漢430074 2武漢第二船舶設(shè)計(jì)研究所，湖北武漢430064 3空軍預(yù)警學(xué)院電子對(duì)抗系，湖北武漢430019

針對(duì)一類(lèi)以控制力矩陀螺（CMG）為姿態(tài)控制執(zhí)行機(jī)構(gòu)的水下航行體，考慮到其大角度機(jī)動(dòng)時(shí)姿態(tài)描述矩陣可能會(huì)出現(xiàn)奇異的問(wèn)題，建立了與其相適應(yīng)的運(yùn)動(dòng)模型。首先，通過(guò)引入四元數(shù)來(lái)建立運(yùn)動(dòng)學(xué)方程，并給出四元數(shù)與歐拉角之間的關(guān)系。隨后，在建立動(dòng)力學(xué)方程時(shí)，將水下航行體視為由水下航行體和CMG組成的多剛體系統(tǒng)，并使用四元數(shù)來(lái)代替動(dòng)力學(xué)方程中的歐拉角項(xiàng)。最后，使用龍格庫(kù)塔法對(duì)所建立的模型進(jìn)行仿真。仿真結(jié)果表明，所建立的模型能有效避免使用歐拉角方法建立模型時(shí)所產(chǎn)生的奇異問(wèn)題。

四元數(shù)；控制力矩陀螺；水下航行體；運(yùn)動(dòng)模型

0 引 言

目前，大多數(shù)水下航行體均采用歐拉角方法來(lái)建立其運(yùn)動(dòng)學(xué)方程，而當(dāng)水下航行體俯仰角出現(xiàn)±90°情況時(shí)，其運(yùn)動(dòng)學(xué)方程就會(huì)出現(xiàn)奇異［1］。由此可知，對(duì)于水面船舶和一些在垂直面內(nèi)不進(jìn)行大姿態(tài)角機(jī)動(dòng)的水下航行體而言，采用歐拉角方法來(lái)建立其運(yùn)動(dòng)學(xué)方程完全可行。但是，隨著水下航行體種類(lèi)和功能的多樣化，一些水下航行體在運(yùn)動(dòng)過(guò)程中會(huì)出現(xiàn)姿態(tài)角±90°的情況，如高空空投反潛魚(yú)雷［2-3］。又如，以控制力矩陀螺（CMG）為姿態(tài)控制執(zhí)行機(jī)構(gòu)的水下航行體，由于其可以實(shí)現(xiàn)任意姿態(tài)角機(jī)動(dòng)［4］，故該水下航行體的俯仰角也完全有可能達(dá)到±90°附近。基于此，文獻(xiàn)［5］提出將CMG作為水下航行體姿態(tài)控制執(zhí)行機(jī)構(gòu)，并進(jìn)行實(shí)驗(yàn)驗(yàn)證。CMG的工作原理是依靠框架帶動(dòng)恒速飛輪轉(zhuǎn)動(dòng)使得飛輪的角動(dòng)量方向發(fā)生改變，進(jìn)而控制力矩，其具有力矩放大、輸出力矩連續(xù)、執(zhí)行機(jī)構(gòu)內(nèi)置［6］等諸多優(yōu)點(diǎn)。同時(shí)，由于CMG可以輸出3個(gè)自由度的力矩，故可以實(shí)現(xiàn)對(duì)任意姿態(tài)角的控制。并且，特別需要指出的是由于CMG在橫搖方向能夠輸出力矩，所以其不必依靠橫傾回復(fù)力矩來(lái)保證水下航行體的穩(wěn)定性。這就意味著在給水下航行體進(jìn)行配重時(shí)，可以將重心和浮心配置在同一高度，此時(shí)水下航行體在垂直面內(nèi)做俯仰運(yùn)動(dòng)也就不受縱傾回復(fù)力矩作用?；谶@一點(diǎn)，水下航行體在垂直面內(nèi)的機(jī)動(dòng)將更容易，即其俯仰角更容易達(dá)到±90°附近。

對(duì)于以CMG為姿態(tài)控制執(zhí)行機(jī)構(gòu)（其具體方法參見(jiàn)文獻(xiàn)［3-4］和文獻(xiàn)［7］，限于篇幅，此處不再贅述）的水下航行體，由于其俯仰角會(huì)達(dá)到±90°附近，而采用歐拉角方法來(lái)建立其運(yùn)動(dòng)學(xué)方程會(huì)產(chǎn)生奇異，因此有必要采用其它方法來(lái)建立其運(yùn)動(dòng)學(xué)方程。本文擬采用四元數(shù)法來(lái)建立其運(yùn)動(dòng)學(xué)方程，同時(shí)在建立其動(dòng)力學(xué)方程時(shí)還會(huì)使用四元數(shù)來(lái)代替方程中的姿態(tài)角項(xiàng)。

1 四元數(shù)

四元數(shù)由Hamilton于1843年提出，是一種超復(fù)數(shù)。但是，直到20世紀(jì)70年代，四元數(shù)才開(kāi)始在控制工程中得到應(yīng)用。運(yùn)用四元數(shù)來(lái)描述水下航行體的姿態(tài)能夠克服歐拉角奇異性的缺點(diǎn)，且其運(yùn)算也不涉及復(fù)雜三角函數(shù)，因此非常適合實(shí)時(shí)在線計(jì)算。四元數(shù)包含一個(gè)三維矢量和一個(gè)標(biāo)量。根據(jù)歐拉旋轉(zhuǎn)定理［8］，剛體繞固定點(diǎn)的任意位移可以繞通過(guò)此點(diǎn)的某一軸轉(zhuǎn)動(dòng)某一角度而得到。四元數(shù)的矢量部分就表示這一轉(zhuǎn)動(dòng)軸的方向，其標(biāo)量部分則與繞轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)的角度大小有關(guān)。因此，四元數(shù)可以定義為［8］

顯然有

以上式中：λ為轉(zhuǎn)動(dòng)軸矢量（又叫歐拉軸矢量）；β為繞轉(zhuǎn)動(dòng)軸轉(zhuǎn)動(dòng)的角度，（°）；上標(biāo)T為矩陣的轉(zhuǎn)置，后文均沿用這種表示方法。

2 基于四元數(shù)的運(yùn)動(dòng)模型

2.1 坐標(biāo)系及參數(shù)定義

如圖1所示，描述一個(gè)水下航行體的運(yùn)動(dòng)通常需建立2個(gè)坐標(biāo)系：與大地固連的慣性系OE-XEYEZE；與水下航行體固連的動(dòng)坐標(biāo)系Ob-XbYbZb。水下航行體的位置在慣性系下描述為η1=[x y z]T，水下航行體的姿態(tài)角（歐拉角描述）描述為 η2=[φ θ ψ]T。其中：φ為橫搖角，（°）；θ 為縱傾角，（°）；ψ 為航向角，（°）。水下航行體的線速度在動(dòng)坐標(biāo)系下描述為 v1=[u v w]T，水下航行體的角速度在動(dòng)坐標(biāo)系下描述為v2=[p q r]T。其中：u為軸向速度，m/s；v為橫漂速度，m/s；w為垂向速度，m/s；p為橫搖角速度 ，（°）·s-1；q 為縱傾角速度，（°）·s-1；r為轉(zhuǎn)艏角速度，（°）·s-1。

圖1 水下航行體坐標(biāo)系與運(yùn)動(dòng)參數(shù)Fig.1 Coordinate and motion parameters of an autonomous underwater vehicle

2.2 運(yùn)動(dòng)學(xué)方程

線速度矢量在慣性系下描述為η˙1，在動(dòng)坐標(biāo)系下描述為v1。由此，

式中：η˙1為對(duì)時(shí)間的導(dǎo)數(shù)；C ∈ R3×3，為動(dòng)坐標(biāo)系向慣性系變換的坐標(biāo)變換矩陣。

根據(jù)歐拉旋轉(zhuǎn)定理，動(dòng)坐標(biāo)系可以通過(guò)繞著某一軸轉(zhuǎn)動(dòng)某一角度，從而與慣性系重合。因此，式（4）中的C可以由下式給出：

式中:I為3×3的單位矩陣；λ×為λ的反對(duì)稱(chēng)矩陣。將式（1）、式（2）和式（3）代入式（5），可得

鑒于C完全由q決定，可將C記為 E1(q)。然后，展開(kāi)式（6）可得

坐標(biāo)變換矩陣C滿(mǎn)足如下關(guān)系式［9］：

聯(lián)立式（6）和式（7），可得

四元數(shù)q中的4個(gè)元素并不是獨(dú)立的，必須滿(mǎn)足以下關(guān)系：

對(duì)式（12）兩邊微分，得

將式（9）～式（11）、式（13）寫(xiě)成矩陣的形式，并根據(jù)四元數(shù)的逆運(yùn)算和乘法運(yùn)算，有

將式（1）和式（14）寫(xiě)在一起，得

J2(η2)為姿態(tài)變換矩陣，則由下式給出：

對(duì)比式（16）和式（18），可以得到歐拉四元數(shù)與歐拉角之間有如下關(guān)系式：

對(duì)于式（19），當(dāng) θ=±90°時(shí)，J1(η2)陷入奇異，此時(shí)該方程無(wú)法描述水下航行體的姿態(tài)。但是，采用式（14）來(lái)描述水下航行體的姿態(tài)則不存在奇異問(wèn)題。另外，由式（14）可以看出采用四元數(shù)建立的運(yùn)動(dòng)學(xué)方程的運(yùn)算只涉及簡(jiǎn)單的乘法運(yùn)算，并不涉及到復(fù)雜三角函數(shù)運(yùn)算，從而更加適合在線實(shí)時(shí)運(yùn)算。

2.3 動(dòng)力學(xué)方程

將水下航行體視為由水下航行體本體與CMG組成的多剛體系統(tǒng)，利用動(dòng)量定理和動(dòng)量矩定理，并充分考慮其與水的相互作用力，可以推導(dǎo)出水下航行體的動(dòng)力學(xué)方程。若采用歐拉角描述水下航行體的姿態(tài)，水下航行體的動(dòng)力學(xué)方程中也會(huì)出現(xiàn)歐拉角。因此，本文利用式（20），并運(yùn)用四元數(shù)來(lái)代替動(dòng)力學(xué)方程中的歐拉角，可以得到下列動(dòng)力學(xué)方程：

式中：τ為水下航行體所受到的控制力（力矩），包括螺旋槳的推力和CMG輸出的控制力矩，可以通過(guò)相應(yīng)的控制算法解算出來(lái)；f為水下航行體6個(gè)自由度上經(jīng)過(guò)解耦后所受到的合力和力矩,可由下式給出：

式中：M為水下航行體的質(zhì)量、慣量（包含附連水）矩陣；F(v)為水下航行體所受到的水動(dòng)力和科氏力以及CMG與水下航行體的耦合力；F(q)為水下航行體所受到的靜力。M和F(v)的具體表達(dá)式見(jiàn)文獻(xiàn)［5］和文獻(xiàn)［10］，F(xiàn)(q)由下式給出：

社會(huì)統(tǒng)計(jì)學(xué)與數(shù)理統(tǒng)計(jì)學(xué)的根本區(qū)別在于前者在統(tǒng)計(jì)研究中以事物的質(zhì)為前提，強(qiáng)調(diào)認(rèn)識(shí)事物質(zhì)的重要性，后者則不關(guān)心事物的質(zhì)．

式中：W和B分別為航行體所受到的重力和浮力，N；xG，yG和zG為航行體重心在動(dòng)坐標(biāo)系中的坐標(biāo)，m；xC，yC和zC為航行體浮心在動(dòng)坐標(biāo)系中的坐標(biāo)，m。

基于此，式（16）和式（22）便構(gòu)成了基于四元數(shù)的運(yùn)動(dòng)模型。

3 計(jì)算機(jī)仿真

給定水下航行體的初始狀態(tài)和控制輸入，通過(guò)式（16）和式（22）便可仿真出水下航行體整個(gè)運(yùn)動(dòng)過(guò)程中的運(yùn)動(dòng)狀態(tài)。為獲得較好的仿真精度，本仿真算例利用龍格庫(kù)塔法來(lái)求解式（16）和式（22），具體仿真方法如下：

1） n=0 。初始化 v(n)，q(n)，ηE(n)，τ(n)。

2）由第n步的值遞推出第n+1步的值。首先計(jì)算下列值：

其中，τ(n+0.5)=0.5(τ(n)+τ(n+1))，Δt為仿真步長(zhǎng)。接著，就可以得到n+1步的值：

3）將q(n+1)單位化。理論上四元數(shù)q必須滿(mǎn)足式（12），但是在數(shù)值計(jì)算時(shí)會(huì)存在一些誤差，為了保證式（12）成立，有必要將q(n+1)單位化。單位化方程為：

4）令n=n+1，返回至第2）步，直到算完整個(gè)仿真時(shí)間段。

下面以美國(guó)REMUS100小型水下航行體為例進(jìn)行仿真分析，仿真結(jié)果分別如圖2～圖6所示。仿真所需要的水動(dòng)力參數(shù)以及艇參數(shù)可參見(jiàn)文獻(xiàn)［10］。首先，應(yīng)實(shí)現(xiàn)水下航行體的重力和浮力平衡，并將重心和浮心配置在同一位置。初始條件?。?/p>

即在水下航行體軸向上有大小為0.3 m/s的初速度?？紤]到水下航行體空間的限制，CMG構(gòu)型為由多個(gè)單框架陀螺構(gòu)成的金字塔型結(jié)構(gòu)［7］，其安裝角為54.7°，單個(gè)陀螺轉(zhuǎn)子轉(zhuǎn)動(dòng)慣量為Ii=3.25× 10-3kg·m2(i=1，2，3，4)。圖2～圖4中，單個(gè)陀螺轉(zhuǎn)子的角動(dòng)量為 10（kg·m2）/s，圖 5～圖 6中，單個(gè)陀螺轉(zhuǎn)子的角動(dòng)量為20（kg·m2）/s。

圖2～圖4是水下航行體的CMG在Yb軸向上有-0.1 N·m大小的輸出力矩（其它方向的輸出力矩為0）時(shí)所得到的運(yùn)動(dòng)曲線圖。圖2中的曲線呈半圓型，說(shuō)明水下航行體在垂直面內(nèi)做回轉(zhuǎn)運(yùn)動(dòng)。從圖2可以看出水下航行體在水下回轉(zhuǎn)了半圈多，很明顯其出現(xiàn)了縱傾角（俯仰角）為-90°的情況。圖3給出了四元數(shù)隨時(shí)間的變化曲線，其中ξ2和η的曲線是重合的，均為零。ξ2和η均為零表明潛艇只有縱傾角發(fā)生改變，即潛艇只是在垂直面內(nèi)機(jī)動(dòng)。從圖中可以看出曲線光滑，且滿(mǎn)足式（12）。圖4為利用式（20）將四元數(shù)轉(zhuǎn)化成歐拉角（縱傾角）后所得到的曲線，曲線顯示水下航行體大約在45 s作用時(shí)其縱傾角達(dá)到－90°。圖2～圖4表明，在CMG的作用下，水下航行體在運(yùn)動(dòng)過(guò)程中出現(xiàn)了縱傾角為－90°的情況，但基于四元數(shù)的運(yùn)動(dòng)模型仍然能描述水下航行體的運(yùn)動(dòng)。

圖2 水下航行體垂直面內(nèi)的運(yùn)動(dòng)軌跡Fig.2 Trajectory of an autonomous underwater vehicle in vertical plane

圖3 四元數(shù)隨時(shí)間的變化曲線Fig.3 Variation of four-parameter with respect to time

圖4 水下航行體縱傾角隨時(shí)間的變化曲線Fig.4 Variation of pitch angle of an autonomous underwater vehicle with respect to time

圖5和圖6為給定期望縱傾角θ=－90°，并在PD控制律作用下的水下航行體運(yùn)動(dòng)曲線。其中，圖5為利用式（20）將四元數(shù)轉(zhuǎn)化成歐拉角（縱傾角）后所得到的曲線?？梢钥闯?，水下航行體的縱傾角無(wú)超調(diào)的達(dá)到期望角度，最后穩(wěn)定在－90°。圖6則描述了這一運(yùn)動(dòng)過(guò)程中，水下航行體在垂直面內(nèi)的運(yùn)動(dòng)軌跡，曲線大約在5.2 m左右開(kāi)始與x軸垂直，說(shuō)明此位置水下航行體的縱傾角達(dá)到－90°，水下航行體垂直于水面，開(kāi)始往更深水域駛?cè)?。圖5～圖6說(shuō)明，針對(duì)基于四元數(shù)的運(yùn)動(dòng)模型，可以設(shè)計(jì)控制器，從而將水下航行體縱傾角控制在－90°。

圖5 期望縱傾角θ為90°時(shí)，實(shí)際縱傾角隨時(shí)間的變化曲線Fig.5 Variation of pitch angle with respect to time，when the expected pitch angle is 90°

圖6 期望縱傾角θ為90°時(shí)，水下航行體在垂直面內(nèi)的運(yùn)動(dòng)軌跡Fig.6 Trajectory of an autonomous underwater vehicle in vertical plane，when the expected pitch angle is 90°

4 結(jié) 語(yǔ)

本文針對(duì)一類(lèi)縱傾角可能會(huì)出現(xiàn)±90°的水下航行體，利用四元數(shù)來(lái)建立其運(yùn)動(dòng)學(xué)方程，并運(yùn)用四元數(shù)來(lái)代替動(dòng)力學(xué)方程中的歐拉角項(xiàng)。隨后，給出了歐拉角和四元數(shù)之間的關(guān)系，以便于兩者之間的相互轉(zhuǎn)換，同時(shí)對(duì)相關(guān)的計(jì)算機(jī)仿真方法作了說(shuō)明。最后，給出了計(jì)算機(jī)的仿真結(jié)果。結(jié)果表明：當(dāng)縱傾角出現(xiàn)－90°時(shí)（+90°一樣），該模型仍然能用于描述水下航行體的運(yùn)動(dòng)，并且可以通過(guò)設(shè)置控制器將其縱傾角控制在－90°。本文所建立的模型能夠避免運(yùn)用歐拉角方法建立的模型所產(chǎn)生的奇異問(wèn)題，因而適用于水下航行體任意角機(jī)動(dòng)，從而為CMG在水下航行體中的應(yīng)用奠定了基礎(chǔ)。

［1］FOSSEN T I.Guidance and control of ocean vehicles［M］.England：John Wiley&Sons Lid Baffins Lane，1994.

［2］黃華紅，楊云川，呂艷慧.一種魚(yú)雷俯仰角出現(xiàn)±90°時(shí)的姿態(tài)仿真方法［J］.魚(yú)雷技術(shù)，2012，20（3）：225-230.

HUANG Huahong，YANG Yunchuan，LV Yanhui.At?titude simulation method for torpedo pitch angle at±90°［J］.Torpedo Technology，2012，20（3）：225-230.

［3］潘常軍，郭迎清.基于Simulink的高空空投AUV全彈道仿真系統(tǒng)研究［J］.計(jì)算機(jī)工程與應(yīng)用，2013，49（8）：222-226.

PAN Changjun，GUO Yingqing.High altitude airlaunched AUV's complete trajectory simulation system based on Simulink［J］.Computer Engineering and Ap?plications，2013，49（8）：222-226.

［4］THOMTON B，URA T，NOSE Y，et al.Internal actua?tion of underwater robots using control moment gyros［C］//Occeans Europe，2005：591-598.

［5］THORNTON B.The development of zeros-G class un?derwater robots：unrestricted attitude control using con?trol moment gyros［D］.Southampton：University of Southampton，2006.

［6］ROSER X，SGHEDONI M.Control moment gyroscopes（CMGs）and their application in future scientific mis?sions［C］//Proc of 3rdESA International Conference.Noordwijk，1997：523-528.

［7］張錦江.單框架控制力矩陀螺系統(tǒng)的構(gòu)型分析和對(duì)比研究［J］.中國(guó)空間科學(xué)技術(shù)，2003，23（3）：52-56.

ZHANG Jinjiang.Research on configuration analysis and comparison of SGCMG system［J］.Chinese Space Science and Technology，2003，23（3）：52-56.

［8］TAYLOR J R.Classical mechanics［M］.United States of America：University Science Books，2006.

［9］劉暾，趙鈞.空間飛行器動(dòng)力學(xué)［M］.哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2003.

［10］PRESTERO T J.Verification of a six degree of free?dom simulation model for the REMUS autonomous un?derwater vehicle［D］.California：Massachusetts Insti?tute of Technology，2001.

Applying the Four-Parameter Approach to Establish the Motion Model of an AUV

XU Zhengwu1，TANG Guoyuan1，DENG Zhiyong2，HUANG Daomin3，HUANG Yongzhong1

1 School of Naval Architecture and Ocean Engineering，Huazhong University of Science and Technology，Wuhan 430074，China 2 Wuhan Second Ship Design and Research Institute，Wuhan 430064，China 3 Department of Electronic Warfare，Air Force Early-Warning Academy，Wuhan 430019，China

This paper focuses on a particular type of Autonomous Underwater Vehicle（AUV）that uses the Control Moment Gyros（CMGs）for attitude control.It is noticed that the AUV may have a large attitude angle，and as a result，a proper motion model must be established to avoid the attitude description matrix singularity.To do so，the four-parameter approach is applied to establish the kinematics equation，and the relationship between four-parameter and Euler's angle is then given.When constructing the dynamics equation，the AUV is regarded as a multi-rigid-body system consisting of the AUV itself and CMGs，while Euler's angle is replaced by four-parameter.For validation，the motion model is simulated by the Runge-Kutta method.The results show that the model effectively avoids the singularity.

four-parameter；Control Moment Gyro（CMG）；Autonomous Underwater Vehicle（AUV）；motion model

U674.76

A

1673－3185（2014）02－12－05

10.3969/j.issn.1673-3185.2014.02.003

http://www.cnki.net/kcms/doi/10.3969/j.issn.1673-3185.2014.02.003.html

期刊網(wǎng)址：www.ship-research.com

2013－08－20 網(wǎng)絡(luò)出版時(shí)間：2014-3-31 16:32

湖北省自然科學(xué)基金資助項(xiàng)目（2013CFB154）

徐正武（1987－），男，碩士生。研究方向：艦船與水下航行體運(yùn)動(dòng)控制。E-mail：137198217@qq.com

唐國(guó)元（1973－），男，博士，副教授，碩士生導(dǎo)師。研究方向：艦船與水下航行體運(yùn)動(dòng)控制，艦船機(jī)電控制系統(tǒng)及特種裝置系統(tǒng)

唐國(guó)元

book=29,ebook=259

［責(zé)任編輯：饒亦楠］