針對高動態(tài)載體應(yīng)用的高精度捷聯(lián)慣導(dǎo)姿態(tài)算法優(yōu)化方法

（南京航空航天大學(xué) 導(dǎo)航研究中心，南京 210016）

（南京航空航天大學(xué) 導(dǎo)航研究中心，南京 210016）

提高高動態(tài)條件下捷聯(lián)慣導(dǎo)姿態(tài)算法的解算精度和解算實(shí)時(shí)性是提高高精度捷聯(lián)慣導(dǎo)系統(tǒng)實(shí)用性能的重要基礎(chǔ)。特別是針對高超飛行器等高速高動態(tài)載體的高精度實(shí)時(shí)導(dǎo)航需求，需要采用較高的慣性器件采樣率，以提高高動態(tài)條件下的姿態(tài)解算精度。由于傳統(tǒng)捷聯(lián)指北姿態(tài)算法中器件采樣率的增加會導(dǎo)致算法運(yùn)算量的大幅增加，影響算法的實(shí)時(shí)性。基于此，提出了基于捷聯(lián)算法優(yōu)化編排原理的雙速姿態(tài)解算方法和工程實(shí)現(xiàn)編排方法。該方法在增加慣性器件采樣率的同時(shí)并不會顯著增加計(jì)算量，能有效滿足高速高動態(tài)載體的實(shí)時(shí)性導(dǎo)航解算需求。仿真試驗(yàn)結(jié)果表明，在高動態(tài)仿真條件下，雙速優(yōu)化姿態(tài)編排算法與傳統(tǒng)指北算法相比，更能有效滿足高動態(tài)下捷聯(lián)慣導(dǎo)算法的實(shí)時(shí)性和高精度解算要求。

指北姿態(tài)算法；雙速姿態(tài)算法；高動態(tài)；實(shí)時(shí)性；優(yōu)化改進(jìn)

在捷聯(lián)慣導(dǎo)系統(tǒng)中，姿態(tài)更新算法相對速度位置更新算法更為復(fù)雜，算法的解算精度和實(shí)時(shí)性也是影響捷聯(lián)慣導(dǎo)姿態(tài)解算性能的兩個(gè)主要因素[1-4]。特別是在高超飛行器等高動態(tài)載體應(yīng)用中，由于載體的位姿運(yùn)動特性極其復(fù)雜，為保證導(dǎo)航結(jié)果能夠真實(shí)反映高動態(tài)載體的運(yùn)動情況，其對捷聯(lián)解算的實(shí)時(shí)性和解算精度都有更高的要求[5-7]。在高動態(tài)慣性捷聯(lián)姿態(tài)算法研究和編排方面，為了提高捷聯(lián)姿態(tài)解算的精度，需要充分利用慣性器件陀螺儀的數(shù)據(jù)，通常通過提高陀螺儀數(shù)據(jù)的采樣率來提高算法精度。

傳統(tǒng)的捷聯(lián)指北姿態(tài)算法，其基本思想是采用等效旋轉(zhuǎn)矢量的方向余弦法或四元數(shù)法進(jìn)行姿態(tài)解算，并采用角速度輸入或角速率輸入的多子樣圓錐補(bǔ)償算法，以及四階龍格庫塔算法等姿態(tài)修正算法提高動態(tài)情況下的姿態(tài)解算精度[8-11]。但此類算法會隨著慣性器件采樣率的提高，其運(yùn)算量急劇增加，從而使得導(dǎo)航處理器在提高算法精度的同時(shí)，難以滿足高動態(tài)載體高頻率導(dǎo)航輸出模式下的實(shí)時(shí)導(dǎo)航需求。

