杜 鵬，楊 靜，邱紅專，熊 凱

(1.北京航空航天大學(xué)自動化科學(xué)與電氣工程學(xué)院，北京 100083；2. 北京控制工程研究所，北京 100081)

0 引言

對于運行在高動態(tài)環(huán)境下的載體而言，捷聯(lián)慣性導(dǎo)航系統(tǒng)的性能容易受惡劣環(huán)境的影響，在姿態(tài)解算的過程中，由于有限轉(zhuǎn)動的不可交換性，不可避免地存在著由圓錐運動引起的圓錐誤差；在速度解算的過程中，由于運載體同時存在線運動和角運動，當(dāng)采用速度增量進行解算時，也必須考慮到系統(tǒng)的劃船誤差的影響。傳統(tǒng)的捷聯(lián)慣導(dǎo)算法通過對圓錐誤差和劃船誤差分別設(shè)計相應(yīng)的算法進行補償來提高算法的精度[1]。文獻[2-3]針對速度解算中產(chǎn)生的劃船誤差，在東北天導(dǎo)航坐標(biāo)系下設(shè)計了不同的算法進行補償。而針對姿態(tài)解算中產(chǎn)生的圓錐誤差，文獻[4-6]針對不同的陀螺設(shè)計了基于角增量和角速度的圓錐效應(yīng)補償算法。

在傳統(tǒng)的導(dǎo)航算法中，對劃船誤差和圓錐誤差分別設(shè)計算法進行補償，采用了傳統(tǒng)的運動學(xué)觀點，將剛體的旋轉(zhuǎn)與平移分開進行考慮。而對于剛體來說，旋轉(zhuǎn)與平移實際上是同時進行的，有學(xué)者提出了可以利用相關(guān)公式將圓錐補償算法轉(zhuǎn)換為劃船誤差補償算法[7-9]，但這并不容易在實際的導(dǎo)航算法中實現(xiàn)，而研究發(fā)現(xiàn)劃船誤差補償算法與圓錐誤差補償算法存在一定的等價性，其補償原理和過程大致相同。而由Clifford在對偶數(shù)基礎(chǔ)上提出的對偶四元數(shù)，提供了一種可以同時描述旋轉(zhuǎn)和平移的工具。文獻[10]將對偶四元數(shù)應(yīng)用于慣性導(dǎo)航解算，推導(dǎo)了更為簡單直觀的基于對偶四元數(shù)的慣導(dǎo)微分方程。

對偶四元數(shù)導(dǎo)航算法能同時對劃船誤差與圓錐誤差進行補償，可以在一定程度上提升導(dǎo)航算法的性能。目前已公開發(fā)表的文獻大多是以飛機等載體為對象，在東北天導(dǎo)航坐標(biāo)系下對對偶四元數(shù)導(dǎo)航算法展開研究的[11]；而運行在高動態(tài)環(huán)境下的彈道導(dǎo)彈等載體，通常采用發(fā)射點慣性坐標(biāo)系作為導(dǎo)航坐標(biāo)系，相關(guān)研究并不多見。本文將在發(fā)射點慣性系下，對基于對偶四元數(shù)的捷聯(lián)慣導(dǎo)算法展開研究。

論文的結(jié)構(gòu)安排如下，首先介紹了發(fā)射點慣性系下的捷聯(lián)慣導(dǎo)解算模型，在此基礎(chǔ)上推導(dǎo)了發(fā)射點慣性坐標(biāo)系下對偶四元數(shù)算法的解算公式，比較了速度更新過程中對偶四元數(shù)算法與傳統(tǒng)算法的精度差異。并設(shè)計了多條機動飛行軌跡，以三子樣算法為例，對對偶四元數(shù)算法和傳統(tǒng)算法的性能進行了仿真對比分析。

1 發(fā)射點慣性系下捷聯(lián)慣導(dǎo)解算

1.1 坐標(biāo)系說明

將文中用到的主要坐標(biāo)系定義如下：

1)發(fā)射點慣性坐標(biāo)系oxliylizli：坐標(biāo)原點位于初始時刻的發(fā)射點；oxli軸是發(fā)射垂直面與發(fā)射點當(dāng)?shù)厮矫娴慕痪€；oyli軸垂直于oxli軸，指向天；ozli軸與oxli軸、oyli軸垂直；且oxliylizli構(gòu)成右手定則，在慣性空間保持不變，以下簡稱發(fā)射點慣性系。

