辅助信息源短时失效下的捷联惯性基组合导航算法

张梦得, 李开龙, 胡柏青, 吕旭

(海军工程大学 电气工程学院, 湖北 武汉 430033)

0 引言

由于捷联惯性导航系统(SINS)不可避免的器件误差积累，因此通常与辅助信息源(全球定位系统(GPS)，多普勒计程仪(DVL)等)实时修正，构成捷联惯性基组合导航系统[1-6]。捷联惯性基组合导航系统的核心之一是组合导航算法，根据组合导航系统状态模型的不同，可以分为基于状态误差量的线性模型和基于捷联惯性导航(简称捷联惯导)基本方程的非线性模型。对于线性模型，经典的线性Kalman滤波即可解决；对于非线性模型，近年来有乘性扩展Kalman滤波[7-10](MEKF)和四元数无味Kalman滤波[11-15](USQUE)等算法。相比于线性模型，非线性模型具有更高的估计精度，虽然计算量较大，但随着硬件计算速度快速提高，非线性模型将具有更广泛的应用空间[15-18]。

但在捷联惯性基组合导航实际应用中，难以避免地会出现辅助信息源失效问题，虽然对于高精度惯导系统，辅助信息源短时失效并不会造成导航精度的急剧下降，但是对于使用低精度SINS的水下无人潜器或无人机等，即使是半分钟的辅助信息源失效，都将造成导航精度急剧的降低[19-21]。因此，该问题也是捷联惯性基组合导航研究的热点之一。

文献[22]通过神经网络算法对SINS导航输出误差进行补偿的方式对GPS失效时的SINS误差进行抑制；文献[23]通过构建偏最小二乘估计器与支持向量回归估计器的联合估计器，分别估计DVL短时失效时输出速度的线性部分与其他部分，进行速度补偿取得较好效果；文献[24]采用SINS/GPS与SINS/里程计(OD)两套组合导航系统相结合的方式，若GPS出现短时失效，通过SINS/OD系统仍可以进行组合导航；文献[25]针对外部传感器失效无法进行动基座初始对准的问题，通过构建车辆运动模型得到虚拟量测量，从而通过Kalman滤波进行动基座初始对准。

针对该问题，本文提出一种高斯过程回归(GPR)辅助捷联惯性基组合导航算法。该方法利用组合导航闭环校正后的SINS速度作为GPR的训练输入集，辅助信息源的速度作为训练输出集，在未发生失效时对高斯过程回归进行训练；辅助信息源失效时利用高斯过程回归预测辅助信息源输出以实现组合滤波量测更新，从而实现失效时量测更新的不间断。在低精度微机电系统(MEMS)/GPS组合车载实验中，利用实验数据比较本文算法与传统算法在失效情况下姿态、位置和陀螺常值漂移估计效果。

1 捷联惯导基本模型

SINS基本模型是非线性模型，以姿态、速度、位置、陀螺输出、加速度计输出为状态量。由于本文研究的是USQUE算法，因此给出四元数姿态表达下的SINS基本模型[26]。

模型的姿态部分为

(1)

(2)

SINS其他部分模型为

(3)

(4)

(5)

(6)

(7)

(8)

陀螺的量测模型为

(9)

(10)

加速度计的量测模型为

(11)

(12)

2 GPR算法基本原理

GPR算法是高斯过程在回归领域的应用，该方法利用核函数来拟合数据，并且能够给出预测结果的不确定度。最大的特点是可以提供预测结果的后验，且具有解析形式，非常适用于低维数且数量不大样本的回归问题。同大部分人工智能算法相同，GPR同样分为训练与预测两个部分[27]。

在训练部分中，训练数据集合D=〈x,y〉，其中x=[x1,x2,…,xn]表示训练数据的输入，y=[y1,y2,…,yn]表示训练数据的输出，n为训练集的数据个数。

输入与输出的关系为

yi=f(xi)+δ，

(13)

式中：f(xi)是函数真实值，i=1,2,…,n；噪声δ服从正态分布，δ～N(0,σn)，σn为方差。

高斯过程是随机变量的集合，要求集合中任意数量的随机变量都必须满足联合高斯分布的随机过程[28]。因此，均值函数与协方差函数就成了高斯过程训练的关键。

y～GP(μ(x),σ(xi,xj))，

(14)

式中：GP(·)代表高斯过程函数；μ(x)是均值函数，通常取0；σ(xi,xj)是协方差函数，xi、xj是训练集x中的任意变量，j=1，2，…，n.

(15)

式中：σf、w、σn为超参数，记为θ={σf,w,σn}，这3个超参数的取值是整个GPR的关键；In为n阶单位矩阵。因此整个问题就转化成了在(15)式条件下求取目标函数的最小值问题。

目标函数为

(16)

式中：p(·)为概率函数。

