CN102426373A - 一种惯性/卫星组合导航系统开闭环混合修正方法 - Google Patents

Abstract

本发明属于导航技术领域，涉及一种惯性/卫星组合导航系统开闭环混合修正方法。本发明采用开闭环修正结合的方法对惯性卫星导航系统的误差量进行修正。在闭环修正中采用了自适应获得卡尔曼滤波器量测噪声方差值的方法，方法简便易行、实施难度较小，可有效抑制卫星导航数据的跳变异常、以及长时间不组合后进入组合状态时修正量的较大波动，提高了闭环修正的准确性和稳定性；在开环修正中对所采集的量测量实行每秒实时更新，修正频率与惯导数据总线频率相同，不同频率的闭环和开环修正采用按卫星导航接收机秒脉冲同步的方法，做到了无缝连接，误差修正迅捷且平滑，解决了闭环修正容易干扰开环修正的难题。

Description

一种惯性/卫星组合导航系统开闭环混合修正方法

技术领域

本发明属于导航技术领域，涉及一种惯性/卫星组合导航系统开闭环混合修正方法。

背景技术

目前，在导航系统中，基于系统噪声或量测噪声方差自适应计算的自适应卡尔曼滤波修正方法，普遍存在着计算量较大、程序编制较为复杂的问题。

文献[1]Weidong Ding，Jinling Wang and Chris Rizos.Improving AdaptiveKalman Estimation in GPS/INS Integration[J].THE JOURNAL OF NAVIGATION(2007)，60，517-529([1]Weidong Ding，Jinling Wang and Chris Rizos.在GPS/INS(惯性导航系统)组合中改善卡尔曼估计。导航杂志(美国)(2007)，60，P.517-P.529)。其主要方法是计算自适应噪声方差阵

分三个步骤：

1)先采用点数为m的滑动窗口计算新息(残差)的均方误差阵：

$E\left\{d_k{d}_k^T\right\}=\frac{1}{m}\underset{i=0}{\overset{m-1}{\mathrm{\Σ}}}{d}_{k-i}{d}_{k-i}^T---\left(1\right)$

其中的新息(残差)为：

$d_k=z_k-H_k{\hat{x}}_k^{-}---\left(2\right)$

状态一步预测为： ${\hat{x}}_k^{-}={\mathrm{\Φ}}_{k-1}{\hat{x}}_{k-1}---\left(3\right)$

z_k为量测值，H_k为量测方程系数矩阵，Φ_k-1为一步转移阵，

为状态估计值。

2)计算缩放比例系数：

$\mathrm{\α}=\frac{\mathrm{trace}\{\frac{1}{m}\underset{i=0}{\overset{m-1}{\mathrm{\Σ}}}{d}_{k-i}{d}_{k-i}^T-R_k\}}{\mathrm{trace}\left\{H_k{\hat{P}}_k^{-}{H}_k^T\right\}}---\left(4\right)$

其中

为一步预测均方误差阵，trace{}表示矩阵的迹。

3)计算自适应噪声方差阵：

${\hat{Q}}_k=Q_{k-1}\sqrt{\mathrm{\α}}---\left(5\right)$

上述(1)式和(4)式的计算都较为复杂，另外如何预先确定点数m也有难度，

只能整体按比例

变化，单独的系统噪声方差无法做自适应变化。

文献[2]Christopher Hide，Terry Moore and Martin Smith．Adaptive KalmanFiltering Algorithms for Integrating GPS and Low Cost INS[C]//2004IEEE：227-233([2]Christopher Hide，Terry Moore and Martin Smith.用于GPS和低成本INS组合的自适应卡尔曼滤波算法[C]//2004IEEE，P.227-P.233)。其主要内容是比较了三类自适应卡尔曼滤波算法。这三类算法为：

A.方差按比例缩放/过程噪声按比例缩放

方法一：

$P_k^{-}=S_k({\mathrm{\Φ}}_{k-1}{P}_{k-1}^{+}{\mathrm{\Φ}}_{k-1}^T+Q_{k-1})---\left(6\right)$

上述比例因子S_k≥1，是事先给定的，虽然简单，但按固定比例增大方差，不是严格意义上的“自适应”。

方法二：

先计算比例因子： $S_k\≥\frac{{v_k^{-}}^T{v}_k^{-}}{\frac{1}{N}\underset{j=0}{\overset{N-1}{\mathrm{\Σ}}}{v_{k-N+j}^{-}}^T{v}_{k-N+j}^{-}}---\left(7\right)$

上式中，N是预先给定的周期数，目的是由目前的第k向前推N个周期；

按照文献[2]的定义，是“卡尔曼滤波器的状态预测和新量测之间的差别”，实际上就是(2)式表示的新息(残差)。

如果将上述S_k用于(6)，则在GPS和低成本INS组合时回会出现不稳定的问题。可用于下式：

$P_k^{-}={\mathrm{\Φ}}_{k-1}{P}_{k-1}^{+}{\mathrm{\Φ}}_{k-1}^T+S_k{Q}_{k-1}$

