CN109269497A - 基于auv切法向速度模型的多尺度无迹卡尔曼滤波估计方法 - Google Patents

Abstract

本发明水下潜航器领域，公开了基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，获取状态序列X(N)和观测值序列Z(N)；将状态序列X(N)和观测值序列Z(N)分解到尺度i上；获取离散系统下两种运动状态的切法向速度模型的状态转移方程；获取不同尺度之间状态方程与量测方程的矩阵转换关系包括系统过程噪声，系统量测方程和系统量测噪声；获取在尺度i下的状态一步预测方程，状态向量的更新方程，协方差的更新方程和MUKF的增益K_k；通过对观测序列的细节信息部分进行阈值处理来降噪；对滤波后的近似信息和降噪处理后的观测值细节信息进行小波重构得到原始尺度上的最优估计。本发明提高状态预测的准确性，降低噪声干扰，提高滤波估计精度。

Description

基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法

技术领域

本发明属于水下潜航器领域，尤其涉及基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法。

背景技术

传统的非线性滤波的方法主要是扩展卡尔曼滤波算法,但是该算法存在着精度不高、稳定性差、对目标机动反应迟缓等缺点.近年来,文献提出了一种非线性滤波算法----Unscented卡尔曼滤波。它是根据Unscented变化和卡尔曼滤波相结合得到的一种算法。这种算法主要运用卡尔曼滤波的思想,但是在求解目标后续时刻的预测值和量测值时,则需要应用采样点来计算。UKF通过设计加权点δ,来近似表示n维目标采样点,计算这些δ点经由非线性函数的传播,通过非线性状态方程获得更新后的滤波值,从而实现了对目标的跟踪。UKF有效地克服了扩展卡尔曼滤波的估计精度低、稳定性差的缺陷。

卡尔曼最初提出的滤波理论只适用于线性系统,Bucy，Sunahara等人提出并研究了扩展卡尔曼滤波，将卡尔曼滤波理论进一步应用到非线性领域。EKF的基本思想是将非线性系统线性化，然后进行卡尔曼滤波，因此EKF是一种次优滤波。其后，多种二阶广义卡尔曼滤波方法的提出及应用进一步提高了卡尔曼滤波对非线性系统的估计性能。二阶滤波方法考虑了Taylor级数展开的二次项，因此减少了由于线性化所引起的估计误差，但大大增加了运算量，因此在实际中反而没有一阶EKF应用广泛。在状态方程或测量方程为非线性时，通常采用扩展卡尔曼滤波。EKF对非线性函数的Taylor展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性，可以将卡尔曼线性滤波算法应用于非线性系统中。这样一来，解决了非线性问题。EKF虽然应用于非线性状态估计系统中已经得到了学术界认可并为人广泛使用，然而该种方法也带来了两个缺点，其一是当强非线性时EKF违背局部线性假设，Taylor展开式中被忽略的高阶项带来大的误差时，EKF算法可能会使滤波发散；另外，由于EKF在线性化处理时需要用雅克比矩阵，其繁琐的计算过程导致该方法实现相对困难。所以，在满足线性系统、高斯白噪声、所有随机变量服从高斯分布这3个假设条件时，EKF是最小方差准则下的次优滤波器，其性能依赖于局部非线性度。无损卡尔曼滤波是一种新型的滤波估计算法。UKF以UT变换为基础，摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，对于一步预测方程，使用无迹(UT)变换来处理均值和协方差的非线性传递，就成为UKF算法。UKF是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要求导计算Jacobian矩阵。UKF没有线性化忽略高阶项，因此非线性分布统计量的计算精度较高。基于上述优点，UKF被广泛应用于导航、目标跟踪、信号处理和神经网络学习等多个领域。