由于目标函数(16)式是非凸的，不可能求出最优解，可以采用粒子群、共轭梯度等算法进行搜索优化，得到目标函数的次优解，通过多次迭代，逐步逼近最优解的值，并得到与之相对应的超参数。训练结束后，代入新的数据x*，可以得到预测值y*：

y*～GP(μ*,σ*)，

(17)

3 GPR-USQUE算法

图1 GPR-USQUE训练与估计流程图Fig.1 Training and estimation of GPR-USQUE algorithm

通过研究发现GPR对于插值预测效果非常好，但是对于出现一定数量的不位于训练数据集范围内数据的情况预测效果不佳，这是由于GPR能够进行回归预测的核心是数据的相关性，一定数量不位于训练数据集的数据会造成该部分数据与GPR训练的黑箱相关性过低，从而导致预测的稳定性急剧下降。这也是大部分人工智能算法的通病。因此，对于GPR的输入量与训练集的选择非常重要。对于输入量的数值区间不要过大，训练集中数据量可以不大，但是数值区间要尽量大些。

在组合导航系统中，通常作为Kalman滤波中量测量的导航信息通常为速度，位置或者速度与位置。对于位置信息，可以认为是无界的，这对于GPR算法并不适用。因此对速度信息进行回归预测，无论从训练集的规模还是数据的相关性上都非常恰当。

以姿态部分为例，短时失效时GPR-USQUE算法见图2，图中δRk和δRk-1分别为第k和k-1次次滤波的罗德里格斯参数姿态。

图2 短时失效时GPR-USQUE算法流程图Fig.2 Flow chart of GPR-USQUE algorithm during short-term failure

4 实验与分析

为了验证GPR-USQUE算法在辅助信息源短时失效情况下的有效性，本文通过车载实验数据比较GPR-USQUE与USQUE算法在辅助信息源短时失效情况下的性能。车上载有型号为XW-IMU5220的MEMS，两台XW-AD U3601型GPS接收机以及XW-ADU7612型姿态航向高精度参考系统(用以提供实验高精度姿态参考)。在MEMS/GPS车载实验中，选取姿态、速度、位置与陀螺漂移作为状态量进行研究。由于将GPS的速度作为辅助信息源，因此，重点关注姿态、位置和陀螺常值漂移的估计结果。MEMS的更新频率为100 Hz，GPS的水平速度误差小于0.1 m/s，位置误差小于2 m，更新率为10 Hz. MEMS/GPS组合导航系统观测采用速度松组合方式。MEMS的IMU相关性能参数见表1.

表1 XW-IMU5220型MEMS相关性能Tab.1 Related performance of XW-IMU5220 MEMS

滤波初始协方差矩阵设置为

P0=diag([Pq0;Pv0;Pp0;Pε0])，

图3 测试轨迹Fig.3 Test track

实验轨迹为驾车绕操场一周，实验时间为250 s. 在实验过程中，170～230 s区间段内将一台GPS关机模拟GPS短时失效，另一台GPS接收机正常工作为实验提供速度与位置基准。为了确保GPR训练集小而有效，在100多秒左右的训练时间内要求车速、航向应有一定的变化，以使训练集包含多种运动情况。图3为实验轨迹，由于是对实验数据的离线处理，选取正常组合模式下0～170 s的SINS闭环校正输出速度与GPS输出速度，共计1 710组数据作为GPR训练集的输入与输出。为了说明算法的有效性与普适性，在170～230 s的失效时间段内，实验车辆进行了转弯、变速、匀速等多种机动，通过对170～230 s的GPS速度输出做出预测，并作为USQUE算法的伪量测量进行组合导航。对于USQUE算法，170～230 s则不进行量测更新，只进行纯惯性导航解算。图4～图7给出了两种方法在姿态与陀螺漂移估计误差的对比结果；图8给出了信息源短时失效期间东向、北向速度GPR的估

图4 俯仰角估计误差EθFig.4 Estimated pitch error Eθ

图5 横滚角估计误差EΦFig.5 Estimated roll error EΦ

图6 航向角估计误差EΨFig.6 Estimated yaw error EΨ

图7 陀螺常值漂移估计Fig.7 Gyro drift estimate

图8 GPR速度估计Fig.8 GPR velocity estimate

计值与GPS参考系统的对比结果；图9给出了USQUE与GPR-USQUE算法估计的实验轨迹；图10给出了USQUE与GPR-USQUE算法对经度与纬度的估计误差。

图9 GPR-USQUE与USQUE算法位置估计效果比较Fig.9 Comparison of position estimates of GPR-USQUE and USQUE

图10 位置估计误差EpFig.10 Estimated position errors Ep

