摘    要：姿態(tài)更新算法是捷聯(lián)慣性導(dǎo)航系統(tǒng)的關(guān)鍵算法，目前姿態(tài)更新算法有歐拉角法、四元數(shù)法、方向余弦法和旋轉(zhuǎn)矢量法。旋轉(zhuǎn)矢量法可以采用多子樣算法實(shí)現(xiàn)對(duì)不可交換誤差的補(bǔ)償。針對(duì)利用陀螺角增量輸出進(jìn)行姿態(tài)更新計(jì)算帶來的不可交換性誤差，考慮到導(dǎo)航坐標(biāo)系在姿態(tài)更新周期內(nèi)旋轉(zhuǎn)比較緩慢的特點(diǎn)，研究了捷聯(lián)慣導(dǎo)姿態(tài)更新的旋轉(zhuǎn)矢量的修正算法，以此為基礎(chǔ)詳細(xì)推導(dǎo)了四子樣的旋轉(zhuǎn)矢量算法，得出利用陀螺角增量求解等效旋轉(zhuǎn)矢量的顯式形式。該顯式形式中直接利用陀螺的增量輸出，便于工程實(shí)際中應(yīng)用。

關(guān)鍵詞：姿態(tài)更新；等效旋轉(zhuǎn)矢量；四子樣；姿態(tài)修正

中圖分類號(hào)：TP 27    文獻(xiàn)標(biāo)識(shí)碼：A

    姿態(tài)更新是實(shí)時(shí)地解算從機(jī)體坐標(biāo)系到導(dǎo)航坐標(biāo)系的方向余弦矩陣，姿態(tài)更新算法是捷聯(lián)慣性導(dǎo)航系統(tǒng)的關(guān)鍵算法。傳統(tǒng)的姿態(tài)更新算法有歐拉角法、方向余弦法和四元數(shù)法，其中，四元數(shù)皮卡算法簡(jiǎn)單、計(jì)算量小，因而在工程實(shí)際中常采用。但是該算法對(duì)不可交換誤差的補(bǔ)償不****，特別是運(yùn)載體高動(dòng)態(tài)時(shí)，這種誤差更加嚴(yán)重。而等效旋轉(zhuǎn)矢量法對(duì)這種誤差作了適當(dāng)補(bǔ)償，特別適用于高動(dòng)態(tài)的環(huán)境下工作。本文推導(dǎo)了四予樣旋轉(zhuǎn)矢量算法，并推導(dǎo)了利用旋轉(zhuǎn)矢量進(jìn)行姿態(tài)解箅時(shí)的修正算法。

    1)向量坐標(biāo)變換的四元數(shù)乘表示法和坐標(biāo)變換矩陣表示法如果將向量r（向量r在R系中的投影）和r6（向量r在6系中的投影）看作零標(biāo)量的四元數(shù)，則rR和r6聞的變換關(guān)系可采用四元數(shù)乘式中，O表示四元數(shù)乘；Q表示從R系到6系的旋轉(zhuǎn)四元數(shù)；Q 8表示其共軛四元數(shù)。

    而坐標(biāo)變換矩陣表示方法為

式中，cR為從b系到R系的坐標(biāo)變化矩陣。

  2)四元數(shù)的三角式及四元數(shù)微分方程Q=cos（0/2）+URsiri（0/2），當(dāng)用其描述剛體定點(diǎn)轉(zhuǎn)動(dòng)，即當(dāng)只關(guān)心b系相對(duì)月系的角位置時(shí)，可認(rèn)為b系是由R系經(jīng)過無中間過程的一次性等效旋轉(zhuǎn)形成，為瞬時(shí)旋轉(zhuǎn)軸和旋轉(zhuǎn)方向，θ為轉(zhuǎn)過的角度。

