基于故障容错卡尔曼滤波的组合导航方法与流程

本发明属于组合导航技术领域，具体涉及一种基于故障容错卡尔曼滤波的组合导航方法。

背景技术：

现代科学技术已经能够给航行体提供了多种导航设备，如惯性导航系统(ins)，多普勒(doppler)雷达导航系统，全球定位系统(gps)。这些导航设备各有优缺点，精度和成本也大不相同。其中惯性导航系统是一种不依赖外部信息，不向外辐射能量的自主式导航系统，而且能提供多种导航参数(位置，速度，姿态)。作为惯性导航系统的一种——捷联式惯性导航系统(sins)正在得到越来越广泛地应用。将惯性导航系统与其他导航系统适当地组合起来，可取长补短，大大提高导航的精度，也可以大大降低惯导系统的成本。所以，组合导航系统已经成为一种趋势。

利用卡尔曼滤波技术对组合导航系统进行最优组合，一种是集中式卡尔曼滤波，另一种是分散化卡尔曼滤波(即联邦卡尔曼滤波)，由于集中式卡尔曼滤波器的状态维数较高，计算负担重，不利于滤波的实时运行，而且容错性能很差。联邦卡尔曼滤波由于设计的灵活性，计算量小，容错性能好，而受到广泛的应用。

传统的联邦卡尔曼容错系统，是在子滤波器往主滤波器输出信息时进行一次故障检测，一旦有故障则对其进行隔离，无故障时再送往主滤波器进行信息融合，得到最佳的导航估计。随着多种导航系统的出现，以及人们对导航精度要求不断提高，提高卡尔曼组合导航系统的容错性能已成为组合导航技术领域主要的研究课题之一。

关于故障容错与联邦卡尔曼的文献有很多，其中张海龙等的《联邦卡尔曼滤波在捷联惯导/全球定位/里程仪组合导航系统中的优化设计》(探测与控制学报，2009)，王鹏等的《惯导系统/全球定位系统/多普勒计程仪的船舶组合导航滤波算法研究》(科学技术与工程，2014)，上述两篇文献讲述了联邦卡尔曼滤波的原理及其特点，并进行了仿真设计，证明了联邦卡尔曼滤波器在不组合导航系统应用中的可行性和优越性，但是系统采用的是联邦卡尔曼的一般结构，其抗干扰能力较弱，容错性能有待改善。本发明是将容错卡尔曼滤波器应用到联邦卡尔曼系统结构里。针对因出现故障而隔离的sins/gps子系统，增加了一个故障检测环节，和一个容错卡尔曼滤波器，一旦出现故障，回到故障点，重新进行容错卡尔曼滤波。以此来提高对该子系统数据的利用程度，从而提高整体系统的鲁棒性能。

技术实现要素：

本发明的目的在于提供一种基于故障容错卡尔曼滤波的组合导航方法。

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

基于故障容错卡尔曼滤波的组合导航方法，其特征在于，包含以下步骤：

步骤1：选择作为状态变量，

其中：δλ：惯性导航系统纬度和经度误差；

δvx,δvy：惯性导航系在地理坐标系东向和北向的速度误差；

εxεyεz：陀螺一阶马尔可夫过程随机误差；

αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；

然后根据系统特性，列写出系统的状态方程：

sins/gps子系统选择位置和速度作为量测量，

即

式中：λi，vix，viy分别为惯导解算的纬度，经度，东向速度，北向速度；λg，vgx，vgy分别为gps测得纬度，经度，东向速度，北向速度；

sins/doppler子系统选择选择速度作为量测量，

即

式中：vix，viy分别为惯导解算的东向速度，北向速度；vdx，vdy，分别为多普勒测得的东向速度，北向速度；

分别进行离散化处理得到：

式中：φⁱk/k-1，hik，γi,k-1，wi,k-1，vi,k分别为第i个子滤波器的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵；

步骤2：构建常规卡尔曼滤波器进行状态估计；常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测和方差的一步预测pk/k-1；利用测量更新求出状态的估计值和方差的估计值pk；具体过程如下：

状态一步预测方程：

状态估值计算方程：

滤波增益方程：

一步预测均方差方程：

