三時間間隔圓錐補償姿態更新算法性能分析

程思微，葉靈軍，甘厚吉

CHENG Siwei,YE Lingjun,GAN Houji

海軍工程大學 電子工程學院，武漢430033

College of Electronic Engineering,Naval University of Engineering,Wuhan 430033,China

1 引言

Ignagni[1]在1990 年首先提出了姿態更新算法的三時間間隔結構，并給出了一組計算公式。其基本方法是把等效旋轉矢量的求解區間分為大、中、小三個時間間隔。每個大時間間隔作為一個更新周期。大時間間隔包含若干個中時間間隔，中時間間隔又包含若干個小時間間隔。在計算時，先對小時間間隔內的角速度積分，得出小時間間隔內的角增量，將這些量相加，就可以得到中時間間隔和大時間間隔的角增量；然后再利用小時間間隔內的角增量的叉乘來對中時間間隔和大時間間隔的角增量進行補償，得到大時間間隔內的等效旋轉矢量，并進行姿態更新。

以文獻[1]提出的三時間間隔結構為基礎，Lee[2]、Park[3]、Wu[4]等人設計了一系列相應的多子樣等效旋轉矢量算法。Ignagni在其總結性文獻[5]中，綜合了Jiang[6]和Lee[2]的部分算法，得出了三時間間隔下的另一組計算公式。

上述姿態更新方法[1-6]均采用三時間間隔結構，各有其優缺點。其中，Ignagni 和Park 等人都只計算了算法漂移，沒有進行仿真驗證。而Lee 和Wu 雖然進行了仿真實驗，但沒有涉及大時間間隔中應該包含多少個中時間間隔這一關鍵問題，僅僅簡單地直接令大時間間隔等于中時間間隔。

為了考察三時間間隔結構在姿態更新中的作用，本文用三時間間隔結構改寫了Lee 提出的四子樣算法和四子樣優化算法，以及Savage[7]的方法，然后對改進算法以及Ignagni、Park、Lee 等人算法分別進行了仿真，并通過與傳統雙速結構算法的對比分析，確定三時間間隔對姿態更新算法精度和效率的影響。

2 三時間間隔圓錐補償算法

2.1 基本原理

姿態更新的過程實質是求解方向余弦陣、歐拉角或者姿態四元數的過程。不管采用哪種描述方法，都要先求解等效旋轉矢量。等效旋轉矢量的微分運動方程如下[8]：

其中Φ為等效旋轉矢量，表示載體tn-1到tn時刻的等效旋轉，?=||Φ||為旋轉角度，ω為旋轉角速度。由于時間間隔很短，一般情況下?為小量。

旋轉矢量的計算公式如下[1]：

采用三時間間隔的思想，設每個大時間間隔包含M個中時間間隔，則

表示第m個中時間間隔內的圓錐補償。它的計算公式和Jordan[9]、Miller[10]、Savage[7]等人提出的等效旋轉矢量的計算公式類似。另外，它也是Ignagni[1，5]系列算法的不同之所在。

2.2 基于三時間間隔結構的算法改進

公式（4）～（6）中的任意一個，再加上式（1）～（3），就構成了一個新的三時間間隔結構的姿態更新算法。例如，先由公式（4）求解中時間間隔內的圓錐補償，然后再根據公式（1）～（3）計算大時間間隔內的等效旋轉矢量，進而對姿態進行更新，就是三時間間隔結構的四子樣等效旋轉矢量姿態更新算法。

3 算法分析

下面對本文將要考察的算法進行分析。

Ignagni 給出的兩組算法的主要不同是中時間間隔的圓錐補償的計算公式不同。在文獻[1]中，算法A 的中時間間隔圓錐補償，此時，中時間間隔等于小時間間隔，算法退化成兩時間間隔，也就是Savage[7]所說的雙速結構。算法A 的計算公式也和Savage 的類似，所不同的是，它缺少上一周期的補償量，且補償系數全是1/2。算法B 是Jiang[6]提出的單子樣公式的三時間間隔的表示。算法C、D、E 是Jordan[9]和Miller[10]算法的三時間間隔的表示，而算法E1 和F 是從算法E 改進而來的，所不同的是，這兩種算法在求解時都只需要計算一次叉乘。算法G、H 又是算法F 改進版本，它們的不同是，用到了上一個大時間間隔的角增量作為補償量。

在文獻[5]中，算法1 是Jiang[6]的二子樣算法的三時間間隔表示，算法3、5、7分別是從Jordan[9]、Miller[10]、Lee[2]的算法改進得到的三子樣姿態更新算法，算法9 是一種新的五子樣算法。算法2、4、6、8、10 又分別是從算法1、3、5、7、9 改進而來的，它們的特點是比原方法多了一個修正量。Park[3]的算法6 是一個六子樣算法。

表1 給出了文獻[1-3，5]，以及本文中的算法按子樣數的分類。其中，IA～IH分別表示文獻[1]中的算法A～H，Lee 表示文獻[2]提出的實時算法，Park6 表示文獻[3]中的算法6，I1～I10 分別表示文獻[5]中的算法1～10，RV43、RV4o3、S23、S33、S43 分別表示本文構造的四子樣算法、優化四子樣算法，以及子樣數為2、3、4 的Savage方法。

