CN100462685C - 一种sins/gps组合导航系统的自适应加权反馈校正滤波方法 - Google Patents

Abstract

本发明涉及一种SINS/GPS组合导航系统的自适应加权反馈校正滤波方法，其特征是首先利用GPS提供的位置和速度观测量，通过卡尔曼滤波方法估计SINS的位置误差、速度误差、姿态误差和器件误差，然后采用系统可观测度分析方法计算系统中各个状态量的可观测度，再将归一化处理后的系统状态可观测度作为加权反馈因子，对SINS系统进行自适应加权反馈校正。本发明具有精度高、不易发散的优点，可在系统不完全可观测的情况下有效地提高飞机、导弹、舰船或地面车辆用SINS/GPS组合导航系统的导航精度。

Description

一种SINS/GPS组合导航系统的自适应加权反馈校正滤波方法

技术领域

本发明涉及一种SINS/GPS组合导航系统信息融合的滤波方法，可用于提高飞机、导弹、舰船或地面车辆用SINS/GPS组合导航系统的导航精度。

背景技术

捷联惯性导航系统(SINS)是一种完全自主的导航系统，可以连续、实时地提供位置、速度和姿态信息，其短时精度很高，且具有隐蔽性好，不受气候条件限制等优点，因而广泛应用于航空、航天、航海等领域。但是，SINS误差随时间增长，因此常与全球卫星定位系统(GPS)组合构成SINS/GPS组合导航系统。

在SINS/GPS组合导航系统中，为了提高组合导航系统的精度，现有方法常常采用反馈校正卡尔曼滤波方法进行信息融合，其本质是将SINS的位置误差、速度误差、姿态误差和器件误差作为状态量，将GPS提供的位置和速度作为观测量，通过最优估计的方法将SINS的各种误差估计出来，然后进行反馈校正。但是，由于SINS/GPS组合导航系统在通常情况下是不完全可观测的，一些状态量的可观测度很小，导致这些状态的估计值精度很低。将这些精度很低的状态估值完全反馈到SINS内部，导致SINS内部误差变大，最终组合导航系统精度下降。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种SINS/GPS组合导航系统的自适应加权反馈校正滤波方法，该方法在组合导航系统不完全可观测的情况下有效地提高导航精度。

本发明的技术解决方案为：一种SINS/GPS组合导航系统的自适应加权反馈校正滤波方法，其特点在于包括下列步骤：

(1)建立SINS的误差方程，利用GPS提供的位置和速度信息作为观测量，通过卡尔曼滤波方法估计SINS的误差。

(2)通过系统可观测度分析方法计算SINS/GPS组合导航系统各个状态量的可观测度，上述的系统可观测度分析方法为基于奇异值分解的系统可观测度分析方法，该方法的具体步骤为：

(a)通过分段线性定常的方法计算条带可观测矩阵(SOM)Q_s(r)，

$Q_s\left(r\right)=\left[\begin{array}{c}{Q}_1\\ Q_2\\ \·\·\·\\ Q_r\end{array}\right]$

其中对应每一时间段j的可观测矩阵定义为：

(b)对条带式可观测矩阵Q_s(r)阵进行奇异值分解，得：

Q_s(r)＝U*S*V^T

其中U＝[u₁，u₂，…，u_m]，V＝[v₁，v₂，…，v_m]都是正交矩阵。

$S=\left[\begin{array}{cc}{\mathrm{\Λ}}_{r\×r}& 0\\ 0& 0\end{array}\right]$

其中Λ＝diag(σ₁，σ₂，…，σ_r)，σ₁>σ₂>…>σ_r>0称为矩阵Q_s(r)的奇异值。

(c)设初始状态为X(t₀)(n维)，量测值为Z(m1维)，则

$Z=Q_S\left(r\right)*X\left(t_0\right)=\left({\mathrm{USV}}^T\right)X\left(t_0\right)=\left(\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\σ}}_i{u}_i{v}_i^T\right)X\left(t_0\right)$

即

$Z=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\σ}}_i\left(v_i^{T}X\left(t_0\right)\right)*u_i$

可得：

$X\left(t_0\right)=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}\left(\frac{u_i^T*Z}{{\mathrm{\σ}}_i}\right)v_i$

(d)对矩阵

