一种基于角速率的改进旋转矢量姿态更新算法

王 君，宋蔚阳，吴骁航

一种基于角速率的改进旋转矢量姿态更新算法

王 君，宋蔚阳，吴骁航

（北京航天长征飞行器研究所，北京，100076）

为了提高姿态更新精度，提出了一种利用前周期角速率进行优化的改进旋转矢量姿态更新算法。通过对基于角速率的旋转矢量更新算法及其算法误差的推导，针对单子样、双子样、改进双子样算法在圆锥运动条件下情况进行了仿真，并开展跑车试验。仿真结果表明，在高动态的圆锥运动条件下，利用前周期角速率的优化双子样算法，比传统的姿态更新算法具有更高的精度；跑车试验结果验证了算法的正确性和有效性。

姿态更新算法；误差补偿；旋转矢量；优化算法

0 引 言

在广泛使用的捷联惯性导航系统（Strapdown Inertial Navigation System，SINS）中，惯性测量器件与载体固连，当使用条件是较高动态的环境时， SINS对导航算法精度的要求较高。在捷联惯性导航系统的姿态、速度和位置更新算法中，姿态算法对整个系统精度的影响最大，是算法研究和设计的核心[1]。自捷联惯性导航系统的概念提出以来，经过三十多年的研究，对捷联导航姿态更新算法己经形成了以四元数导航算法、多子样旋转矢量圆锥补偿算法、划船补偿算法为代表的捷联惯性导航姿态更新算法。

国内外许多学者提出了不同的旋转矢量算法[2~5], 都是基于陀螺的角增量信息。然而对于目前的常用惯性器件输出的角速率信息，无法直接使用。因此本文针对以光纤陀螺为代表的惯性器件，使用角速率信息推导出一种改进旋转矢量的表达式，经过仿真研究和跑车试验，结果表明此算法具有更高的精度和应用价值。

1 姿态更新算法

常用的经典姿态更新算法有四元数法、旋转矢量法等。

1.1 姿态更新的四元数方法

传统四元数方法中，首先构建载体四元数微分方程，再选用不同的解法来求解四元数微分方程。

经典的四元数微分方程表示形式如下：

将上述四元数微分方程写成矩阵形式为

其中，

通常采用毕卡算法由角增量计算变换四元数；通过三角函数的级数展开为有限项计算，包括一阶、二阶、三阶等近似算法。需注意到，姿态更新的四元数算法的假设条件时载体系在姿态更新的时间段内为定轴转动时才严格成立。

1.2 姿态更新的旋转矢量法

当考虑载体在更新周期内不是定轴转动，由角增量直接求解四元数，容易引起转动不可交换误差。因此研究者们提出了旋转矢量法，即通过角增量求解等效旋转矢量、再使用等效旋转矢量更新姿态的方法。

使用角增量构建旋转矢量的方法，以姿态更新周期内使用的角增量数量区分，可以分为单子样法、双子样法、三子样法等。

观察式（4）、式（6）中姿态变换四元数的形式可以发现，构造姿态变换四元数时两者的区别是前者使用角增量，后者使用旋转矢量，构造方法一致。

在姿态更新周期内，分别使用常量、一次项、二次项拟合角速度，可得使用不同数量角增量构建旋转矢量的方法[6,7]，单子样算法、双子样算法、三子样算法分别为

单子样旋转矢量算法与四元数法等价。

2 基于角速率的改进旋转矢量姿态更新算法

2.1 改进旋转矢量姿态更新算法

基于角速率的改进旋转矢量姿态更新算法，在考虑到圆锥运动会诱发捷联惯导姿态更新产生严重漂移，使用该运动环境对姿态更新算法进行优化。考虑到常用的惯性器件输出为角速度，使用角速度构建旋转矢量。

推导在圆锥运动环境下的旋转矢量优化方式。假设运载体存在圆锥运动，描述该运动的四元数如下：

姿态更新的四元数更新方程即式（4）可转换成：

上述表达式为无误差的旋转矢量增量。

使用角增量代替旋转矢量进行姿态更新时的误差可表示为

2.2 基于角速率输出的姿态更新算法

考虑到角增量为使用角速度假设条件下计算的，当陀螺输出的信息为角速率时，利用角速率信息经过插值积分可以提取角增量信息。

3 试验仿真

3.1 圆锥运动仿真

双子样算法与改进型算法的姿态更新误差如图2所示。多次仿真可观察到优化双子样迭代算法的误差比双子样算法减小一倍。

图2 双子样与改进型算法的姿态更新误差曲线

3.2 跑车试验

为验证实际使用的动态环境下算法的可靠性和有效性，设计跑车试验对算法进行验证。试验车上搭载有高精度惯导系统（航向精度0.1°，1σ；水平姿态精度0.05°，1σ）；高精度惯导系统内部包含卫星导航接收机，可满足对中低精度光纤惯组算法精度的验证。高精度惯导的数据更新频率为200 Hz，中低精度光纤惯组的数据更新频率为200 Hz（姿态更新频率为100 Hz），对高精度惯导的输出结果进行拟合后，与试验设备进行比较。图3为车载试验高精度惯导（左侧）与中低等精度光纤惯组（零偏1.5（°）/h）的安装图示。

