基于改进自适应滤波的SINS/DVL组合导航算法研究

魏延辉，刘 静，郝晟功

（哈尔滨工程大学 自动化学院，哈尔滨150001）

采用捷联惯性导航系统SINS（strapdown inertial navigation system）与多普勒计程仪DVL（doppler velocity log）组合能够实现完全自主导航、短时定位定向精度高、抗干扰能力强，是水下导航的最广泛的方式[1]。但是，复杂的水下环境会导致DVL 测速失准，甚至会出现信号短期中断现象，影响SINS/DVL组合导航系统的精度。

针对DVL 出现的测速失准现象，文中提出了一种改进的自适应滤波算法，引入χ2故障检测，当观测量存在异常时，通过设置自适应调节因子矩阵对先验状态均方误差矩阵进行自适应调节从而抑制误差。最后，通过仿真试验验证了所提算法能有效抑制DVL 测速失准时产生的误差， 并通过Kalman滤波、Sage-Husa 自适应滤波和改进的自适应滤波对比仿真试验，验证该方法的有效性。

1 SINS/DVL 组合导航数学模型

SINS/DVL 组合导航系统的误差模型分为SINS误差模型和DVL 误差模型2 部分[2]。由于SINS 的垂直通道不稳定且DVL 的垂直速度测量误差较大，故文中仅考虑水平通道问题。

根据SINS 长时间工作的误差特性， 选择状态变 量为：经度、纬度误差（δL，δλ），速度误差（δvx，δvy），平台失准角（α，β，γ），陀螺漂移（εx，εy，εz）。其误差微分方程为

式中：R 为地球半径；Ω 为地球自转角速度；λ 为当地纬度；Ax，Ay分别为加速度计测量值在当地地理坐标轴上的分量；βg为陀螺漂移反相关时间常数；wg为白噪声。

令DVL 速度偏移误差和偏流角误差用一阶马尔科夫过程来表示，刻度系数误差为常值[3]，则对DVL 速度偏移误差δvd， 刻度系数误差δc 及偏流角误差δΔ进行建模，有

式中：wd和wΔ分别为二者的激励白噪声。因此建立的系统状态方程为

其中

式中：（Fins（t））7×7为惯性导航系统状态转移矩阵；（Fε（t））3×3为陀螺漂移反相关时间矩阵；（Fd（t））3×3为DVL 误差反相关矩阵。

SINS/DVL 采用速度组合方式[4]。在导航坐标系下， 将SINS 解算出的速度与DVL 测量得到的速度作差，将此差值作为Kalman 滤波器的量测值，因此得到系统的量测方程，即

其中

式中：Vsx，Vsy为惯导的测量速度在平台坐标系中的投影；Kd为考虑偏流角的航迹向[5]。

将式（4）和式（5）进行离散化，得到SINS/DVL组合导航系统的Kalman 滤波器为

式中：Φk，k-1为系统从k-1 时刻到k 时刻的n×n 维状态转移矩阵；Hk为系统k 时刻的m×n 维观测矩阵；Wk-1为模型噪声，Vk为观测噪声， 二者为相互独立的零均值高斯白噪声。其方差分别为Qk和Rk，满足以下统计特性：

其中

式中：qk，qj分别为Wk和Wj的数学期望；rk为Vk的数学期望；Qk和Rk为系统噪声和观测噪声方差矩阵。

2 改进的Sage-Husa 自适应滤波算法

经典Kalman 滤波算法在系统的结构参数和噪声统计特性都准确已知的情况下才能获得状态的最优估计； 而利用Sage-Husa 自适应滤波对SINS/DVL 组合导航系统进行信息融合，能不断地利用量测数据估计和校正噪声统计特性与模型参数，从而保证滤波始终处于正常状态。

在Sage-Husa 自适应滤波算法中， 每次滤波都要估计系统噪声特性和观测噪声特性，若系统阶数较高则会导致计算量大幅增加。然而，在实际系统中，如果滤波不出现异常，则无需对噪声特性进行调整，因此引入滤波收敛判据：