可看到(7)式的计算较为复杂，且如何预先确定的周期数N(同文献[2]中的点数m相似)也有难度。

B.多个模型的自适应估计(MMAE，Multiple Model Adaptive Estimation)

此方法是使多个采用不同随机模型的卡尔曼滤波器同时运行，利用建立在新息(残差)上的概率密度函数来决定哪种卡尔曼滤波器的组合可以使用，第n个卡尔曼滤波器的概率密度函数为：

$f_n\left(z_n\right)=\frac{1}{\sqrt{{\left(2\mathrm{\π}\right)}^m\left|C_{v_k^{-}}\right|}}{e}^{-\frac{1}{2}{v}_k^{-}{C}_{v_k^{-}}^{-1}{v_k^{-}}^T}---\left(8\right)$

上式中m是量测量的数目，更新协方差阵的估计为：

$C_{v_k^{-}}=\frac{1}{N}\underset{j=k-N+1}{\overset{k}{\mathrm{\Σ}}}{v}_j^{-}{v}_j^{-T}---\left(9\right)$

第n个卡尔曼滤波器为正确的概率p_n(k)用下列迭代公式计算：

$p_n\left(k\right)=\frac{f_n\left(z_k\right)p_n(k-1)}{\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{f}_j\left(z_k\right)p_j(k-1)}---\left(10\right)$

对于N个卡尔曼滤波器，设在k时刻的第j个卡尔曼滤波器的估计值为

则k时刻的最优估计值为：

${\hat{x}}^{+}\left(k\right)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{p}_j\left(k\right){\hat{x}}_j^{+}\left(k\right)---\left(11\right)$

文献[2]中计算机的CPU为1.4GHz，利用mPosⁱ组合软件，7个卡尔曼滤波按顺序花费计算20ms，也可采用并行计算。文献[2]认为此方法的最大限制在于处理时间的显著增加。

C.自适应卡尔曼滤波器

这类算法是采用额外的方程来估计量测方程噪声协方差阵或者状态方程噪声协方差阵，是对传统卡尔曼滤波器的扩展。

其中一种算法是采用极大似然准则估计状态方程噪声的协方差阵：

${\hat{Q}}_k={\hat{C}}_{\mathrm{\Δ}{x}_k}+P_k^{+}-\mathrm{\Φ}{P}_{k-1}^{+}{\mathrm{\Φ}}^T---\left(12\right)$

式中

是状态修正协方差阵，其估计为

${\hat{C}}_{\mathrm{\Δ}{x}_k}\≈\frac{1}{N}\underset{j=k-N+1}{\overset{k}{\mathrm{\Σ}}}\mathrm{\Δ}{x}_j\mathrm{\Δ}{x}_j^T---\left(13\right)$

式中Δx_j为状态修正序列，定义为：

$\mathrm{\Δ}{x}_j=x_j^{-}-x_j^{+}$

使用替换其中K_k卡尔曼滤波器的增益阵，则可用更新协方差阵

来近似状态修正序列：

${\hat{C}}_{\mathrm{\Δ}{x}_k}\≈K_k{\hat{C}}_{v_k^{-}}{K}_k^T---\left(14\right)$

其中更新协方差阵的估计为(9)式：

(13)式被称为建立在残差基础上的估计，而(13)式被称为建立在更新基础上的估计。

(12)式的一个潜在的局限性在于方程难于保证是正定的，因此就用近似：

${\hat{Q}}_k\≈{\hat{C}}_{\mathrm{\Δ}{x}_k}---\left(15\right)$

实际上对

的估计并不容易。

文献[3]Helen Basil，M.R.Ananthasayanam and S.N.Puri.Adaptive KalmanFilter Tuning In Integration of Low-Cost MEMS-INS/GPS[C]//AIAA Guidance，Navigation，and Control Conference and Exhibit.16-19 August2004，Providence，Rhode Island(Helen Basil，M.R.Ananthasayanam and S.N.Puri.在低成本的MEMS-INS/GPS组合中的自适应卡尔曼滤波器的调整[C]//AIAA引导，导航和控制讨论和展览会.2004年8月，普罗维登斯(美国罗得岛州的首府)，罗得岛州，美国)，其自适应卡尔曼滤波器的调整涉及到初始估计均方误差阵P₀、状态方程噪声协方差阵Q和量测方程噪声协方差阵R。采用了性能指标：

$J=\frac{1}{N}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{v}_k^T{[H\left(k\right)P^{-}\left(t_k\right)H^T\left(k\right)+R]}^{-1}{v}_k---\left(16\right)$

式中

另外对量测方程噪声协方差阵R采用自适应估计：

式中更新序列r_k＝v_k，

另外对状态方程噪声协方差阵Q的自适应估计为

式中

Φ_k，k-1为状态转移阵。

从(17)式和(18)式可看出，对R和Q的估计涉及到动态窗口总点数N的确定，其计算过程比较复杂。