估计均方差方程：

步骤3：构建故障检测函数进行检测：

定义故障检测函数

其中：rk为残差，即

e{rk}＝μ,e{[rk-μ][rk-μ]^t}＝ak；

λk是服从自由度为m的χ²分布，即λk～χ²(m)；m为量测zk的维数；

故障判定准则为：

其中，td为预先设置的门限值，在给定误警率pf时，由χn分布来确定；

步骤4：构建容错卡尔曼滤波器进行状态估计：根据系统的代价函数确定实数γ，γ是反映滤波稳健程度的参数，取值为2.5，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计；

对于

其中，tk为满秩阵，yk是状态变量xk的一种线性组合；

进行如下递推估计：

其中：

步骤5：将子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计：

其中：分别为子滤波器1、子滤波器2的状态估计；

p1，p2分别为子滤波器1、子滤波器2的方差估计；

为全局状态估计，pg为全局状态估计协方差阵；

步骤6：用联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：采用间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就得到最终导航参数的估计值。

与现有技术相比，本发明的有益效果在于：

本发明是将容错卡尔曼滤波器应用到联邦卡尔曼系统结构里。针对因出现故障而隔离的sins/gps子系统，增加了一个故障检测环节，和一个容错卡尔曼滤波器，一旦出现故障，回到故障点，重新进行容错卡尔曼滤波。以此来提高对该子系统数据的利用程度，从而提高整体系统的鲁棒性能。

附图说明

图1为本发明的系统结构图；

图2为有重置的联邦卡尔曼滤波器的一般结构；

图3为无重置的联邦卡尔曼滤波器的一般结构。

具体实施方式

下面结合附图对本发明作出详细说明：

具体实施例一：

本发明的目的在于提供基于故障容错卡尔曼滤波的组合导航方法。针对的是sins/gps/doppler多组合导航系统，该系统的结构图如图1所示。其中图2为有重置的联邦卡尔曼滤波器结构和图3为无重置的联邦卡尔曼滤波器结构。本发明是在无重置的联邦卡尔曼滤波器结构上进行优化设计，针对其中的一个子滤波器增加了一个容错卡尔曼滤波器和一个故障检测函数。

本发明采用的技术方案包括下列步骤：

步骤1、选择作为状态变量，

其中：δλ：惯性导航系统维度和经度误差；

δvx,δvy：惯性导航系在地理坐标系东向和北向的速度误差；

εxεyεz：陀螺一阶马尔可夫过程随机误差；

αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；

然后根据系统特性，列写出系统的状态方程：

sins/gps子系统选择位置和速度作为量测量，

即

式中：λi，vix，viy分别为惯导解算的纬度，经度，东向速度，北向速度；

λg，vgx，vgy分别为gps测得纬度，经度，东向速度，北向速度。

sins/doppler子系统选择选择速度作为量测量，

即

式中：vix，viy分别为惯导解算的东向速度，北向速度；

vdx，vdy，分别为多普勒测得的东向速度，北向速度。

分别进行离散化处理得到

式中：φⁱk/k-1，hik，γi,k-1，wi,k-1，vi,k为第i个子系统的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵。

步骤2、构建常规卡尔曼滤波器进行状态估计。常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测和方差的一步预测pk/k-1；利用测量更新求出状态的估计值和方差的估计值pk。具体过程如下：

状态一步预测方程：

状态估值计算方程：

滤波增益方程：

一步预测均方差方程：

估计均方差方程：

步骤3、构建构建故障检测函数进行检测：

采用残差χ²检验法，定义滤波器的残差为：式中的预报值为

通过对残差rk的均值的检验就可以确定系统是否发生了故障。

对rk作以下二元假设：

h0，无故障时：

h0，有故障时：e{rk}＝μ,e{[rk-μ][rk-μ]^t}＝ak

定义故障检测函数：

式中，λk是服从自由度为m的χ²分布，即λk～χ²(m)。m为量测zk的维数。

故障判定准则为：

其中，td为预先设置的门限值，可由误警率pf确定(当限定误警率pf＝α时，由pf＝p[λk＞td/h0]＝α即可求出td)