申请号201210585170.1的专利公开了一种用于水下滑翔器的高精度组合导航定位方法，惯性测量单元IMU的输出经过粗处理和细处理后得到较高精度的数据，融合基于Runge-Kutta法的航位推算得到的数据，再经自适应卡尔曼AKF和无迹卡尔曼UKF二级滤波，滤波后反馈回IMU来校正IMU的累积误差，最终输出较准的位置速度姿态等信息。本发明用于水下滑翔器的自主导航，具有高精度、实时性、稳定性等优点，使得整体导航系统能长航时、高精度、低功耗稳定运行，能迅速准确地得到水下航行器当前位姿信息，同时为其提供航迹和位置参数。但该方法前期采集数据时误差较大，导致输出结果的可信度下降。

申请号201210460353.0的专利公开了一种自主水下航行器自主导航定位方法，选取自主水下航行器运动模型；选取自主水下航行器测量模型；通过自主水下航行器运动模型和测量模型构建动态系统；滤波器参数初始化；选取渐消因子；更新滤波器时间；更新滤波器量测数据；由当前时刻更新到下一时刻，重复执行步骤(5)至步骤(7)，直到到达终止时刻，结束计算，输出结果。通过引入渐消因子实时调整滤波增益矩阵，强迫输出残差序列保持相互正交，以此来减小老数据的权值，相对地增加新数据的权值，提高了滤波器的估计精度和稳定性。但是由于水下环境变化多样，该方法的适应性较差。

发明内容

本发明的目的在于公开预测准确度高，抗干扰能力强和降噪能力强的基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法。

本发明的目的是这样实现的：

基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，包含如下步骤：

步骤(1)：获取状态序列X(N)和观测值序列Z(N)：

将状态量与观测量作小波分解为不同频域上的信息，并在最初尺度作无极卡尔曼滤波后重构到原始尺度，获取状态序列X(N)和观测值序列Z(N)，N为当前原始尺度序号，且状态序列X(N)和观测值序列Z(N)的长度M是2的整数次幂，且M≥2^N-i，i为小波分解后的尺度，表示原始序列进行了N-i层小波分解，i为整数；每进行一层小波变换，尺度减小1。

步骤(2)：将状态序列X(N)和观测值序列Z(N)分解到尺度i上：

根据小波变换原理，利用MALLAT算法数字滤波器处理：

上式中，X_V(j)表示尺度j上的状态近似信号，X_D(j-1)表示尺度j-1上的状态细节信号，j为整数；h(l)是用于小波分解获得近似信息的低通滤波系数，g(l)是用于小波分解获得细节信息的高通滤波系数；

将状态序列X(N)和观测值序列Z(N)分解到尺度i上：

上式中，Z_V(i)表示尺度i上的观测值近似信号，Z_D(i)表示尺度i上的观测值细节信号。

步骤(3)：获取离散系统下两种运动状态的切法向速度模型的状态转移方程：

已知法向加速度a_n(t)和切向加速度a_t(t)，离散系统中，当目标进行切向加速度a_t(t)运动时，切法向速度模型的状态转移方程为：

上式中，表示a_t(t)在x轴方向的分量，表示a_t(t)在y轴方向的分量；

当目标进行法向加速度a_n(t)运动时，切法向速度模型的状态转移方程为：

上式中，表示a_n(t)在x轴方向的分量，表示a_n(t)在y轴方向的分量。

步骤(4)：获取不同尺度之间状态方程与量测方程的矩阵转换关系包括系统过程噪声，系统量测方程和系统量测噪声：

尺度j分解到尺度j-1的系统状态方程表达式为：

A(j-1)＝A(j)A(j)；

上式中，A表示系统状态转移矩阵；

尺度j分解到尺度j-1的系统过程噪声表达式为：

Q(j-1)＝A(j)∑_lh²(l)Q(j)A^T(j)+∑_lh²(l)Q(j)；

上式中，Q表示系统过程噪声方差，h(l)表示低通滤波器；

尺度j分解到尺度j-1的系统量测方程表达式为：