本发明根据卡尔曼滤波器的基本原理和特点，对量测方程噪声协方差阵R采用了简化处理，直接求量测噪声方差值，采用使量测噪声方差值与量测值和估计均方误差阵P直接相关的再次估计的处理方法，实施难度较小，对卫星导航数据波动的反映较快且抑制波动的作用较显著，另外在长时间不组合后进入组合状态时能够有效抑制误差估计量的剧烈波动，这些都有效提高了卡尔曼滤波器闭环修正的准确性和稳定性。实际的应用已证明了其有效性。

发明内容

本发明的目的是提出一种实施难度小的惯性/卫星组合导航系统开闭环混合修正方法。本发明的技术解决方案是，

1)自适应获得卡尔曼滤波器量测噪声方差值

在惯性/卫星组合导航系统中预先设定的量测噪声固定方差值为R₀₁，R₀₂，...R_0m，卡尔曼滤波器已获得的量测值为Z₁，Z₂，...Z_m，卡尔曼滤波器已获得的与量测值对应的估计均方误差阵P中对应的对角线元素为PL₁，PL₂，...PL_m，待计算的量测噪声方差值为R₁，R₂，...R_m，一个闭环修正周期内的新闭环修正值为SB₁，SB₂，...SB_M，预置闭环修正量状态字A的初始值为0，预置闭环修正计时器B的初始值为0。

获得量测噪声方差值的步骤如下：

⑧等待和判断卫星接收机秒脉冲是否在0.9s～1.1s内到。如果是，则转入下一步；如果否，则卫星接收机出现故障，停止组合导航，转入纯惯性导航；

⑨等待和判断在卫星接收机秒脉冲到来后的950m内卫星导航数据包是否到。如果是，则转入下一步；如果到950ms数据包一直未到，则按照惯导输出数据总线周期作时间更新，B＝B+1，转到①；

⑩判断是否满足闭环修正的卫星状态字有效和闭环修正周期秒数的条件。如果满足，则转入下一步；如果不满足，则按照惯导输出数据总线周期作时间更新，B＝B+1，转到①；

使得|Z₁|²与PL₁的单位相同、|Z₂|²与PL₂的单位相同、...|Z_m|²与PL_m的单位相同；

分别判断量测值和估计均方误差阵P中对应的对角线元素，获得量测噪声方差值：

是否|Z₁|²/PL₁≤1。如果是，则R₁＝R₁₀；

如果否，则R₁＝R₁₀*|Z₁|²/PL₁。

是否|Z₂|²/PL₂≤1。如果是，则R₂＝R₂₀；

如果否，则R₂＝R₂₀*|Z₂|²/PL₂。

…

是否|Z_m|²/PL_m≤1。如果是，则R_m＝R_m0；

如果否，则R_m＝R_m0*|Z_m|²/PL_m。

将量测噪声方差值R₁，R₂，...R_m置于卡尔曼滤波器中处理，获得本闭环修正周期内的闭环修正量SB₁，SB₂，...SB_M，闭环修正量状态字A＝1，同时闭环修正计时器B自动累加1；

重复①-⑥步骤。

2)开环修正量更新

在惯性/卫星组合导航系统中设定按秒脉冲更新的初始更新开环修正量是S₁、S₂、…S_K，实际更新开环修正量SS₁、SS₂、…SS_K，累积的开环修正总量是LS₁、LS₂、…LS_K，初始值均为0，预先设置开环修正系数a，a大于0并小于惯导数据总线频率的倒数。

开环修正量更新的步骤如下：

(1)、等待和判断卫星接收机秒脉冲是否在0.9s～1.1s内到。如果是，则转入下一步；如果否，分为两种情况：如果秒脉冲未到，但还未结束0.9s～1.1s的时间遍历，则转到(10)，做开环修正量固定值的累加；如果已结束0.9s～1.1s的时间遍历，秒脉冲仍未到，则卫星接收机出现故障，停止组合导航，转入纯惯性导航；

(2)、锁定惯性/卫星组合导航系统上的卫星接收机秒脉冲到来时刻的惯导没有经过开环修正的相应数据Ins₁，Ins₂，...Ins_K，并存储到惯性/卫星组合导航系统上的内存单元中；

(3)、等待和判断在卫星接收机秒脉冲到来后的950m内卫星导航数据包是否到。如果是，则转入下一步；如果到950ms数据包一直未到，则停止开环修正量累加，按照惯导输出数据总线周期作时间更新，B＝B+1，转到(1)；如果还未到950ms且数据包一直未到，则转到(10)；

(4)、根据卫星导航数据包的状态字判断卫星导航数据是否可用。如果是，则进入下一步；如果否，则停止开环修正量累加，按照惯导输出数据总线周期作时间更新，B＝B+1，转到(1)；

(5)、在惯性/卫星组合导航系统中的卫星接收机秒脉冲对应卫星导航数据包到来时，从数据包中取出卫星导航的对应数据Sat₁，Sat₂，...Sat_K；

(6)、根据取出的卫星导航对应数据和存储的惯导对应数据，计算初始开环修正量S₁、S₂、…S_K，S₁＝Ins₁-Sat₁，S₂＝Ins₂-Sat₂，…S_K＝Ins_K-Sat_K；

(7)、判断状态字A是否为1且闭环修正计时器B是否小于300秒；