步骤4、中构建容错卡尔曼滤波器进行状态估计。通过步骤3对常规卡尔曼滤波的sins/doppler的估计进行检测，若出现故障则回到故障点，进入容错卡尔曼滤波器进行状态估计。下面构建容错卡尔曼滤波器，根据系统的代价函数确定实数γ(γ是反映滤波稳健程度的参数)，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计。

对于离散系统模型：

其中，tk为满秩阵，yk是状态变量xk的一种线性组合。

设tk(ff)表示将输入序列映射至滤波误差序列{e(k)}的传递函数。定义tk(ff)的h∞范数为：

其中e(k)为滤波误差，即

给定实数γ＞0，使得||tk(ff)||∞＜γ。并将γ代入以下递推公式进行状态估计：

其中：式中，γ根据实际情况取值。

步骤5、在各个子系统的状态估计确认无故障后，将子滤波器状态的局部估计值及其协方差阵送入主滤波器进行信息融合，得到全局状态估计。如图2所示，是有重置联邦卡尔曼的一般结构。联邦卡尔曼滤波器由六种结构，不同的结构以及信息分配系数的确定对联邦滤波器的特性(容错性，最优性，计算量等)会产生不同的影响。本发明采用一种没有重置且βm＝0,βi＝1/n的结构如图3所示，这种结构不需将主滤波器的全局估计值及其协方差pg反馈到子滤波器，由于各子滤波器不会互相影响，可以提高系统的容错性，

其中：分别为子滤波器1、子滤波器2的状态估计；

p1，p2分别为子滤波器1、子滤波器2的方差估计；

为全局状态估计，pg为全局状态估计协方差阵。

步骤6、将联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：本发明采用的是间接法滤波，将误差作为状态变量，经过联邦卡尔曼滤波系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就可以得到最终导航参数的估计值。

具体实施例二：

本发明的目的在于提供一种基于故障容错卡尔曼滤波的组合导航方法。针对的是sins/gps/doppler多组合导航系统，本发明采用的技术方案包括下列步骤：

步骤1、选择作为状态变量，

其中：δλ：惯性导航系统纬度和经度误差；

δvx,δvy：惯性导航系在地理坐标系东向和北向的速度误差；

εxεyεz：陀螺一阶马尔可夫过程随机误差；

αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；

然后根据系统特性，列写出系统的状态方程：

sins/gps子系统选择位置和速度作为量测量，

即

式中：λi，vix，viy分别为惯导解算的纬度，经度，东向速度，北向速度；

λg，vgx，vgy分别为gps测得纬度，经度，东向速度，北向速度。

sins/doppler子系统选择选择速度作为量测量，

即

式中：vix，viy分别为惯导解算的东向速度，北向速度；

vdx，vdy，分别为多普勒测得的东向速度，北向速度。

分别进行离散化处理得到：

式中：φⁱk/k-1，hik，γi,k-1，wi,k-1，vi,k分别为第i个子滤波器的状态转移阵，量测阵，系统噪声驱动阵，系统噪声阵，量测噪声阵。

步骤2、构建常规卡尔曼滤波器进行状态估计。常规卡尔曼滤波过程分为时间更新和测量更新两个过程：利用时间更新求出状态的一步预测和方差的一步预测pk/k-1；利用测量更新求出状态的估计值和方差的估计值pk。具体过程如下：

状态一步预测方程：

状态估值计算方程：

滤波增益方程：

一步预测均方差方程：

估计均方差方程：

步骤3、中构建构建故障检测函数进行检测：

定义故障检测函数

其中：rk为残差，即

e{rk}＝μ,e{[rk-μ][rk-μ]^t}＝ak。

λk是服从自由度为m的χ²分布，即λk～χ²(m)。m为量测zk的维数。

故障判定准则为：

其中，td为预先设置的门限值，在给定误警率pf时，可由χn分布来确定。

步骤4、构建容错卡尔曼滤波器进行状态估计：根据系统的代价函数确定实数γ(γ是反映滤波稳健程度的参数，可以取值为2.5)，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计。

对于

其中，tk为满秩阵，yk是状态变量xk的一种线性组合。

进行如下递推估计：

其中：

步骤5、将子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计：