C(j-1)＝C(j)；

上式中，C表示观测矩阵；

尺度j分解到尺度j-1的系统量测噪声表达式为：

R(j-1,k)＝∑_lh²(l)R(j,2k-l)；

上式中，R表示量测噪声方差。

步骤(5)：获取在尺度i下的状态一步预测方程，状态向量的更新方程，协方差的更新方程和MUKF的增益K_k：

在k时刻尺度i上进行多尺度卡尔曼滤波的状态一步预测方程：

X_V(i,k-1|k)＝A(i,k-1)X_V(i,k-1)+W(i,k-1)；

上式中，k表示离散的时间节点；X_V(i,k-1|k)表示k时刻尺度i下的状态预测；W(i,k-1)表示系统噪声随机序列，且：

E{w(i,k)}＝0；

E{w(i,k)w^T(i,k)}＝Q(i,k)δ_kj,k,j≥0；

对尺度i上的Sigma点进行采样，并选取数量为2n_x+1的sigma点χⁱ，并选取sigma点χⁱ对应的权值wⁱ：

sigma点χⁱ对应的权值：

上式中，n_x为状态的维数，为状态序列均值，P为方差，λ＝a²(n+κ)-n_x是缩放比例参数，a是采样点的分布状态的控制参数，κ是使得矩阵(n+λ)P为半正定矩阵的待选参数；β是非负的权系数；m为均值，c为协方差；

尺度i上的系统量测预测方程为：

Z_V(i,k)＝H(i,k)X_V(i,k)+v(i,k)；

上式中，H为观测矩阵，v为观测噪声；

尺度i上的MUKF算法中的预测量均值和协方差P_z(i,k),P_xz(i,k)为：

则：

状态向量的更新方程：

MUKF的增益K_k：K_k＝P_xz(i,k)P_z(i,k)^-1；

协方差的更新方程：

步骤(6)：通过对观测序列的细节信息部分进行阈值处理来降噪：

阈值ω：

ω＝μ·σ；

上式中，σ为噪声方差，μ是取值受经验因素影响的降噪参数。

步骤(7)：对滤波后的近似信息和降噪处理后的观测值细节信息进行小波重构得到原始尺度上的最优估计：

X_V(i)＝∑X_V(i-1)h(-l)+∑X_D(i-1)g(-l)。

本发明的有益效果为：

本发明结合实际AUV运动规律，建立基于切法向速度的运动模型，提高状态预测的准确性；本发明将多尺度分析方法与无迹卡尔曼滤波方法结合，考虑不同尺度频率对滤波估计的影响，降低噪声偏差对滤波系统的干扰，提高滤波估计精度；本发明对观测信号细节信息做阈值降噪处理，进一步消除噪声对组合导航系统的影响。

附图说明

图1是基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法原理图；

图2是多尺度小波分解示意图；

图3a是序列分解的MALLAT算法的滤波器等效示意图；

图3b是序列重构的MALLAT算法的滤波器等效示意图。

具体实施方式

下面结合附图来进一步描述本发明：

实施例1：

如图1，基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，包含如下步骤：

步骤(1)：由于AUV运动的切向和法向加速度模型为非线性模型，本方法采用无迹卡尔曼滤波算法。无迹卡尔曼滤波是水下导航中最常用到的信息融合方法之一，在实际应用中证明了它的有效性，但另一方面无迹卡尔曼滤波也存在局限性，未考虑到水下导航信息低频与高频信号之间的相关性与差异性，因此本发明将信号频域分析方法——多尺度分析，与无迹卡尔曼滤波相结合，基于本发明提出的AUV切法向速度运动模型，将状态量与观测量作小波分解为不同频域上的信息，并在最粗尺度作无极卡尔曼滤波后重构到原始尺度。