2)載體坐標(biāo)系oxbybzb：坐標(biāo)原點位于載體質(zhì)心；oxb軸與載體縱軸一致，指向機頭方向；oyb軸垂直于oxb軸，位于載體縱對稱面內(nèi)，指向上方；ozb軸與oxb軸、oyb軸構(gòu)成右手定則。

3)地心慣性坐標(biāo)系oxcyczc：地球坐標(biāo)系oxeyeze在發(fā)射瞬時所固定的坐標(biāo)系。

1.2 解算方法

在發(fā)射點慣性系下，建立捷聯(lián)慣導(dǎo)的微分方程如下

(1)

(2)

(3)

根據(jù)上述發(fā)射點慣性下的捷聯(lián)慣導(dǎo)微分方程，利用加速度計測量的比力和陀螺測量的角速度，分別通過速度更新和位置更新、姿態(tài)更新兩個主要環(huán)節(jié)可以實現(xiàn)對導(dǎo)航參數(shù)的遞推解算。

1)速度和位置更新

通過求解式(1)和式(2)實現(xiàn)速度和位置的更新。為此，需要將載體坐標(biāo)系下的比力轉(zhuǎn)換到發(fā)射點慣性系下

(4)

求解重力加速度在地心慣性系下的分量

(5)

(6)

(7)

由式(4)和式(5)可以計算得到發(fā)射點慣性系下的加速度

(8)

對式(8)積分就可以解算出速度和位置

(9)

2)姿態(tài)更新

根據(jù)描述姿態(tài)方式的不同，姿態(tài)更新算法有歐拉角法、方向余弦法和四元數(shù)法。由于四元數(shù)法算法簡單、計算量小、無奇點，因此，在姿態(tài)更新中得到了廣泛應(yīng)用。對式(3)求解就可以對四元數(shù)進行更新。

在開始進行姿態(tài)更新時，需要對四元數(shù)進行初始化，這里可以利用載體在初始狀態(tài)下的歐拉角初值來解算四元數(shù)初值。

若載體三次轉(zhuǎn)動對應(yīng)的歐拉角順序是俯仰角φ0、航向角ψ0和滾轉(zhuǎn)角γ0，即載體從發(fā)射點慣性系到載體坐標(biāo)系的轉(zhuǎn)動過程如下

由此可以得到由歐拉角表示的發(fā)射點慣性系到載體系的姿態(tài)轉(zhuǎn)換矩陣為

(10)

對應(yīng)的四元數(shù)的初值為

q1=cos(γ0/2)cos(ψ0/2)cos(φ0/2)+

sin(γ0/2)sin(ψ0/2)sin(φ0/2)

q2=sin(γ0/2)cos(ψ0/2)cos(φ0/2)-

cos(γ0/2)sin(ψ0/2)sin(φ0/2)

q3=cos(γ0/2)sin(ψ0/2)cos(φ0/2)+

sin(γ0/2)cos(ψ0/2)sin(φ0/2)

q4=cos(γ0/2)cos(ψ0/2)sin(φ0/2)-

sin(γ0/2)sin(ψ0/2)cos(φ0/2)

(11)

對微分方程進行求解就可以對四元數(shù)進行更新。

2 基于對偶四元數(shù)的捷聯(lián)慣導(dǎo)算法

當(dāng)載體處在高動態(tài)的環(huán)境中時，由于有限轉(zhuǎn)動的不可交換性，在姿態(tài)解算中不可避免地存在著由圓錐運動引起的圓錐誤差；在速度解算的過程中，也必須考慮到系統(tǒng)的劃船誤差的影響。傳統(tǒng)的運動學(xué)觀點是將兩者分開進行考慮，而對偶四元數(shù)可以統(tǒng)一考慮圓錐誤差與劃船誤差進行補償。

2.1 對偶四元數(shù)

對偶數(shù)是由W.Clifford在1873年提出的，因其形式類似于復(fù)數(shù)，可以看作是復(fù)數(shù)域的擴充。其形式如下[12-14]