Savage在文獻(xiàn)[12]中總結(jié)和提出的雙速捷聯(lián)導(dǎo)航算法的基本思想是將導(dǎo)航系和機(jī)體系的旋轉(zhuǎn)分為兩個(gè)循環(huán)回路進(jìn)行更新，其編排機(jī)理與傳統(tǒng)捷聯(lián)姿態(tài)算法相比，在增加陀螺采樣率的條件下，不僅能夠提高姿態(tài)解算的精度，而且能夠有效地減少算法計(jì)算量，滿足高動態(tài)姿態(tài)實(shí)時(shí)性的需求，在高動態(tài)載體應(yīng)用中具備優(yōu)越性[13-14]。

目前適用于高超飛行器等高動態(tài)載體的慣性導(dǎo)航姿態(tài)解算的編排理論及其應(yīng)用的研究文獻(xiàn)較少，因此本文針對高動態(tài)載體的特殊導(dǎo)航應(yīng)用需求，以及傳統(tǒng)指北姿態(tài)編排算法計(jì)算效率低的問題，結(jié)合雙速姿態(tài)算法編排原理，設(shè)計(jì)了適用于高超飛行器等高動態(tài)載體的雙速編排算法和工程應(yīng)用方法，以滿足高動態(tài)載體的導(dǎo)航應(yīng)用需求。

1 高動態(tài)捷聯(lián)姿態(tài)算法的優(yōu)化改進(jìn)方法

1.1 傳統(tǒng)捷聯(lián)指北姿態(tài)算法的編排

傳統(tǒng)捷聯(lián)指北姿態(tài)解算的編排流程見圖1所示。

綜合上述分析可以看出，傳統(tǒng)捷聯(lián)指北姿態(tài)算法的運(yùn)算量會隨著采樣率的增加而迅速增加，無法滿足高動態(tài)載體在提高姿態(tài)解算精度的同時(shí)，提高其解算實(shí)時(shí)性的要求。

1.2 高動態(tài)捷聯(lián)雙速姿態(tài)解算的編排優(yōu)化方法

1.2.1 優(yōu)化編排方案

高超飛行器等高動態(tài)載體在機(jī)動情況下，需要通過增加慣性傳感器采樣率，來提高導(dǎo)航參數(shù)的解算精度。如何在提高慣導(dǎo)解算精度(即增加器件采樣數(shù))的同時(shí)，保證算法的實(shí)時(shí)性，是捷聯(lián)姿態(tài)算法應(yīng)用于高動態(tài)載體中亟需解決的關(guān)鍵問題。為了減少計(jì)算量，本節(jié)設(shè)計(jì)了基于雙速（two-speed）的優(yōu)化捷聯(lián)導(dǎo)航姿態(tài)算法，算法的優(yōu)化編排方案見圖2所示。

圖1 傳統(tǒng)捷聯(lián)指北姿態(tài)解算的編排流程Fig.1 Flowchart of north oriented attitude algorithm

圖2 高動態(tài)捷聯(lián)雙速姿態(tài)解算的編排優(yōu)化方案流程Fig.2 Flowchart of two-speed attitude algorithm

如圖2所示，在實(shí)際的工程應(yīng)用中，為了對圓錐誤差進(jìn)行有效補(bǔ)償同時(shí)減少計(jì)算量，可以將機(jī)體系旋轉(zhuǎn)矢量 φm的更新劃分為兩部分角增量 αm和 βm進(jìn)行計(jì)算，即圖中所示的 α( m)和 β( m)，而這兩部分角增量的更新周期可設(shè)置為陀螺的采樣周期 Tk。則整個(gè)姿態(tài)角的更新過程可以劃分為三個(gè)循環(huán)回路，三個(gè)循環(huán)回路的更新周期之間的關(guān)系為 Tm= KTk、Tn= MTm，其中M、K可以根據(jù)具體應(yīng)用需求進(jìn)行設(shè)置。 Tk、 Tm、 Tn三者之間的關(guān)系如圖3所示。

圖3 周期Tk、Tm、Tn之間的時(shí)間關(guān)系圖Fig.3 Relationship among Tk, Tm and Tn

