基于UT变换改进多模型滤波的水下航行器导航方法

王泽元，许昭霞

(1．中国人民解放军61818 部队，北京102102;2．中国人民解放军61081 部队，北京100094)

0 引 言

由于组合导航技术具有导航精度高，可靠性强等优点，被广泛应用于水下航行器导航系统［1］。对于组合导航系统的状态估计，目前多采用卡尔曼滤波或扩展卡尔曼滤波对非线性模型进行线性化处理，这种处理方法存在2 个问题:一是由于忽略了模型的高阶导数，估计精度随着时间增长难以保证;二是滤波过程中需要反复计算系统方程的雅可比矩阵，滤波计算量偏大。

为克服卡尔曼滤波算法精度较低，易于发散，计算量大等缺点，Julier 针对非线性高斯模型提出了无迹卡尔曼滤波方法(UKF)［2］。已有学者运用无迹卡尔曼滤波方法解决水下航行器导航系统滤波问题［3－4］，文献［3］针对复杂加性噪声，采用基于球面分布单形采样，简化了水下航行器组合导航的UKF 算法;文献［4］针对无人水下航行器目标跟踪控制需求，仿真验证了UKF 估计方法的有效性。随着多传感器获取信息融合提高导航精度，文献［5］将UKF 引入到多模型滤波算法中，既能提高滤波算法的精度，也可并行求解，但是此方法的总体计算量偏大。此外，上述方法没有考虑利用辅助信息对估计结果进行修正。

本文针对上述问题，将UKF 引入水下航行器组合导航系统滤波中，提出基于UT 变换的简化多模型算法，同时设计融合算法，利用辅助信息对估计结果进行修正，提高整体估计精度。首先对非线性观测方程进行UT 变化，利用Sigma 点构建伪观测量，更新模型概率，得到偏差估计结果并代入到多模型滤波中，得到状态估计结果。其次利用环境变量分析辅助信息可用度，提高系统鲁棒性，并在此基础上设计加权因子融合算法，合理利用辅助信息，提高系统整体估计精度。最后通过仿真验证测试算法的有效性。

1 组合导航系统滤波模型的UT 变换

对于水下组合导航系统，线性化后离散系统数学模型如下［6－7］:

式中:xk为状态向量;zk为观测向量;Φk为状态转移矩阵;Γk为噪声干扰矩阵;Hk为观测矩阵;ωk，υk为零均值高斯白噪声，且。

水下航行器组合导航系统建模为式(1)时，存在模型的近似和理想化假设，直接用于滤波会导致估计精度降低。考虑到系统非线性对观测方程的影响，将式(1)的观测方程表示为:

式中，h(·)为观测函数;ρvk和ρsk为速度偏差和刻度系数偏差。

组合导航滤波系统中非线性特性表现在观测方程，因此只需要对预测量进行采样，然后利用观测函数h(·)对采样点进行处理，得到观测更新后的系统统计特性。下面通过UT 变化产生Sigma 点对观测方程进行近似，重新构建伪观测量用于估计。

预测方程如下:

通过以下方法选取2nx+1 个sigma 点χi和相对应的权值wi，进行Sigma 点采样:

为了避免UT 转换后的方差阵为非正定阵，采用Julier［8］提出的尺度转换方法对原始的sigma 点及其权值进行如下转换:

其中，λ=α2(n+κ)- n，α 决定sigma 的散布程度，通常取α ≤1，κ 为比例因子，通常nx+κ=3，β 描述协方差计算χ0的权重，为求一阶统计特性的权系数，为求二阶统计特性的权系数。

观测量更新如下:

观测量和状态量的交互协方差为:

若k 时刻观测量为zk，则状态估计结果及其协方差为:

2 辅助信息修正的改进多模型滤波算法

对组合导航系统滤波模型的UT 变换及状态估计过程中，偏差值未知，下面首先基于sigma 点的伪观测量进行偏差值的估计;然后利用辅助信息对估计结果进行修正，消除累积误差，提高整体估计精度。

2.1 基于sigma 点的伪观测量与偏差值估计

由式(8)可以看出，在偏差值ρvk，ρsk已知情况下，可通过UKF 方法得到系统状态估计结果。下面利用式(5)生成的Sigma 点构建伪观测量，然后通过改进多模型原理对偏差值进行估计。

假设ρvk，ρsk服从半马尔科夫假设，利用离散值对其进行建模:

式中二维参数ρj的总数N=n·m。

在观测量zk已知情况下，构建伪观测量的量测信息及其协方差阵:

式中，p(ρk-1=ρα| Z0:k-1)为tk-1时刻偏差为ρα的概率，由递推计算得到;θαi(tk)为tk时刻偏差ρα到ρj的马尔可夫转移概率，由概率转移矩阵求得。

在ρj和p(ρk=ρj| Z0:k)的基础上计算两类偏差的估计值和:

2.2 辅助信息修正的改进多模型滤波算法

1)辅助信息的可信度分析

