一种基于模糊自适应滤波的无人艇组合导航方法与流程

本发明涉及导航系统技术领域，具体涉及一种基于模糊自适应滤波的无人艇组合导航方法。

背景技术：

无人艇(usv)具有体积小、隐身性好、智能、无人员伤亡等优点，在军事方面能够灵活作战，部署机动，使用方便，可以在危险区域或者不适宜派遣有人舰船的区域独立自主执行任务，拓展了海上作战范围，具有良好的费效比。我国作为海岸线广阔、海上争端频发的发展中大国，有必要加强无人艇技术研究，保护自身的海洋权益。而为其设计一款高性能的导航系统具有重要现实意义。就目前而言，对于无人艇导航系统的主要要求是小体积、高精度、高可靠性，能够适应不同海况，针对其特点，常采用捷联惯导系统(sins)和全球定位系统(gps)组成组合导航系统。

在传统组合导航系统中，卡尔曼滤波技术得到广泛的应用。当组合导航系统的数学模型精确已知，并忽略计算误差时，用常规卡尔曼滤波对系统的状态进行估计，可以得到状态的精确估计值。如果系统数学模型不精确或者噪声统计特性变化，常规卡尔曼滤波精度将大大降低，甚至发散。建立组合导航系统准确的数学模型需要做大量的试验，尤其是建立准确的系统噪声和量测噪声的统计特性。为了解决此问题，通常采用自适应滤波技术，在进行滤波的同时，利用观测数据带来的信息，不断地在线估计和修正模型参数、噪声统计特性以提高滤波精度，得到对象状态的最优估计值。

近年来，许多学者对自适应滤波进行了深入的研究。文献“抗野值自适应卡尔曼滤波方法的研究，中国惯性技术学报，2003”提出了抗野值自适应滤波方法，这种方法通过对信息的监测判断是否有野值出现，当有野值出现时，通过自适应调整增益矩阵，达到消除野值影响的目的。但是它仅仅解决了由于野值引起滤波发散的情况，而对其它原因引起的滤波发散无能为力。文献“arobustandself-stunningkalmanfilterforautonomousspacecraftnavigation，degreeofdoctorofwashingtonuniversity，2001”提出了多模型和基于信息的自适应滤波方法，由于这种方法多处出现矩阵求逆计算，实时性和稳定性难以满足要求。另外，这种方法要求系统可观测，而且它仅仅适合白噪声。在文献“神经网络辅助卡尔曼滤波技术在组合导航系统中的应用研究，中国惯性技术学报，2003”中指出，将人工智能技术和滤波技术融合是滤波技术的发展趋势。神经网络作为人工智能技术中的一个领域，其主要优点在于它对系统的模型没有特别要求，只要有足够的用于训练的先验数据，就可以用训练的神经网络近似代替原系统。但训练样本的获取一直是神经网络应用的一个瓶颈。

对无人艇组合导航系统而言，由于自身艇小更易受到复杂海况的影响，系统量测噪声统计特性随实际工作环境而改变，初始的先验值并不能代表实际工作时的噪声情况。经过对捷联式惯性导航系统的大量反复试验，可以获得试验时系统噪声的统计特性，但是实际工作时量测噪声的统计特性是未知的，因此需要一种在线估计量测噪声统计特性的自适应滤波方法。

技术实现要素：

本发明的目的是为了解决上述问题，提出一种基于模糊自适应滤波的无人艇组合导航方法。本发明根据实时得到的量测新息的实际方差与理论方差的比值和差值，由设计的模糊推理系统(fis)在线实时调整量测噪声矩阵。这就使滤波算法不需要得到准确的量测噪声矩阵的先验知识，且对于时变得量测噪声也能够得到精确的估计值。采用递推的方式逐步调整系统量测噪声方差阵逼近真实值，并可通过调整参数来调整逼近过程在快速性和稳定性之间达到最佳平衡，且以量测新息的实际方差与理论方差的差值和比值作为模糊推理系统的输入，使系统更具通用性。