(12)

其中，α為對偶數(shù)的實數(shù)部分;α′為對偶數(shù)的對偶部分;ε為對偶數(shù)單位，且必須有：ε2=ε3=,…，0。

剛體在空間中的旋轉(zhuǎn)和平移可以用圖 1表示，剛體繞過點c的單位向量l旋轉(zhuǎn)θ角后，沿l的方向平移距離d(或者先沿l的方向平移距離d后，再旋轉(zhuǎn)θ角)。

圖1 剛體在空間的旋轉(zhuǎn)與平移Fig.1 Rotation and translation of rigid bodies in space

剛體的這種運動類似于螺釘和螺母的運動，因此又被稱為螺旋運動，可以用螺旋矢量統(tǒng)一表述，螺旋矢量定義為

(13)

其中，單位螺旋軸為

(14)

對偶角為

(15)

對偶四元數(shù)是以對偶數(shù)和四元數(shù)為基礎(chǔ)的。其定義如下

(16)

其中，ε為對偶數(shù)單位，q和q′都是普通的四元數(shù)

q=q0+q1i+q2j+q3k

(17)

q′=q4+q5i+q6j+q7k

(18)

在圖 1中，平移和轉(zhuǎn)動分別對應(yīng)以下關(guān)系

tL=q*°to°q

(19)

to=q°tL°q*

(20)

其中：to=[0,to]為零標(biāo)量的四元數(shù)。

根據(jù)以上關(guān)系，對偶四元數(shù)可以表示為如下形式

(21)

其中，實部q是描述轉(zhuǎn)動的規(guī)范四元數(shù);對偶部q′是轉(zhuǎn)動四元數(shù)和平移向量的函數(shù)。

對偶四元數(shù)可以由螺旋運動的參數(shù)表示為

(22)

由式(19)和式(20)可得

to=2q′°q*tL=2q*°q′

(23)

對偶四元數(shù)微分方程為

(24)

式中，對偶向量為

(25)

與傳統(tǒng)四元數(shù)更新算法類似，運用螺旋矢量來更新四元數(shù)。螺旋矢量的微分方程如下

(26)

(27)

(28)

因此，式(27)可表示為

(29)

對式(29)在時間間隔[tk,tk+1]內(nèi)積分得到

(30)

εf(τ)]dτ]×[ω(t)+εf(t)]}dt

[ω(t)+εf(t)]}dt

=Δθm+εΔVm+Φcon+εΔVsculm

(31)

式中

(32)

其中，Φcon和ΔVsculm分別代表圓錐誤差和劃船誤差。從式(32)可以看出，對偶四元數(shù)算法將旋轉(zhuǎn)和平移統(tǒng)一起來，能夠同時補償圓錐誤差和劃船誤差。

與旋轉(zhuǎn)矢量法類似，螺旋矢量的計算也由單子樣、雙子樣和三子樣等相應(yīng)的優(yōu)化算法來完成，其優(yōu)化系數(shù)與等效旋轉(zhuǎn)矢量法的優(yōu)化系數(shù)完全相同，在得到螺旋矢量后就可以用其來更新對偶四元數(shù)。下面以三子樣算法為例，給出對偶四元數(shù)的更新步驟為：

螺旋矢量三子樣優(yōu)化算法

(33)

步驟二：由式(22)可得

(34)

將對偶四元數(shù)實數(shù)部分和對偶部分拆開得

(35)

將得到的螺旋矢量代入式(35),計算q(ΔT)和q′(ΔT)。

步驟三：設(shè)tk+1時刻的對偶四元數(shù)可表示為

(36)

將式(36)按照定義拆分成標(biāo)量部分和對偶部分得到

(37)

將得到的q(ΔT)和q′(ΔT)代入式(37),計算tk+1時刻的對偶四元數(shù)。

與東北天坐標(biāo)系下對偶四元數(shù)的更新不同，在發(fā)射點慣性系下，不用再考慮地球自轉(zhuǎn)的影響，也不用再利用分離坐標(biāo)系的思想，所有的更新均在發(fā)射點慣性系下進行。

2.2 對偶四元數(shù)初始化

