针对高动态载体应用的高精度捷联惯导姿态算法优化方法

（南京航空航天大学 导航研究中心，南京 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