三時(shí)間間隔圓錐補(bǔ)償姿態(tài)更新算法性能分析

程思微，葉靈軍，甘厚吉

CHENG Siwei,YE Lingjun,GAN Houji

海軍工程大學(xué) 電子工程學(xué)院，武漢430033

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

1 引言

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

以文獻(xiàn)[1]提出的三時(shí)間間隔結(jié)構(gòu)為基礎(chǔ)，Lee[2]、Park[3]、Wu[4]等人設(shè)計(jì)了一系列相應(yīng)的多子樣等效旋轉(zhuǎn)矢量算法。Ignagni在其總結(jié)性文獻(xiàn)[5]中，綜合了Jiang[6]和Lee[2]的部分算法，得出了三時(shí)間間隔下的另一組計(jì)算公式。

上述姿態(tài)更新方法[1-6]均采用三時(shí)間間隔結(jié)構(gòu)，各有其優(yōu)缺點(diǎn)。其中，Ignagni 和Park 等人都只計(jì)算了算法漂移，沒有進(jìn)行仿真驗(yàn)證。而Lee 和Wu 雖然進(jìn)行了仿真實(shí)驗(yàn)，但沒有涉及大時(shí)間間隔中應(yīng)該包含多少個(gè)中時(shí)間間隔這一關(guān)鍵問題，僅僅簡單地直接令大時(shí)間間隔等于中時(shí)間間隔。

為了考察三時(shí)間間隔結(jié)構(gòu)在姿態(tài)更新中的作用，本文用三時(shí)間間隔結(jié)構(gòu)改寫了Lee 提出的四子樣算法和四子樣優(yōu)化算法，以及Savage[7]的方法，然后對改進(jìn)算法以及Ignagni、Park、Lee 等人算法分別進(jìn)行了仿真，并通過與傳統(tǒng)雙速結(jié)構(gòu)算法的對比分析，確定三時(shí)間間隔對姿態(tài)更新算法精度和效率的影響。

2 三時(shí)間間隔圓錐補(bǔ)償算法

2.1 基本原理

姿態(tài)更新的過程實(shí)質(zhì)是求解方向余弦陣、歐拉角或者姿態(tài)四元數(shù)的過程。不管采用哪種描述方法，都要先求解等效旋轉(zhuǎn)矢量。等效旋轉(zhuǎn)矢量的微分運(yùn)動(dòng)方程如下[8]：

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

旋轉(zhuǎn)矢量的計(jì)算公式如下[1]：

采用三時(shí)間間隔的思想，設(shè)每個(gè)大時(shí)間間隔包含M個(gè)中時(shí)間間隔，則

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

2.2 基于三時(shí)間間隔結(jié)構(gòu)的算法改進(jìn)

公式（4）～（6）中的任意一個(gè)，再加上式（1）～（3），就構(gòu)成了一個(gè)新的三時(shí)間間隔結(jié)構(gòu)的姿態(tài)更新算法。例如，先由公式（4）求解中時(shí)間間隔內(nèi)的圓錐補(bǔ)償，然后再根據(jù)公式（1）～（3）計(jì)算大時(shí)間間隔內(nèi)的等效旋轉(zhuǎn)矢量，進(jìn)而對姿態(tài)進(jìn)行更新，就是三時(shí)間間隔結(jié)構(gòu)的四子樣等效旋轉(zhuǎn)矢量姿態(tài)更新算法。

3 算法分析

下面對本文將要考察的算法進(jìn)行分析。

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

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

表1 給出了文獻(xiàn)[1-3，5]，以及本文中的算法按子樣數(shù)的分類。其中，IA～I(xiàn)H分別表示文獻(xiàn)[1]中的算法A～H，Lee 表示文獻(xiàn)[2]提出的實(shí)時(shí)算法，Park6 表示文獻(xiàn)[3]中的算法6，I1～I(xiàn)10 分別表示文獻(xiàn)[5]中的算法1～10，RV43、RV4o3、S23、S33、S43 分別表示本文構(gòu)造的四子樣算法、優(yōu)化四子樣算法，以及子樣數(shù)為2、3、4 的Savage方法。