进行分析，观察它的各列元素的大小，判断出每一个奇异值σ_i对应的初始状态向量X(t_0，i)，奇异值σ_i的大小直接表明了状态向量X(t_0，i)可观测程度的高低。

(3)采用归一化处理方法对的每个系统状态的可观测度进行归一化处理，然后将归一化处理后的系统状态可观测度作为加权反馈因子，采用自适应加权反馈校正方法对SINS系统进行反馈校正。

在上述的SINS/GPS组合导航系统的自适应加权反馈校正滤波方法中，自适应加权反馈校正方法可以表示为

其中，ΔY_i为系统第i个状态量ΔX_i对应的反馈量；k_i为归一化处理后状态量ΔX_i的可观测度；

为状态量ΔX_i的估值。

上述的归一化处理方法可以表示为 $k_i=\frac{{\mathrm{\σ}}_i}{{\mathrm{\σ}}_{\mathrm{si}}}.$ 其中，k_j为归一化处理后第i个系统状态的可观测度；σ_i为归一化处理之前的第i个系统状态的可观测度；σ_si为载体做“S”形机动时第i个系统状态的可观测度，即第i个系统状态的最大可观测度。

本发明的原理是：在SINS/GPS组合导航系统中，现有方法通常采用反馈校正卡尔曼滤波方法将SINS和GPS两个子系统的数据进行信息融合，以获得更高精度的导航结果，其基本原理是将SINS/GPS组合导航系统的误差(位置误差、速度误差、姿态误差和器件误差等)作为系统的状态量，将GPS提供的位置和速度信息作为观测量，采用卡尔曼滤波方法估计出这些误差(状态量)，并把估计出来的误差作为反馈量在系统中进行校正，以修正系统的误差，提高系统的导航精度。

但是，SINS/GPS组合导航系统在大多情况下都是不完全可观测的，只有当载体做“S”形机动时，系统的可观测度达到最大，SINS/GPS组合导航系统才转变为完全可观测系统。而SINS/GPS组合导航系统的可观测度直接决定了卡尔曼滤波方法的估计精度。有些系统状态不可观测或可观测度很低，其估计精度也很低，若将这些精度很低的状态估值反馈，反而降低了组合导航系统的精度。因此，只有根据每个系统状态的可观测度的决定其反馈量，建立系统状态量的反馈量与可观测度的定量关系，根据每个系统状态的可观测度大小判断反馈量的大小，才能从根本提高组合导航系统的精度。

因此，所述的一种SINS/GPS组合导航系统自适应加权反馈校正滤波的基本原理是：首先，将SINS/GPS组合导航系统的误差作为状态量，建立系统的状态方程，将GPS提供的位置和速度信息作为观测量，通过卡尔曼滤波方法估计SINS/GPS组合导航系统的误差(状态量)。然后，采用系统可观测度分析方法计算每个状态量的可观测度，再将归一化处理后的系统状态可观测度作为加权反馈因子，将加权反馈因子与状态估值的积作为反馈量，进行自适应反馈校正。由于自适应加权反馈校正滤波根据每个系统状态的可观测度的决定其反馈量，建立系统状态量的反馈量与可观测度的定量关系，因此从根本提高了组合导航系统的精度。

本发明与现有技术相比的优点在于：本发明建立系统状态量的反馈量与可观测度的定量关系，将归一化处理后的系统状态可观测度作为加权反馈因子，对SINS系统进行自适应加权反馈校正，可在系统不完全可观测的情况下有效地提高飞机、导弹、舰船或地面车辆用SINS/GPS组合导航系统的导航精度。

附图说明

图1为本发明的原理框图；

图2为本发明的卡尔曼滤波基本算法的解算流程图。

具体实施方式

如图1、2所示，本发明的具体实施方法如下：

(1)建立SINS/GPS组合导航系统的数学模型，包括系统状态方程和量测方程，分别如式1和式2所示。

系统状态方程：

$\stackrel{.}{X}=\mathrm{FX}+\mathrm{GW}---\left(1\right)$

其中，X为系统状态矢量，W为系统噪声矢量，F为系统转移矩阵，G为噪声转换矩阵：

$X={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\mathrm{\δv}}_x& \mathrm{\δ}{v}_y& {\mathrm{\δv}}_z& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T$