下面不加證明地給出四元數(shù)的微分方程：

  3)姿態(tài)四元數(shù)如果上述中b系表征運(yùn)載體機(jī)體坐標(biāo)系，R系表征運(yùn)載體的導(dǎo)航坐標(biāo)系n，則Q為運(yùn)載體的姿態(tài)四元數(shù)。

    設(shè)tk時(shí)刻的機(jī)體坐標(biāo)系為b(k)，導(dǎo)航坐標(biāo)系為n(k)，tk+1時(shí)刻的機(jī)體坐標(biāo)系為b(k+1)，導(dǎo)航坐標(biāo)系為n(k+1)。記6(k)至b(k+1)的旋轉(zhuǎn)四元數(shù)為g(h)，n( k)至b(k)的旋轉(zhuǎn)四元數(shù)Q(tk)，即為tk；時(shí)刻的姿態(tài)四元數(shù)，n（k十1）至b(k+1)的旋轉(zhuǎn)四元數(shù)為Q(tk+1)，即為tk=1

，時(shí)刻的姿態(tài)四元數(shù)，n(k)至n(k+1)的旋轉(zhuǎn)四元數(shù)為p(h)，其中，h=t_k-1-t_k為姿態(tài)更新周期：根據(jù)式(2)可得：

    還可以得到：

   

    由式(1)可以得到下式：

 

    依據(jù)式(1)還能得到如下各式：

    聯(lián)立以上各式可得到：

    四元數(shù)的乘法結(jié)合律，上式可以寫作：

    比較上式和式(5)可得：

設(shè)姿態(tài)更新周期h=t_k-1-t_k內(nèi)，導(dǎo)航坐標(biāo)系的變化較緩慢，設(shè)導(dǎo)航坐標(biāo)系的變換四元數(shù)p(h)=cos（θ/2）+u^Rsin（θ/2），所以由四元數(shù)表示剛體轉(zhuǎn)動(dòng)的意義可得θ=O，所以p(h)=1+θ，所以式(9)可寫成：

    由式(6)~式(8)可知，此時(shí)得到的姿態(tài)四元數(shù)上是從b(λ+1)坐標(biāo)系到T(k)坐標(biāo)系的姿態(tài)四元數(shù)，也就是說姿態(tài)更新很短的時(shí)間周期內(nèi)忽略了導(dǎo)航坐標(biāo)系的變化，下面研究如何修正由于忽略導(dǎo)航坐標(biāo)系的變化而引起的姿態(tài)矩陣的解算誤差。

    姿態(tài)四元數(shù)理應(yīng)按照式(9)進(jìn)行更新，由于導(dǎo)航坐標(biāo)系在姿態(tài)更新周期內(nèi)旋轉(zhuǎn)比較緩慢，采用式(10)來進(jìn)行更新，但是經(jīng)過若干姿態(tài)更新周期后要作適當(dāng)修正，修正方法分析如下：

    由式(10)算得到的Q（t_k+I）。假設(shè)修正計(jì)算的周期為t_k-1-t_k=Mh，其中，h為姿態(tài)更新周期，M為某一正整數(shù)，那么根據(jù)式(10)更新解算：

    此時(shí)得到的姿態(tài)四元數(shù)對(duì)應(yīng)的姿態(tài)矩陣,也就說忽略r導(dǎo)航坐標(biāo)系的更新，而t_i=1時(shí)刻正確的姿態(tài)矩陣應(yīng)該為

    導(dǎo)航坐標(biāo)系的變化可以根據(jù)下面近似得到。

    假設(shè)在修正周期內(nèi)（Mh）內(nèi)，運(yùn)載體的經(jīng)度和緯度的增量分別為△λ和△L，且均為微小角，則導(dǎo)航坐標(biāo)系在這段時(shí)間間隔內(nèi)的旋轉(zhuǎn)矢量為

1)  旋轉(zhuǎn)矢量的微分方程形式  由式( 10)可知，進(jìn)行姿態(tài)四元數(shù)的更新，只要能夠求得旋轉(zhuǎn)矢量q(h)就可以進(jìn)行姿態(tài)的實(shí)時(shí)更新，而q（h）由等效旋轉(zhuǎn)矢量φ確定，所以姿態(tài)更新問題最終歸結(jié)到求解等效旋轉(zhuǎn)矢量φ

    下面不加證明地給出等效旋轉(zhuǎn)矢量的微分方程，即****的Borlz方程：

    對(duì)其中的三角畫數(shù)展開，可以得到更為簡(jiǎn)潔的近似方程。

    2)等效旋轉(zhuǎn)矢量的四子樣算法研究按照式(1l)和式(12)求解旋轉(zhuǎn)矢量有諸多不便，主要是：