其中：分别为子滤波器1、子滤波器2的状态估计；

p1，p2分别为子滤波器1、子滤波器2的方差估计；

为全局状态估计，pg为全局状态估计协方差阵。

步骤6、将联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：本发明采用的是间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就可以得到最终导航参数的估计值。

具体实施例三：

本发明公开了一种基于故障容错卡尔曼滤波的组合导航方法。该方法针对的是针对的是惯性/卫星/多普勒组合导航系统，该系统由两个子滤波器和一个主滤波器组成。本发明对其中的惯性/卫星导航子系统进行容错设计，增加了一个容错卡尔曼滤波器，以提高其抗干扰能力。在系统状态模型，先验信息均已知的情况下，优先采用常规卡尔曼滤波器进行状态估计，然后进行一次故障检测，无故障时，直接送往主滤波器进行数据融合；若出现故障，则采用容错卡尔曼滤波器对该子滤波器进行替换，回到故障点重新进行状态估计。输出的数据再进行一次故障检测，无故障时，再送往主滤波器进行数据融合。有故障时，直接将该子系统隔离掉，从而降低了系统误判的风险，提高了对导航子系统数据的利用程度，从而提高了组合导航系统的鲁棒性。

基于故障容错卡尔曼滤波的组合导航方法，其特征在于：

步骤1、选择组合导航状态量列写状态方程，选择量测量列写量测方程，并进行离散化处理；

步骤2、构建常规卡尔曼滤波器进行状态估计；

步骤3、构建故障检测函数进行故障检测；

步骤4、构建容错卡尔曼滤波器进行状态估计；

步骤5、将各子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计；

步骤6、将联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数。

步骤1中选择作为参考系统的状态变量，

其中：δλ：惯性导航系统纬度和经度误差；

δvx,δvy：惯性导航系在地理坐标系东向和北向的速度误差；

εxεyεz：陀螺一阶马尔可夫过程随机误差；

αβλ：捷联惯性导航在地理坐标系下东，北，天向的平台误差角；

然后根据系统特性，列写出导航系统的状态方程。

sins/gps子滤波器选择位置和速度作为量测量，

即

式中：λi，vix，viy分别为惯导解算的纬度，经度，东向速度，北向速度；

λg，vgx，vgy分别为gps测得纬度，经度，东向速度，北向速度。

sins/doppler子滤波器选择选择速度作为量测量，

即

式中：vix，viy分别为惯导解算的东向速度，北向速度；

vdx，vdy，分别为多普勒测得的东向速度，北向速度。

然后把系统的状态方程和量测方程进行离散化处理，得到系统的状态空间模型。

步骤2中构建常规卡尔曼滤波器进行状态估计：卡尔曼滤波分为两个过程，时间更新和测量更新。利用时间更新求出状态的一步预测和方差的一步预测pk/k-1；利用测量更新求出状态的估计值和方差的估计值pk。

步骤3中构建构建故障检测函数进行检测：

定义故障检测函数：其中：rk为残差，ak为残差rk的方差。

λk是服从自由度为m的χ²分布，即λk～χ²(m)。m为量测zk的维数。

故障判定准则为：

其中，td为预先设置的门限值，在给定误警率pf时，可由χn分布来确定。

步骤4中构建容错卡尔曼滤波器进行状态估计：根据系统的代价函数确定实数γ(γ是反映滤波稳健程度的参数，可以取值为2.5)，并将其带入滤波递推公式中，得到确定的容错卡尔曼滤波器，从而进行状态估计。

步骤5中将子滤波器的数据送入主滤波器进行信息融合，得到全局状态估计：

其中：分别为子滤波器1、子滤波器2的状态估计；

p1，p2分别为子滤波器1、子滤波器2的方差估计；

为全局状态估计，pg为全局状态估计协方差阵。

步骤6中将联邦卡尔曼的全局误差的估计值去校正惯性导航系统输出的导航参数：本发明采用的是间接法滤波，将误差作为状态变量，经过联邦卡尔曼系统，得到误差的估计值，然后再反馈至惯导系统，用惯导系统输出的导航参数减去误差的估计值，就可以得到最终导航参数的估计值。