一種壓縮型全姿態(tài)角四元數(shù)的表示方法

劉公緒，蔚保國(guó)，史凌峰，劉 洪，何 瑞，董亞軍

(1.衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，河北 石家莊 050081；2．西安電子科技大學(xué) 電子工程學(xué)院，陜西 西安 710071)

用全姿態(tài)角來(lái)描述運(yùn)載體在三維空間中任意位置的姿態(tài)，其概念普遍應(yīng)用在導(dǎo)航、定位和制導(dǎo)等相關(guān)領(lǐng)域[1]。載體的姿態(tài)多用歐拉角表示，但歐拉角法具有奇異性，即當(dāng)俯仰角為 ±90°時(shí)，會(huì)出現(xiàn)萬(wàn)向節(jié)鎖，因而歐拉方程不能全姿態(tài)表示姿態(tài)角。學(xué)者們常借助歐拉角與四元數(shù)之間的轉(zhuǎn)化進(jìn)行運(yùn)載體的全姿態(tài)表示[2-3]。姿態(tài)四元數(shù)的表示問(wèn)題通常包括姿態(tài)空間的描述問(wèn)題，以及姿態(tài)角與四元數(shù)的映射問(wèn)題，它們是姿態(tài)估計(jì)和控制算法的基石[4-7]。在用四元數(shù)表示運(yùn)載體姿態(tài)時(shí)，雖然四元數(shù)可以有效避免姿態(tài)角的奇異問(wèn)題，但四元數(shù)的狀態(tài)空間對(duì)姿態(tài)角空間是雙重覆蓋的，即對(duì)任意一組姿態(tài)角，都有兩組四元數(shù)與之對(duì)應(yīng)，這會(huì)導(dǎo)致在姿態(tài)控制時(shí)出現(xiàn)姿態(tài)散開(kāi)問(wèn)題[8,9]。姿態(tài)散開(kāi)是指四元數(shù)的二重覆蓋性會(huì)在兩組四元數(shù)向量的鄰域形成一個(gè)吸引域和一個(gè)排斥域，導(dǎo)致當(dāng)初始條件接近平衡點(diǎn)時(shí)會(huì)忽然反方向繞開(kāi)去，繞一整圈后再回到平衡點(diǎn)。為解決這一問(wèn)題，筆者提出一種壓縮型全姿態(tài)角四元數(shù)的表示方法。其基本思想如下：運(yùn)載體的實(shí)際姿態(tài)可分別惟一用姿態(tài)角空間和四元數(shù)空間進(jìn)行描述，從這個(gè)角度講，姿態(tài)角空間和四元數(shù)空間都不應(yīng)該是冗余的，應(yīng)該是壓縮的，即包含且僅包含運(yùn)載體的實(shí)際姿態(tài)信息。因此引入壓縮型姿態(tài)角空間和壓縮型四元數(shù)空間兩個(gè)概念，并通過(guò)方向余弦矩陣與四元數(shù)的轉(zhuǎn)化關(guān)系實(shí)現(xiàn)兩個(gè)壓縮空間的一一映射，并且兩個(gè)壓縮空間都與運(yùn)載體的實(shí)際姿態(tài)一一對(duì)應(yīng)。最后通過(guò)數(shù)值仿真實(shí)驗(yàn)驗(yàn)證了所提出方法的可行性。

1 壓縮型姿態(tài)角空間

這里采用東-北-天坐標(biāo)系為導(dǎo)航坐標(biāo)系(n系)。起初，載體坐標(biāo)系(b系)與導(dǎo)航坐標(biāo)系重合，運(yùn)載體的右前上各自指向東-北-天方向。載體繞X,Y,Z軸旋轉(zhuǎn)分別得到俯仰角(θ)，橫滾角(φ)和航向角(ψ)，如圖1所示。關(guān)于姿態(tài)角的擴(kuò)展在學(xué)術(shù)研究或工程應(yīng)用中并沒(méi)有明確規(guī)定，原則上需要表示運(yùn)載體在三維空間中的全姿態(tài)。在壓縮型姿態(tài)角空間中，對(duì)其中任意一個(gè)姿態(tài)角進(jìn)行壓縮均可以實(shí)現(xiàn)對(duì)運(yùn)載體實(shí)際姿態(tài)信息的唯一描述?？紤]到在方向余弦矩陣中，決定俯仰角大小的只有一項(xiàng)，對(duì)其擴(kuò)展會(huì)增加難度，而決定另外兩個(gè)姿態(tài)角大小的都各有兩項(xiàng)，對(duì)其擴(kuò)展相對(duì)容易。因此采取對(duì)俯仰角壓縮，即不擴(kuò)展，而對(duì)橫滾角和航向角進(jìn)行擴(kuò)展的處理方法為

