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