本发明是一种基于模糊自适应滤波的无人艇组合导航方法，具体包括以下几个步骤：

步骤一：建立系统状态方程和量测方程：

在sins/gps组合导航模式下，sins作为主导航系统建立系统的状态方程，并选取经纬度误差、速度误差以及失准角为状态变量，利用gps与sins所提供速度和位置的差值作为量测变量，建立组合导航系统的量测方程；

状态方程：xk＝φk,k-1xk-1+γk-1wk-1

量测方程：zk＝hkxk+vk

其中，xk为系统状态向量，zk为量测向量，hk为量测矩阵，φk,k-1为状态转移矩阵，γk-1为系统噪声矩阵，wk-1为系统噪声向量，vk为量测噪声向量；wk-1和vk是不相关的高斯白噪声序列，均值和方差分别为：e{wk}＝0,e{wkwj^t}＝qkδkj,e{vk}＝0,e{vkvj^t}＝rkδkj，cov{wivj}＝0；式中，qk为系统噪声方差阵；rk为量测噪声方差矩阵，δkj是δ函数；

步骤二：根据常规卡尔曼滤波算法，建立模糊自适应滤波算法：

首先更新状态一步预测值及其均方误差，利用k时刻量测信息zk和一步预测计算新息序列rk，求取增益矩阵和滤波方程；

一步预测：

预测均方误差：

增益矩阵：

新息序列：

滤波方程：

滤波均方误差：pk＝(i-kkhk)pk/k-1

上述算法描述中，为滤波过程中对状态向量xk的估计量，为状态一步预测，kk为滤波增益，pk为滤波误差的协方差阵，pk/k-1为预测误差协方差阵，rk为新息序列，i为单位矩阵；

步骤三：确定模糊推理系统的输入输出参数：

定义残差方差的理论值为和残差方差的实际值为设计两个模糊推理系统(fis)，均为单输入单输出模式。将理论值和实际值的比值rork作为模糊推理系统1的输入，输出为αk；理论值和实际值的差值dork作为模糊推理系统2的输入，输出为b；

残差方差的理论值：

残差方差的实际值：

理论值和实际值的比值：

理论值和实际值的差值：

其中，为对最新的n个残差向量方差求平均值(n由经验选取，一般在10～30之间，主要起平滑作用)，i0＝k-n+1；ri即为残差序列；tr(·)表示对矩阵求迹；

步骤四：设计模糊推理系统：

模糊推理系统1的输入为上一步求出的rork，输出为αk，对输入输出模糊化，两者的隶属度函数均为三角形隶属度函数，反模糊化采用重心法，其模糊控制规则为：

ifrork∈d(decrease),thenαk∈d(decrease)

ifrork∈m(maintain),thenαk∈m(maintain)

ifrork∈i(increase),thenαk∈i(increase)

模糊推理系统2的输入为上一步求出的dork，输出为b，对输入输出模糊化，两者的隶属度函数均为三角形隶属度函数，反模糊化采用重心法，其模糊控制规则为：

ifdork∈n(negtive),thenb∈i(increase)

ifdork∈z(zero),thenb∈m(maintain)

ifdork∈p(positive),thenb∈i(increase)

步骤五：更新量测噪声估计值：

根据上一步求出的αk和b，更新量测噪声估计值，具体表达式为：

量测噪声估计值：

其中，表示第k步的量测噪声估计值，αk表示量测噪声的调整系数，b为指数调整系数，表示对αk的放大程度。若b>1，表示放大αk对的调整作用，这样量测噪声变化时，可以在较少的步数内迅速逼近真实值。若b<1，表示缩小αk对的调整作用。b的取值过大，则可能导致以真实量测噪声值为中心作小幅度震荡；b取值过小，则会导致调整到真实值的过渡时间稍长。

步骤六：重复执行滤波过程：

不断重复以上步骤二、三、五过程,利用自适应滤波方法对量测噪声进行在线估计,并实时修正捷联惯导系统输出的导航信息,直至sins/gps组合导航过程结束。

本发明的优点在于：