為了簡便起見，約定每個大時間間隔包含的中時間間隔的個數稱為樣本數（M值），而每個中時間間隔包含的小時間間隔的個數為子樣數（N值）。如果樣本數為1，則三時間間隔退化成兩時間間隔，Jordan、Miller 等人提出的傳統的等效旋轉矢量算法就是在此基礎上構造的。因為三時間間隔結構與傳統的算法結構的主要差別是多了一個樣本數的概念，下面主要考察樣本數對算法精度和效率的影響。

表1 各算法按子樣數的分類

4 實驗分析

4.1 精度分析

精度分析實驗分三組進行，第一組測試文獻[1]中的算法，第二組測試文獻[5]和文獻[3]中的算法，第三組測試本文構造的算法。假設載體作典型圓錐運動，實驗參數設置為：圓錐頻率0.1 Hz，半錐角1°，步長0.01 s，運行時間1 h。

圖1、2 給出了文獻[1]中算法的歐拉角最大絕對值誤差MAE隨樣本數的變化曲線。其中，RV3 表示三子樣旋轉矢量算法，樣本數不變化；IA～IH 分別表示文獻中的算法A～H，樣本數的變化范圍為[1，20]。從圖1 可以看到：

（1）除了算法B、G、H 以外，其他算法的誤差隨樣本數的變化都不明顯。

（2）就算是算法B、G、H，算法誤差也不會隨著樣本數增加而無限減小，它們存在一個極限，從圖1 上看，它們的精度都不會超過RV3 的精度。

圖1 文獻[1]中算法誤差

圖2 算法A 和算法C～F 誤差

（3）算法C～F 的精度與RV3 相當。

從圖2 可以看到：

（1）算法B、G、H 以外的其他算法的精度隨樣本數的變化雖然不是很明顯，但是誤差隨著樣本數增加，還是有下降的趨勢。

（2）算法C～F 按精度可以分為兩部分，算法C 的精度比其他算法精度差。這是因為在算法C 中子樣數為2，而其他算法子樣數為3。也就是說，算法C 的中時間間隔中只包含2 個小時間間隔，所以它的精度稍微差一點。

由此可以得出以下結論：

（1）如果不用中時間間隔的補償，那么算法精度隨樣本數變化不大（算法A）。

（2）如果只用單子樣，那么算法精度會隨樣本數增大而提高，但是不會超過RV3 的精度（算法B）。

（3）如果使用二子樣或者三子樣，算法精度隨樣本數變化也不大，大致相當于RV3 的精度（算法C～F）。

（4）如果使用上一大時間間隔的補償量，在樣本數較小時會有比較大的誤差，隨著樣本數的增加，誤差會降低，但是最后的精度也不如沒有使用該補償量算法的精度（算法G、H）。因此，沒有必要使用上一大時間間隔的補償量。

圖3 文獻[5]和文獻[3]中算法誤差

圖4 文獻[3]中算法6 和文獻[5]中算法3、7、9 誤差

圖3 和圖4 給出了文獻[5]和文獻[3]中算法的誤差隨樣本數的變化曲線。其中，RV3 表示三子樣旋轉矢量算法，樣本數不變化。I1～I10 分別表示文獻[5]中的算法1～10，Park6 表示文獻[3]中的算法6，樣本數的變化范圍為[1，20]。從中可以得出以下結論：

（1）增加樣本數，能夠提高部分算法的精度（算法4、5、6、10），也會降低部分算法的精度（算法1、2、8）。

（2）樣本數過大時，誤差反而會更大（除算法5、6 外的所有算法，這一點在圖4 中特別明顯）。

（3）Ignagni 對Park 算法的改進有一些提高了精度（算法8），也有一些降低了精度（算法6、10）。

（4）Park 的算法6 隨著樣本數增加誤差增加，但是增加的幅度不大。該算法的精度要低于三子樣旋轉算法，這說明過多地增加子樣數，對提高算法精度并沒有意義。

（5）文獻[5]中所有算法隨著樣本數的增加，精度都趨近于三子樣旋轉算法精度。

在圖5 中，RV3 表示三子樣旋轉矢量算法，它是雙速結構，樣本數不變化。其他算法都是三時間間隔結構，樣本數的變化范圍為[1，20]。其中RV43 表示四子樣算法，RV4o3 表示四子樣優化算法，Lee 表示文獻[2]提出的實時算法，S23、S33、S43 分別表示子樣數為2、3、4 的Savage算法?？梢钥闯觯?/p>

（1）所有算法的誤差變化的幅度都很小，最大誤差和最小誤差之差在10-8量級，遠小于算法誤差。

（2）隨著樣本數的增加，所有算法的誤差先降低，后增加。

（3）四子樣算法的精度都差不多。

（4）在M較小時，二子樣和三子樣Savage 算法比四子樣算法精度低，在M較大時，精度反而較高。

圖5 本文構造算法的誤差

4.2 效率分析

