吕 旭， 胡柏青， 徐大伟， 李开龙， 赵 涛

(1.中国人民解放军海军工程大学电气工程学院， 武汉 430033； 2.锦州航星集团锦州航星船舶科技有限公司， 锦州 121000; 3.海装沈阳局驻锦州地区军代表室， 锦州 121000)

捷联惯性导航系统是推算导航系统的范例，该系统既可以单独工作，也可以作为组合导航的一部分进行使用。捷联惯性导航系统/全球卫星导航系统(strapdown inertial navigation system / global navigation satellite system，SINS/GNSS)的组合导航方式具有输出连续、高可靠性，定位精确等优点，是一组较理想且被广泛应用的组合导航模式[1-2]。组合导航系统进行信息融合过程中，滤波方法的优劣是导航性能的关键技术体现[3-4]。卡尔曼滤波(Kalman filter，KF)算法既适用于平稳随机过程，也是适应于非平稳过程，一经提出便得到广泛应用[5-7]。但是经典的KF只能解决线性状态估计问题。无味卡尔曼滤波(unscented Kalman filter，UKF)[8]在非线性系统中具有较高的估计精度和稳定性，成为研究热点。捷联惯性基组合导航在实际应用中，估计精度和实时性都是需要考虑的问题。UKF的计算复杂度和较高的精度使其广泛应用于导航的非线性滤波系统中，这是其成为研究热点的一个重要原因。Crassidis[9]首次提出了一种基于四元数的航天器姿态估计UKF，称为四元数无味估计器(unscented quaternion estimator，USQUE)。在文献[10-11]中，对四元数表示姿态的USQUE算法进行了改进，降低了USQUE算法的计算复杂度。将该方法应用于初始对准和SINS/GNSS组合导航等。

在应用 USQUE 进行组合导航姿态估计时，虽然USQUE实现简单，操作方便，但无法克服量测噪声统计量不准确带来的滤波精度下降问题。文献[12]中提及一种将新息滑动窗的(innovation-based adaptive estimation，IAE)扩展到USQUE姿态估计器，IAE通过对滑动窗内新息的统计实现自调节，但是在方法滑动窗中新息量越大，算法估计精度越高，同时也增加了复杂度，且对于量测噪声方差阵内独立变化的噪声不能单独调节，该算法计算量大，实现起来较为困难。在USQUE 算法的基础上，采用变分贝叶斯(variational Bayesian，VB)近似估计器，对 USQUE 算法进行改进，采用高斯迭代过程近似估计当前时刻量测噪声协方差矩阵，设计了基于变分贝叶斯的自适应四元数无味卡尔曼滤波器(variational Bayesian adaptive USQUE，VB-AUSQUE)的模型。

综上所述，针对组合导航系统受外界环境干扰，造成量测噪声未知及时变的问题，将严重影响USQUE方法的姿态估计性能。现利用VB对 USQUE 中未知或时变的量测噪声进行近似，通过高斯迭代进行校准，将近似结果重新输入到滤波器模型中，改善姿态估计精度。通过捷联式惯性导航系统/全球定位系统(SINS / GNSS)组合导航仿真与舰载实验，并验证所提算法的有效性。

1 SINS/GNSS组合导航系统

采用直接式松组合SINS/GNSS系统。使用GNSS位置和速度作为组合算法的量测输入，与SINS校正类型或GNSS辅助无关。建立系统方程和量测方程，其中姿态部分为四元数表示。

(1)

(2)

对于中低精的惯导设备，陀螺仪漂移和加速度计零也是系统状态估计中的主要元素。其误差方程分别表示为

(3)

(4)

量测模型采用速度和位置松组合模式，得到线性离散量测方程，即

(5)

式(5)中：ηvk和ηpk分别是零均值速度和位置高斯白噪声。

2 变分贝叶斯四元数无味估计器

2.1 变分贝叶斯原理

变分贝叶斯(variational Bayesian，VB)估计算法是一种以迭代的方式对后验分布进行优化估计的方法，算法的核心思想是将复杂的联合条件概率密度分解为几个可以求解的独立参数的概率密度乘积的形式[13]。文献[14]中，开发了一种基于VB近似的自适应扩展卡尔曼滤波器(variational Bayesian adaptive extended Kalman filter, VBAEKF)，用于测量噪声的时变自适应估计。对于处理量测噪声协方差矩阵Rk统计特性不确定的问题，本质就是在量测量为y1:k-1的条件概率密度P(xk,Rk|y1:k)的求解问题。在量测噪声协方差矩阵Rk统计特性未知或者时变的情况下，无法直接对P(xk,Rk|y1:k)求解。VB估计算法可以将这种复杂的概率密度函数转换为对滤波状态量的概率密度分布与量测噪声概率密度分布乘积的形式，即

P(xk,Rk|y1:k)≈ϑx(xk)ϑR(Rk)

(6)

式(6)中:ϑx(xk)为滤波状态量的概率密度分布， ϑR(Rk)为量测噪声的概率密度分布。两者满足关系

(7)

依据最小化后验概率密度分布与真实概率密度分布之间的Kullback-Leibler(KL)散度距离的原则，可以将式(6)近似分解为一个高斯概率密度分布和一个Inverse-Wishart(IW)概率密度分布的乘积的形式，即

(8)

式(8)中：tr(·)表示矩阵的迹；mk和Mk是IW概率密度分布的两个参数，由此可见IW中包含了量测噪声协方差Rk，因此在Rk不确定或者时变的情况下，使用VB估计可以自适应的估计Rk阵。

2.2 改进的USQUE姿态估计器