式中：s 为调节因子，反映判据的严格程度。式（7）成立，则说明实际新息的方差大于理论值的s 倍，即滤波出现异常，量测噪声变化较大，需要对其进行调整。新息方差理论值为

则滤波收敛条件为

由于水下环境复杂，SINS/DVL 组合导航系统的量测噪声统计特性受实际作业环境影响可能发生改变， 初始的先验值可能无法反映实际的噪声情况，针对此情况，在Sage-Husa 自适应滤波的基础上进行了改进，利用量测信息实时估计量测噪声的统计特性，从而精确估计系统的状态变量。

结合滤波异常判断条件， 用Sage-Husa 自适应滤波估计量测噪声协方差阵，则改进后的算法为

其中

改进的算法在新息计算后，增加了对滤波异常的判断：当滤波正常时，无需对滤波器做调整；当滤波异常时，即量测噪声突变，则利用新息对Rk进行调整。滤波初始时刻的dk为最大值，此时是估计的Rk对当前信息的依赖程度较高， 之后dk逐渐减小，新息对Rk的调整幅度逐渐减小，这样增加了对量测噪声变化的跟踪能力。改进后的算法只在噪声统计特性发生变化时对Rk进行调整，减轻了原算法中对Rk频繁调整引起的计算负担。

3 改进的自适应Kalman 滤波

相对于经典Kalman 滤波算法， 自适应Kalman滤波重新估计观测噪声协方差Rk，在一定程度上增加了计算量。在DVL 测量速度发生突变而导致观测量失准时， 自适应Kalman 滤波的抑制效果并不理想且会累积误差。文中在自适应Kalman 滤波算法的基础上进行了改进：在观测量正常情况下，使用经典Kalman 滤波算法：在观测量失准情况下，通过对先验均方误差矩阵Pk，k-1进行实时地自适应调节从而使得新息协方差与实际新息方阵相符合。

SINS/DVL 组合导航系统的系统模型采用闭环反馈校正，系统模型误差较小，可以忽略不计。因此，可以利用新息的统计特性来判断观测值是否异常。当DVL 正常工作时，测量新息是零均值白噪声序列；当DVL 突然失准时，测量新息不再是零均值的白噪声。引入χ2故障检测，将上述条件转变为假设检验问题，即

式中：H0为原假设， 表示DVL 正常工作情况；H1为备择假设，表示DVL 失准情况；Pvk为测量信息的协方差矩阵。

令假设检验统计量为

其中Tk服从自由度为t 的χ2分布，即

式中λk为非中心化参数。取显著性水平为α，则DVL 正常工作与否的边界条件为

即观测量是否正常的判别准则为

当Tk＞TD时，观测量存在异常，为了抑制误差，需在此时采取措施进行调节，使得测量新息与实际相符，即满足diag（vkvkT）=diag（Pvk）。

由于新息中携带观测量信息，直接对其进行调节会导致滤波失真，在此通过对先验状态均方误差矩阵Pk，k-1进行调节使测量新息协方差与新息方阵相符。采用的方法是，通过增加系数矩阵λm实现对Pk，k-1的调节控制，具体步骤如下：

首先，设定一个调节因子λm，则最优的先验均方误差矩阵为

令diag（vkvkT）=diag（Pvk）

若λm＜1，即调节因子元素≯1 时，则调节因子失效，此时对先验状态均方误差矩阵Pk，k-1的调节是无效的，Pk，k-1保持不变。

综上所述，调节因子λm的最终公式为

文中对自适应滤波算法的改进仅仅是调节了偏离实际的先验状态均方误差矩阵Pk，k-1，在本质上它与经典Kalman 滤波算法是一致的，都采用间接法中的闭环反馈机制。并且，改进的自适应滤波算法根据当前时刻的新息调节Pk，k-1，相对于传统Kalman滤波只增加了少量计算量，明显提高了计算精度。