接收到的辅助信号z2观测方程可表示为:

图1 基于UT 变化的多模型滤波改进算法流程图Fig.1 The flow chart of improved multiple model kalman filtering algorithm based on UT

式中:H2=［I，0］;υ2k为零均值高斯白噪声。采用卡尔曼滤波对辅助信息进行处理，得到状态估计量和估计方差P2k。

环境信息变量描述传感器数据的可用性，通过引入环境信息变量对传感器进行分析，可以判断传感器信息的可行度，确保估计系统的整体精度。定义环境变量

式中，γk，Sk为观测新息及其协方差阵，表达式为

2)辅助信息的融合算法

在融合过程中需设置启动阀λF，当λF=0，表示没有获得辅助信号，直接输出多模滤波结果;当λF=1，表示得到辅助信号时，利用环境变量进行检测，如果辅助信息可信度低，滤波过程中不考虑辅助信息，即

若辅助信号可用，则可以利用如式(28)所提加权因子法提高估计精度。

其中α1，α2为标量加权因子，表达式为

融合后状态估计方差为

辅助信息修正的改进多模型滤波算法结构图如图2所示。

图2 辅助信息修正的改进多模型滤波算法流程图Fig.2 Improved multiple model kalman filtering algorithm used the auxiliary information

2.3 算法实现

辅助信息修正的改进多模型滤波算法的计算步骤如下:

第1 步:初始化

确定Sigma 点采样的参数，计算初始状态估计值、协方差阵。

第2 步:k 时刻滤波一步预测值

第4 步:观测值更新

第5 步:辅助信息修正

判断阀值λF，通过式(27)或式(28)得到融合后的状态和状态估计方差。

通过式(10)～式(12)计算状态估计结果及其协方差。

3 仿真结果与分析

仅考虑在固定水深下的航行，水下航行器组合导航系统的仿真模型和条件同文献［9］，仿真时间取600 s。为了验证本文提出方法的有效性，采用了本文方法和常规的多模型滤波算法进行了仿真，2 种方法在纬度和经度方向的位置误差仿真结果如图3和图4所示。

由仿真曲线可知，采用本文算法可以有效地降低系统状态误差，加快滤波状态收敛速度。

在估计精度方面，定义性能指标函数Jρv，Jρs为:

采用了本文方法和常规多模型滤波算法对偏差估计精度进行比较，表1 给出估计精度统计指标。

图3 本文改进算法的位置误差曲线Fig.3 Errors of position of improved algorithm

图4 多模型滤波算法的位置误差曲线Fig.4 Errors of position of traditional multiple model filtering algorithm

从表1 中的数据结果可知，本文算法通过UT变换对多模型算法进行修正，提高了估计精度。

表1 滤波算法估计精度Tab.1 Estimated accuracy of filtering algorithm

4 结 语

本文提出了一种基于UT 变换的简化多模型算法，并利用辅助信息对估计结果进行修正。针对非线性观测方程进行UT 变化，利用Sigma 点构建伪观测量，进而更新模型概率，得到偏差估计结果并代入到多模型滤波中，得到状态估计结果;然后利用环境变量分析辅助信息的可用度，设计加权因子融合算法，合理利用辅助信息，提高系统整体估计精度。

仿真结果表明所提方法有效地降低系统状态误差，加快滤波状态收敛速度，同时对多模型算法进行修正，提高了估计精度。

［1］LI Kang-hua，WANG Jin-ling，LI Wan-li，et al．A noval INS and doppler sensors calibration method for long range underwater vehicle navigation［J］．Sensors，2013，13(14):583－600．

［2］JULIER S J，UHHNANN J K．Unscented filtering and nonlinear estimation［J］．Proceedings of the IEEE，2004，92(3):401－422．

［3］刘明雍，胡俊伟，李闻白．一种基于改进无迹卡尔曼滤波的自主水下航行器组合导航方法研究［J］．兵工学报，2011，32(2):252－256．

［4］王征．基于PF和UKF 的水下目标运动估计方法研究［J］．自动化技术与应用，2013，32(12):12－17．

［5］朱安福，景占荣，高田，等．基于UKF 的交互多模型算法［J］．系统仿真学报，2009，21(2):655－657．

［6］王其，徐晓苏，张涛，等．模糊自适应滤波在水下航行器组合导航系统中的应用［J］．中国惯性技术学报，2008，16(3):320－325．

［7］KIM H S，KIM I H．Design of adaptive fuzzy IMM algorithm for tracking the maneuvering target with time-varying measurement noise［J］．International Journal of Control，Automation and Systems，2007，5(3):307－316．

［8］JULIER S J，UHLMANN J K，Durrant－Whyte H F．A new approach for filtering nonlinear systems［C］．Proceedings of the American Control Conference，Evanston，USA，1995:1628－1632．

［9］HAO Yan-ling，MU Hong-wei．Application of integrated alignment in AUV based on GP－SRCDKF［J］．Advances in Information Sciences and Service Sciences，2012，4(21):598－605．