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

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

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

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

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

(1)

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

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

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

圖2 坐標系的象限

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

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

(2)

(3)

(4)

(5)

從載體坐標系到導(dǎ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)航坐標系與載體坐標系的轉(zhuǎn)換關(guān)系，可以得到從載體坐標系到導(dǎo)航坐標系的四元數(shù)變換矩陣:

(14)

對式(14)展開，得到由姿態(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ù)空間式(17)仍不能完全消除二重覆蓋性，因此，對四元數(shù)空間進一步壓縮，得到壓縮型四元數(shù)空間為

(18)

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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