(1)

為方便描述，將式(1)稱為壓縮型姿態(tài)角空間。

關(guān)于式(1)對(duì)運(yùn)載體全姿態(tài)唯一描述的機(jī)理做如下說(shuō)明。這里俯仰角的取值范圍是[-π/2, π/2]，若只考察俯仰角，顯然其二、三象限與一、四象限是疊加在一起的，具有二義性，不能描述俯仰角的全姿態(tài)，關(guān)于坐標(biāo)系象限的定義見(jiàn)圖2。若整體考察運(yùn)載體的三個(gè)姿態(tài)角的取值，可以發(fā)現(xiàn)俯仰角在±π/2時(shí)，橫滾角和航向角也會(huì)發(fā)生180°的相位變化。因此，完全可以綜合考察三個(gè)姿態(tài)角的大小來(lái)惟一確定運(yùn)載體在導(dǎo)航坐標(biāo)系中的姿態(tài)。下面將介紹如何對(duì)傳統(tǒng)四元數(shù)空間進(jìn)行約束，構(gòu)造壓縮型四元數(shù)空間，消除四元數(shù)對(duì)姿態(tài)角的二重覆蓋性。

圖1 坐標(biāo)系示意圖及姿態(tài)角的定義

圖2 坐標(biāo)系的象限

2 壓縮型四元數(shù)空間

在圖1中，繞坐標(biāo)系X, Y, Z軸分別作基本旋轉(zhuǎn),可得到相應(yīng)的坐標(biāo)變換矩陣，如式(2)～(4)所示[10]:

(2)

(3)

(4)

(5)

從載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣:

(6)

rn=q?rb?q*,

(7)

其中，?表示四元數(shù)乘法。與姿態(tài)轉(zhuǎn)換矩陣類似，上式可轉(zhuǎn)化為

(8)

(9)

依次繞X, Y, Z軸做基本旋轉(zhuǎn)，得到四元數(shù)表示分別為

(10)

(11)

(12)

(13)

根據(jù)導(dǎo)航坐標(biāo)系與載體坐標(biāo)系的轉(zhuǎn)換關(guān)系，可以得到從載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的四元數(shù)變換矩陣:

(14)

對(duì)式(14)展開(kāi)，得到由姿態(tài)角轉(zhuǎn)化到四元數(shù)的方程組如下：

(15)

結(jié)合式(6)和式(9)，可以得到由四元數(shù)轉(zhuǎn)化為姿態(tài)角的方程組[12]：

(16)

聯(lián)合壓縮型姿態(tài)角空間式(1)，式(15)和四元數(shù)模值約束條件，可得出四元數(shù)空間取值為

(17)

事實(shí)上，約束后的四元數(shù)空間式(17)仍不能完全消除二重覆蓋性，因此，對(duì)四元數(shù)空間進(jìn)一步壓縮，得到壓縮型四元數(shù)空間為

(18)

顯然壓縮的形式有多種，這里不再一一列出。

至此，已經(jīng)闡明了壓縮型姿態(tài)角空間和壓縮型四元數(shù)空間。下面即進(jìn)行數(shù)值仿真實(shí)驗(yàn)，驗(yàn)證所提出方法的可行性。

3 數(shù)值仿真和分析

3.1 姿態(tài)角奇異性測(cè)試