整个实验过程可以分为3个阶段：0～170 s是正常组合导航阶段；170～230 s是GPS失效阶段；230～250 s是失效后恢复正常组合导航阶段。GPR-USQUE算法与USQUE算法在0～170 s工作阶段估计效果是一样的，这是因为GPR处于训练阶段，并没有进行工作，只是单纯地进行USQUE算法组合导航。230～250 s是失效后恢复正常组合导航阶段,在该阶段GPR不再发挥作用，而是又一次进入训练阶段为下一次可能发生的GPS短时失效做准备，因此该阶段不是本文研究的重点。在170～230 s阶段中GPS失效，GPR开始工作。通过分析姿态估计结果图4～图6，使用GPR构造伪速度信息进行量测更新的GPR-USQUE算法估计精度相比于纯惯性导航解算的USQUE算法要高出很多。对于陀螺漂移的估计见图7，以估计误差的均方根(RMS)作为评价估计效果的标准，计算结果见表2. 图8体现的是GPS失效阶段GPR对速度的估计精度，图9和图10主要用于分析比较GPS短时失效时，GPR-USQUE算法与单纯惯性导航解算的USQUE算法对位置估计的精度。

表2 GPS失效段内导航误差RMS统计值Tab.2 Navigation error RMS statistics in GPS failure phase

由于MEMS本身精度不高，60 s不进行量测更新使得MEMS发散程度很高，导致导航信息输出精度急剧下降，使用GPR-USQUE算法可以较好地抑制这种情况。在使用GPR-USQUE算法后，俯仰角估计误差减小了84.3%，横滚角估计误差减小了94.3%，航向角估计误差减小了70.7%. 东向陀螺漂移估计精度提高了10.3%，北向陀螺漂移估计精度提高了10.8%，天向陀螺漂移估计精度提高了5.2%. 图8是GPR速度估计图与局部放大图，在短时失效时间段内，速度进行了一定程度的变化。图8表明了GPR在GPS失效时对速度信息的估计与参考系统提供的速度信息的关系，可以计算出GPR估计结果：北向速度最大误差为0.150 9 m/s，RMS为0.054 9 m/s；东向速度最大估计误差为0.201 2 m/s，RMS为0.075 2 m/s. 这也是GPR-USQUE算法在短时失效情况下有较高估计精度的原因。由图8可以看到东向、北向速度估计中放大部分，由于GPR估计不准确，最大误差达到了0.15 m/s，与之对应的3倍标准差范围也相应增大，表明此处GPR估计值的取值置信度比较低，这也是GPR与其他人工智能方法相比的优势。对于图9和图10中体现的位置估计效果，GPR-USQUE算法表现出了较高的估计水平：经度最大估计误差为5.72 m，RMS为3.574 m；纬度最大估计误差为3.56 m，RMS为2.323 m. USQUE算法在GPS失效段由于只能单纯进行惯导解算，从图9可以看出，解算轨迹已经发散。通过分析图10的位置估计误差可知：USQUE算法在正常组合导航阶段，位置估计误差正常；在GPS失效段，经度和纬度估计误差均值超过了100 m.

在计算时间方面，对250 s的实验数据处理，USQUE算法的计算时间为61.36 s，GPR-USQUE算法的计算时间为242.19 s. USQUE算法的计算复杂度为状态向量维数的2次幂，GPR在学习和预测过程中的计算复杂度分别是训练数据的3次幂和2次幂，计算复杂度的增加是导致GPR-USQUE算法计算时间较长的原因。由于GPR训练集所需的数据量并不大，使得GPR-USQUE算法并不需要增加较多的时间就可以将估计精度提高到较高的水平。

5 结论

本文针对捷联惯性基组合系统中发生的短时失效情况，提出了GPR-USQUE算法。在GPS或DVL短时失效期间，利用GPR提供的量测信息辅助SINS进行组合导航以获取准确的导航信息。为了克服短时失效带来的不利影响，采用失效前时刻的组合导航闭环校正的SINS速度信息作为训练集的输入，以GPS或DVL提供的速度信息作为输出，训练基于高斯过程的伪观测模型。失效时，利用训练的伪观测模型替代观测模型实现组合滤波量测更新。通过基于车载MEMS/GPS组合导航系统的实测实验对算法有效性进行了检验，结果表明：在含有60 s GPS短时失效的实验过程中，俯仰角估计误差减小了84.3%，横滚角估计误差减小了94.3%，航向角估计误差减小了70.7%. 东向陀螺漂移估计精度提高了10.3%，北向陀螺漂移估计精度提高了10.8%，天向陀螺漂移估计精度提高了5.2%. 经度最大估计误差为5.72 m，RMS为3.574 m,提高了90.1%；纬度最大估计误差为3.56 m,RMS为2.323 m，提高了90.4%. 证明算法具有较高的估计精度，可以有效地解决组合导航中辅助信息源短时失效问题。