$F=\left[\begin{array}{cc}{F}_{\mathrm{INS}}& F_S\\ o_{6\×6}& F_M\end{array}\right],$ $F_S=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],$ F_M＝[O_6×15]， $G=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]$

系统的量测方程

z＝HX+ζ               (2)

其中：z为观测矢量，H为观测矩阵，ζ为量测噪声：

z＝[δL δλ δh δV_E δV_N δV_U]^T

$H=\left[\begin{array}{ccc}{0}_{3\×6}& I_{3\×3}& 0_{3\×6}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×9}\end{array}\right]$

$\mathrm{\ζ}={\left[\begin{array}{cccccc}{\mathrm{\ζ}}_L& {\mathrm{\ζ}}_{\mathrm{\λ}}& {\mathrm{\ζ}}_h& {\mathrm{\ζ}}_{V_E}& {\mathrm{\ζ}}_{V_N}& {\mathrm{\ζ}}_{V_U}\end{array}\right]}^T$

(2)在SINS/GPS组合导航系统数学模型的基础上，采用卡尔曼滤波方法进行信息融合，获得SINS/GPS组合系统的状态量估计值

卡尔曼滤波算法的流程图如图2所示，该算法的公式编排如式(3)～(7)所示。

状态一步预测方程

${\stackrel{\mathrm{\Λ}}{X}}_{k/k-1}={\mathrm{\φ}}_{k,k-1}{\stackrel{\mathrm{\Λ}}{X}}_{k-1}---\left(3\right)$

状态估值计算方程

${\stackrel{\mathrm{\Λ}}{X}}_k={\stackrel{\mathrm{\Λ}}{X}}_{k/k-1}+K_k(Z_k-H_k{\stackrel{\mathrm{\Λ}}{X}}_{k/k-1})---\left(4\right)$

滤波增量方程

${\stackrel{\mathrm{\Λ}}{K}}_k={\stackrel{\mathrm{\Λ}}{P}}_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(5\right)$

一步预测均方误差方程

${\stackrel{\mathrm{\Λ}}{P}}_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(6\right)$

估计均方误差方程

${\stackrel{\mathrm{\Λ}}{P}}_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(7\right)$

(3)采用基于奇异值分解的可观测度分析方法计算SINS/GPS组合导航系统中各个状态量的可观测度。

分段线性定常系统(PWCS)可观测性分析方法是基于奇异值分解的可观测度分析方法的基础。SINS/GPS组合导航系统是时变系统，判断定常系统可观测性的分析方法都不适用，分段线性定常系统(PWCS)可观测性分析方法是专门用于判断时变系统可观测性的一种方法。在一个足够小的时间区间内，如果线性时变系统的系数矩阵变化量可以忽略不计，那么在该时间区间内就可以把时变系统当作定常系统处理，这样的系统称为分段式定常系统。

一个离散的PWCS可用如下模型表示：

X(k+1)＝F_jX(k)+G_jU(k)+Γ_jw(k)

                                        (8)

Z_j(k)＝H_jX(k)

式中X(k)∈Rⁿ，F_j∈R^n×n，G_j∈R^n×s，U(k)∈R^s，w(k)∈R^l，Γ_j∈R^n×l，Z_j(k)∈R^m，H_j∈R^m×n。j＝1，2，……，r，表示系统分段间隔序号。对每个时间段j，矩阵F_j、G_j和H_j都是恒定的，但对应不同的时间段，每个矩阵可以是不同的。系统总的可观测性矩阵(TOM)和条带化可观测性矩阵(SOM)分别表示为：

$Q_{\mathrm{tom}}\left(r\right)=\left[\begin{array}{c}{Q}_1\\ Q_2{F}_1^{n-1}\\ \·\·\·\\ Q_r{F}_{r-1}^{n-1}{F}_{r-2}^{n-1}...F_1^{n-1}\end{array}\right]---\left(9\right)$

$Q_{\mathrm{som}}\left(r\right)=\left[\begin{array}{c}{Q}_1\\ Q_2\\ \·\·\·\\ Q_r\end{array}\right]---\left(10\right)$

其中对应每一时间段j的可观测矩阵定义为：

根据系统方程和量测方程以及上述可观测矩阵的定义，由初值表示的系统输出为：

Z＝Q_tom(r)*X(t₀)              (12)

