一种新的捷联惯性导航系统姿态四元数方程求解方法

周召发， 胡文， 张志利， 徐梓皓， 陈河

(火箭军工程大学 兵器发射理论与技术国家重点学科实验室， 陕西 西安 710025)

0　引言

姿态算法是捷联惯性导航系统的关键技术之一。在捷联惯性导航系统中，载体姿态的计算精度将直接影响着系统导航精度，因此提高载体姿态矩阵的计算精度是捷联惯性导航系统研究的重要内容[1-3]。由于载体的运动过程具有不确定性和刚体有限转动的不可交换性，现有的捷联惯性导航系统姿态算法会引入不可交换误差(也称圆锥误差)[4]，而减小不可交换误差是提高解算载体姿态精度的有效方法。

自1971年Bortz[5]提出等效旋转矢量的概念并建立分析圆锥运动误差的理论基础以来，大量国外学者对此进行了研究。例如：Jordan提出了二子样算法[4]，Miller[6]提出了三子样算法，Lee等[7]提出了四子样算法，Jiang等[8]提出了前一周期内陀螺角增量输出的优化算法。在此基础上，国内一些学者对角速率输入的圆锥补偿算法进行了一系列研究，提出了有效的优化算法[9-10]。

多子样等效旋转矢量算法利用多项式对载体的角速度进行拟合，利用旋转矢量微分方程构造旋转矢量的泰勒级数展开式求解等效旋转矢量，通过等效旋转矢量与四元数之间的关系求解姿态四元数[3,11]，再运用上述两个解算技巧减小不可交换误差。传统四元数算法是利用毕卡法(等价于单子样旋转矢量法)解算四元数微分方程，计算载体姿态四元数。毕卡法解算四元数微分方程既没有对载体的角速度做多项式拟合，又没有对姿态四元数进行泰勒级数展开。这两大缺陷导致了四元数法的计算精度较低。因此，本文拟改进四元数微分方程的求解方法，弥补传统四元数算法的缺陷。

1　三子样旋转矢量算法

三子样旋转矢量法利用单周期内3次角增量采样值计算等效旋转矢量，利用等效旋转矢量计算出姿态四元数。三子样旋转矢量法对刚体在空间转动时产生的不可交换误差做出了补偿，减小了计算误差。下面给出三子样旋转矢量法的主要推导过程。

三子样旋转矢量法从等效旋转矢量与四元数之间的关系进行推导。根据方程

Q(tk+1)=Q(tk)⊗q(h),

(1)

(2)

载体的角速率用抛物线拟合如下：

(3)

式中：a、b、c均为拟合载体角速度的参数；0≤τ≤h，h=tk+1-tk.

对等效旋转矢量Φ做泰勒展开：

(4)

采用文献[1]中求解旋转矢量的推导过程，得到等效旋转矢量的求解结果为

(5)

2　三子样四元数法

传统四元数法中，一般采用毕卡法来解算四元数微分方程，毕卡法的实质是单子样旋转矢量法。多子样旋转矢量法可以增加子样数来提高载体姿态计算精度。该方法在计算姿态四元数之前，首先要计算当前周期的等效旋转矢量，然后借助等效旋转矢量计算当前时刻的姿态四元数。等效旋转矢量法求解姿态四元数不是直接求解，而是通过当前周期的等效旋转矢量进行求解。四元数法直接解算四元数微分方程，求解出当前时刻载体的姿态四元数。比较四元数法和等效矢量旋转矢量法求解载体姿态四元数的计算过程可以发现：四元数法计算更直接、更简洁，但是计算精度比等效旋转矢量法低。若提高四元数法的计算精度，使之达到与多子样旋转矢量法相同的精度等级，则四元数算法的优点就会凸显出来。因此，三子样四元数法是借鉴三子样旋转矢量法求解过程找到的一种新的计算四元数方法。

多子样旋转矢量法的计算过程有3个突出的优点：1)在计算等效旋转矢量时，对载体角速度用多项式进行拟合，最大限度地减小了载体的计算误差；2)应用泰勒展开式计算等效旋转矢量，使得计算等效旋转矢量的精度更高；3)利用等效旋转矢量建立输出角增量和旋转矢量的关系。其中前两点是多子样旋转矢量法的主要优点。本文将多子样旋转矢量法中泰勒展开式和多项式拟合角速度的解算技巧应用到解算四元数微分方程中，建立起角增量输出情况下四元数微分方程新的计算方法——三子样四元数法。