(1)本发明提出的一种基于模糊自适应滤波的无人艇组合导航方法，能够有效的解决无人艇受外部环境影响，量测噪声实时变化带来的导航精度下降甚至滤波发散等问题，克服了传统算法的缺点和不足；

(2)本发明提出的基于模糊自适应滤波的无人艇组合导航方法，通过监测理论残差和实际残差的协方差一致程度，应用模糊推理系统不断调整量测噪声和滤波器的增益系数，对卡尔曼滤波器进行在线自适应调整，保证了sins/gps组合导航精度，更为简单易行，具有通用性。

附图说明

图1：本发明提出的无人艇自适应滤波方法导航系统的结构框图。

图2：本发明提出的无人艇自适应滤波方法流程图。

图3：本发明中模糊推理系统1输入输出隶属度函数。

图4：本发明中模糊推理系统2输入输出隶属度函数。

图5：本发明中自适应滤波和常规卡尔曼滤波曲线图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明提出的一种无人艇组合导航系统的模糊自适应滤波方法，结构框图和算法流程图分别如图1和图2所示，具体包括以下几个步骤：

步骤一：建立系统状态方程和量测方程：

在sins/gps组合导航模式下，sins作为主导航系统建立系统的状态方程，并选取经纬度误差、速度误差以及失准角为状态变量，利用gps与sins所提供速度和位置的差值作为量测变量，建立组合导航系统的量测方程；

首先，建立连续系统的状态方程和量测方程：

状态方程：

量测方程：z(t)＝h(t)x(t)+v(t)

其中，状态变量：

状态转移阵：

系统噪声阵：

姿态角矩阵：

量测系数阵：

式中，δλ表示纬度、经度位置误差；δve、δvn表示东向速度、北向速度误差；ve、vn表示东向、北向速度；表示东向、北向及垂向失准角；fe、fn、fu表示加速度计在东向、北向和垂向的加速度；wie表示地球自转角速度；r表示地球半径；γ、θ、φ表示纵摇角、横摇角和航向角；w(t)和v(t)分别为系统噪声和量测噪声；

将连续系统状态方程和量测方程离散化：

状态方程：xk＝φk,k-1xk-1+γk-1wk-1

量测方程：zk＝hkxk+vk

其中，xk为系统状态向量，zk为量测向量，量测矩阵hk＝h(t)，系统噪声矩阵γk-1＝g(tk)δt，状态转移矩阵φk,k-1＝i+f(tk)δt(采样时间tk＝k，采样周期δt＝1)，i为单位矩阵，wk-1为系统噪声向量，vk为量测噪声向量；wk-1和vk是不相关的高斯白噪声序列，均值和方差分别为：

e{wk}＝0,e{wkwj^t}＝qkδkj,e{vk}＝0,e{vkvj^t}＝rkδkj，cov{wivj}＝0；

其中，e、cov分别表示均值和方差、qk为系统噪声方差阵；rk为量测噪声方差矩阵，δkj是δ函数；

步骤二：根据常规卡尔曼滤波算法，建立模糊自适应滤波算法：

首先更新状态一步预测值及其均方误差，利用k时刻量测信息zk和一步预测计算新息序列rk，求取增益矩阵和滤波方程；

一步预测：

预测均方误差：

增益矩阵：

新息序列：

滤波方程：

滤波均方误差：pk＝(i-kkhk)pk/k-1

其中，为滤波过程中对状态向量xk的估计量，为状态一步预测，kk为滤波增益，pk为滤波误差的协方差阵，pk/k-1为预测误差协方差阵，rk为新息序列，i为单位矩阵；

步骤三：确定模糊推理系统的输入输出参数：

定义残差方差的理论值为和残差方差的实际值为设计两个模糊推理系统(fis)，均为单输入单输出模式。将理论值和实际值的比值rork作为模糊推理系统1的输入，输出为αk；理论值和实际值的差值dork作为模糊推理系统2的输入，输出为b；

残差方差的理论值：

残差方差的实际值：