任意一種捷聯(lián)慣性導(dǎo)航算法都要有一個迭代遞推的過程,由前一時刻的導(dǎo)航信息推算得到當(dāng)前時刻的導(dǎo)航信息。在傳統(tǒng)的導(dǎo)航算法中，進行解算時需要知道初始時刻的三軸速度、位置及姿態(tài)角(俯仰、偏航、滾轉(zhuǎn)角)。而利用對偶四元數(shù)進行解算時,需要的初始信息為同時包含平動和轉(zhuǎn)動信息的初始對偶四元數(shù)。

對偶四元數(shù)實部的初始化與1.1節(jié)中姿態(tài)更新一樣。

對偶四元數(shù)對偶部的初始化如下

(38)

其中，Vli(0)為初始時刻載體在發(fā)射點慣性系下的速度。

2.3 速度更新算法比較

在傳統(tǒng)的速度更新算法中，兩個時刻之間的速度增量為

(39)

對于對偶四元數(shù)來說，由式(23)可得對偶四元數(shù)更新算法的速度增量為[14]

(40)

將式(40)中的三角函數(shù)取四階近似，并忽略高階小量，則在對偶四元數(shù)更新算法中速度增量的計算公式如下

(41)

將式(31)代入式(41)可得

[(Δθm+Φcon)×(ΔVm+ΔVsculm)]

(42)

與式(39)相比，對偶四元數(shù)速度更新算法中速度增量的計算增加了式(42)右邊的后四項，這四項說明了對偶四元數(shù)計算的速度增量更為準(zhǔn)確，從而提高了速度更新的精度。

3 仿真驗證

為了驗證對偶四元數(shù)導(dǎo)航算法的性能，本節(jié)將設(shè)計復(fù)雜程度不同的多條機動軌跡，并在三子樣條件下，對基于對偶四元數(shù)的導(dǎo)航算法和基于旋轉(zhuǎn)矢量的傳統(tǒng)導(dǎo)航算法的性能進行對比研究。為了更直觀地驗證采用對偶四元數(shù)對于減弱不可交換誤差以提高導(dǎo)航精度的作用，這里暫不考慮算法中基于重力模型計算的重力分量對導(dǎo)航解算結(jié)果的影響。因此，在本節(jié)的仿真實驗中，將重力加速度近似為常值進行測試。

(1)單軸姿態(tài)機動

在載體進行單軸姿態(tài)機動時，每次僅繞俯仰軸、偏航軸和滾轉(zhuǎn)軸中的一個軸，按照一定的角速度變化規(guī)律持續(xù)做周期運動，姿態(tài)角也隨之作周期性的變化，且在規(guī)定的時間內(nèi)達到設(shè)定幅值θ。在單軸姿態(tài)機動(簡稱軌跡1)下，單周期的基本軌跡參數(shù)如表1所示，按照該參數(shù)共持續(xù)25個周期，總運行時間為1000s，導(dǎo)航解算周期為0.03s。

表2列出了載體分別繞俯仰軸、偏航軸與滾轉(zhuǎn)軸做姿態(tài)機動，當(dāng)θ分別為30°、60°和80°時的導(dǎo)航解算誤差結(jié)果。由于純捷聯(lián)慣導(dǎo)算法的解算誤差是隨著時間累積而增大的，故表2中數(shù)據(jù)統(tǒng)計的是終點時刻的導(dǎo)航參數(shù)誤差。圖2～圖4所示為當(dāng)俯仰角變化幅值為30°時，兩種解算方法的導(dǎo)航誤差曲線。圖中紅線為對偶四元數(shù)導(dǎo)航算法的解算結(jié)果，藍(lán)線為傳統(tǒng)三子樣算法的解算結(jié)果。

表1 軌跡1參數(shù)說明

表2 載體單軸機動時導(dǎo)航誤差

圖2 速度誤差(軌跡1)Fig.2 Velocity error(trajectory 1)

圖3 位置誤差(軌跡1)Fig.3 Position error(trajectory 1)

圖4 姿態(tài)誤差(軌跡1)Fig.4 Attitude error(trajectory 1)