由于上述算法精度都與三子樣旋轉矢量算法近似，而且隨樣本數變化不大。出于簡潔和方便的考慮，在效率分析時，只選取三子樣旋轉矢量算法和根據三子樣旋轉矢量算法改進得到的算法F 進行比較。

圖6 給出了這兩個算法隨著樣本數變化所消耗的CPU 時間的曲線。其中，RV3 表示三子樣旋轉矢量算法，樣本數不變化。IF 表示文獻[1]中的算法F，樣本數的變化范圍為[1，20]?？梢钥闯觯?/p>

（1）算法F 的執行效率要明顯高于三子樣旋轉矢量算法。這是因為算法F 是三時間間隔結構，它的姿態更新周期比三子樣旋轉矢量算法長，因此姿態更新的次數少，計算三角函數的次數少。

（2）隨著樣本數增加，算法F 的耗時呈指數下降趨勢。但當樣本數超過8 以后，變化就十分平緩了。所以樣本數也不需要取得太大。

圖6 三子樣旋轉矢量算法和算法F 耗時

5 結論

Ignagni 的三時間間隔結構和傳統的雙速結構的區別在于用三時間間隔代替了兩時間間隔。當樣本數或子樣數為1 時，三時間間隔結構就退化成雙速結構。當樣本數為1 時，就是熟悉的等效旋轉矢量算法，當子樣數為1 的時候，和Savage的方法類似。

采用三時間間隔結構以后，隨著樣本數的增加，不同算法的精度可能變高，也可能變低，但幅度都不大，而且最后都趨近于三子樣等效旋轉矢量算法的精度。尤其是從傳統算法直接改進過來的算法，例如圖1 中的算法C、D、E，以及圖5 中的所有算法，它們的精度幾乎不受樣本數的影響。對傳統算法進行修正后，增加樣本數可能會提高精度，但主要也是抵消了修正量的影響，例如圖1 中的算法G、H，圖3 中的算法6、10。與雙速結構類似，過分地增加樣本數和子樣數并不能提高精度，反而會引入計算誤差。

但是，采用了三時間間隔以后，姿態更新的周期就加長了，也就是說，在固定的時間內，姿態更新的次數減少了，這就意味著它可以少計算一些三角函數?？梢哉f，三時間間隔結構的本質是利用叉乘代替了三角函數運算。而Ignagni的那些修正算法（圖1 中的算法E'、F），又減少了叉乘的計算次數。因此，如果選擇合理的樣本數和子樣數，三時間間隔結構能夠在一定程度上減少運算量。但由于樣本數和子樣數越多，叉乘的計算次數也會越多，算法結構也越復雜，而且在樣本數和子樣數比較大時，再增加樣本數和子樣數，效率不能有太大幅度的提高，因此它們都不宜選得太大。

綜上所述，Ignagni提出的三時間間隔結構是雙速結構的改進形式，繼承并發展了利用角增量的叉乘作為補償量的思想。它不能顯著地提高姿態更新算法的精度，過分地增加樣本數和子樣數，只會增加算法的復雜度和計算誤差。但它延長了姿態更新的周期，減少了三角函數運算的次數，選擇合理的樣本數和子樣數，可以在一定程度上減少運算量。因此，在實際工程應用中，本文提出的三時間間隔結構算法能夠兼顧時間和精度要求，適于求解那些對時間效率要求較高，而對精度要求不那么苛刻的問題。當姿態變化要求較大的樣本數以提高計算效率而姿態變化較為平緩則樣本數較小，并且根據本文實驗分析，在大多數情形下取樣本數不超過8 個。

[1] Ignagni M B.Optimal strapdown attitude integration algorithms[J].Journal of Guidance，1990，13（2）：363-369.

[2] Lee J G，Yoon Y J，Mark J G，et al.Extension of strapdown attitude algorithm for high-frequence base motion[J].Journal of Guidance，1990，13（4）：738-743.

[3] Park C G，Kim K J，Chung D，et al.Generalized coning compensation algorithm for strapdown system[C]//AIAA，Guidance，Navigation and Control Conference.San Diego：AIAA，1996：29-31.

[4] Wu Y X，Hu X P，Hu D W，et al.Strapdown inertial navigation system algorithms based on dual quaternion[J].IEEE Transactions on Aerospace and Electronic Systems，2005，41（1）：110-132.

[5] Ignagni M B.Efficient class of optimized coing compensation algorithm[J].Journal of Guidance，Control，and Dynamics，1996，19（2）：424-429.

[6] Jiang Y F，Lin Y P.Improved strapdown coning algorithm[J].IEEE Transactions on Aerospace and Electronic Systems，1992，28（2）：484-490.

[7] Savage P G.Strapdown inertial navigation integration algorithm design part 1：attitude algorithms[J].Journal of Guidance，Control，and Dynamics，1998，21（1）：19-28.

[8] Bortz J E.A new mathematical formulation for strapdown inertial navigation[J].IEEE Transactions on Aerospace and Electronic Systems，1971，AES-7（1）：61-66.

[9] Jordan J W.An accurate strapdown direction cosine algorithm，NASA TN-D-5384[R].1969.

[10] Miller R B.A new strapdown attitude algorithm[J].Journal of Guidance，1983，6（4）：287-291.