理论值和实际值的比值：

理论值和实际值的差值：

其中，为对最新的n个残差向量方差求平均值(n由经验选取，一般在10～30之间，主要起平滑作用)，i0＝k-n+1；ri即为残差序列；tr(·)表示对矩阵求迹；

步骤四：设计模糊推理系统：

模糊推理系统1的输入为上一步求出的rork，输出为αk，对输入输出模糊化，两者的隶属度函数均为三角形隶属度函数，反模糊化采用重心法，隶属度函数如图3，其模糊控制规则为：

ifrork∈d(decrease),thenαk∈d(decrease)

ifrork∈m(maintain),thenαk∈m(maintain)

ifrork∈i(increase),thenαk∈i(increase)

模糊推理系统2的输入为上一步求出的dork，输出为b，对输入输出模糊化，两者的隶属度函数均为三角形隶属度函数，反模糊化采用重心法，隶属度函数如图4，其模糊控制规则为：

ifdork∈n(negtive),thenb∈i(increase)

ifdork∈z(zero),thenb∈m(maintain)

ifdork∈p(positive),thenb∈i(increase)

当模型准确时，和的值趋近于相等，rork的值在1附近，自适应调整系数使得维持不变。如果量测噪声少量增大，则随之增大，进而rork增大，自适应调整系数增大(此时，b由fis2控制的较小，防止震荡)，则调整到合理值。如果量测噪声增大较大，则随之增大，进而rork增大，自适应调整系数增大(此时，b由fis2适量增大，减少过渡时间)，则调整到合理值。量测噪声减小的过程与增大过程类似。模糊自适应滤波器的原理框图如图1所示。

步骤五：更新量测噪声估计值：

更新量测噪声估计值，具体表达式为：

量测噪声估计值：

其中，表示第k步的量测噪声估计值，αk表示量测噪声的调整系数，b为指数调整系数，表示对αk的放大程度。若b>1，表示放大αk对的调整作用，这样量测噪声变化时，可以在较少的步数内迅速逼近真实值。若b<1，表示缩小αk对的调整作用。b的取值过大，则可能导致以真实量测噪声值为中心作小幅度震荡；b取值过小，则会导致调整到真实值的过渡时间稍长。

步骤六：重复执行滤波过程：

不断重复以上步骤二、三、五过程,利用自适应滤波方法对量测噪声进行在线估计，并实时修正捷联惯导系统输出的导航信息,直至sins/gps组合导航过程结束。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

本发明的有益效果可以通过matlab和fuzzy工具箱仿真实验进行验证，在0～250s区间保持初始模型不变，在251～500s区间将量测噪声变为初始值的9倍。分别用常规卡尔曼滤波和本发明提出的滤波方法对两段过程进行仿真。仿真曲线如图5所示。sins/gps组合导航系统中仿真参数设置如下：

初始位置为经度120°、纬度60°，初始速度为东向14m/s、北向14m/s，初始东北天平台角误差分别为2′、2′、5′，初始速度误差为0.01m/s，ins初始经度和纬度误差为2′。采样间隔为1s，仿真时间为500s。仿真开始条件为：p0＝diag[2²,2²,5²,0.01²,0.01²,2²,2²]，r0＝diag[0.2²,0.2²,2²,2²]。

仿真结果表明，采用传统的卡尔曼滤波，在0～250s保持随机噪声初始模型不变时，误差较小。251～500s后，随机噪声模型突变，量测噪声的方差发生了变化，仍然采用初始的量测噪声方差值滤波，滤波结果会随着噪声的增大而出现更大的波动，甚至可能发散。而采用本发明的滤波方法，后半段量测噪声突变时，滤波过程未发生明显变化，波形较为平稳，位置误差保持在较低水平。本发明提出的模糊自适应滤波方法通过在线调整量测噪声的方差阵，能很好地适应载体在不确定环境下的工作状态。即使系统量测噪声发生了变化，也不会影响滤波器的估计结果，仍然有很高的估计精度与很好的自适应性，具有广阔的工程应用前景。