基于自适应姿态估计的MIMU/GPS紧组合导航算法

单 斌，张复建，杨 波，王跃钢，薛 亮，腾红磊

（火箭军工程大学 导弹工程学院，西安 710025）

由于低成本的微型惯性测量单元（Miniature Inertial Measurement Units,MIMU，简称“微惯组”）的漂移和零偏误差较大，基于MIMU的惯性导航系统误差随时间不断发散。为解决这个问题，许多学者采用卫星与惯导进行组合导航[1-2]，抑制误差发散。由于松组合模式简单，因而受到工程上的广泛使用，然而，当可见卫星数量少于4颗时，松组合模式系统不能正常工作，没有GPS系统稳定的输出对微惯导系统输出进行修正，使得MIMU/GPS松组合导航系统变成纯惯导解算，长时间的导航精度难以满足实际需要。也有不少学者研究了惯性/卫星组合导航系统在缺星情况下的导航方法以及改进的卡尔曼滤波算法[3-4]。

在可见卫星数少于4颗时，MIMU/GPS紧组合系统的可观性会有所降低，尤其是当载体大角度转向时，姿态角误差明显增大，甚至引起速度误差的增大，降低组合导航系统的性能[5]。针对此类问题，Qifan Zhou[6]采用高度和航向角信息辅助紧组合导航系统，提出一种新的冗余测量噪声估计方法应用到自适应卡尔曼滤波中。Jaradat[7]则提出自回归的神经网络数据融合用于低成本的惯组/卫星组合导航系统，增加姿态角误差观测量，提高组合导航系统的可观性[8-9]，可以有效地提高组合导航系统性能。然而，采用GPS系统的姿态测量来构造系统姿态误差观测量[10]容易受到信号干扰而降低其测姿精度。也可以采用基于加速度计输出的方式[11]，反解出载体的水平姿态角，达到增加姿态角误差观测量而提高系统可观性的目的，不足之处是当载体处于非重力加速运动状态时，加速度计输出包含了非重力加速度项，虽然可以作近似补偿修正，但是反解出的姿态角精度随着载体运动加速度的变化而降低，从而导致了系统的姿态角观测量不准。而卡尔曼滤波算法或其他滤波算法均是以观测信息对状态量实现最小方差估计，当观测信息不准时，系统滤波精度下降[12-14]。由此可见，增加姿态角误差观测信息时，系统导航性能与载体的运动状态及环境相关。

针对上述存在的问题，本文提出基于多模型的姿态自适应估计，来辅助MIMU/GPS紧组合导航系统，设计智能模糊推理系统，根据微惯组输出数据信息，识别载体运动状态，自动分配多模型自适应估计的姿态和滤波算法解算的姿态权重，将二者进行姿态融合，并进行相关实验验证。

1 MIMU/GPS紧组合导航系统算法

通常，MIMU惯性导航解算方案是对导航参数的微分方程采取积分方法实现更新，MIMU导航参数微分方程具体如下：

式中，Vn=[VVV]T为载体在导航系的速度，LE、λN、hU分别为载体的纬度、经度、高度，Q为姿态四元数，ωine=[0,ωiec osL,ωiesinLT]，ωibb为陀螺仪的输出信息，为地球自转角速率，，g为重力加速度，为加速度计输出。

很多文献对 MIMU/GPS紧组合系统状态空间模型的描述较为详细[15]，在此不再赘述。MIMU/GPS紧组合导航系统的伪距、伪距率误差观测量表示如下：

2 多模型自适应姿态解算方案

2.1 姿态解算

欧拉角微分方程表示如下：

将式(5)积分可得到载体的姿态角，但其中含有陀螺仪的漂移误差，尤其当载体处于静止状态时，通过时间积分法得到的姿态角误差尤为明显[16]。Tseng[17]对陀螺仪、加速度计和磁强计的频率响应进行了仿真，结果表明陀螺仪的频率响应特性与其他两个传感器的在本质上是相反的，具有互补的特点。因此，为了补偿陀螺仪漂移误差带来的影响，将加速度计与磁强计解算得到的姿态角，与式(5)得到的姿态角进行融合，通过加速度计和磁强计输出计算得到的姿态角方式表示如下：

式中，m=[mx,my,mz]T为磁强计输出。值得注意的是，在计算载体的姿态角时，考虑到载体前进方向时的非重力加速度对姿态解算产生主要影响，忽略其余次要因素，因此式(6)中的fyb作如下近似补偿：

式中，符号 [v](2)表示向量v的第2个元素；表示微惯组采样频率；vin表示在GPS接收信号历元间隔内，微惯组导航解算的第i个三维速度向量值；L表示在相邻两个历元间，经过微惯组采样值计算载体速度参数的个数。

2.2 线性互补滤波原理

当原始信号分别被高频和低频噪声干扰时，其中一个传感器输出为高频干扰，另一个为低频干扰，这时分别将两个传感器经过低通和高通滤波器，可以得到原始信号较为理想的估计值。这种将具有频率互补特性的传感器输出信息进行融合是一种常用的信息融合技术。

