（南京航空航天大學 導航研究中心，南京 210016）

（南京航空航天大學 導航研究中心，南京 210016）

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

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

在捷聯(lián)慣導系統(tǒng)中，姿態(tài)更新算法相對速度位置更新算法更為復雜，算法的解算精度和實時性也是影響捷聯(lián)慣導姿態(tài)解算性能的兩個主要因素[1-4]。特別是在高超飛行器等高動態(tài)載體應(yīng)用中，由于載體的位姿運動特性極其復雜，為保證導航結(jié)果能夠真實反映高動態(tài)載體的運動情況，其對捷聯(lián)解算的實時性和解算精度都有更高的要求[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ù)法進行姿態(tài)解算，并采用角速度輸入或角速率輸入的多子樣圓錐補償算法，以及四階龍格庫塔算法等姿態(tài)修正算法提高動態(tài)情況下的姿態(tài)解算精度[8-11]。但此類算法會隨著慣性器件采樣率的提高，其運算量急劇增加，從而使得導航處理器在提高算法精度的同時，難以滿足高動態(tài)載體高頻率導航輸出模式下的實時導航需求。

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

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

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

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

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

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

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

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

高超飛行器等高動態(tài)載體在機動情況下，需要通過增加慣性傳感器采樣率，來提高導航參數(shù)的解算精度。如何在提高慣導解算精度(即增加器件采樣數(shù))的同時，保證算法的實時性，是捷聯(lián)姿態(tài)算法應(yīng)用于高動態(tài)載體中亟需解決的關(guān)鍵問題。為了減少計算量，本節(jié)設(shè)計了基于雙速（two-speed）的優(yōu)化捷聯(lián)導航姿態(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所示，在實際的工程應(yīng)用中，為了對圓錐誤差進行有效補償同時減少計算量，可以將機體系旋轉(zhuǎn)矢量 φm的更新劃分為兩部分角增量 αm和 βm進行計算，即圖中所示的 α( m)和 β( m)，而這兩部分角增量的更新周期可設(shè)置為陀螺的采樣周期 Tk。則整個姿態(tài)角的更新過程可以劃分為三個循環(huán)回路，三個循環(huán)回路的更新周期之間的關(guān)系為 Tm= KTk、Tn= MTm，其中M、K可以根據(jù)具體應(yīng)用需求進行設(shè)置。 Tk、 Tm、 Tn三者之間的關(guān)系如圖3所示。

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

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

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

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

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

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

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

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

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

2.1 仿真實驗設(shè)計

為了在高動態(tài)環(huán)境下，將傳統(tǒng)指北姿態(tài)算法與改進的雙速姿態(tài)算法進行對比驗證，設(shè)計了大機動姿態(tài)變化的仿真航跡。通過理想航跡仿真產(chǎn)生慣性器件的陀螺儀和加速度計數(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)角仿真時間為300 s，三個姿態(tài)角的理想仿真結(jié)果及其局部放大圖如圖4所示。

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

為進一步驗證傳統(tǒng)指北姿態(tài)算法與改進雙速姿態(tài)算法的編排差異，在姿態(tài)變化過程固定的情況下，分別設(shè)計了慣性器件采樣頻率 Tk為0.5 kHz、1 kHz和2 kHz的三組姿態(tài)解算試驗。在這三組仿真試驗中，傳統(tǒng)指北姿態(tài)算法與改進雙速姿態(tài)算法的具體仿真條件為：傳統(tǒng)指北姿態(tài)算法采用單子樣圓錐補償算法，則其解算周期 Tn與器件采樣周期 Tk相同；改進雙速姿態(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)角誤差最大值進行統(tǒng)計，其詳細數(shù)值統(tǒng)計與分析結(jié)果如表2所示。圖5給出了不同采樣頻率下的傳統(tǒng)指北姿態(tài)算法與改進雙速姿態(tài)算法的橫滾角誤差對比曲線，其中實線代表改進的雙速姿態(tài)算法誤差，虛線為傳統(tǒng)指北姿態(tài)算法誤差。

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

圖7給出了不同采樣頻率下的傳統(tǒng)指北姿態(tài)算法與改進雙速姿態(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)計結(jié)果可以看出，在采樣率相同情況下，本文提出的姿態(tài)雙速算法隨著采樣率的提高，解算精度也相應(yīng)提高。

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

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

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

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

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

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

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

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

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

（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)慣導算法[J].中國慣性技術(shù)學報，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)慣導系統(tǒng)姿態(tài)算法比較[J].中國慣性技術(shù)學報，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]. 宇航學報, 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)慣導姿態(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—），男，教授，博士生導師。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

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

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