由表2和圖2、圖3可以看出，使用對偶四元數(shù)導(dǎo)航算法解算的速度和位置的精度明顯高于傳統(tǒng)的三子樣算法。其中位置誤差由24m減小到9m左右，速度誤差也減小了很多，這就驗證了第2節(jié)的理論分析。從圖4可以看出，兩種方法解算得到的姿態(tài)誤差曲線幾乎重合，這說明采用對偶四元數(shù)導(dǎo)航算法后，姿態(tài)精度并未得到提高，這是因為兩種算法采用的姿態(tài)更新原理是一樣的，都是利用對偶四元數(shù)的實部即轉(zhuǎn)動四元數(shù)進行姿態(tài)的解算。

以上通過單軸姿態(tài)機動的仿真實驗，驗證了對偶四元數(shù)算法可以有效提高單軸姿態(tài)持續(xù)機動情況下速度和位置的解算精度，但并不能提高姿態(tài)精度。

(2)三軸姿態(tài)機動

為了進一步驗證對偶四元數(shù)算法在復(fù)雜機動情況下的性能，設(shè)計了三軸同時進行姿態(tài)機動的軌跡，即每次同時繞俯仰軸、偏航軸和滾轉(zhuǎn)軸按照一定的角速度變化規(guī)律持續(xù)做周期運動，姿態(tài)角也隨之做周期性的變化，且在規(guī)定的時間內(nèi)達到設(shè)定幅值θ。在三軸復(fù)雜姿態(tài)機動(簡稱軌跡2)下，單周期的基本軌跡參數(shù)如表3所示，按照該參數(shù)共持續(xù)25個周期，總運行時間為1000s，導(dǎo)航解算周期為0.03s。

表4列出了載體按照表3中的參數(shù)進行三軸姿態(tài)機動時的終點導(dǎo)航誤差。圖5～圖7所示為載體按軌跡2機動時，兩種導(dǎo)航解算方法下的導(dǎo)航誤差曲線對比圖。圖中紅線為對偶四元數(shù)導(dǎo)航算法的解算結(jié)果，藍(lán)線為傳統(tǒng)三子樣算法的解算結(jié)果。

表3 軌跡2說明

表4 載體三軸姿態(tài)機動時導(dǎo)航誤差

圖5 速度誤差(軌跡2)Fig.5 Velocity error( trajectory 2)

圖6 位置誤差(軌跡2)Fig.6 Position error(trajectory 2)

圖7 姿態(tài)誤差(軌跡2)Fig.7 Attitude error(trajectory 2)

由圖5和圖6可以看出，對偶四元數(shù)算法解算出的三軸速度和位置誤差明顯小于傳統(tǒng)算法的解算結(jié)果，其中終點位置誤差由240多m減小到60多m。由此可見，當(dāng)載體按照軌跡2機動時，采用對偶四元數(shù)算法解算的速度和位置精度有顯著提高，且隨著時間的推移和機動復(fù)雜性的增加，這種精度優(yōu)勢更加明顯。

由圖7可以看出，兩種算法下的姿態(tài)誤差曲線幾乎重合，這是由于兩種算法中的姿態(tài)更新方法無本質(zhì)區(qū)別，因此，對偶四元數(shù)導(dǎo)航算法并不能提高姿態(tài)的解算精度。

4 結(jié)論

本文以發(fā)射點慣性系為導(dǎo)航系的彈類載體為對象，針對彈載捷聯(lián)慣性導(dǎo)航系統(tǒng)在高動態(tài)環(huán)境下容易產(chǎn)生圓錐誤差與劃船誤差的問題,設(shè)計了基于對偶四元數(shù)的捷聯(lián)慣性導(dǎo)航算法，并通過仿真實驗驗證了方法的有效性。通過對三子樣下的基于對偶四元數(shù)的捷聯(lián)慣性導(dǎo)航算法和傳統(tǒng)算法的理論分析與實驗結(jié)果對比分析表明：

1)對偶四元數(shù)導(dǎo)航算法可以同時補償系統(tǒng)的圓錐誤差與劃船誤差，速度和位置的解算精度明顯高于傳統(tǒng)算法，但姿態(tài)精度相當(dāng)。

2)載體所作的機動越復(fù)雜、持續(xù)時間越長，基于對偶四元數(shù)的捷聯(lián)慣導(dǎo)算法的精度優(yōu)勢越明顯。