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

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

0 引言

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

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

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

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

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

1.1 坐標(biāo)系說(shuō)明

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

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

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

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

1.2 解算方法

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

(1)

(2)

(3)

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

1)速度和位置更新

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

(4)

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

(5)

(6)

(7)

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

(8)

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

(9)

2)姿態(tài)更新

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

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

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

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

(10)

對(duì)應(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)

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

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

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

2.1 對(duì)偶四元數(shù)

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

(12)

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

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

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

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

(13)

其中，單位螺旋軸為

(14)

對(duì)偶角為

(15)

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

(16)

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

q=q0+q1i+q2j+q3k

(17)

q′=q4+q5i+q6j+q7k

(18)

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

tL=q*°to°q

(19)

to=q°tL°q*

(20)

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

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

(21)

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

對(duì)偶四元數(shù)可以由螺旋運(yùn)動(dòng)的參數(shù)表示為

(22)

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

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

(23)

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

(24)

式中，對(duì)偶向量為

(25)

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

(26)

(27)

(28)

因此，式(27)可表示為

(29)

對(duì)式(29)在時(shí)間間隔[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)可以看出，對(duì)偶四元數(shù)算法將旋轉(zhuǎn)和平移統(tǒng)一起來(lái)，能夠同時(shí)補(bǔ)償圓錐誤差和劃船誤差。

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

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

(33)

步驟二：由式(22)可得

(34)

將對(duì)偶四元數(shù)實(shí)數(shù)部分和對(duì)偶部分拆開(kāi)得

(35)

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

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

(36)

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

(37)

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

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

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

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

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

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

(38)

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

2.3 速度更新算法比較

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

(39)

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

(40)

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

(41)

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

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

(42)

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

3 仿真驗(yàn)證

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

(1)單軸姿態(tài)機(jī)動(dòng)

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

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

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

表2 載體單軸機(jī)動(dòng)時(shí)導(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可以看出，使用對(duì)偶四元數(shù)導(dǎo)航算法解算的速度和位置的精度明顯高于傳統(tǒng)的三子樣算法。其中位置誤差由24m減小到9m左右，速度誤差也減小了很多，這就驗(yàn)證了第2節(jié)的理論分析。從圖4可以看出，兩種方法解算得到的姿態(tài)誤差曲線幾乎重合，這說(shuō)明采用對(duì)偶四元數(shù)導(dǎo)航算法后，姿態(tài)精度并未得到提高，這是因?yàn)閮煞N算法采用的姿態(tài)更新原理是一樣的，都是利用對(duì)偶四元數(shù)的實(shí)部即轉(zhuǎn)動(dòng)四元數(shù)進(jìn)行姿態(tài)的解算。

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

(2)三軸姿態(tài)機(jī)動(dòng)

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

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

表3 軌跡2說(shuō)明

表4 載體三軸姿態(tài)機(jī)動(dòng)時(shí)導(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可以看出，對(duì)偶四元數(shù)算法解算出的三軸速度和位置誤差明顯小于傳統(tǒng)算法的解算結(jié)果，其中終點(diǎn)位置誤差由240多m減小到60多m。由此可見(jiàn)，當(dāng)載體按照軌跡2機(jī)動(dòng)時(shí)，采用對(duì)偶四元數(shù)算法解算的速度和位置精度有顯著提高，且隨著時(shí)間的推移和機(jī)動(dòng)復(fù)雜性的增加，這種精度優(yōu)勢(shì)更加明顯。

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

4 結(jié)論

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

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

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