图3 车载试验

图4为航向角试验结果。从图4可见，在约3000 s行车过程后，基于角速率改进旋转矢量姿态更新算法航向角的偏差约0.06°；单子样算法航向角在0.07°左右，精度提高10%左右。进行了3次不同路线跑车试验，使用同样的离线分析方法对姿态结果进行分析，将基于角速率的改进旋转矢量姿态更新算法、单子样算法计算的姿态结果分别与高精度惯导的输出进行对比，计算姿态误差，误差结果与第1次试验接近，航向角精度有10%左右的提升，俯仰角和滚动角精度无明显区别，统计分析结果见表1。由于跑车试验只能提供一定程度的动态，在本次试验中基于角速率的改进旋转矢量算法与双子样算法的精度基本相当。

图4 航向姿态角解算对比

表1 跑车试验结果

Tab.1 Result of Vehicle Tests

试验统计值类型俯仰角偏差/（°）航向角偏差/（°）滚动角偏差/（°） 试验一（3023~3123s）单子样-0.6060.0710.019 改进双子样-0.6050.0610.018 试验二（2219~2319s）单子样-0.3850.0450.011 改进双子样-0.3830.0400.010 试验三（2536~2636s）单子样-0.5980.0530.012 改进双子样-0.5960.0460.011

4 结 论

本文提出了一种基于角速率的改进旋转矢量姿态更新算法。在圆锥运动环境下对双子样旋转矢量系数进行补偿，同时直接使用当前姿态更新周期和前一时刻惯组数据更新周期的三组角速度信息建立旋转矢量计算方法。通过数学仿真试验和跑车试验进行验证，试验结果表明在高动态的圆锥运动条件下，利用前周期角速率的优化双子样算法，比传统的双子样姿态更新算法具有更高的精度。结合跑车试验，验证了算法的正确性和有效性，同时算法具有较高的应用价值。

[1] 严恭敏. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.

Yan Gongmin. Stapdown inertial navigation algorithm andintegrated navigation principle[M]. Xi’an: Northwestern polytechnical University Press, 2019.

[2] Jiang Y F, Lin Y P. Improved st rap-down coning algorithms[J]. IEEE Transaction on Arospace and Electronic System, 1992, 28(2): 484 -490.

[3] Yeon F J, Yu P L. Improved strapdown coning algorithms[J]. IEEE Transations on Aerospace and Electronic Systems, 1992, AES-28(2): 738-743.

[4] Savage P G. Coning algorithm design by explicit frequency shaping[J]. Journal of Guidance, Control, and Dynamics, 2010, 33(4): 1123-1132.

[5] 王立东. 基于角增量和角速率的旋转矢量算法的等效性[J]. 宇航学报, 2014, 35(3): 340-344.

Wang Lidong. Equivalence analysis of rotation vector algorithms based on angle increment and angular velocith[J]. Journal of Astronautics, 2014, 35(3): 340-344.

[6] 顾冬晴. 捷联惯导姿态更新的双子样二次迭代优化算法研究[J]. 中国惯性技术学报, 2002(3): 5-8.

Gu Dongqing. Optimal two-sample iteration algorithm for SINS attitude updating[J]. Journal of Chinese Inertial Technology, 2002(3): 5-8.

[7] 张昀申. 基于改进等效旋转矢量的姿态更新算法研究[J]. 舰船电子工程, 2017(9): 30-32.

Zhang Yunshen. Strapdown algorithm research based on improved equivalent rotation vector[J]. Ship Electronic Engineering, 2017(9): 30-32.

[8] 严恭敏. 捷联惯导中求解圆锥误差系数的通用算法[J]. 导航定位学报, 2017, 5(3): 1-4.

Yan Gongmin. A general numerical method to obtaining optimized coning compensation coefficients for strapdown attitude algorithm[J]. Journal of Navigation and Positioning, 2017, 5(3): 1-4.

An Improved Attitude Algorithm of Rotation Vector Based onAngular Velocity

Wang Jun, Song Wei-yang, Wu Xiao-hang

(Beijing Institute of Space Long March Vehicle , Beijing, 100076)

To improve the accuracy of attitude updating, the optimal rotation vector algorithms are proposed based on pre-angular. The rotation vector algorithms using pre-angular and calculation errors are deduced. The simulation analyses of single-sample, two-sample, optimal two-sample are made. The simulation results indicate that the performance of the optimal algorithm using pre-angular increment is better than traditional algorithm. The vehicle test verified the correctness and effectiveness of the algorithm.

strapdown attitude algorithm; error correction; rotation vector; optimal algorithm

1004-7182(2020)03-0038-05

10.7654/j.issn.1004-7182.20200308

O242 .1

A

王 君（1985-），女，工程师，主要研究方向为捷联惯性导航技术、组合导航系统。

宋蔚阳（1983-），男，高级工程师，主要研究方向为飞行器导航、制导与控制。

吴骁航（1990-），男，博士，工程师，主要研究方向为导航系统设计与仿真、组合导航算法设计。

2020-04-16；

2020-05-08