(8)、如果是，就从开环修正量S₁、S₂、…S_K先减去当前时刻的累积开环修正量LS₁、LS₂、…LS_K，再减去即将使用的闭环修正量SB₁，SB₂，...SB_M中与开环修正量相对应的量SB₁，SB₂，...SB_K，得到新的开环修正量SS₁、SS₂、…SS_K；计算出本时刻开始的实际开环修正量a*SS₁、a*SS₂、…、a*SS_K；用开环修正量a*SS₁、a*SS₂、…、a*SS_K和闭环修正量SB₁，SB₂，...SB_K同时做开环和闭环修正，实际上是把开环修正量a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中，持续用累积开环修正量LS₁、LS₂、…LS_K对惯导的相应输出数据分别作开环修正，并置A＝0，B＝0；

(9)、如果否，则只从量测数据中减去当前时刻的累积开环修正量LS₁、LS₂、…LS_K，得到新的开环修正量SS₁、SS₂、…SS_K；计算出本时刻开始的实际开环修正量a*S_S1、a*SS₂、…、a*SS_K；只用开环修正量a*SS₁、a*SS₂、…、a*SS_K做开环修正，实际上是把开环修正量a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中，持续用累积开环修正量LS₁、LS₂、…LS_K对惯导的相应输出数据分别作开环修正，并置B＝B+1；

(10)、按照惯导输出数据总线周期作时间更新，再次将开环修正量a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中，持续用累积开环修正量LS₁、LS₂、…LS_K对惯导的相应输出数据分别作开环修正；

(11)、回到(1)。

本发明具有的优点和有益效果：

本发明采用开闭环修正结合的方法对惯性卫星导航系统的误差量进行修正。在闭环修正中采用了自适应获得卡尔曼滤波器量测噪声方差值的方法，方法简便易行、实施难度较小，可有效抑制卫星导航数据的跳变异常、以及长时间不组合后进入组合状态时修正量的较大波动，提高了闭环修正的准确性和稳定性；在开环修正中对所采集的量测量实行每秒实时更新，修正频率与惯导数据总线频率相同，不同频率的闭环和开环修正采用按卫星导航接收机秒脉冲同步的方法，做到了无缝连接，误差修正迅捷且平滑，解决了闭环修正容易干扰开环修正的难题。本发明对于提高惯性/卫星组合导航的误差估计和误差修正的准确性、快速性、平稳性有较好的效果。

附图说明

图1是本发明流程图；

图2是本发明闭环修正流程图；

图3是本发明开环修正流程图。

具体实施方式

1)自适应获得卡尔曼滤波器量测噪声方差值

在惯性/卫星组合导航系统中预先设定的量测噪声固定方差值为R₀₁，R₀₂，...R_0m，卡尔曼滤波器已获得的量测值为Z₁，Z₂，...Z_m，卡尔曼滤波器已获得的与量测值对应的估计均方误差阵P中对应的对角线元素为PL₁，PL₂，...PL_m，待计算的量测噪声方差值为R₁，R₂，...R_m，一个闭环修正周期内的新闭环修正值为SB₁，SB₂，...SB_M，预置闭环修正量状态字A的初始值为0，预置闭环修正计时器B的初始值为0。

说明：卡尔曼滤波器中估计均方误差阵P和量测值Z₁，Z₂，...Z_m是两个关键因素。另外闭环修正量状态字A是为了开、闭环修正混合修正时，在开环修正量中准确扣除闭环修正量，做到无缝衔接。而闭环修正计时器B是为了防止在计算出闭环修正量后，长时间没有开环修正和闭环修正，导致惯导误差与闭环修正量相差较大，不再使用旧的闭环修正量。

获得量测噪声方差值的步骤如下：

①等待和判断卫星接收机秒脉冲是否在0.9s～1.1s内到。如果是，则转入下一步；如果否，则卫星接收机出现故障，停止组合导航，转入纯惯性导航；

说明：为了使用方便，卫星接收机秒脉冲一般都设置为1整秒，而且一定是规则发送，如果秒脉冲出现不规则的情况，一定是卫星接收机出现了问题，就不能用于组合导航，由于惯导的计时精度有限，将判据有意识放宽，如果惯导的计时精度较高，可将判据加严至0.95s～1.05s甚至0.99s～1.01s以上则效果更佳。

②等待和判断在卫星接收机秒脉冲到来后的950ms内卫星导航数据包是否到来。如果是，则转入下一步；如果到950ms数据包一直未到，则按照惯导输出数据总线周期作时间更新，B＝B+1，转到①；

说明：目前大多数的卫星接收机的卫星导航数据包一般到能在秒脉冲到来后的500ms内到，但有些旧型卫星接收机可能延迟到接近800ms，但若超过1s就出现了跨秒现象，导致无法使用，而且如果延迟时间较长，就会给使用带来的较大的困难，因此限定延迟时间不能超过950ms是较宽的限制，如果已知卫星接收机的卫星导航数据包在秒脉冲到来后的400ms内到，可将延迟时间的判据定为450ms，则效果更好。