假设y=(θ,γ,ψ)为运动载体的姿态角信息，=(,,)是由陀螺仪计算的姿态角速率，=(θ,γ,ψ)是由加速度计和磁强计计算的姿态角，则由线性互补滤波器原理可以给出载体姿态角的估计值如下：

由于参数α∈[0,1]，且通常保持常值不变，因此为了得到更好的滤波结果，线性互补滤波器一般被非线性互补滤波器或者自适应非线性互补滤波器所取代，尤其当传感器具有时变偏差或系统具有强非线性时，线性互补滤波算法不能提供可靠的姿态估计。然而，线性互补滤波器的实现结构简单，在工程上容易实现，具有潜在的较大优势。

2.3 自适应姿态估计

在很多自适应卡尔曼滤波算法中，基于自适应多模型估计方案广泛用于修正噪声矩阵，其主要原理是将多个不同参数的滤波器输出结果划分权重，依据每个滤波器输出的条件概率进行融合。

为充分利用线性互补滤波器的优势，本文提出一种多模型的自适应方案，其多模型的含义如下：通过设置参数αi(i=1,2,…,m)，由式(8)得到k时刻yˆ的估计值(αi)，则k时刻由m个不同参数可得到对应的m个姿态估计值，最终k时刻的姿态估计值融合计算式为

式中，(αi)是第i个带有参数αi的滤波器输出结果，是由加速度计/磁强计估计的姿态角，m是多模型的滤波器个数，p(αi|zk*)是第i个滤波器在k时刻的姿态估计权重。该权重值(p(αizk*)=wki)通过如下迭代方程更新：

式中，是第i个滤波器在k时刻的姿态估计，wi是k第i个滤波器在k时刻的计算权重。

为便于推导式(9)，定义姿态角残差为：

式中，是由多模型自适应线性互补滤波器估计的姿态角。每一个滤波器的权重因子通过姿态角残差更新计算，滤波器的输出结果为：

式中，y是系统状态；zk*表示量测值；p(y|zk*)表示在已知zk*的条件下，y的条件概率函数。当条件概率取决于参数α和zk*时，式(13)可以改写为：

根据贝叶斯理论有：

式中，Ak是残差rk的协方差矩阵。式(16)即为计算每个子滤波器的权重系数。

将式(15)带入到式(14)中，得到：

式(17)的离散化后即为式(9)。随着量测值的更新递推，当某个滤波器的残差较小时，对应的滤波器权重系数较大，反之亦然。

3 模糊自适应方案

当载体处于静止，匀速或者大角度匀速转向（或近似匀速转向）运动状态时，相比于MIMU/GPS紧组合解算，多模型的姿态解算方法能提供更为精确的姿态信息，此时，多模型的姿态估计数据可以提高 MIMU/GPS系统的导航性能；当载体处于非重力直线加速或近似直线加速状态时，在多模型的姿态估计中，由于忽略了加速度计所测量的载体非重力次要加速度项，多模型的姿态估计精度下降，此时，MIMU/GPS紧组合解算的姿态精度较高，应当占据更高的权重。为了达到上述目的，合理分配二者的权重大小，载体的运动状态首先需要明确知道，这是提高MIMU/GPS系统导航性能的关键。

本文设计的模糊推理系统主要包含四个过程：定义输入变量；输入变量模糊化；通过模糊规则解算输出变量；输出变量去模糊化。

基于上述思想，本文通过模糊逻辑原理，构建一种模糊推理算法，根据载体的运动状态，在MIMU/GPS紧组合导航解算的姿态和多模型自适应解算的姿态之间进行智能决策，确定二者的权重大小，从而达到自适应的提高MIMU/GPS组合导航的性能。其总体方案设计框图见图1。

图1 MIMU/GPS组合导航系统总体方案Fig.1 Overall scheme of MIMU/GPS integrated navigation system

根据微惯组的量测信息，可以判断载体的运动状态，则模糊逻辑推理系统的输入定义为：

为有效评估 MIMU/GPS紧组合导航解算的姿态和多模型自适应估计的姿态信息，定义模糊推理系统的输出为和，分别用于评估二者的信息融合权重。在上述自适应推理思想下，基于IF-THEN规则设计输入变量和输出变量的模糊集，，。在模糊集和中，和分别表示模糊推理系统输入和输出参数的数量，表示模糊规则数量，表示一种符号标志，即隶属函数所对应的模糊集，因此符号和 分别表示输入和输出变量所对应的隶属函数值。

模糊推理系统根据载体运动状态程度，分别对应三个模糊集：小（S）、中（L）、大（M），并设计九条模糊规则如下：

当模糊系统输入量接近零时，表明载体处于静止或者匀速运动状态；当模糊系统输入量的大小增加时，表明载体处于变速机动或者非重力加速状态。相关隶属函数如图2所示。

在每条规则中，分别计算输入和输出变量所对应模糊集的隶属函数值：

图2 输入、输出变量隶属函数Fig. 2 Membership function of input variables

则模糊系统的总体输出为：