為了簡便起見，約定每個(gè)大時(shí)間間隔包含的中時(shí)間間隔的個(gè)數(shù)稱為樣本數(shù)（M值），而每個(gè)中時(shí)間間隔包含的小時(shí)間間隔的個(gè)數(shù)為子樣數(shù)（N值）。如果樣本數(shù)為1，則三時(shí)間間隔退化成兩時(shí)間間隔，Jordan、Miller 等人提出的傳統(tǒng)的等效旋轉(zhuǎn)矢量算法就是在此基礎(chǔ)上構(gòu)造的。因?yàn)槿龝r(shí)間間隔結(jié)構(gòu)與傳統(tǒng)的算法結(jié)構(gòu)的主要差別是多了一個(gè)樣本數(shù)的概念，下面主要考察樣本數(shù)對算法精度和效率的影響。

表1 各算法按子樣數(shù)的分類

4 實(shí)驗(yàn)分析

4.1 精度分析

精度分析實(shí)驗(yàn)分三組進(jìn)行，第一組測試文獻(xiàn)[1]中的算法，第二組測試文獻(xiàn)[5]和文獻(xiàn)[3]中的算法，第三組測試本文構(gòu)造的算法。假設(shè)載體作典型圓錐運(yùn)動(dòng)，實(shí)驗(yàn)參數(shù)設(shè)置為：圓錐頻率0.1 Hz，半錐角1°，步長0.01 s，運(yùn)行時(shí)間1 h。

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

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

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

圖1 文獻(xiàn)[1]中算法誤差

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

（3）算法C～F 的精度與RV3 相當(dāng)。

從圖2 可以看到：

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

（2）算法C～F 按精度可以分為兩部分，算法C 的精度比其他算法精度差。這是因?yàn)樵谒惴– 中子樣數(shù)為2，而其他算法子樣數(shù)為3。也就是說，算法C 的中時(shí)間間隔中只包含2 個(gè)小時(shí)間間隔，所以它的精度稍微差一點(diǎn)。

由此可以得出以下結(jié)論：

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

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

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

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

圖3 文獻(xiàn)[5]和文獻(xiàn)[3]中算法誤差

圖4 文獻(xiàn)[3]中算法6 和文獻(xiàn)[5]中算法3、7、9 誤差

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

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

（2）樣本數(shù)過大時(shí)，誤差反而會(huì)更大（除算法5、6 外的所有算法，這一點(diǎn)在圖4 中特別明顯）。

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

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

（5）文獻(xiàn)[5]中所有算法隨著樣本數(shù)的增加，精度都趨近于三子樣旋轉(zhuǎn)算法精度。

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

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

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

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

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

圖5 本文構(gòu)造算法的誤差

4.2 效率分析

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

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

（1）算法F 的執(zhí)行效率要明顯高于三子樣旋轉(zhuǎn)矢量算法。這是因?yàn)樗惴‵ 是三時(shí)間間隔結(jié)構(gòu)，它的姿態(tài)更新周期比三子樣旋轉(zhuǎn)矢量算法長，因此姿態(tài)更新的次數(shù)少，計(jì)算三角函數(shù)的次數(shù)少。

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

圖6 三子樣旋轉(zhuǎn)矢量算法和算法F 耗時(shí)

5 結(jié)論

Ignagni 的三時(shí)間間隔結(jié)構(gòu)和傳統(tǒng)的雙速結(jié)構(gòu)的區(qū)別在于用三時(shí)間間隔代替了兩時(shí)間間隔。當(dāng)樣本數(shù)或子樣數(shù)為1 時(shí)，三時(shí)間間隔結(jié)構(gòu)就退化成雙速結(jié)構(gòu)。當(dāng)樣本數(shù)為1 時(shí)，就是熟悉的等效旋轉(zhuǎn)矢量算法，當(dāng)子樣數(shù)為1 的時(shí)候，和Savage的方法類似。

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

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

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

[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.