一种基于改进旋转矢量的低成本多数据融合姿态更新算法

王君 赵苑辰 李涛

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

1 引言

在导航系统的应用中，考虑到低成本、高环境适应性等客观条件，通常使用低精度MEMS 惯组作为传感器来进行捷联导航解算。此类型应用中惯性测量器件与载体固连，当载体存在较大机动时，姿态精度对导航算法的要求较高。捷联惯性导航系统的数据更新算法一般包括姿态更新、速度更新和位置更新算法；其中姿态更新中的误差会加快速度误差、位置误差增加的速度，因此姿态精度对系统精度的影响最大，是算法研究和设计的核心[1]。在自捷联惯性导航系统的概念提出以来，对捷联导航姿态更新算法己经形成了以四元数导航算法、多子样旋转矢量圆锥补偿算法、划船补偿算法为代表的捷联惯性导航姿态更新算法。

国内外许多学者提出了不同的旋转矢量算法[2][3]，都是基于陀螺的角增量信息，然而对于目前的常用惯性器件输出的角速率信息，无法直接使用。因此本文针对以MEMS 陀螺、MEMS 加表为代表的低精度惯性器件，使用角速率信息推导出一种改进旋转矢量的表达式，增加了基于单基线GNSS 位置速度测量的角速度卡尔曼滤波估计补偿方法。经过数学仿真研究和跑车试验数据分析，试验结果表明本文提出的数据融合算法，能够结合改进双子样姿态更新和单基线GNSS 的优势，通过多数据融合算法有效提高载体姿态估计精度，具备较高的工程应用价值。

2 姿态更新算法

捷联惯导姿态更新的经典算法包括姿态更新四元数法、姿态更新旋转矢量法等[1]。

2.1 四元数法

2.2 旋转矢量法

通常载体角运动较为复杂，在更新周期内不是定轴转动的。2.1节中，使用角增量直接求解姿态变换四元数，则会产生误差。姿态更新的旋转矢量算法可避免产生不可交换误差，其原理是首先使用固联于载体的传感器得到角增量信息，其次利用同为描述转动过程的角增量和等效旋转矢量之间的关系求解后者，最后使用等效旋转矢量对载体的姿态进行更新。

在姿态更新的旋转矢量法中，可以使用多种方法利用角增量来构建旋转矢量；最直观的是以姿态更新周期内使用角增量数量进行区分，使用传感器输出的一次角增量结果信息构造旋转矢量成为单子样法，使用两次更新结果为双子样法，使用三次更新结果为三子样法，以此类推。

等效旋转矢量φ 表示载体从tm-1时刻运动至tm时刻描述姿态变化的等效旋转，由其定义可知转动的方向为u=φ/φ，转动大小为等效旋转矢量φ 的模φ。当且仅当φ 为微小量时，可以对等效旋转矢量的微分方程展开后的高阶微小量进行省略；则微分方程近似为：[3]

观察（4）式与（7），两式为姿态变换四元数的不同构成形式。以发现两者的主要区别是构造时四元数时使用的元素不同，（4）为使用角增量，（7）为使用旋转矢量。若计算旋转矢量使用常量拟合角速度，即单子样旋转矢量算法，则与四元数法等价。若使用一次项、二次项拟合角速度，可得双子样、三子样算法构建的旋转矢量：[1]

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

在载体姿态存在复杂运动时，如存在周期性振动，则会诱发捷联惯导姿态更新产生严重漂移，从而使姿态更新产生偏差。由于周期性振动可近似为圆锥运动，则可以使用基于圆锥误差补偿的多子样算法进行优化。考虑到常用的惯性器件输出为角速度，使用角速度构建旋转矢量。

3.1 旋转矢量优化方法

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

4 低成本数据融合算法

载体的实时姿态可由姿态四元数进行表示，依据（1）式四元数运动学微分方程特性，载体的姿态估计是非线性问题。非线性系统的分析有多种经典方法，常用的包括扩展卡尔曼滤波、二阶滤波、迭代滤波等。考虑到实际工程应用的制约，从运算量、算法复杂度和鲁棒性方面考虑，选择扩展卡尔曼滤波对姿态估计进行分析。扩展卡尔曼滤波的思路为将非线性系统在参考点处进行泰勒级数展开，略去高阶项取一阶线性部分作为近视，再开展线性卡尔曼滤波估计。

由于低精度MEMS 传感器的零偏量级大，必须进行补偿才能提高姿态更新精度。若载体使用场景为车载、稳定平台、航行器等，可依据载体机动设计加速度输出的机动判断，进而对水平姿态进行修正；同时采用单基线GNSS 测量的速度矢量，求解载体与地理北向的方位角，估计载体的姿态信息。低成本数据融合的EKF 算法流程图如图1所示。

图1：低成本数据融合的EKF 算法流程图

4.1 扩展卡尔曼滤波状态方程的建立

4.2 观测方程的建立和滤波算法流程

5 试验仿真

5.1 圆锥运动仿真

搭建试验工况对比不同算法的精度，依据第3 节中提出的基于角速率的改进旋转矢量姿态更新算法进行仿真，仿真条件为（仿真参数Ω=1Hz，条件，惯组更新周期2ms，姿态更新周期4ms）。姿态更新的双子样算法与改进型算法的姿态更新误差如图2所示。可观察到改进旋转矢量姿态更新算法的误差比经典双子样算法减小一倍。

图2：两种算法的姿态更新误差曲线

5.2 跑车试验

为验证实际工况下数据融合算法的可靠性和姿态精度，开展了地面跑车试验进行验证。为了获得比较基准，在试验车上搭载了高精度惯导系统（航向姿态精度0.3°，3σ；水平姿态精度0.15°，3σ）。高精度惯导的数据更新频率为200Hz；低精度MEMS 惯组主要器件精度为：陀螺零偏60°/h，3σ；加表零偏6mg，3σ，数据更新频率为200Hz（姿态更新频率为100Hz）。由于对高精度惯导和低精度MEMS 的输出没有同步信号，对高精度关组的输出结果进行拟合，与低精度MEMS 进行比较。图3 为跑车试验姿态角试验结果。从结果中可看出，在约600s 的实验中，改进数据融合算法航向角的偏差在1°以内，水平姿态偏差在0.5°以内；双子样算法姿态精度漂移6°左右；改进数据融合算法的精度有明显的提高。由于本方法中采用了加速度计的输出进行机动判断，判断的准确度决定了水平修正的正确性；航向修正也需要结合GNSS 的输出进行机动判断，当且仅当载体进行直线运动时才能进行航向的修正，判断的准确度也决定了航向角修正的正确性。为解决航向角估计的动态限制，可采用其他传感器提供航向信息，例如参考文献[5]提出的基于磁力计的方法。

图3：双子样与数据融合算法的姿态曲线

6 结束语

本文提出了一种基于改进旋转矢量的低成本多数据融合姿态更新算法。为提高姿态更新精度，在双子样旋转矢量系数补偿的基础上，增加前一时刻角速度信息建立改进旋转矢量更新公式。并增加了基于单基线GNSS 速度测量的卡尔曼滤波角速度估计补偿。通过数学仿真分析，结果表明在圆锥运动条件下，利用前周期角速率的优化双子样算法，比传统算法的姿态精度更高。结合跑车实验，验证了基于改进旋转矢量的多数据融合姿态更新算法的正确性和有效性，结合过载、GNSS 对载体机动进行判断，提高了姿态角修正的正确性。本文提出的算法具有较高的应用价值，同时可以推广至其它传感器提供航向数据的信息融合算法。