③判断是否满足闭环修正的卫星状态字有效和闭环修正周期秒数的条件。如果满足，则转入下一步；如果不满足，则按照惯导输出数据总线周期作时间更新，B＝B+1，转到①；

说明：可根据需要预先设置几种闭环修正的条件，比如闭环修正的时间周期设置为10s，至少要收到6秒状态字有效的卫星导航数据等等。只有满足全部条件，才能开始做闭环修正。

④使得|Z₁|²与PL₁的单位相同、|Z₂|²与PL₂的单位相同、...|Z_m|²与PL_m的单位相同；

说明：这是为了保证计算正确的前提条件。

⑤分别判断量测值和估计均方误差阵P中对应的对角线元素，获得量测噪声方差值：

是否|Z₁|²/PL₁≤1。如果是，则R₁＝R₁₀；

如果否，则R₁＝R₁₀*|Z₁|²/PL₁。

是否|Z₂|²/PL₂≤1。如果是，则R₂＝R₂₀；

如果否，则R₂＝R₂₀*|Z₂|²/PL₂。

…

是否|Z_m|²/PL_m≤1。如果是，则R_m＝R_m0；

如果否，则R_m＝R_m0*|Z_m|²/PL_m。

⑥将量测噪声方差值R₁，R₂，...R_m置于卡尔曼滤波器中处理，获得本闭环修正周期内的闭环修正量SB₁，SB₂，...SB_M，闭环修正量状态字A＝1，同时闭环修正计时器B自动累加1；

说明：闭环修正量SB₁，SB₂，...SB_M是根据卡尔曼滤波器的标准计算获得的，其中只有部分量与开环修正有关。

⑦重复①-⑥步骤。

2)开环修正量更新

在惯性/卫星组合导航系统中设定按秒脉冲更新的初始更新开环修正量是S₁、S₂、…S_K，实际更新开环修正量SS₁、SS₂、…SS_K，累积的开环修正总量是LS₁、LS₂、…LS_K，初始值均为0，预先设置开环修正系数a，a大于0并小于惯导数据总线频率的倒数。

说明：要用到闭环修正量状态字A和闭环修正计时器B的值，另外还要用到全部闭环修正量SB₁，SB₂，...SB_M中的部分量SB₁，SB₂，...SB_K。A一般是一个远小于1的正数。

开环修正量更新的步骤如下：

(1)、等待和判断是否卫星接收机秒脉冲在0.9s～1.1s内到。如果是，则转入下一步；如果否，分为两种情况：如果秒脉冲未到，但还未结束0.9s～1.1s的时间遍历，则转到(10)，做开环修正量固定值的累加；如果已结束0.9s～1.1s的时间遍历，秒脉冲仍未到，则卫星接收机出现故障，停止组合导航，转入纯惯性导航；

说明：因为开环修正频率一般比闭环修正的频率高，在秒脉冲未到、还未结束0.9s～1.1s的时间遍历、因此无法判定秒脉冲发送不正常的情况下，要继续做开环修正量固定值的累加。

(2)、锁定惯性/卫星组合导航系统上的卫星接收机秒脉冲到来时刻的惯导没有经过开环修正的相应数据Ins₁，Ins₂，...Ins_K，并存储到惯性/卫星组合导航系统上的内存单元中；

说明：要注意存储的惯导数据Ins₁，Ins₂，...Ins_K没有经过开环修正，即为纯惯性导航数据或只经过闭环修正的数据。

(3)、等待和判断在卫星接收机秒脉冲到来后的950m内卫星导航数据包是否到来。如果是，则转入下一步；如果到950ms数据包一直未到，则停止开环修正量累加，按照惯导输出数据总线周期作时间更新，B＝B+1，转到(1)；如果还未到950ms且数据包一直未到，则转到(10)；

说明：如果卫星接收机的数据包来得较快，可把等待时间相应减少，如减少至500ms。第三种情形是指还未到950ms的时间判据，在数据包未到的时，继续做开环修正量固定值的累加，同秒脉冲未到、还未结束0.9s～1.1s的时间遍历的情形类似。

(4)、根据卫星导航数据包的状态字判断卫星导航数据是否可用。如果是，则进入下一步；如果否，则停止开环修正量累加，按照惯导输出数据总线周期作时间更新，B＝B+1，转到(1)；

说明：如果卫星导航数据不可用，与丢星(收不到卫星导航数据)或人工选择纯惯性导航相同，都要立刻停止开环修正量累加，只用原来累积的开环修正量对惯导的输出数据做开环修正。

(5)、在惯性/卫星组合导航系统中的卫星接收机秒脉冲对应卫星导航数据包到来时，从数据包中取出卫星导航的对应数据Sat₁，Sat₂，...Sat_K；

(6)、根据取出的卫星导航对应数据和存储的惯导对应数据，计算初始开环修正量S₁、S₂、…S_K，S₁＝Ins₁-Sat₁，S₂＝Ins₂-Sat₂，…S_K＝Ins_K-Sat_K；

(7)、判断状态字A是否为1且闭环修正计时器B是否小于300秒；