對比圖1和圖2可以看出，在一個(gè)姿態(tài)解算周期中，編排優(yōu)化的捷聯(lián)雙速姿態(tài)算法僅僅在角增量α和β更新過程中需要與陀螺的采樣時(shí)刻保持一致，而機(jī)體系旋轉(zhuǎn)矢量更新、導(dǎo)航系旋轉(zhuǎn)矢量更新以及或的更新周期則可以遠(yuǎn)大于陀螺采樣周期，因此在陀螺采樣數(shù)增加的情況下，整個(gè)姿態(tài)解算過程中的計(jì)算量不會顯著增加。

1.2.2 優(yōu)化編排算法

高動態(tài)捷聯(lián)雙速姿態(tài)解算編排方案中是將機(jī)體系B和導(dǎo)航系N的旋轉(zhuǎn)更新分為兩個(gè)循環(huán)回路（中速循環(huán)m和低速循環(huán)n）進(jìn)行解算，則和的更新如式(7)所示。

機(jī)體系旋轉(zhuǎn)矢量 φm劃分的兩部分角增量 αm和βm（如圖2所示）的計(jì)算如式(12) (13)所示。

與傳統(tǒng)的多子樣角增量或角速率圓錐補(bǔ)償算法不同，對于 αm和 βm的計(jì)算可以直接劃分為若干個(gè)周期的 αk和 βk進(jìn)行更新(如式（14）所示)，而無需分別對不同子樣數(shù)的圓錐補(bǔ)償式進(jìn)行推導(dǎo)，從而提高了圓錐補(bǔ)償算法的適應(yīng)性，其中 Tk和 Tm的關(guān)系如圖3所示。

雙速姿態(tài)算法式(7)中的上、下標(biāo)n、n -1、m、m -1分別指代 tn、tn-1、tm和 tm-1時(shí)刻。雖然這樣的表達(dá)式理論推導(dǎo)成立，但在實(shí)際程序設(shè)計(jì)中，式(7)應(yīng)該在同一時(shí)刻進(jìn)行解算，而由于B系和N系的更新頻率不同，則B系的 tm時(shí)刻和N系的 tn時(shí)刻并不同步，因此該理論公式難于進(jìn)行算法的工程實(shí)現(xiàn)。針對雙速（two-speed）捷聯(lián)姿態(tài)算法編排機(jī)理難于工程實(shí)現(xiàn)的問題，對于式(7)中方向余弦矩陣和四元數(shù)的更新進(jìn)行了改進(jìn)推導(dǎo)(見式(15)所示)，使其易于工程實(shí)現(xiàn)。式中，對于每個(gè)n循環(huán)周期中的 tn-1時(shí)刻與 tm-1時(shí)刻，從圖 3所示的 Tk、 Tm、 Tn三者關(guān)系的時(shí)間軸上可以看出二者為同一時(shí)刻，因此且式(15)最后一個(gè)和中的n = m + M。通過式(15)的推導(dǎo)可以看出，改進(jìn)后的算法編排不僅可以保持原雙速算法中B系和N系的雙速循環(huán)，而且可以使和的更新在同一時(shí)刻進(jìn)行，實(shí)現(xiàn)算法的具體工程應(yīng)用。

綜合捷聯(lián)雙速姿態(tài)算法的優(yōu)化編排方案和優(yōu)化編排算法可以看出，優(yōu)化編排的捷聯(lián)雙速姿態(tài)算法在增加陀螺采樣率，提高姿態(tài)解算精度的同時(shí)，也能夠有效滿足實(shí)時(shí)性較高的要求，在高超飛行器等高動態(tài)載體導(dǎo)航系統(tǒng)中具有重要的工程應(yīng)用價(jià)值。

2 高動態(tài)環(huán)境姿態(tài)算法仿真對比與驗(yàn)證

2.1 仿真實(shí)驗(yàn)設(shè)計(jì)