USQUE算法将滤波分成内外两层，内层采用误差修正罗德里格斯参数(modified Rodrigues parameter, MRP)传递采样点，外层利用四元数完成姿态更新，有效解决了归一化约束问题[15]。其算法的状态方程为

(9)

式(9)中:Xk∈Rn和Yk∈Rm；状态方程为f(·)；Wk-1和Vk分别为高斯白噪声，即

(10)

为简化说明，下文仅对改进自适应USQUE算法的关键姿态估计流程进行介绍。

2.2.1 时间更新

1)产生Sigma点

Pk-1/k-1),i=0,1,…,2n

(11)

2)外层姿态更新

(12)

(13)

3)内层姿态递推

(14)

(15)

状态估计及其滤波方差表达式为

(16)

(17)

式中：ωi是Sigma点的权值。

计算概率密度分布参数mk和Mk：

mk/k-1=ρ(mk-1-n-1)+n+1

(18)

Mk/k-1=BMk-1BT

(19)

2.2.2 量测更新

量测更新使用迭代滤波框架。

初始设置为：

和mk=1+mk/k-1。

对于j=1:N，迭代以下内容(N表示迭代次数)。

(20)

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

当j=N时，迭代结束，N设置为

2.2.3 姿态更新

(30)

3 仿真与舰载测试

3.1 仿真实验

为验证算法有效性，利用轨迹发生器模拟飞行器大机动运动，其轨迹如图1所示，包含匀速、加速、爬升、转弯、滚转等状态，图2和图3分别给出载体姿态和速度变化。轨迹初始地理位置参数为东经108.91°、北纬34.25°和高度1 000 m，初始姿态角为0°，初始速度为0，仿真步长为0.01 s，总时长为800 s。

图1 轨迹仿真Fig.1 Simulation trajectory

图2 姿态变化Fig.2 The change of attitude

图3 载体速度变化Fig.3 The change of velocity

图4 姿态估计误差对比Fig.4 Comparison on attitude estimate errors

由图4中可知，对于中低精度SINS/GNSS组合导航系统，在大初始姿态误差角下，两种算法的姿态滤波均收敛，其中，俯仰角和横滚角均在50 s左右稳定收敛。航向角估计效果相对水平姿态角要差，本文所提算法和传统USQUE算法分别在150和200 s左右收敛至0.06°和0.08°以内，VB-AUSQUE在200 s左右达到稳定，而USQUE在400 s相对稳定。航向角估计差的原因主要在于航向角和水平姿态角估计精度分别取决于陀螺仪和加速计的性能。仿真数据可知VB-AUSQUE滤波算法收敛速度及姿态估计性能更优，能够有效识别系统中未知和时变噪声，提高了USQUE算法本身的自适应性能。

3.2 舰载实验

为了验证本文算法的有效性，采用低精度SINS与GNSS组合舰载实验数据来比较USQUE和VB-AUSQUE滤波算法在组合导航中姿态估计效果。舰载实验中使用的传感器主要技术指标如表1所示，该试验场地在长江，实际船速9节(1节=1.852 km/h)左右。实验过程设计如下:打开实验系统后，试验船靠泊约 20 min，实验船出航后，运动约为3 h，选取800 s的数据进行实验验证，整个运动轨迹如图5所示。

表1 传感器主要性能指标

图5 舰船运动轨迹Fig.5 Trajectory of the ship

通常导航系统受外部环境的复杂影响，未知和时变测量噪声在船载试验中很常见，且在测量中有一些异常值。对于GPS数据，初始噪声设置为100倍，并且采用时变噪声如式(31)。初始条件:横向误差0.1°，偏航误差1°,使用GPS初始化位置和速度。姿态误差对比如图6所示。

(31)

由图6可以看出，在SINS/GNSS直接式组合导航舰载试验中，本文所提及的两种方法在俯仰角和横滚角误差精度估计上相当。由于航向角可观测性弱，在载体运动直线段，航向角基本不变，滤波估计效果不好，估计误差有增大趋势，最大误差约达到60′，当舰船转弯时可观测性增强，本文所提的VB-USQUE算法明显收敛速度较快较快，航向误差估计较小。进一步说明将VB近似估计方法应用于USQUE姿态估计框架，能够高斯迭代近似估计量测噪声，有效抑制了噪声对算法估计性能的干扰，提高了算法本身的自适应能力。

图6 舰载姿态估计误差对比Fig.6 Comparison on attitude estimate errors of shipborne test

为了进一步说明本文算法的有效性，对比位置误差如图7所示。该位置误差为定义为

(32)

式(32)中：δλ为经度误差，m；δL为纬度误差，m；δp为位置误差。

由图7知，该方法不但提高了本身姿态的估计性能，位置估计经度也得到了提高。

图7 位置估计误差对比Fig.7 Comparison on position estimate errors

综上，总结舰船在800 s实验数据的基础上，对比本文所提及量中算法在航向方面的估计性能，其均方根误差及计算时间对比如表2所示。可知，VB-AUSQUE和USQUE姿态估计方法在计算时间相当的情况下，本文算法姿态和位置估计误差更小，性能更优。

表2 航向均方根误差和算法运行时间

4 结论

USQUE滤波算法在组合导航姿态估计应用中，量测噪声的统计特性将直接影导航姿态的解算和滤波精度。在噪声统计特性未知或时变的 SINS /GNSS 组合导航系统中，VB-AUSQUE算法可实时对量测噪声近似逼近，调节时变方差阵。为此，通过仿真实验和实际舰载试验，相比标准的USQUE算法可明显提高姿态估计精度，提高了算法的自适应能力，具有较高的工程应用价值。