4 仿真试验与结果分析

4.1 仿真试验

为验证该算法的有效性， 在MatLab 环境下进行SINS/DVL 组合导航系统仿真试验， 仿真试验参数设定见表1。

表1 仿真参数的设定Tab.1 Simulation parameter setting

4.2 仿真结果分析

为了更真实地模拟DVL 测速突然失准的状况，在5000～6000 s，12000～13000 s，23000～24000 s，30000～31000 s，将DVL 的噪声增大为原来的10 倍，然后恢复正常。在此仿真条件下，采用经典卡尔曼滤波、改进的Sage-Husa 自适应滤波和改进的自适应卡尔曼滤波算法分别对SINS/DVL 组合导航系统进行仿真。速度误差、位置误差的仿真结果如图1～图4所示。

由图1和图2可见，增加10 倍噪声后，速度和位置误差都存在明显跳变， 且曲线呈现发散趋势，即DVL 速度信息失准时，会导致组合导航的精度下降，甚至会造成组合导航失效。

图1 正常情况下的速度误差和位置误差Fig.1 Velocity error and position error in normal conditions

图2 十倍噪声情况下的速度误差和位置误差Fig.2 Velocity error and position error in case of ten times noise

通过对比不同算法仿真结果， 可以发现，DVL噪声突然增大为原来的10 倍，量测噪声突变，采用3 种不同的滤波算法得到的速度和位置误差均发生突变，组合导航的精度受到影响。

为了直观地比较滤波性能，表2列出了3 种滤波算法各个导航参数的误差绝对值的最终稳定值、绝对值最大值。由表可知，经典Kalman 滤波误差较大，经纬度误差分别为89.334 m 及95.415 m；改进的Sage-Husa 自适应滤波精度次之， 经纬度误差分别为72.045 m 及73.125 m；改进的自适应滤波算法精度最高，经纬度误差分别为33.482 m 及43.385 m。

图3 十倍噪声情况下改进Sage-Husa 滤波算法的速度误差和位置误差Fig.3 Velocity error and position error of improved Sage-Husa filtering algorithm in case of ten times noise

图4 十倍噪声情况下改进自适应滤波算法的速度误差和位置误差Fig.4 Velocity error and position error of improved adaptive filtering algorithm in case of ten times noise

表2 三种滤波算法的效果对比Tab.2 Comparison of three filtering algorithms

由图3，图4及表2可以看出，在仿真试验中由于DVL 噪声突变，导致Kalman 滤波的精度降低，组合导航精度迅速降低，且有发散趋势。相对于经典Kalman 滤波算法，改进的Sage-Husa 算法精度有所提高，但累积了异常时候产生的误差。因此，采用改进的Sage-Husa 算法，能够稍微抑制DVL 测速失准产生的误差，但是效果有限。

而在此所提出的改进的自适应滤波算法，通过χ2故障检测可以判断出DVL 噪声突变情况，并根据当前时刻的新息调节先验状态均方误差矩阵Pk，k-1，能够有效抑制误差并且不积累DVL 噪声异常时产生的误差，既保证了精度，又能在一定程度上减少运算量，抑制误差效果比较明显，且误差波动范围更小，系统表现的更加稳定。

5 结语

文中所提出的改进的自适应滤波算法，解决了SINS/DVL 组合导航系统由于受到复杂海况影响出现的测速突然失准问题。该算法通过引入χ2故障检测判断观测量异常的情况，并设置自适应调节因子矩阵对先验状态均方误差矩阵进行自适应调节从而抑制误差。由仿真结果可见，改进的自适应滤波算法精度是最高的，改进的Sage-Husa 自适应滤波算法次之，经典Kalman 滤波算法精度较低。改进的自适应滤波算法能够有效提高系统组合导航定位精度， 具有一定的抗干扰能力和较好的工程适用性，从而算法的实际应用提供很好的解决方案和参考。