步骤(2)：如图2，每进行一层小波变换，尺度序号减小1。获取输入状态序列X(N)和观测值序列Z(N)，N为当前原始尺度序号，为了进行多次小波变换分解，序列的长度M必须为2的整数次幂，且M≥2^N-i，i为小波分解后的尺度，即表示原始序列进行了N-i层小波分解。

根据小波变换原理，利用MALLAT算法数字滤波器处理方案，如下式：

式中下标V表示近似信号，下标D表示细节信号；h(l)是用于小波分解获得近似信息的低通滤波系数，g(l)是用于小波分解获得细节信息的高通滤波系数。其原理框图图如图3a所示，其中2↓表示二抽取，又叫隔二抽取。目的是为保证输出信号总长度与原始信号长度保持一致。

将状态序列X(N)和观测值序列Z(N)分解到尺度i上，分解形式如图2所示，由尺度i上的近似信息部分和各层细节信息部分组成。可表示为下式：

式中下标V表示近似信号，下标D表示细节信号，括号内为尺度序号。

步骤(3)：AUV运动过程中，在法向加速度a_n(t)和切向加速度a_t(t)已知的情况下，连续系统的运动方程可描述为：

其中φ(t)为目标的前进方向角，V(t)为AUV的瞬时速度。

将其推广到离散时间系统，当目标进行切向加速度a_t(t)运动时，切法向速度模型的状态转移方程可描述为：

上式中，表示a_t(t)在x轴方向的分量，表示a_t(t)在y轴方向的分量。

当目标进行法向加速度a_n(t)运动时，切法向速度模型的状态转移方程可描述为：

上式中，表示a_n(t)在x轴方向的分量，表示a_n(t)在y轴方向的分量。

步骤(4)：通过小波变换从尺度j分解到尺度j-1，可得到不同尺度之间状态方程与量测方程的矩阵转换关系：

尺度j分解到尺度j-1的系统状态方程表达式为：

A(j-1)＝A(j)A(j)；

式中A表示系统状态转移矩阵。

尺度j分解到尺度j-1的系统过程噪声表达式为：

Q(j-1)＝A(j)∑_lh²(l)Q(j)A^T(j)+∑_lh²(l)Q(j)；

式中Q表示系统过程噪声方差，h(l)表示低通滤波器。

尺度j分解到尺度j-1的系统量测方程表达式为：

C(j-1)＝C(j)；

式中C表示观测矩阵。

尺度j分解到尺度j-1的系统量测噪声表达式为：

R(j-1,k)＝∑_lh²(l)R(j,2k-l)；

式中R表示量测噪声方差。

通过上式可以将状态方程和观测方程逐层分解至目标尺度i。

步骤(5)：具体对于经过小波分解后的状态序列与观测序列来说，在尺度i下的一步状态预测方程与更新方程如下：

因滤波对象是小波分解后的近似信号，下式中的状态序列与观测序列均省略下脚标V。

在k时刻尺度i上进行多尺度卡尔曼滤波的状态一步预测：

X(i,k-1|k)＝A(i,k-1)X(i,k-1)+W(i,k-1)；

式中k表示离散的时间节点；X(i,k-1|k)表示k时刻尺度i下的状态预测；W(i,k-1)表示系统噪声随机序列，且满足：

E{w(i,k)}＝0；

E{w(i,k)w^T(i,k)}＝Q(i,k)δ_kj,k,j≥0；

对尺度i上的Sigma点进行采样，并选取数量为2n_x+1的sigma点χⁱ，并选取其相对应的权值wⁱ，采样过程如下：

相应权值w如下式：

上式中n_x为状态的维数，为状态序列均值，P为方差，参数λ＝a²(n+κ)-n_x是一个缩放比例参数，用来降低总的预测误差，a的选取控制了采样点的分布状态，κ为待选参数，其具体取值虽然没有界限，但通常确保矩阵(n+λ)P为半正定矩阵。待选参数β≥0是个非负的权系数。w的下标m为均值，c为协方差。

