針對高動態載體應用的高精度捷聯慣導姿態算法優化方法

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

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

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

指北姿態算法；雙速姿態算法；高動態；實時性；優化改進

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

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

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

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

1 高動態捷聯姿態算法的優化改進方法

1.1 傳統捷聯指北姿態算法的編排

傳統捷聯指北姿態解算的編排流程見圖1所示。

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

1.2 高動態捷聯雙速姿態解算的編排優化方法

1.2.1 優化編排方案

高超飛行器等高動態載體在機動情況下，需要通過增加慣性傳感器采樣率，來提高導航參數的解算精度。如何在提高慣導解算精度(即增加器件采樣數)的同時，保證算法的實時性，是捷聯姿態算法應用于高動態載體中亟需解決的關鍵問題。為了減少計算量，本節設計了基于雙速（two-speed）的優化捷聯導航姿態算法，算法的優化編排方案見圖2所示。

圖1 傳統捷聯指北姿態解算的編排流程Fig.1 Flowchart of north oriented attitude algorithm

圖2 高動態捷聯雙速姿態解算的編排優化方案流程Fig.2 Flowchart of two-speed attitude algorithm

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

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

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

1.2.2 優化編排算法

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

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

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

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

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

2 高動態環境姿態算法仿真對比與驗證

2.1 仿真實驗設計

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

姿態角速率變化模型中各參數的設定范圍如表1所示。

表1 姿態角速率模型參數的設定范圍Tab.1 Parameter range of attitude angular velocity

姿態角仿真時間為300 s，三個姿態角的理想仿真結果及其局部放大圖如圖4所示。

圖4 高動態環境下姿態角理想仿真結果Fig.4 Ideal attitude angle simulation in high dynamic environment

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

2.2 仿真結果對比與分析

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

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

圖7給出了不同采樣頻率下的傳統指北姿態算法與改進雙速姿態算法的航向角誤差對比曲線。

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

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

從圖5~圖7中的局部放大圖以及表2的統計結果可以看出，在采樣率相同情況下，本文提出的姿態雙速算法隨著采樣率的提高，解算精度也相應提高。

另外，對兩種姿態算法的解算耗時進行分析，繪制統計結果如圖8示。圖8顯示了兩種算法運行300 s的總耗時與采樣頻率之間的對應關系。

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

① 在高動態運動環境下，提高傳感器數據采樣頻率可以提高姿態算法的解算精度。在器件采樣頻率相同條件下，隨著采樣率的提高，姿態雙速算法解算精度也相應提高。

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

表2 兩種姿態算法的姿態角誤差最大值統計及對比結果Tab.2 Comparison results of attitude angle maximum errors

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

3 仿真對比與驗證結論

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

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

（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] 于永軍，劉建業，熊智，等.高動態載體高精度捷聯慣導算法[J].中國慣性技術學報，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] 孫麗，秦永元.捷聯慣導系統姿態算法比較[J].中國慣性技術學報，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] 楊勝，房建成. 一種單子樣旋轉矢量姿態算法[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.

針對高動態載體應用的高精度捷聯慣導姿態算法優化方法

邢 麗，熊 智，劉建業，賴際舟，孫永榮

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

聯 系 人：熊智（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）；江蘇高校優勢學科建設工程資助項目；中央高校基本科研業務費專項資金資助（NZ2014406）資助；江蘇省研究生培養創新工程（CXLX13_156）

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