為了在高動態(tài)環(huán)境下，將傳統(tǒng)指北姿態(tài)算法與改進(jìn)的雙速姿態(tài)算法進(jìn)行對比驗(yàn)證，設(shè)計(jì)了大機(jī)動姿態(tài)變化的仿真航跡。通過理想航跡仿真產(chǎn)生慣性器件的陀螺儀和加速度計(jì)數(shù)據(jù)。姿態(tài)角初始值設(shè)為：橫滾角0°、俯仰角0°、航向角 20°，姿態(tài)角速率變化模型如式(16)所示，其中γ˙、θ˙、φ˙分別為橫滾、俯仰和航向角速率。

姿態(tài)角速率變化模型中各參數(shù)的設(shè)定范圍如表1所示。

表1 姿態(tài)角速率模型參數(shù)的設(shè)定范圍Tab.1 Parameter range of attitude angular velocity

姿態(tài)角仿真時(shí)間為300 s，三個(gè)姿態(tài)角的理想仿真結(jié)果及其局部放大圖如圖4所示。

圖4 高動態(tài)環(huán)境下姿態(tài)角理想仿真結(jié)果Fig.4 Ideal attitude angle simulation in high dynamic environment

為進(jìn)一步驗(yàn)證傳統(tǒng)指北姿態(tài)算法與改進(jìn)雙速姿態(tài)算法的編排差異，在姿態(tài)變化過程固定的情況下，分別設(shè)計(jì)了慣性器件采樣頻率 Tk為0.5 kHz、1 kHz和2 kHz的三組姿態(tài)解算試驗(yàn)。在這三組仿真試驗(yàn)中，傳統(tǒng)指北姿態(tài)算法與改進(jìn)雙速姿態(tài)算法的具體仿真條件為：傳統(tǒng)指北姿態(tài)算法采用單子樣圓錐補(bǔ)償算法，則其解算周期 Tn與器件采樣周期 Tk相同；改進(jìn)雙速姿態(tài)算法中，按照圖2所示，將更新周期 Tm、解算周期 Tn均設(shè)置為Tm= Tn=0.02 s(50 Hz)，Tm= KTk(K = 10,20,40)。

2.2 仿真結(jié)果對比與分析

將兩種姿態(tài)解算算法的姿態(tài)角誤差最大值進(jìn)行統(tǒng)計(jì)，其詳細(xì)數(shù)值統(tǒng)計(jì)與分析結(jié)果如表2所示。圖5給出了不同采樣頻率下的傳統(tǒng)指北姿態(tài)算法與改進(jìn)雙速姿態(tài)算法的橫滾角誤差對比曲線，其中實(shí)線代表改進(jìn)的雙速姿態(tài)算法誤差，虛線為傳統(tǒng)指北姿態(tài)算法誤差。

同理，圖6給出了不同采樣頻率下的傳統(tǒng)指北姿態(tài)算法與改進(jìn)雙速姿態(tài)算法的俯仰角誤差對比曲線。

圖7給出了不同采樣頻率下的傳統(tǒng)指北姿態(tài)算法與改進(jìn)雙速姿態(tài)算法的航向角誤差對比曲線。

圖6 兩種姿態(tài)算法的俯仰角誤差對比曲線Fig.6 Comparison curves of pitch angle errors

圖7 兩種姿態(tài)算法的航向角誤差對比曲線Fig.7 Comparison curves of head angle errors

從圖5~圖7中的局部放大圖以及表2的統(tǒng)計(jì)結(jié)果可以看出，在采樣率相同情況下，本文提出的姿態(tài)雙速算法隨著采樣率的提高，解算精度也相應(yīng)提高。

另外，對兩種姿態(tài)算法的解算耗時(shí)進(jìn)行分析，繪制統(tǒng)計(jì)結(jié)果如圖8示。圖8顯示了兩種算法運(yùn)行300 s的總耗時(shí)與采樣頻率之間的對應(yīng)關(guān)系。

通過對比圖5~圖8以及表2的仿真結(jié)果可以得出如下幾點(diǎn)結(jié)論：