则尺度i上的系统量测预测方程为：

Z(i,k)＝H(i,k)X(i,k)+v(i,k)；

上式中H为观测矩阵，v为观测噪声。

尺度i上的MUKF算法中的预测量均值和协方差P_z(i,k),P_xz(i,k)为：

计算MUKF的增益K_k，并对状态向量和协方差进行更新：

K_k＝P_xz(i,k)P_z(i,k)^-1；

步骤(6)：将观测序列的细节信息部分{Z_D(i),Z_D(i+1),Z_D(i+2),...,Z_D(N-2),Z_D(N-1)}进行阈值处理：在实际应用中，水下导航传感器性能各不相同，故采用如下公式确定各信号的阈值ω：

ω＝μ·σ；

其中σ为噪声方差，μ的取值因原始信号的不同以及小波分解不同层而选用不同，受经验因素的影响。对超过阈值的数据采用前一刻有效数据替换，达到降噪的功能。

步骤(7)：对滤波后的近似信息和降噪处理后的观测值细节信息进行小波重构，如下式所示：

X_V(i)＝∑X_V(i-1)h(-l)+∑X_D(i-1)g(-l)；

上式右边的两个部分实质上都是卷积运算，用滤波器形式表示如图3b所示。图中2↑是二差值，即表示隔点插零。

只要给出初始状态序列和观测值序列，根据AUV切法向速度模型状态方程就可以在多尺度分解至尺度i，利用近似信息，也就是信号中的低频信号进行无迹卡尔曼滤波，在运算过程中由于去除了信号高频部分，对于水下导航传感器，观测信号在小波分解后的细节信息主要包含了噪声和干扰误差，因此减少了信号中噪声偏差对整个滤波系统的干扰，有效抑制导航系统误差的发散，提高导航精度。再通过对细节信息采取相应合理的阈值处理，达到降噪效果，进一步消除噪声对组合导航系统的影响。

与现有技术相比，本发明结合实际AUV运动规律，建立基于切法向速度的运动模型，提高状态预测的准确性；本发明将多尺度分析方法与无迹卡尔曼滤波方法结合，考虑不同尺度频率对滤波估计的影响，降低噪声偏差对滤波系统的干扰，提高滤波估计精度；本发明对观测信号细节信息做阈值降噪处理，进一步消除噪声对组合导航系统的影响。

以上所述并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，其特征在于：包含如下步骤：

步骤(1)：获取状态序列X(N)和观测值序列Z(N)；

步骤(2)：将状态序列X(N)和观测值序列Z(N)分解到尺度i上；

步骤(3)：获取离散系统下两种运动状态的切法向速度模型的状态转移方程；

步骤(4)：获取不同尺度之间状态方程与量测方程的矩阵转换关系包括系统过程噪声，系统量测方程和系统量测噪声；

步骤(5)：获取在尺度i下的状态一步预测方程，状态向量的更新方程，协方差的更新方程和MUKF的增益K_k；

步骤(6)：通过对观测序列的细节信息部分进行阈值处理来降噪；

步骤(7)：对滤波后的近似信息和降噪处理后的观测值细节信息进行小波重构得到原始尺度上的最优估计。

2.根据权利要求1所述的基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，其特征在于：所述的步骤(1)具体为：

将状态量与观测量作小波分解为不同频域上的信息，并在最初尺度作无极卡尔曼滤波后重构到原始尺度，获取状态序列X(N)和观测值序列Z(N)，N为当前原始尺度序号，且状态序列X(N)和观测值序列Z(N)的长度M是2的整数次幂，且M≥2^N-i，i为小波分解后的尺度，表示原始序列进行了N-i层小波分解，i为整数；每进行一层小波变换，尺度减小1。

3.根据权利要求1所述的基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，其特征在于：所述的步骤(2)具体为：

根据小波变换原理，利用MALLAT算法数字滤波器处理：

