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

徐正武，唐國元，鄧智勇，黃道敏，黃永忠

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

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

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

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

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

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

0 引 言

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

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

1 四元數(shù)

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

顯然有

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

2 基于四元數(shù)的運動模型

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

如圖1所示，描述一個水下航行體的運動通常需建立2個坐標系：與大地固連的慣性系OE-XEYEZE；與水下航行體固連的動坐標系Ob-XbYbZb。水下航行體的位置在慣性系下描述為η1=[x y z]T，水下航行體的姿態(tài)角（歐拉角描述）描述為 η2=[φ θ ψ]T。其中：φ為橫搖角，（°）；θ 為縱傾角，（°）；ψ 為航向角，（°）。水下航行體的線速度在動坐標系下描述為 v1=[u v w]T，水下航行體的角速度在動坐標系下描述為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 水下航行體坐標系與運動參數(shù)Fig.1 Coordinate and motion parameters of an autonomous underwater vehicle

2.2 運動學(xué)方程

線速度矢量在慣性系下描述為η˙1，在動坐標系下描述為v1。由此，

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

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

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

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

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

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

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

對式（12）兩邊微分，得

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

將式（1）和式（14）寫在一起，得

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

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

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

2.3 動力學(xué)方程

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

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

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

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

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

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

3 計算機仿真

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 語

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

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

［2］黃華紅，楊云川，呂艷慧.一種魚雷俯仰角出現(xiàn)±90°時的姿態(tài)仿真方法［J］.魚雷技術(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］.計算機工程與應(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)型分析和對比研究［J］.中國空間科學(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］劉暾，趙鈞.空間飛行器動力學(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ò)出版時間：2014-3-31 16:32

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

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

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

唐國元

book=29,ebook=259

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