说明：A＝1说明已经计算出了闭环修正量，等待开环修正和闭环修正同时进行的条件出现；B＜300s说明等待闭环修正的时间还未达到5min，根据惯导的具体情况，可改变时间判据，如果惯导性能比较稳定，可延长等待时间，比如延长到10min甚至20min以上。

(8)、如果是，就从开环修正量S₁、S₂、…S_K先减去当前时刻的累积开环修正量LS₁、LS₂、…LS_K，再减去即将使用的闭环修正量SB₁，SB₂，...SB_M中与开环修正量相对应的量SB₁，SB₂，...SB_K，得到新的开环修正量SS₁、SS₂、…SS_K；计算出本时刻开始的实际开环修正量a*SS₁、a*SS₂、…、a*SS_K；用开环修正量a*SS₁、a*SS₂、…、a*SS_K和闭环修正量SB₁，SB₂，...SB_K同时做开环和闭环修正，实际上是把开环修正量a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中，持续用累积开环修正量LS₁、LS₂、…LS_K对惯导的相应输出数据分别作开环修正，并置A＝0，B＝0；

说明：当前时刻的累积开环修正量LS₁、LS₂、…LS_K包括了从收到秒脉冲到当前时刻的新增的累积开环修正量，因为开环修正的累积量一直按照惯导数据总线的频率不停地累积，直到丢星、卫星导航数据被判定为不合格或人工选择纯惯性导航时才停止累积计算。这里所说的同时做开环和闭环修正是指新增加的开环修正量a*SS₁、a*SS₂、…、a*SS_K和闭环修正量SB₁，SB₂，...SB_K要同时起作用，对于开环修正实际上是把a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中。

(9)、如果否，则只从量测数据中减去当前时刻的累积开环修正量LS₁、LS₂、…LS_K，得到新的开环修正量SS₁、SS₂、…SS_K；计算出本时刻开始的实际开环修正量a*SS₁、a*SS₂、…、a*SS_K；只用开环修正量a*SS₁、a*SS₂、…、a*SS_K做开环修正，实际上是把开环修正量a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中，持续用累积开环修正量LS₁、LS₂、…LS_K对惯导的相应输出数据分别作开环修正，并置B＝B+1；

(10)、按照惯导输出数据总线周期作时间更新，再次将开环修正量a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中，持续用累积开环修正量LS₁、LS₂、…LS_K对惯导的相应输出数据分别作开环修正；

说明：这是不更新量测量的固定开环修正量累积更新、以及持续的开环修正。

(11)回到(1)。

说明：要重新等待秒脉冲、等待数据包，原来秒脉冲到来后的计时要一直保留，直到下一个正常的秒脉冲到来再重新计时，而且数据包的时间也以秒脉冲到来后的计时为准。

实施例：

分别可参照下列例子实施：

1)自适应获得卡尔曼滤波器量测噪声方差值

可选取经度、纬度为量测量，m＝2，取R₀₁＝R₀₂＝20²(m²)，闭环修正周期10s。

则：

①判断是否卫星接收机秒脉冲在0.9s～1.1s内到，结果：是；

②判断是否卫星数据包在950ms内到，结果：是；

③判断是否满足卫星状态字有效和闭环修正周期秒数的条件，结果：是；

④使得|Z₁|²与PL₁的单位相同、|Z₂|²与PL₂的单位相同的单位相同。

假设|Z₁|²＝(0.002°)²(纬度量测)，|Z₂|²＝(0.003°)²(经度量测)，

PL₁＝(4.712×10^-6rad)²  PL₁＝(6.283×10^-6rad)²

换算成同单位：|Z₁|²＝(0.002°)²＝(3.491×10^-5rad)²

|Z₂|²＝(0.003°)²＝(5.236×10^-5rad)²

⑤判断是否|Z₁|²/PL₁≤1，结果：否，则R₁＝R₁₀×|Z₁|²/PL₁＝20²×(3.491×10^-5)²/(4.712×10^-6)²＝21955.79(m²)

判断是否|Z₂|²/PL₂≤1，结果：否，则

R₂＝R₂₀×|Z₂|²/PL₂＝20²×(5.236×10^-5)²/(6.283×10^-6)²＝27779.55(m²)

⑥将量测噪声方差值R₁，R₂置于卡尔曼滤波器中处理，获得本闭环修正周期内的闭环修正值SB₁，SB₂，...SB_M，置A＝1，B＝B+1。因为这是涉及到卡尔曼滤波器的标准计算步骤，且与本专利无直接关系，因此将这部分计算略去。

2)开环修正量更新

选取惯导和卫星导航的经度、纬度、东向速度、北向速度为量测量，开环修正周期50ms，a＝0.002。设置按秒脉冲更新的初始更新开环修正量是S₁、S₂、S₃、S₄，实际更新开环修正量SS₁、SS₂、SS₃、SS₄，累积的开环修正总量是LS₁、LS₂、LS₃、LS₄，初始值均为0。已经计算出的对应闭环修正量为：SB₁、SB₂、SB₃、SB₄。

则：

(1)、判断接收机秒脉冲是否在0.9s～1.1s内到，结果：是；