2.1　四元数微分方程求导公式

四元数微分方程为

(6)

(7)

式中：ωx、ωy、ωz分别表示载体x轴、y轴、z轴的角速度。

记

(8)

则四元数微分方程可以写为

(9)

设Q和M(×)n阶可导，对(9)式求各阶导数，得到：

(10)

(11)

(12)

(13)

(14)

(15)

观察(9)式～(15)式可以发现，自四元数微分方程的3阶导数开始，第n阶导数的结果按照对Q求导次数由多到少的顺序排列，各多项式的系数项排列恰好是杨辉三角形中的第n-1行。据此可写出四元数微分方程第n阶导数的公式为

(16)

式中：C表示组合。运用(16)式即可以求出四元数微分方程的任意阶导数。

2.2　三子样四元数法的推导过程

四元数微分方程的各阶导数求解完毕，借鉴三子样旋转矢量法的计算技巧来解算四元数微分方程，并将这种解算方法称为三子样四元数法。

设Q(tk)和Q(tk+1)分别为tk和tk+1时刻从n系至b系的旋转四元数，当用(3)式表示载体角速度时，对Q(tk+1)做泰勒展开：

(17)

记角增量为

(18)

(19)

(20)

联立(7)式、(8)式以及(19)式，得到：

(21)

(22)

(23)

(24)

通过(18)式可以得到下列方程：

(25)

通过(25)式反解出a,b,c，得到：

(26)

通过(21)式～(24)式，可将(16)式进行化简，得到三子样四元数微分方程各阶导数的通式为

(27)

结合(27)式，可以写出四元数微分方程各阶导数，在计算和编程时用迭代法求解姿态四元数。化简(12)式～(15)式，将其中包含M的高阶导数项去掉，得到姿态四元数的计算公式。限于篇幅，这里不再给出化简后的形式。

3　1阶～6阶三子样四元数法表达式

下面求解旋转四元数Q(tk+1)的泰勒展开式。结合(21)式～(24)式，将(9)式～(15)式代入(17)式，化简整理，根据保留泰勒展开式项数的多少得到不同阶数的三子样四元数算法。1阶～6阶三子样四元数法的具体形式如下：

Q(tk+1)={I+[ah×]}Q(tk),

(28)

(29)

(30)

(31)

(32)

Q(tk+1)=[10[2c×]2+20[2c×][b×][a×]+
10[2c×][a×][b×]+10[2c×][a×]3+
15[b×][2c×][a×]+15[b×]3+
15[b×]2[a×]2+5[b×][a×][2c×]+
10[b×][a×][b×][a×]+5[b×][a×]2[b×]+
5[b×][a×]4+6[a×][2c×][b×]+
6[a×][2c×][a×]2+4[a×][b×][2c×]+
8[a×][b×]2[a×]+4[a×][b×][a×][b×]+
4[a×][b×][a×]3+3[a×]2[2c×][a×]+
3[a×]2[b×]2+3[a×]2[b×][a×]2+
[a×]3[2c×]+2[a×]3[b×][a×]+
[a×]4[b×]+[a×]6]Q(tk).

(33)

联立(21)式～(24)式和(26)式，得到[ah×]、[bh2×]和[ch3×]的具体形式：

(34)

将(34)式代入(33)式，就可以得到角增量形式的三子样四元数法计算结果。

4　实验仿真及分析

为了比较三子样四元数算法与三子样旋转矢量法在解算载体姿态角上的精度差异，下面进行仿真实验。由于圆锥运动是捷联惯性导航姿态更新系统最恶劣的外部工作环境，设计如下仿真实验：

观察图1(b)和图1(c)可以发现，载体姿态误差中横滚角误差随着时间的延长而发散，原因是三子样四元数法进行泰勒展开后保留的阶数少；逐渐提高三子样四元数算法保留的阶数，直至保留到9阶时，横滚角误差收敛，仿真结果如图1(c)所示。

图2所示为不同阶数时三子样四元数法的横滚角误差图。从图2可知，三子样四元数法取到8阶时横滚角误差开始趋于稳定，取到9阶时横滚角误差已经稳定。从横滚角的收敛阶数可知，三子样四元数算法保留阶数至少为9阶。

