一種改進的旋轉矢量姿態更新算法*

耿騰飛 劉 明

（云南民族大學 昆明 650031）

1 引言

自1971年Bortz提出旋轉矢量微分方程后[1]，有大量學者對圓錐誤差補償算法進行了一系列的研究[2～7]，有效地提高了姿態解算的精度，然而上述算法表達式都是基于陀螺的角增量信息，近年來新型陀螺儀輸出大多為角速率形式，將原有算法直接應用到角速率陀螺算法誤差會變大。針對上述現象，文獻[8～9]對硬件增強角速率輸入圓錐算法做了深入研究，詳細推導了基于角速率輸入的圓錐算法誤差補償系數的通式。文獻[10]對周期項的誤差補償進行再次優化，提出一種改進的姿態算法。文獻[11]提出了一種同時把角速率和角增量信號作為輸入的圓錐誤差算法，提高了算法精度，此外還有許多文獻研究角速率輸入的圓錐誤差算法[12～13]。文中詳細推導了角速率輸入的圓錐誤差補償系數的表示方式，在此基礎上利用陀螺前周期角增量信息得到了改進的求解圓錐誤差補償系數的算法，具有一定工程應用價值。

2 圓錐誤差算法優化準則

3 角速率圓錐誤差補償算法

4 改進圓錐補償算法

5 應用

下面給出1～4子樣圓錐誤差系數，結果見表1。

表1 1～4子樣圓錐誤差系數

通過比較式（28）和式（32）可以看出同子樣數的改進算法精度高于常規算法，更加實用。

以N=2時的算法為例，分別對傳統算法和改進算法進行模擬仿真，取a=1°，h=0.02s，算法誤差與圓錐頻率之間的關系如圖1所示。

圖1 算法誤差與圓錐運動頻率的關系

由上圖可以看出，兩類算法誤差都隨著圓錐運動的頻率增大而增大，但是在頻率相同的情況下，改進算法的算法誤差與傳統算法相比有明顯的降低。

6 結語

文中詳細推導了純角速率輸入下的圓錐算法公式，并給出了誤差補償的一般公式，列出了1～4子樣的補償系數，并在此基礎上提出了一種改進的角速率圓錐誤差補償算法，可求得任意子樣數下的補償系數方程和誤差表達式，通過比較可知新算法有明顯的優勢，具有一定的應用價值。