(2)、在内存中存储惯导没有经过开环修正的与接收机秒脉冲同步的相应数据Ins₁，Ins₂，Ins₃，Ins₄，分别是经度E104.3612°、纬度N31.2573、东向速度Ve＝283.2m/s、北向速度Vn＝-192.7m/s；

(3)、卫星接收机数据包是否在950ms内到，结果：是；

(4)、卫星接收机数据包的状态字是否有效，结果：是；

(5)、从数据包中取出卫星导航的对应数据Sat₁，Sat₂，Sat₃，Sat₄，分别是经度E104.3635°、纬度N31.2519°、东向速度Ve＝282.1m/s、北向速度Vn＝-194.1m/s；

(6)、计算初始开环修正量：

S₁＝Ins₁-Sar₁＝-0.0023°

S₂＝Ins₂-Sat₂＝0.0054°

S₃＝Ins₃-Sat₃＝1.13m/s

S₄＝Ins₄-Sat₄＝1.48m/s

(7)、是否A＝1且B＜600，实际A＝1，B＝176，结果：是；

(8)、取出当前时刻的累积开环修正量：

LS₁＝-0.0019°

LS₂＝0.0051°

LS₃＝1.03m/s

LS₄＝1.42m/s

同时取出已经计算出的对应闭环修正量：

SB₁＝-0.0001°

SB₂＝0.0002°

SB₃＝0.06m/s

SB₄＝0.02m/s

得到实际更新开环修正量

SS₁＝S₁-LS₁-SB₁＝-0.0003°

SS₂＝S₂-LS₂-SB₂＝0.0001°

SS₃＝S₃-LS₃-SB₃＝0.04m/s

SS₄＝S₄-LS₄-SB₄＝0.04m/s

计算实际开环修正量

a*S₁＝-6×10^-7(°)＝-1.047×10^-8(rad)

a*S₂＝2×10^-7(°)＝3.491×10^-8(rad)

a*S₃＝8×10^-5(m/s)

a*s₄＝8×10^-5(m/s)

用开环修正量a*SS₁、a*SS₂、a*SS₃、a*SS₄和闭环修正量SB₁、SB₂、SB₃、SB₄同时做开环和闭环修正，实际上是把开环修正量a*SS₁、a*SS₂、a*SS₃、a*SS₄分别累加到累积开环修正量LS₁、LS₂、LS₃、LS₄中：

LS₁＝LS₁+a*SS₁＝-0.0019006(°)

LS₂＝LS₂+a*SS₂＝0.0051002(°)

LS₃＝LS₃+a*SS₃＝1.03008(m/s)

LS₄＝LS₄+a*SS₄＝1.42008(m/s)

持续用累积开环修正量LS₁、LS₂、LS₃、LS₄对惯导的相应输出数据分别作开环修正；

置A＝0，B＝0(因为已做过闭环修正)；

(9)、按照惯导输出数据总线周期作时间更新，再次将开环修正量a*SS₁、a*SS₂、a*SS₃、a*SS₄分别累加到累积开环修正量LS₁、LS₂、LS₃、LS₄中：

LS₁＝LS₁+a*SS₁＝-0.0019012(°)

LS₂＝LS₂+a*SS₂＝0.0051004(°)

LS₃＝LS₃+a*SS₃＝1.03016(m/s)

LS₄＝LS₄+a*SS₄＝1.42016(m/s)

持续用累积开环修正量LS₁、LS₂、LS₃、LS₄对惯导的相应输出数据分别作开环修正。

(10)、回到(1)；要重新等待秒脉冲、等待数据包，原来秒脉冲到来后的计时要一直保留，直到下一个正常的秒脉冲到来再重新计时，而且数据包的时间也以秒脉冲到来后的计时为准。

Claims (1)

1.一种惯性/卫星组合导航系统开闭环混合修正方法，其特征是，

1)自适应获得卡尔曼滤波器量测噪声方差值

在惯性/卫星组合导航系统中预先设定的量测噪声固定方差值为R₀₁，R₀₂，...R_0m，卡尔曼滤波器已获得的量测值为Z₁，Z₂，...Z_m，卡尔曼滤波器已获得的与量测值对应的估计均方误差阵P中对应的对角线元素为PL₁，PL₂，...PL_m，待计算的量测噪声方差值为R₁，R₂，...R_m，一个闭环修正周期内的新闭环修正值为SB₁，SB₂，...SB_M，预置闭环修正量状态字A的初始值为0，预置闭环修正计时器B的初始值为0。

获得量测噪声方差值的步骤如下：

①等待和判断卫星接收机秒脉冲是否在0.9s～1.1s内到。如果是，则转入下一步；如果否，则卫星接收机出现故障，停止组合导航，转入纯惯性导航；

②等待和判断在卫星接收机秒脉冲到来后的950m内卫星导航数据包是否到。如果是，则转入下一步；如果到950ms数据包一直未到，则按照惯导输出数据总线周期作时间更新，B＝B+1，转到①；

