捷聯(lián)慣導(dǎo)四子樣旋轉(zhuǎn)矢量姿態(tài)更新算法
張澤，段廣仁
（哈爾濱工業(yè)大學(xué)控制理論與制導(dǎo)技術(shù)研究中心，黑龍江哈爾濱15000）
摘 要：姿態(tài)更新算法是捷聯(lián)慣性導(dǎo)航系統(tǒng)的關(guān)鍵算法，目前姿態(tài)更新算法有歐拉角法、四元數(shù)法、方向余弦法和旋轉(zhuǎn)矢量法。旋轉(zhuǎn)矢量法可以采用多子樣算法實現(xiàn)對不可交換誤差的補償。針對利用陀螺角增量輸出進行姿態(tài)更新計算帶來的不可交換性誤差，考慮到導(dǎo)航坐標(biāo)系在姿態(tài)更新周期內(nèi)旋轉(zhuǎn)比較緩慢的特點，研究了捷聯(lián)慣導(dǎo)姿態(tài)更新的旋轉(zhuǎn)矢量的修正算法，以此為基礎(chǔ)詳細推導(dǎo)了四子樣的旋轉(zhuǎn)矢量算法，得出利用陀螺角增量求解等效旋轉(zhuǎn)矢量的顯式形式。該顯式形式中直接利用陀螺的增量輸出，便于工程實際中應(yīng)用。
關(guān)鍵詞：姿態(tài)更新；等效旋轉(zhuǎn)矢量；四子樣；姿態(tài)修正
中圖分類號：TP 27 文獻標(biāo)識碼：A
1引言
姿態(tài)更新是實時地解算從機體坐標(biāo)系到導(dǎo)航坐標(biāo)系的方向余弦矩陣，姿態(tài)更新算法是捷聯(lián)慣性導(dǎo)航系統(tǒng)的關(guān)鍵算法。傳統(tǒng)的姿態(tài)更新算法有歐拉角法、方向余弦法和四元數(shù)法，其中，四元數(shù)皮卡算法簡單、計算量小，因而在工程實際中常采用。但是該算法對不可交換誤差的補償不****，特別是運載體高動態(tài)時，這種誤差更加嚴重。而等效旋轉(zhuǎn)矢量法對這種誤差作了適當(dāng)補償，特別適用于高動態(tài)的環(huán)境下工作。本文推導(dǎo)了四予樣旋轉(zhuǎn)矢量算法，并推導(dǎo)了利用旋轉(zhuǎn)矢量進行姿態(tài)解箅時的修正算法。
2基本關(guān)系式
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)用其描述剛體定點轉(zhuǎn)動，即當(dāng)只關(guān)心b系相對月系的角位置時，可認為b系是由R系經(jīng)過無中間過程的一次性等效旋轉(zhuǎn)形成，為瞬時旋轉(zhuǎn)軸和旋轉(zhuǎn)方向，θ為轉(zhuǎn)過的角度。
下面不加證明地給出四元數(shù)的微分方程：
3)姿態(tài)四元數(shù)如果上述中b系表征運載體機體坐標(biāo)系，R系表征運載體的導(dǎo)航坐標(biāo)系n，則Q為運載體的姿態(tài)四元數(shù)。
3旋轉(zhuǎn)矢量和姿態(tài)四元數(shù)的關(guān)系
設(shè)tk時刻的機體坐標(biāo)系為b(k)，導(dǎo)航坐標(biāo)系為n(k)，tk+1時刻的機體坐標(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；時刻的姿態(tài)四元數(shù)，n（k十1）至b(k+1)的旋轉(zhuǎn)四元數(shù)為Q(tk+1)，即為tk=1
，時刻的姿態(tài)四元數(shù)，n(k)至n(k+1)的旋轉(zhuǎn)四元數(shù)為p(h)，其中，h=tk-1-tk為姿態(tài)更新周期：根據(jù)式(2)可得：
還可以得到：
由式(1)可以得到下式：
依據(jù)式(1)還能得到如下各式：
聯(lián)立以上各式可得到：
四元數(shù)的乘法結(jié)合律，上式可以寫作：
比較上式和式(5)可得：
設(shè)姿態(tài)更新周期h=tk-1-tk內(nèi)，導(dǎo)航坐標(biāo)系的變化較緩慢，設(shè)導(dǎo)航坐標(biāo)系的變換四元數(shù)p(h)=cos（θ/2）+uRsin（θ/2），所以由四元數(shù)表示剛體轉(zhuǎn)動的意義可得θ=O，所以p(h)=1+θ，所以式(9)可寫成：
由式(6)~式(8)可知，此時得到的姿態(tài)四元數(shù)上是從b(λ+1)坐標(biāo)系到T(k)坐標(biāo)系的姿態(tài)四元數(shù)，也就是說姿態(tài)更新很短的時間周期內(nèi)忽略了導(dǎo)航坐標(biāo)系的變化，下面研究如何修正由于忽略導(dǎo)航坐標(biāo)系的變化而引起的姿態(tài)矩陣的解算誤差。
4姿態(tài)四元數(shù)修正算法
姿態(tài)四元數(shù)理應(yīng)按照式(9)進行更新，由于導(dǎo)航坐標(biāo)系在姿態(tài)更新周期內(nèi)旋轉(zhuǎn)比較緩慢，采用式(10)來進行更新，但是經(jīng)過若干姿態(tài)更新周期后要作適當(dāng)修正，修正方法分析如下：
由式(10)算得到的Q（tk+I）。假設(shè)修正計算的周期為tk-1-tk=Mh，其中，h為姿態(tài)更新周期，M為某一正整數(shù)，那么根據(jù)式(10)更新解算：
此時得到的姿態(tài)四元數(shù)對應(yīng)的姿態(tài)矩陣,也就說忽略r導(dǎo)航坐標(biāo)系的更新，而ti=1時刻正確的姿態(tài)矩陣應(yīng)該為
導(dǎo)航坐標(biāo)系的變化可以根據(jù)下面近似得到。
假設(shè)在修正周期內(nèi)（Mh）內(nèi)，運載體的經(jīng)度和緯度的增量分別為△λ和△L，且均為微小角，則導(dǎo)航坐標(biāo)系在這段時間間隔內(nèi)的旋轉(zhuǎn)矢量為
5旋轉(zhuǎn)矢量微分方程及其四子樣算法研究
1) 旋轉(zhuǎn)矢量的微分方程形式 由式( 10)可知，進行姿態(tài)四元數(shù)的更新，只要能夠求得旋轉(zhuǎn)矢量q(h)就可以進行姿態(tài)的實時更新，而q（h）由等效旋轉(zhuǎn)矢量φ確定，所以姿態(tài)更新問題最終歸結(jié)到求解等效旋轉(zhuǎn)矢量φ
下面不加證明地給出等效旋轉(zhuǎn)矢量的微分方程，即****的Borlz方程：
對其中的三角畫數(shù)展開，可以得到更為簡潔的近似方程。
2)等效旋轉(zhuǎn)矢量的四子樣算法研究按照式(1l)和式(12)求解旋轉(zhuǎn)矢量有諸多不便，主要是：
①激光陀螺的輸出一般為角增量，如果將角增量折算成角速率，則微商將引起嚴重的噪聲放大效應(yīng)。
②即使能夠得到角速率的輸出，對于式(11)和式(12)所示微分方程也只能求其數(shù)值解，所以必須對角速率采樣，這樣造成姿態(tài)更新周期內(nèi)丟失了很多信息。
③式(10)說明，姿態(tài)更新只需求時刻，機體坐標(biāo)系旋轉(zhuǎn)所對應(yīng)的等效旋轉(zhuǎn)矢量，而不必要知道這個時間段內(nèi)旋狀矢量是如何變化的。因此，可采用Taylor級數(shù)展開法求解旋轉(zhuǎn)矢量[2]。
設(shè)φ (h)為[tk=1，tk]時間段內(nèi)的等效旋轉(zhuǎn)矢量，其中，h =tk+1-tk姿態(tài)更新周期，運載體的角速度用三次拋物線擬合：
其中，τ [O，h]，對φ (h)在h一O進行Taylor級數(shù)展開：
將式(13)代入得：
則由式(13)和式(14)：
由于姿態(tài)更新周期特別短，由可以設(shè)為小量，根據(jù)式(12)計算西(r)在r=0時的各階導(dǎo)數(shù)時，可略去第3項，并且近似使用φ(τ)=△θ(τ)，這樣式(12)可寫成：
上式求各階導(dǎo)數(shù)，并考慮式(15)和式(16)可以得到如下諸式：為便于書寫，把ωnbb(tk+τ)簡寫為ω，把△θ(τ)簡寫作△θ，φ(τ)簡寫為φ。
對式(17)求各階導(dǎo)數(shù)，并利用式(15)和式(16)代人可以得到以下諸式：
將式(15)和式(16)代人上述諸式：
所以可以得到：
用陀螺輸出代替上式中的參數(shù)：在一個姿態(tài)更新周期內(nèi)對陀螺輸出進行四次采樣，記：
求解上述諸式：
將其代入式(18)得到四子樣旋轉(zhuǎn)矢量算法：
至此，就得到了等效旋轉(zhuǎn)矢量和陀螺輸出角增量間的關(guān)系，實時解算等效旋轉(zhuǎn)矢量，，就可以求得姿態(tài)更新周期內(nèi)載體坐標(biāo)系的旋轉(zhuǎn)四元數(shù)q(h)，然后根據(jù)式(10)進行姿態(tài)四元數(shù)的更新，從而得到運載體姿態(tài)的更新。
6結(jié)語
在姿態(tài)更新周期內(nèi)，采用了三次拋物線來擬合運載體的角速度，得到了旋轉(zhuǎn)矢量四子樣算法，實際上采用不同的函數(shù)來擬合運載體的角運動，就決定了旋轉(zhuǎn)矢量解算的子樣數(shù)，文中用三次拋物線來擬合運載體的角運動，對應(yīng)的是四子樣算法。
運載體的角運動具有很大的任意性，采用某一特定函數(shù)來擬合它，本身就有一定的近似性，在實際導(dǎo)航系統(tǒng)應(yīng)用過程中，應(yīng)該根據(jù)運載體運動特性來選擇子樣數(shù)，也就是在精度和實時性間進行折衷選擇。如運輸機、轟炸機、艦船等角運動比較緩慢的運載體，單子樣算法即滿足其導(dǎo)航精度的需要，而對于一些精確制導(dǎo)式武器等則需要高子樣算法。
|