①激光陀螺的輸出一般為角增量，如果將角增量折算成角速率，則微商將引起嚴(yán)重的噪聲放大效應(yīng)。

②即使能夠得到角速率的輸出，對(duì)于式(11)和式(12)所示微分方程也只能求其數(shù)值解，所以必須對(duì)角速率采樣，這樣造成姿態(tài)更新周期內(nèi)丟失了很多信息。

③式(10)說明，姿態(tài)更新只需求時(shí)刻，機(jī)體坐標(biāo)系旋轉(zhuǎn)所對(duì)應(yīng)的等效旋轉(zhuǎn)矢量，而不必要知道這個(gè)時(shí)間段內(nèi)旋狀矢量是如何變化的。因此，可采用Taylor級(jí)數(shù)展開法求解旋轉(zhuǎn)矢量[2]。

    設(shè)φ (h)為[tk=1，tk]時(shí)間段內(nèi)的等效旋轉(zhuǎn)矢量，其中，h =tk+1-tk姿態(tài)更新周期，運(yùn)載體的角速度用三次拋物線擬合：

    其中，τ [O，h]，對(duì)φ (h)在h一O進(jìn)行Taylor級(jí)數(shù)展開：

    將式(13)代入得：

    則由式(13)和式(14)：

 

    由于姿態(tài)更新周期特別短，由可以設(shè)為小量，根據(jù)式(12)計(jì)算西(r)在r=0時(shí)的各階導(dǎo)數(shù)時(shí)，可略去第3項(xiàng)，并且近似使用φ(τ)=△θ(τ)，這樣式(12)可寫成：

  上式求各階導(dǎo)數(shù)，并考慮式(15)和式(16)可以得到如下諸式：為便于書寫，把ω_nb^b(tk+τ)簡(jiǎn)寫為ω，把△θ(τ)簡(jiǎn)寫作△θ，φ(τ)簡(jiǎn)寫為φ。

  對(duì)式(17)求各階導(dǎo)數(shù)，并利用式(15)和式(16)代人可以得到以下諸式：

  將式(15)和式(16)代人上述諸式：

   所以可以得到：

  用陀螺輸出代替上式中的參數(shù)：在一個(gè)姿態(tài)更新周期內(nèi)對(duì)陀螺輸出進(jìn)行四次采樣，記：

  求解上述諸式：

  將其代入式(18)得到四子樣旋轉(zhuǎn)矢量算法：

    至此，就得到了等效旋轉(zhuǎn)矢量和陀螺輸出角增量間的關(guān)系，實(shí)時(shí)解算等效旋轉(zhuǎn)矢量，，就可以求得姿態(tài)更新周期內(nèi)載體坐標(biāo)系的旋轉(zhuǎn)四元數(shù)q(h)，然后根據(jù)式(10)進(jìn)行姿態(tài)四元數(shù)的更新，從而得到運(yùn)載體姿態(tài)的更新。

    在姿態(tài)更新周期內(nèi)，采用了三次拋物線來擬合運(yùn)載體的角速度，得到了旋轉(zhuǎn)矢量四子樣算法，實(shí)際上采用不同的函數(shù)來擬合運(yùn)載體的角運(yùn)動(dòng)，就決定了旋轉(zhuǎn)矢量解算的子樣數(shù)，文中用三次拋物線來擬合運(yùn)載體的角運(yùn)動(dòng)，對(duì)應(yīng)的是四子樣算法。

  運(yùn)載體的角運(yùn)動(dòng)具有很大的任意性，采用某一特定函數(shù)來擬合它，本身就有一定的近似性，在實(shí)際導(dǎo)航系統(tǒng)應(yīng)用過程中，應(yīng)該根據(jù)運(yùn)載體運(yùn)動(dòng)特性來選擇子樣數(shù)，也就是在精度和實(shí)時(shí)性間進(jìn)行折衷選擇。如運(yùn)輸機(jī)、轟炸機(jī)、艦船等角運(yùn)動(dòng)比較緩慢的運(yùn)載體，單子樣算法即滿足其導(dǎo)航精度的需要，而對(duì)于一些精確制導(dǎo)式武器等則需要高子樣算法。