捷聯慣導四子樣旋轉矢量姿態更新算法
張澤，段廣仁
（哈爾濱工業大學控制理論與制導技術研究中心，黑龍江哈爾濱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)可得：
| 