相比于三子样旋转矢量法，三子样四元数算法的计算量较大，因此有必要对三子样四元数法的实时性进行说明。本文在仿真实验编程中，运用迭代法给出了三子样四元数法解算姿态四元数，而三子样旋转矢量法采用了文献[1]的计算公式，仿真实验共进行6次，实验结果如表1所示。

从表1可以看出：6次仿真试验中，三子样旋转矢量法平均用时2.02×10-4s，三子样四元数法平均用时3.37×10-4s. 因此可以得出结论：虽然三子样四元数法平均用时比三子样旋转矢量法长，但可以满足实时性要求。本文的对比仿真实验在理论上证实了三子样四元数法的可行性，得出了9阶三子样四元数法的计算精度优于三子样旋转矢量法的结果。

表1三子样旋转矢量法和9阶三子样四元数法仿真实验计算用时平均值

Tab.1Average calculating times of three-subsample rotation vector algorithm and three-subsample quaternion algorithm s

5　结论

1) 本文提出的三子样四元数法的解算精度优于三子样旋转矢量法。相比于旋转矢量法，该算法不仅较大程度地提高了四元数的计算精度，而且免去了计算等效旋转矢量这个中间步骤，直接求解姿态四元数，计算过程更直接简洁。

2) 仿真实验结果表明：9阶三子样四元数法的误差比三子样旋转矢量法低一个数量级。

参考文献(References)

[1]秦永元.惯性导航[M].北京:科学出版社，2006: 287-316.

QIN Yong-yuan. Inertial navigation[M].Beijing: Science Press,2006: 287-316.(in Chinese)

[2]Song M, Wu W Q, Pan X F. Approach to recovering maneuver accuracy in classical coning algorithms[J]. Journal of Guidance Control & Dynamics, 2013, 36(6):1872-1880.

[3]Wang M S, Wu W Q,Wang J L,et al. High-order attitude compensation in coning and rotarion coexisting environment[J].IEEE Transactions on Aerospace and Electronic Systems, 2015,51(2):1178-1190.

[4]王立冬，刘军，鲁军.捷联惯导摇摆基座自对准中圆锥误差补偿算法[J].兵工学报，2012, 33(7)：826-830.

WANG Li-dong, LIU Jun, LU Jun. Coning error compensation algorithm of SINS self-alignment on swaying base[J]. Acta Armamentarii, 2012, 33(7):826-830.(in Chinese)

[5]Bortz J E. A new mathematical formulation for strapdown inertial navigation[J]. IEEE Transactions on Aerospace and Electronic Systems, 1971, 7(1)：61-66．

[6]Miller R B．A new strapdown attitude algorithm[J]．Journal of Guidance，Control，and Dynamics，1983，6(4)：287-291．

[7]Lee J G，Yoon Y J，Mark J G．Extension of strapdown attitude algorithm for high-frequency base motion[J]．Journal of Guidance，Control，and Dynamics，1990，13(4)：738-743．

[8]Jiang Y F, Lin Y P. Improved strapdown coning algorithms[J]. IEEE Transactions on Aerospace & Electronic Systems, 1992, 28(2):484-490．

[9]曾庆化，刘建业，熊智，等.一种角速率激光陀螺惯导系统高精度姿态算法[J].上海交通大学学报，2006,40(12): 2159-2163.

ZENG Qing-hua, LIU Jian-ye, XIONG Zhi, et al. A high-accuracy attitude algorithm of ring laser gyro strap-down inertial navigation system with pure angle rate output[J]. Journal of Shanghai Jiao Tong University, 2006,40(12):2159-2163.(in Chinese)

[10]陈建锋，陈熙源，祝雪芬. 硬件增强角速率圆锥优化算法的姿态解算精度分析及改进[J]. 东南大学学报，2012, 42(4): 632-636.

CHEN Jian-feng, CHEN Xi-yuan, ZHU Xue-fen. Accuracy analysis of attitude computation and improvement for coning algorithm optimized with hardware-enhanced angular rates[J]. Journal of Southeast University, 2012,42(4):632-636.(in Chinese)

[11]王洪辉， 杨绍卿，吴成富，等. 旋转矢量在高动态全姿态飞行器运动方程中的应用[J].兵工学报,2016,37(3):439-446.

WANG Hong-hui, YANG Shao-qing, WU Cheng-fu, et al. Application of rotation vector in equations of motion for all-attitude aircrafts[J].Acta Armamentarii, 2016,37(3):439-446.(in Chinese)