① 在高動態(tài)運(yùn)動環(huán)境下，提高傳感器數(shù)據(jù)采樣頻率可以提高姿態(tài)算法的解算精度。在器件采樣頻率相同條件下，隨著采樣率的提高，姿態(tài)雙速算法解算精度也相應(yīng)提高。

② 在相同采樣頻率條件下，改進(jìn)的雙速姿態(tài)算法計(jì)算耗時(shí)明顯小于指北姿態(tài)算法，并且由圖8可以看出，相對指北算法，改進(jìn)的雙速算法在高采樣率情況下可以更好地體現(xiàn)其實(shí)時(shí)性較強(qiáng)的優(yōu)勢，非常適合應(yīng)用于動態(tài)性要求較高的載體中。

表2 兩種姿態(tài)算法的姿態(tài)角誤差最大值統(tǒng)計(jì)及對比結(jié)果Tab.2 Comparison results of attitude angle maximum errors

圖8 兩種姿態(tài)算法的計(jì)算效率性能對比Fig.8 Comparison diagram of computational efficiencies

3 仿真對比與驗(yàn)證結(jié)論

針對高超飛行器等高動態(tài)載體對姿態(tài)算法解算精度和實(shí)時(shí)性提出的較高要求，本文分析了傳統(tǒng)指北姿態(tài)算法編排流程及其在實(shí)時(shí)性方面存在的不足；研究了雙速姿態(tài)算法，并對雙速姿態(tài)算法的工程實(shí)現(xiàn)進(jìn)行了優(yōu)化改進(jìn)；通過研究改進(jìn)的雙速姿態(tài)算法編排理論，對比分析了其在實(shí)時(shí)性方面優(yōu)于傳統(tǒng)指北姿態(tài)算法；在理論分析基礎(chǔ)上，模擬仿真了捷聯(lián)慣性導(dǎo)航系統(tǒng)的高動態(tài)高頻角振動環(huán)境，驗(yàn)證并對比了兩類姿態(tài)算法。

通過仿真結(jié)果分析，驗(yàn)證了在慣性器件高采樣率條件下，改進(jìn)雙速姿態(tài)算法具有較高的實(shí)時(shí)性，可以有效滿足高動態(tài)捷聯(lián)慣導(dǎo)算法的實(shí)時(shí)性和高精度要求，具有重要的工程應(yīng)用價(jià)值。

（References）：

[1] Wang Zhenhuan, Chen Xijun, Zeng Qingshuang. Comparison of strapdown inertial navigation algorithm based on rotation vector and dual quaternion[J]. Chinese Journal of Aeronautics, 2013, 26(2): 442-448.

[2] Li Jie, Liu Jun, Wang Bo. High precise attitude algorithm for SINS[C]//First International Conference on Pervasive Computing, Signal Processing and Applications. Harbin, China, 2010: 636-639.

[3] Wu Wei, Wu Aidi. Attitude algorithm of SINS system based on curve fitting and interpolation[J]. Journal of Communication and Computer, 2010, 7(5): 8-11.

[4] Wang Rong, Liu Jianye, Xiong Zhi, et al. Double-layer fusion algorithm for EGI-based system[J]. Aircraft Engineering and Aerospace Technology, 2013, 85(4): 258-266.

[5] Peng Hui, Xiong Zhi, Wang Rong, LIU Jianye, Cheng ZHANG. A new dynamic calibration method for the INS of the hypersonic cruise vehicles[J]. Aerospace Science and Technology, 2014, 32(1):121-130.

[6] 于永軍，劉建業(yè)，熊智，等.高動態(tài)載體高精度捷聯(lián)慣導(dǎo)算法[J].中國慣性技術(shù)學(xué)報(bào)，2011，19(2)：136-139.

YU Yong-jun, LIU Jian-ye, XIONG Zhi, LI Rong-bing. High-precision SINS algorithm for high dynamic carriers[J]. Journal of Chinese Inertial Technology, 2011, 19(2): 136-139.

