[发明专利]一种惯性/卫星组合导航系统开闭环混合修正方法

权利要求书

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)。