若矩阵Q_tom(r)的秩等于n，则由上式可知，X(t₀)有唯一确定的解，表明系统状态是完全可观测的。显然，直接利用Q_tom(r)阵研究离散PWCS的可观测性计算量相当大，而采用SOM来代替TOM来分析系统的可观测性，可以使问题得到简化。

用SOM矩阵代替TOM矩阵，随着时间段的增加，可观测性矩阵的维数仍然很高，对其实施奇异值分解的工作量也是相当大的。因此，这里采用一种改进的基于奇异值分解的系统可观测度分析方法。

设某时间段动态系统的可观测性矩阵为Q_som，初始状态为X(t₀)(n维)，量测值为Z(m1维)，则

Z＝Q_som*X(t₀)                (13)

对Q_som阵进行奇异值分解，得：

Q_som＝U*S*V^T              (14)

其中U＝[μ₁，u₂，…，u_m]，V＝[v₁，v₂，…，v_m]都是正交矩阵。

$S=\left[\begin{array}{cc}{\mathrm{\Λ}}_{r\×r}& 0\\ 0& 0\end{array}\right]---\left(15\right)$

其中Λ＝diag(σ₁，σ₂，…，σ_r)，σ₁>σ₂>…>σ_r>0称为矩阵Q_som的奇异值。将式(14)带入式(13)中，得：

$Z=\left({\mathrm{USV}}^T\right)X\left(t_0\right)=\left(\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\σ}}_i{u}_i{v}_i^T\right)X\left(t_0\right)---\left(16\right)$

即

$Z=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\σ}}_i\left(v_i^{T}X\left(t_0\right)\right)*u_i---\left(17\right)$

根据式(16)可得：

$X\left(t_0\right)=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}\left(\frac{u_i^T*Z}{{\mathrm{\σ}}_i}\right)v_i---\left(18\right)$

传统的分析方法是，根据式(18)计算每一个奇异值σi对应的初始状态向量X(t_0，i)。从数值上看，较大的奇异值可以获得较好的状态估计，反之，对于特别小的奇异值，可能会引起多个X(t₀)的奇异，最终落入不可观测空间内。

从线性系统理论的角度分析，状态量X(t₀)的可观测性应该只取决于系统结构，而与观测量Z无关，根据式(16)对矩阵

进行分析，观察它的各列元素的大小，就可以判断出每一个奇异值σ_i对应的初始状态向量X(t_0，i)，这种改进的可观测度分析方法不仅计算简单，更重要的是可以在没有实验测得量测数据的情况下分析系统状态的可观测度。

(4)将系统状态量的可观测度进行归一化处理。

通常，如果某一个系统状态可观测，该系统状态对应的可观测度即为1。但是在传统的系统可观测度分析方法中，未对每个系统状态的可观测度进行归一化处理。由于每个系统状态量纲不同，导致每个系统状态的可观测度大小不一，甚至数量级不同。最为明显的是，当载体进行“S”形机动时，此时SINS/GPS组合导航系统为完全可观测系统，每个状态量的可观测度都应该为1。但是采用传统的系统可观测度分析方法计算出来的每个系统状态的可观测度，由于量纲的原因大多不等于1，有的很大，有的很小。为了使得可观测度的定义能更确切地表示系统状态的可观测程度，采用可观测度的归一化处理方法，如式(19)所示。

$k_i=\frac{{\mathrm{\σ}}_i}{{\mathrm{\σ}}_{\mathrm{si}}}---\left(19\right)$

其中，k_i为归一化处理后第i个系统状态的可观测度；σ_i为归一化处理之前的第i个系统状态ΔX_i的可观测度；σ_si为载体做“S”形机动时第i个系统状态ΔX_i的可观测度，即第i个系统状态ΔX_i的最大可观测度。

(5)将归一化处理后的系统可观测度作为加权反馈因子，将加权反馈因子与状态估值的乘积作为反馈量，对SINS/GPS组合导航系统进行自适应加权反馈校正。

传统的反馈校正方法不管系统状态估值的精度，直接进行全状态反馈。当系统中部分系统状态可观测度低时，这些系统状态的估计精度也很低，将这些精度很低的系统状态估值反馈到系统中，最终导致系统的精度下降。