[7] 孫麗，秦永元.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)算法比較[J].中國慣性技術(shù)學(xué)報(bào)，2006，14(3)：6-10.

SUN Li, QIN Yong-yuan. Comparison of attitude algorithms of SINS[J]. Journal of Chinese Inertial Technology, 2006, 14(3): 6-10.

[8] 楊勝，房建成. 一種單子樣旋轉(zhuǎn)矢量姿態(tài)算法[J]. 宇航學(xué)報(bào), 2010, 31(3): 780-785.

YANG Sheng, FANG Jian-cheng. A single-sample rotation vector attitude algorithm[J]. Journal of Astronautics, 2010, 31(3): 780-785.

[9] Ma L, Li Z, Wu T. A coning compensation algorithm for SINS in high dynamic motion[J]. Journal of Control Engineering and Applied Informa, 2011, 13(3): 32-40.

[10] Yang S, Fang J C. A single-sample rotation vector attitude algorithm[J]. Journal of Astronautics, 2010, 32(3): 780-785.

[11] Ben Y Y, Sun F, Gao W, et al. Generalized method for improved coning algorithm using angular rate[J]. IEEE Transactions on Aerospace and Electronic Systems, 2009, 45(4): 1565-1572.

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

[13] Savage P G. A unified mathematical framework for strapdown algorithm design[J]. Journal of Guidance, Control, and Dynamics, 2006, 29(2): 237-249.

[14] Savage P G. Explicit frequency shaped coning algorithms for pseudoconing environment[J]. Journal of Guidance, Control and Dynamics, 2011, 34(3): 774-782.

針對高動態(tài)載體應(yīng)用的高精度捷聯(lián)慣導(dǎo)姿態(tài)算法優(yōu)化方法

邢 麗，熊 智，劉建業(yè)，賴際舟，孫永榮

Optimized method of high-precision attitude algorithm for high dynamic vehicles

XING Li, XIONG Zhi, LIU Jian-ye, LAI Ji-zhou, SUN Yong-rong
(Navigation Research Center, Nanjing University of Aeronautics & Astronautics, Nanjing 210016, China)

The performance of high-precision strapdown inertial navigation system(SINS) is closely related to the precision and real-time performance of attitude algorithm under high dynamic environment. According to the high-precision and real-time navigation requirement of hypersonic cruise vehicles(HCVs), the high-precision attitude algorithm requires improving sample rates of inertial components. However, in the traditional north oriented attitude algorithm, the computational complexity would be significantly increased with the sample rates of inertial components, and this would affects the real-time performance of the algorithm. To solve this problem, a two-speed attitude optimizing algorithm and its engineering implementtation method are proposed. The method can effectively meet the real-time navigation requirement of high-speed and high-dynamic vehicles. Simulation results show that, compared with traditional north oriented attitude algorithm, the two-speed attitude optimizing algorithm can more effectively meet the real-time and high-precision navigation requirement in high dynamic environment.

north oriented attitude algorithm; two-speed attitude algorithm; high dynamic; real-time; optimization and improvement

聯(lián) 系 人：熊智（1976—），男，教授，博士生導(dǎo)師。E-mail：xznuaa@nuaa.edu.cn

1005-6734(2014)06-0701-06

10.13695/j.cnki.12-1222/o3.2014.06.001

U666.1

A

2014-05-10；

2014-09-26

國家自然科學(xué)基金項(xiàng)目（61374115，91016019，61210306075，60904091，61203188）；國家留學(xué)基金資助項(xiàng)目；江蘇省六大人才高峰資助項(xiàng)目（2013-JY-013）；江蘇高校優(yōu)勢學(xué)科建設(shè)工程資助項(xiàng)目；中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資金資助（NZ2014406）資助；江蘇省研究生培養(yǎng)創(chuàng)新工程（CXLX13_156）

邢麗（1987—），女，博士生，從事高精度慣導(dǎo)研究。Email：nuaaxl@nuaa.edu.cn