當(dāng)俯仰角為±π/2時(shí)會(huì)出現(xiàn)奇異問(wèn)題，這里對(duì)姿態(tài)角奇異性測(cè)試的方法如下。使俯仰角在-π/2與π/2之間交替變化，使橫滾角和航向角在(-π,π]內(nèi)隨機(jī)變化，用來(lái)模擬當(dāng)俯仰角為±π/2時(shí)，任意橫滾角和任意航向角下姿態(tài)角的奇異情況。若所提的方法具有奇異性，則解算的四元數(shù)是無(wú)窮多組解中的一組，可以通過(guò)四元數(shù)與姿態(tài)角之間的轉(zhuǎn)換關(guān)系反解算四元數(shù)得姿態(tài)角，然后對(duì)比前后姿態(tài)角的誤差。若出現(xiàn)奇異問(wèn)題，則意味著在反解算時(shí)，作為輸入的是無(wú)窮多組解中的一組解，得到的姿態(tài)角與生成的姿態(tài)角完全相等的概率幾乎為零，則反解算前后姿態(tài)角誤差勢(shì)必較大，否則誤差很小。因此可以通過(guò)生成姿態(tài)角與反解算姿態(tài)角的前后誤差來(lái)判斷是否出現(xiàn)奇異性問(wèn)題。測(cè)試結(jié)果如圖3所示，可見(jiàn)誤差在10-11量級(jí)，誤差主要來(lái)源是弧度與角度之間轉(zhuǎn)換時(shí)導(dǎo)致的誤差，以及四元數(shù)歸一化時(shí)導(dǎo)致的截?cái)嗾`差。該實(shí)驗(yàn)驗(yàn)證了壓縮型四元數(shù)空間繼承了四元數(shù)表示姿態(tài)角的非奇異性優(yōu)點(diǎn)。

圖3 姿態(tài)角奇異性測(cè)試

3.2 兩個(gè)壓縮空間的一一映射關(guān)系驗(yàn)證

為了驗(yàn)證壓縮型姿態(tài)角空間與壓縮型四元數(shù)空間的一一映射關(guān)系，開(kāi)展了以下實(shí)驗(yàn)，如圖4和圖5所示。首先隨機(jī)生成壓縮型姿態(tài)角空間，然后根據(jù)式(15)將姿態(tài)角映射到壓縮型四元數(shù)空間，再根據(jù)式(16)，將映射的四元數(shù)反映射到壓縮型姿態(tài)角空間，將得到的姿態(tài)角與生成的姿態(tài)角做差，即得到誤差。由圖4可見(jiàn)誤差在10-11量級(jí)，誤差主要來(lái)源是弧度與角度之間轉(zhuǎn)換時(shí)導(dǎo)致的誤差，以及四元數(shù)歸一化時(shí)導(dǎo)致的截?cái)嗾`差。接著隨機(jī)生成壓縮型四元數(shù)空間，根據(jù)式(16)將四元數(shù)映射到壓縮型姿態(tài)角空間內(nèi)，再根據(jù)式(15)，將得到的姿態(tài)角反映射到壓縮型四元數(shù)空間內(nèi)，最后將得到的四元數(shù)與生成的四元數(shù)做差，即得到誤差。由圖5可見(jiàn)誤差在10-15量級(jí)，誤差主要來(lái)源同上述分析。

圖4 壓縮型姿態(tài)角空間與壓縮型四元數(shù)空間的一一映射關(guān)系驗(yàn)證

圖5 壓縮型姿態(tài)角空間與壓縮型四元數(shù)空間的一一映射關(guān)系驗(yàn)證

4 結(jié)束語(yǔ)

針對(duì)傳統(tǒng)姿態(tài)角空間與四元數(shù)空間的非一一映射關(guān)系而引起的姿態(tài)散開(kāi)等問(wèn)題，首先引入壓縮型姿態(tài)角空間和壓縮型四元數(shù)空間兩個(gè)概念，提出一種新穎的壓縮型全姿態(tài)角四元數(shù)的表示方法。基于壓縮型姿態(tài)角空間的概念，分析了姿態(tài)角的壓縮過(guò)程及壓縮姿態(tài)角空間對(duì)運(yùn)載體全姿態(tài)唯一描述的機(jī)理；基于壓縮型四元數(shù)空間的概念，通過(guò)增加四元數(shù)空間的約束條件，消除四元數(shù)的二重覆蓋性；并通過(guò)方向余弦矩陣與四元數(shù)的相互轉(zhuǎn)化關(guān)系實(shí)現(xiàn)兩個(gè)壓縮空間的一一映射。然后通過(guò)數(shù)值仿真實(shí)驗(yàn)驗(yàn)證了所提出方法的可行性。測(cè)試結(jié)果表明該方法可以唯一表示運(yùn)載體在三維空間中的全姿態(tài)信息，無(wú)奇異性問(wèn)題，無(wú)姿態(tài)散開(kāi)問(wèn)題，可以普遍應(yīng)用在導(dǎo)航、制導(dǎo)等領(lǐng)域，具有一定的科學(xué)研究?jī)r(jià)值和工程實(shí)踐意義。文中所述的姿態(tài)描述方法消除了四元數(shù)對(duì)姿態(tài)角的二重覆蓋性，但四元數(shù)的二重覆蓋性在某些算法中有一定作用，如基于旋轉(zhuǎn)矢量的姿態(tài)估計(jì)方法，它利用四元數(shù)的二重覆蓋性，減少了剛體旋轉(zhuǎn)的不可交換誤差。因此在實(shí)際應(yīng)用中，應(yīng)根據(jù)筆者提出的壓縮映射思想，選擇合適的坐標(biāo)系進(jìn)行姿態(tài)描述，然后利用相關(guān)的姿態(tài)解算和控制算法達(dá)到預(yù)期目的。