由于卡尔曼滤波方法估计的精度归根到底是由每个系统状态的可观测度决定的，只有根据每个系统状态的可观测度的决定其反馈量，建立系统状态量的反馈量与可观测度的定量关系，才能从根本提高组合导航系统的精度。因此，自适应加权反馈校正方法可以表示为：

其中，ΔY_i为系统第i个状态量ΔX_i对应的反馈量；k_i为归一化处理后状态量ΔX_i的可观测度；

为状态量ΔX_i的估值。

最后，将ΔY_i在SINS/GPS组合导航系统中进行反馈，校正SINS/GPS组合导航系统的误差，提高了组合导航系统的精度。

Claims (2)

1.一种SINS/GPS组合导航系统的自适应加权反馈校正滤波方法，其特征在于包括下列步骤：

(1)建立SINS的误差方程，利用GPS提供的位置和速度信息作为观测量，通过卡尔曼滤波方法估计SINS的误差；

(2)通过系统可观测度分析方法计算SINS/GPS组合导航系统各个状态量的可观测度，所述的系统可观测度分析方法为基于奇异值分解的系统可观测度分析方法；

(3)采用归一化处理方法对的每个系统状态的可观测度进行归一化处理，然后将归一化处理后的系统状态可观测度作为加权反馈因子，采用自适应加权反馈校正方法对SINS系统进行反馈校正，所述的自适应加权反馈校正方法公式为： $\mathrm{\Δ}{Y}_i=k_i\mathrm{\Δ}{\hat{X}}_i,$ 其中，ΔY_i为系统第i个状态量ΔX_i对应的反馈量，

为状态量ΔX_i的估值， $k_i=\frac{{\mathrm{\σ}}_i}{{\mathrm{\σ}}_{\mathrm{si}}}$ 为归一化处理后状态量ΔX_i的可观测度，σ_i为归一化处理之前的第i个系统状态ΔX_i的可观测度，σ_si为载体做“S”形机动时第i个系统状态ΔX_i的可观测度，即第i个系统状态的最大可观测度。

2.根据权利要求1所述的一种SINS/GPS组合导航系统的自适应加权反馈校正滤波方法，其特征在于：所述的基于奇异值分解的系统可观测度分析方法的具体步骤为：

(a)通过分段线性定常的方法计算条带可观测矩阵Q_s(r)，

$Q_s\left(r\right)=\left[\begin{array}{c}{Q}_1\\ Q_2\\ \·\·\·\\ Q_r\end{array}\right]$

其中对应每一时间段j的可观测矩阵定义为：

$Q_j=\left[\begin{array}{cccc}{\left(H_j\right)}^T& {\left(H_j{F}_j\right)}^T& \·\·\·& {\left(H_j{F}_j^{n-1}\right)}^T\end{array}\right],$

其中，H_j和F_j分别为第j个时间段的系统观测矩阵和状态转移矩阵；

(b)对条带式可观测矩阵Q_s(r)阵进行奇异值分解，得：

Q_s(r)＝U*S*V^T

其中U＝[u₁，u₂，…，u_m]，V＝[v₁，v₂，…，v_m]都是正交矩，S为对角阵：

$S=\left[\begin{array}{cc}{\mathrm{\Λ}}_{r\×r}& 0\\ 0& 0\end{array}\right]$

其中Λ＝diag(σ₁，σ₂，…，σ_r)，σ₁>σ₂>…>σ_r>0称为矩阵Q_s(r)的奇异值；

(c)设初始状态为X(t₀)，为n维，量测值为Z，为m1维，n>0，m1>0则 $Z=Q_s\left(r\right)*X\left(t_0\right)=\left({\mathrm{USV}}^T\right)X\left(t_0\right)=\left(\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\σ}}_i{u}_i{v}_i^T\right)X\left(t_0\right)$

即 $Z=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\σ}}_i\left(v_i^{T}X\left(t_0\right)\right)*u_i$

可得： $X\left(t_0\right)=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}\left(\frac{u_i^T*Z}{{\mathrm{\σ}}_i}\right)v_i$

(d)对矩阵

进行分析，观察它的各列元素的大小，判断出每一个奇异值σ_i对应的初始状态向量X(t_0，i)，奇异值σ_i的大小直接表明了状态向量X(t_0，i)可观测程度的高低。

Images