上式中，X_V(j)表示尺度j上的状态近似信号，X_D(j-1)表示尺度j-1上的状态细节信号，j为整数；h(l)是用于小波分解获得近似信息的低通滤波系数，g(l)是用于小波分解获得细节信息的高通滤波系数；

将状态序列X(N)和观测值序列Z(N)分解到尺度i上：

上式中，Z_V(i)表示尺度i上的观测值近似信号，Z_D(i)表示尺度i上的观测值细节信号。

4.根据权利要求1所述的基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，其特征在于：所述的步骤(3)具体为：

已知法向加速度a_n(t)和切向加速度a_t(t)，离散系统中，当目标进行切向加速度a_t(t)运动时，切法向速度模型的状态转移方程为：

上式中，表示a_t(t)在x轴方向的分量，表示a_t(t)在y轴方向的分量；

当目标进行法向加速度a_n(t)运动时，切法向速度模型的状态转移方程为：

上式中，表示a_n(t)在x轴方向的分量，表示a_n(t)在y轴方向的分量。

5.根据权利要求1所述的基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，其特征在于：所述的步骤(4)具体为：

尺度j分解到尺度j-1的系统状态方程表达式为：

A(j-1)＝A(j)A(j)；

上式中，A表示系统状态转移矩阵；

尺度j分解到尺度j-1的系统过程噪声表达式为：

Q(j-1)＝A(j)∑_lh²(l)Q(j)A^T(j)+∑_lh²(l)Q(j)；

上式中，Q表示系统过程噪声方差，h(l)表示低通滤波器；

尺度j分解到尺度j-1的系统量测方程表达式为：

C(j-1)＝C(j)；

上式中，C表示观测矩阵；

尺度j分解到尺度j-1的系统量测噪声表达式为：

R(j-1,k)＝∑_lh²(l)R(j,2k-l)；

上式中，R表示量测噪声方差。

6.根据权利要求1所述的基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，其特征在于：所述的步骤(5)具体为：

在k时刻尺度i上进行多尺度卡尔曼滤波的状态一步预测方程：

X_V(i,k-1|k)＝A(i,k-1)X_V(i,k-1)+W(i,k-1)；

上式中，k表示离散的时间节点；X_V(i,k-1|k)表示k时刻尺度i下的状态预测；W(i,k-1)表示系统噪声随机序列，且：

E{w(i,k)}＝0；

E{w(i,k)w^T(i,k)}＝Q(i,k)δ_kj,k,j≥0；

对尺度i上的Sigma点进行采样，并选取数量为2n_x+1的sigma点χⁱ，并选取sigma点χⁱ对应的权值wⁱ：

sigma点χⁱ对应的权值：

上式中，n_x为状态的维数，为状态序列均值，P为方差，λ＝a²(n+κ)-n_x是缩放比例参数，a是采样点的分布状态的控制参数，κ是使得矩阵(n+λ)P为半正定矩阵的待选参数；β是非负的权系数；m为均值，c为协方差；

尺度i上的系统量测预测方程为：

Z_V(i,k)＝H(i,k)X_V(i,k)+v(i,k)；

上式中，H为观测矩阵，v为观测噪声；

尺度i上的MUKF算法中的预测量均值和协方差P_z(i,k),P_xz(i,k)为：

则：

状态向量的更新方程：

MUKF的增益K_k：K_k＝P_xz(i,k)P_z(i,k)^-1；

协方差的更新方程：

7.根据权利要求1所述的基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，其特征在于：所述的步骤(6)中的阈值ω：

ω＝μ·σ；

上式中，σ为噪声方差，μ是取值受经验因素影响的降噪参数。

8.根据权利要求1所述的基于AUV切法向速度模型的多尺度无迹卡尔曼滤波估计方法，其特征在于：所述的步骤(7)为：

X_V(i)＝∑X_V(i-1)h(-l)+∑X_D(i-1)g(-l)。