基于四元素法的捷聯(lián)慣導姿態(tài)更新算法

所謂姿態(tài)更新是指將運載體上慣性單元的輸出，實時轉(zhuǎn)換成運載體的姿態(tài)。這里的姿態(tài)通常指機體坐標系(b系)相對于導航坐標系(n系)的角位置。

對于一個姿態(tài)求解系統(tǒng)而言，其內(nèi)部的姿態(tài)更新算法，無疑是其整個系統(tǒng)的核心，如何根據(jù)系統(tǒng)輸入以及應用場合的特點選用一種速度、精度都能充分滿足要求的算法是系統(tǒng)設(shè)計人員必須認真考慮的一件事情。目前，常用姿態(tài)更新算法主要有歐拉角算法、方向余弦法、四元素法以及旋轉(zhuǎn)矢量法。

歐拉角算法通過求解歐拉角微分方程直接計算航向角、俯仰角和橫滾角，但由于其微分方程特點，當俯仰角接近90o度時方程出現(xiàn)退化，故其只適用于水平姿態(tài)變化不大的情況，而不適用于全姿態(tài)的姿態(tài)確定。

方向余弦法避免了退化問題，可計算運載體全姿態(tài)，但是由于算法計算量大，實時計算困難，工程中很少采用。

四元素法計算量比余弦法小，且算法簡單，易于操作，是比較實用的工程方法。但其對有限轉(zhuǎn)動引起的不可交換誤差的補償程度不夠，所以只適用于低動態(tài)運載體的姿態(tài)解算。對于高動態(tài)運載體，其算法漂移十分嚴重。

旋轉(zhuǎn)矢量法根據(jù)運載體角速度擬合方式，分為單子樣算法（常數(shù)擬合），二子樣算法（直線擬合），三子樣算法（拋物線擬合）。因此可以根據(jù)需要采用合適的多子樣算法實現(xiàn)對不可交換誤差做有效補償。旋轉(zhuǎn)矢量法精度通常優(yōu)于四元素法，但是通常計算量較四元素法更大，其較四元素法更適合角機動頻繁或者存在嚴重角振動的場合。值得一提的是，當旋轉(zhuǎn)矢量法采用單子樣計算姿態(tài)時，就等同于四元素法。

由于民航飛機以及直升機正常工作狀態(tài)常處于低動態(tài)，故在此類飛機的捷聯(lián)慣導系統(tǒng)中常采用四元素法作為姿態(tài)更新算法，本文也將主要針對應用于此類系統(tǒng)的四元素法進行介紹。

所謂四元素，即由四個元構(gòu)成的數(shù)：

(2.1)

其中q0、q1、q2、q3是實數(shù)，i、j、k既是互相正交的單位向量，又是虛單位，因此四元素既可以看作是四維空間中的一個向量，又可看作一個超復數(shù)。

在計算運載體姿態(tài)時，當只關(guān)心機體坐標系相對于導航坐標系的的角位置時，可以認為機體坐標系(b系)是由導航坐標系(n系)經(jīng)過無中間過程一次性等效旋轉(zhuǎn)形成的，四元素Q包含了這種等效旋轉(zhuǎn)的全部信息。其姿態(tài)變換公式見下式：

(3.1)

式(3.1)中，為b系中矢量，為b系相對于n系旋轉(zhuǎn)后，在n系中的矢量投影結(jié)果。式中坐標旋轉(zhuǎn)矩陣具體形式如下：

(3.2)

當已知運載體的航向角Y、俯仰角q、橫滾角g時，機體坐標系相對于導航坐標系的一次性等效旋轉(zhuǎn)矩陣還可以表示成如下形式：

(3.3)

記，故當已知姿態(tài)變換矩陣時，根據(jù)式(3.2)、式(3.3)可以求出當前姿態(tài)的四元素以及歐拉角，其中四元素求解方程如下：

(3.4)

式中q0、q1、q2、q3的符號可以按下式確定，其中的值可以任意假設(shè)：

(3.5)

由旋轉(zhuǎn)矩陣式(3.3)得到的歐拉角求解公式如下：

(3.6)

式(3.6)中的航向角和橫滾角的真值按表1和表2確定。

表1

表3.2

捷聯(lián)陀螺的輸出通常是角速度，因此，為了計算運載體姿態(tài)，我們需要引入四元素微分方程。引入微分方程的好處是，根據(jù)上一時刻姿態(tài)四元素，通過定時采樣機體坐標系的三軸角增量即可求出新的姿態(tài)四元素(即機體坐標系相對于導航坐標系的四元素)。

四元素微分方程見下式：

(4.1)

式(4.1)中：

(4.2)

其中分別為定時采樣時三個軸的角增量，I為單位矩陣。因此由時刻陀螺儀定時采樣的角增量，結(jié)合式(4.1)，即可求出時刻的四元素。

利用四元素微分方程進行姿態(tài)更新前，需要知道系統(tǒng)的初始四元素，求取初始四元素的方法以捷聯(lián)慣導為例，通常是利用捷聯(lián)慣導的加速度計在無加速狀態(tài)時的輸出結(jié)果計算出系統(tǒng)初始的歐拉角，再利用式(3.3)求出初始的姿態(tài)旋轉(zhuǎn)矩陣，再將的矩陣元素代入式(3.4)、式(3.5)即可求出初始四元素。

求出初始四元素后，定時采樣系統(tǒng)陀螺儀的角速度即可得到等時間間隔內(nèi)的系統(tǒng)角增量，代統(tǒng)姿態(tài)。

本文僅對基于四元素法更新的一般情況進行了討論，實際應用中還必須充分考慮到四元素法由于算法本身問題造成的誤差，例如利用四元素法進行姿態(tài)更新時，陀螺儀的角增量為定時采樣的，然而實際陀螺儀的輸出并非完全線性關(guān)系，即不為常數(shù)，同時四元素法對旋轉(zhuǎn)軸的旋轉(zhuǎn)次序是敏感的，即當的結(jié)果相同時，不同的輸出次序?qū)е虏煌乃脑馗陆Y(jié)果。而在利用四元素法進行姿態(tài)更新時，近似的認為其在等時間內(nèi)三軸同時產(chǎn)生角增量。這勢必產(chǎn)生前文所述的不可交換誤差�？s短采樣時間間隔可以有效降低近似誤差，然而卻增加了計算量，對系統(tǒng)提出了更高要求，因此實際使用中必須根據(jù)應用場合對四元素法進行適當修正，才能得到滿意的結(jié)果。