③判断是否满足闭环修正的卫星状态字有效和闭环修正周期秒数的条件。如果满足，则转入下一步；如果不满足，则按照惯导输出数据总线周期作时间更新，B＝B+1，转到①；

④使得|Z₁|²与PL₁的单位相同、|Z₂|²与PL₂的单位相同、…|Z_m|²与PL_m的单位相同；

⑤分别判断量测值和估计均方误差阵P中对应的对角线元素，获得量测噪声方差值：

是否|Z₁|²/PL₁≤1。如果是，则R₁＝R₁₀；

如果否，则R₁＝R₁₀*|Z₁|²/PL₁。

是否|Z₂|²/PL₂≤1。如果是，则R₂＝R₂₀；

如果否，则R₂＝R₂₀*|Z₂|²/PL₂。

…

是否|Z_m|²/PL_m≤1。如果是，则R_m＝R_m0；

如果否，则R_m＝R_m0*|Z_m|²/PL_m。

⑥将量测噪声方差值R₁，R₂，...R_m置于卡尔曼滤波器中处理，获得本闭环修正周期内的闭环修正量SB₁，SB₂，...SB_M，闭环修正量状态字A＝1，同时闭环修正计时器B自动累加1；

⑦重复①-⑥步骤。

2)开环修正量更新

在惯性/卫星组合导航系统中设定按秒脉冲更新的初始更新开环修正量是S₁、S₂、…S_K，实际更新开环修正量SS₁、SS₂、…SS_K，累积的开环修正总量是LS₁、LS₂、…LS_K，初始值均为0，预先设置开环修正系数a，a大于0并小于惯导数据总线频率的倒数。

开环修正量更新的步骤如下：

(1)、等待和判断卫星接收机秒脉冲是否在0.9s～1.1s内到。如果是，则转入下一步；如果否，分为两种情况：如果秒脉冲未到，但还未结束0.9s～1.1s的时间遍历，则转到(10)，做开环修正量固定值的累加；如果已结束0.9s～1.1s的时间遍历，秒脉冲仍未到，则卫星接收机出现故障，停止组合导航，转入纯惯性导航；

(2)、锁定惯性/卫星组合导航系统上的卫星接收机秒脉冲到来时刻的惯导没有经过开环修正的相应数据Ins₁，Ins₂，...Ins_K，并存储到惯性/卫星组合导航系统上的内存单元中；

(3)、等待和判断在卫星接收机秒脉冲到来后的950m内卫星导航数据包是否到。如果是，则转入下一步；如果到950ms数据包一直未到，则停止开环修正量累加，按照惯导输出数据总线周期作时间更新，B＝B+1，转到(1)；如果还未到950ms且数据包一直未到，则转到(10)；

(4)、根据卫星导航数据包的状态字判断卫星导航数据是否可用。如果是，则进入下一步；如果否，则停止开环修正量累加，按照惯导输出数据总线周期作时间更新，B＝B+1，转到(1)；

(5)、在惯性/卫星组合导航系统中的卫星接收机秒脉冲对应卫星导航数据包到来时，从数据包中取出卫星导航的对应数据Sat₁，Sat₂，...Sat_K；

(6)、根据取出的卫星导航对应数据和存储的惯导对应数据，计算初始开环修正量S₁、S₂、…S_K，S₁＝Ins₁-Sat₁，S₂＝Ins₂-Sat₂，…S_K＝Ins_K-Sat_K；

(7)、判断状态字A是否为1且闭环修正计时器B是否小于300秒；

(8)、如果是，就从开环修正量S₁、S₂、…S_K先减去当前时刻的累积开环修正量LS₁、LS₂、…LS_K，再减去即将使用的闭环修正量SB₁，SB₂，...SB_M中与开环修正量相对应的量SB₁，SB₂，...SB_K，得到新的开环修正量SS₁、SS₂、…SS_K；计算出本时刻开始的实际开环修正量a*SS₁、a*SS₂、…、a*SS_K；用开环修正量a*SS₁、a*SS₂、…、a*SS_K和闭环修正量SB₁，SB₂，...SB_K同时做开环和闭环修正，实际上是把开环修正量a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中，持续用累积开环修正量LS₁、LS₂、…LS_K对惯导的相应输出数据分别作开环修正，并置A＝0，B＝0；

(9)、如果否，则只从量测数据中减去当前时刻的累积开环修正量LS₁、LS₂、…LS_K，得到新的开环修正量SS₁、SS₂、…SS_K；计算出本时刻开始的实际开环修正量a*SS₁、a*SS₂、…、a*SS_K；只用开环修正量a*SS₁、a*SS₂、…、a*SS_K做开环修正，实际上是把开环修正量a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中，持续用累积开环修正量LS₁、LS₂、…LS_K对惯导的相应输出数据分别作开环修正，并置B＝B+1；

(10)、按照惯导输出数据总线周期作时间更新，再次将开环修正量a*SS₁、a*SS₂、…、a*SS_K分别累加到累积开环修正量LS₁、LS₂、…LS_K中，持续用累积开环修正量LS₁、LS₂、…LS_K对惯导的相应输出数据分别作开环修正；

(11)、回到(1)。

Images