式中，nr表示非零的输出变量隶属函数值个数。由质心法得到输出变量的去模糊化结果（见图3，部分输入、输出变量系数关系见表1和表2）为：

通过模糊推理系统对载体运动状态的评估，最终得到两种姿态解算方案所对应的权重值ς1*和ς2*，则MIMU/GPS组合导航系统的姿态输出为：

式中，f(Q)表示滤波解算的姿态角，Ori表示MIMU/GPS导航系统最终输出的姿态角。

图3 模糊系统输出曲面Fig.3 Output surface of Fuzzy system

表1 输出变量ς1Tab.1 Output variable ς1

表2 输出变量ς2Tab.2 Input variable ς2

4 仿真验证

仿真参数设置如下：初始位置为纬度 34.25°、经度108.91°、高度380 m；初始速度和姿态角均为0，初始速度误差均为0.1 m/s；三个方向的初始失准角均为 1°；陀螺仪常值漂移为 5(°)/h，随机游走误差为0.5(°)/h1/2，加速度计常值零偏误差为1 mg/h，随机游走误差为0.1 mg/Hz1/2，GPS接收机时钟误差的等效距离误差为10 m，时钟等效距离率误差为0.01 m/s，MIMU采样频率为50 Hz，GPS接收机更新频率为1 Hz，多模型自适应姿态估计方案的滤波器个数为m=10，每个滤波器输出的初始权重取为0.1，滤波器参数服从均匀分布α～U(0.01,0.99)。

为了验证本文算法的有效性，将引言部分提到的方法作为实验方案的对照试验，假设可见卫星数量为 3颗的情况下，基于以下 4种实验方案，对MIMU/GPS组合导航系统进行验证：1）方案一，伪距+伪距率的方案；2）方案二，航向角+伪距+伪距率的方案；3）方案三，全姿态+伪距+伪距率的方案；4）方案四，本文提出的方案。

载体运动参照无人机等飞行器的实际飞行机动形式，主要考虑静止、加减速、爬升、平飞、转弯、高度机动、俯冲，其运动轨迹见图4。

图4 载体运动轨迹Fig.4 Vehicle plane motion trajectory

基于上述实验方案，当有效卫星数为 3颗时，4种方案均能有效解算载体的位置，表明紧组合方案在可见卫星数低于4颗时，依然能实现导航解算。当载体处于水平机动转向时，方案一导航解算时会出现较大的误差，尤其是姿态角误差受影响变化较大，对此，增加航向角和全姿态的观测信息，分别见方案二和方案三，有效改善了姿态角误差大的问题。然而，无论是增加航向角信息观测，还是增加全姿态信息观测，在一定程度上都能提高 MIMU/GPS组合导航系统的可观测性，但是当载体处于大角度变向运动时，系统姿态观测信息的准确性出现下滑，导致方案二和方案三解算的姿态角误差出现较大波动，系统鲁棒性不强，

见图5～7，且载体速度误差也出现较大波动，最大速度误差在1 m/s以上，见图8～10，载体位置误差波动变化不显著，见图11～13。在此，按照式(24)定义载体速度误差和位置误差，其误差统计结果见表3。

从方案二和方案三的导航误差估计结果来看，增加航向角或全姿态角的观测信息，在可见卫星数少于4颗时，可以有效提高MIMU/GPS系统的导航性能，但是当载体处于大角度变向运动时，方案三不如方案二，尤其是载体的水平速度误差受影响较大，因此基于加速度计输出解算的水平姿态角精度与载体运动状态直接相关，进而直接决定水平姿态观测信息的可靠程度。针对此问题，本文所提出的方案是通过建立多模型的自适应姿态估计方案，根据微惯组输出信息来预判载体的运动状态，通过模糊规则，自适应的分配相应权重，进一步实现载体的高精度位置和速度输出，提高MIMU/GPS组合导航系统的总体性能。本文提出方法解算的姿态角误差在15′之内，速度误差在0.1 m/s之内，位置误差在5 m之内。

表3 速度和位置误差统计Tab.3 Error statistics of velocity and position

图5 俯仰角误差Fig.5 Error of pitch angle

图6 横滚角误差Fig.6 Error of roll angle

图7 航向角误差Fig.7 Error of yaw angle

图8 东向速度误差Fig.8 Velocity error of eastward

图9 北向速度误差Fig.9 Velocity error of northward

图10 天向速度误差Fig.10 Velocity error of upward

图11 纬度误差Fig.11 Error of latitude

图12 经度误差Fig.12 Error of longitude

图13 高度误差Fig.13 Error of height

5 结 论

本文针对 MIMU/GPS组合导航过程中卫星数低于4颗时的情况，为了提高组合导航系统性能，保证系统持续长时间输出高精度的导航结果，分析对比了基于姿态观测信息辅助的紧组合导航方法的局限性，提出了基于多模型自适应姿态估计辅助的模糊智能紧组合导航方法，通过实验验证了本文算法的有效性，并且该方法不受载体大角度转向运动状态的影响，能够持续输出高精度的导航信息。