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

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

Info

Publication number
CN102426373A
CN102426373A CN2011102671897A CN201110267189A CN102426373A CN 102426373 A CN102426373 A CN 102426373A CN 2011102671897 A CN2011102671897 A CN 2011102671897A CN 201110267189 A CN201110267189 A CN 201110267189A CN 102426373 A CN102426373 A CN 102426373A
Authority
CN
China
Prior art keywords
loop correction
open loop
ss
ls
correction
Prior art date
Application number
CN2011102671897A
Other languages
English (en)
Inventor
魏伟
杨卫平
张晓冬
谭宏光
薛涛
Original Assignee
中国航空工业第六一八研究所
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 中国航空工业第六一八研究所 filed Critical 中国航空工业第六一八研究所
Priority to CN2011102671897A priority Critical patent/CN102426373A/zh
Publication of CN102426373A publication Critical patent/CN102426373A/zh

Links

Abstract

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

Description

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

技术领域

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

背景技术

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

[0003] 文献[l]ffeidong Ding, Jinling Wang and Chris Rizos. Improving Adaptive Kalman Estimation in GPS/INS Integration[J]. THE JOURNAL OF NAVIGATION(2007), 60, 517-529 ([IJffeidong Ding, Jinling Wang and Chris Rizos.在 GPS/INS (惯性导航系统)组合中改善卡尔曼估计。导航杂志(美国M2007),60,P.517-P.5^)。其主要方法是计算自适应噪声方差阵&。分三个步骤:

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

ι m~\

[0005] E{dkdTk} 二一 ( 1 )

[0006] 其中的新息(残差)为:

[0007] dk =Zk-HkXk (2)

[0008] 状态一步预测为名=Φ^Λ-丨 (3 )

[0009] Zk为量测值,Hk为量测方程系数矩阵,φ^为一步转移阵,iw为状态估计值。

[0010] 2)计算缩放比例系数: 1

[0011]

[0012] [0013]

a =

/=0

) 4 C-

trace{HkPk-H'k}

其中为一步预测均方误差阵,traced表示矩阵的迹。 3)计算自适应噪声方差阵:

[0014] Qk=Qk_xyfa (5)

[0015] 上述(1)式和(4)式的计算都较为复杂,另外如何预先确定点数m也有难度,么只能整体按比例‘变化,单独的系统噪声方差无法做自适应变化。

[0016] 文献[2]Christopher Hide,Terry Moore and Martin Smith. Adaptive Kalman Filtering 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)。其主要内容是比较了三类自适应卡尔曼滤波算法。这三类算法为:

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

[0018]方法一:

Figure CN102426373AD00051

[0020] 上述比例因子&>1,是事先给定的,虽然简单,但按固定比例增大方差,不是严格意义上的“自适应”。

[0021]方法二 :

Figure CN102426373AD00052

[0023] 上式中,N是预先给定的周期数,目的是由目前的第k向前推N个周期;< 按照文献 [2]的定义,是“卡尔曼滤波器的状态预测和新量测之间的差别”,实际上就是(¾式表示的新息(残差)。

[0024] 如果将上述&用于(6),则在GPS和低成本INS组合时回会出现不稳定的问题。可用于下式:

[

Figure CN102426373AD00053

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

[0027] B.多个模型的自适应估计(MMAE,Multiple Model Adaptive Estimation)

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

Figure CN102426373AD00054

[0030] 上式中m是量测量的数目,更新协方差阵的估计为:

Figure CN102426373AD00055

[0032] 第η个卡尔曼滤波器为正确的概率Pn(k)用下列迭代公式计算: pn(k)= /她(10)

[0033]

[0034] 对于N个卡尔曼滤波器,设在k时刻的第j个卡尔曼滤波器的估计值为巧(幻,则k 时刻的最优估计值为:

Figure CN102426373AD00056

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

[0037] C.自适应卡尔曼滤波器

[0038] 这类算法是采用额外的方程来估计量测方程噪声协方差阵或者状态方程噪声协方差阵,是对传统卡尔曼滤波器的扩展。[0039] 其中一种算法是采用极大似然准则估计状态方程噪声的协方差阵:

[0040]

Figure CN102426373AD00061

[0041] 式中是状态修正协方差阵,其估计为 Λ 1 k

[0042]

Figure CN102426373AD00062

[0043] 式中Δ Xj为状态修正序列,定义为:

[0044]

Figure CN102426373AD00063

[0045] 使用替

Figure CN102426373AD00064

其中Kk卡尔曼滤波器的增益阵,则可用更新协方差阵来近似状态修正序列:

[0046]

Figure CN102426373AD00065

[0047] 其中更新协方差阵的估计为(9)式:

[0048] (13)式被称为建立在残差基础上的估计,而(1¾式被称为建立在更新基础上的估计。

[0049] (12)式的一个潜在的局限性在于方程难于保证是正定的,因此就用近似:

[0050]

Figure CN102426373AD00066

[0051 ] 实际上对4的估计并不容易。

[0052] 文献[3]Helen Basil, Μ. R. Ananthasayanam and S. N. Puri. Adaptive Kalman Filter 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. ¢1¾]¾^:¾ MEMS-INS/ GPS组合中的自适应卡尔曼滤波器的调整[C]//AIAA引导,导航和控制讨论和展览会.2004 年8月,普罗维登斯(美国罗得岛州的首府),罗得岛州,美国),其自适应卡尔曼滤波器的调整涉及到初始估计均方误差阵Ptl、状态方程噪声协方差阵Q和量测方程噪声协方差阵R。 采用了性能指标:

[0053]

Figure CN102426373AD00067

[0054] 式中

Figure CN102426373AD00068

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

[0055]

Figure CN102426373AD00069

[0056]式中更新序列 = vk,

Figure CN102426373AD000610

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

[0058]

Figure CN102426373AD000611

[0059] 式中

Figure CN102426373AD00071

,Η为状态转移阵。

[0060] 从(17)式和(18)式可看出,对r和q的估计涉及到动态窗口总点数n的确定,其计算过程比较复杂。

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

发明内容

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

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

[0064] 在惯性/卫星组合导航系统中预先设定的量测噪声固定方差值为Rtll,R02, . . . R0ffl, 卡尔曼滤波器已获得的量测值为Z1, Z2, ...zm,卡尔曼滤波器已获得的与量测值对应的估计均方误差阵P中对应的对角线元素为PL1, PL2, ...PLm,待计算的量测噪声方差值为R1, R2, ... Rm,一个闭环修正周期内的新闭环修正值为SB1, SB2, ... ,预置闭环修正量状态字 A的初始值为0,预置闭环修正计时器B的初始值为0。

[0065] 获得量测噪声方差值的步骤如下:

[0066] ⑧等待和判断卫星接收机秒脉冲是否在0. 9s〜1. Is内到。如果是,则转入下一步;如果否,则卫星接收机出现故障,停止组合导航,转入纯惯性导航;

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

[0068] ⑩判断是否满足闭环修正的卫星状态字有效和闭环修正周期秒数的条件。如果满

足,则转入下一步;如果不满足,则按照惯导输出数据总线周期作时间更新,B = B+1,转到 ①;

[0069] ⑪使得I Z112与PL1的单位相同、I Z212与PL2的单位相同、…| Zj2与PLffl的单位相同;

[0070] ⑫分别判断量测值和估计均方误差阵p中对应的对角线元素,获得量测噪声方差值:

[0071 ]是否 ι z11 Vpl1 彡 ι。如果是,则 r1 = r10 ;

[0072]如果否,则 r1 = r10* ι z11 Vpl1。

[0073]是否 IZ21 VPL2 ( ι。如果是,则 R2 = R20 ;

[0074]如果否,则 R2 = R20* IZ21 Vpl2。

[0075] …

[0076]是否 i zm 12/plm ( 1。如果是,则 rm = rm0 ;[0077]如果否,则 Rm = Rm0* I Zm 12/PLm。

[0078] ⑬将量测噪声方差值R1A2,. . . Rm置于卡尔曼滤波器中处理,获得本闭环修正周期内的闭环修正量SB1, S&,,闭环修正量状态字A = 1,同时闭环修正计时器B自动累加1 ;

[0079] ⑭重复①-⑥步骤。

[0080] 2)开环修正量更新

[0081] 在惯性/卫星组合导航系统中设定按秒脉冲更新的初始更新开环修正量是S” S2>…、,实际更新开环修正量SSi、SS2、,累积的开环修正总量是LSi、LS2、"4¾,初始值均为0,预先设置开环修正系数a,a大于0并小于惯导数据总线频率的倒数。

[0082] 开环修正量更新的步骤如下:

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

[0084] (2)、锁定惯性/卫星组合导航系统上的卫星接收机秒脉冲到来时刻的惯导没有经过开环修正的相应数据Ins1, Ins2,. . . InsK,并存储到惯性/卫星组合导航系统上的内存单元中;

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

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

[0087] (5)、在惯性/卫星组合导航系统中的卫星接收机秒脉冲对应卫星导航数据包到来时,从数据包中取出卫星导航的对应数据Sat1, Sat2, . . . SatK ;

[0088] (6)、根据取出的卫星导航对应数据和存储的惯导对应数据,计算初始开环修正量 S” S2、*** SK, S1 = Ins1-Sat1, S2 = Ins2-Sat2, ".Sk = InsK_SatK ;

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

[0090] (8)、如果是,就从开环修正量S1A2.…、先减去当前时刻的累积开环修正量IA、 LS2, "4¾,再减去即将使用的闭环修正量SB1;S4,. . . SBm中与开环修正量相对应的量SB1, SB2, ...S&,得到新的开环修正量SSp SS2、…化^计算出本时刻开始的实际开环修正量 a*SS”a*SS2、…、;用开环修正量 a+SSpa+SSy …、和闭环修正量 SBpSh,. . . SBk 同时做开环和闭环修正,实际上是把开环修正量a^SSpa^S^、…、分别累加到累积开环修正量IA、LS2、."IAi中,持续用累积开环修正量IA、LS2、…!^对惯导的相应输出数据分别作开环修正,并置A = 0,B = 0 ;

[0091] (9)、如果否,则只从量测数据中减去当前时刻的累积开环修正量LSp LS2、… LSK,得到新的开环修正量SSp SS2,…化^计算出本时刻开始的实际开环修正量a*&l、 a*SS2、…、;只用开环修正量E^SSpE^SSy…、做开环修正,实际上是把开环修正量a^SSpa^S^、…、别累加到累积开环修正量IA、LS2、-LSk中,持续用累积开环修正量IA、LS2、"4¾对惯导的相应输出数据分别作开环修正,并置B = B+1 ;

[0092] (10)、按照惯导输出数据总线周期作时间更新,再次将开环修正量a*SSi、 a*SS2、…、分别累加到累积开环修正量ΙΑ、LS2, -LSk中,持续用累积开环修正量 LS1^LS2,…对惯导的相应输出数据分别作开环修正;

[0093] (11)、回到(1)。

[0094] 本发明具有的优点和有益效果:

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

附图说明

[0096] 图1是本发明流程图;

[0097] 图2是本发明闭环修正流程图;

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

具体实施方式

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

[0100] 在惯性/卫星组合导航系统中预先设定的量测噪声固定方差值为Rtll,R02, . . . R0ffl, 卡尔曼滤波器已获得的量测值为Z1, Z2, ...zm,卡尔曼滤波器已获得的与量测值对应的估计均方误差阵P中对应的对角线元素为PL1, PL2, ...PLm,待计算的量测噪声方差值为R1, R2, ... Rm,一个闭环修正周期内的新闭环修正值为SB1, SB2, ... ,预置闭环修正量状态字 A的初始值为0,预置闭环修正计时器B的初始值为0。

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

[0102] 获得量测噪声方差值的步骤如下:

[0103] ①等待和判断卫星接收机秒脉冲是否在0. 9s〜1. Is内到。如果是,则转入下一步;如果否,则卫星接收机出现故障,停止组合导航,转入纯惯性导航;

[0104] 说明:为了使用方便,卫星接收机秒脉冲一般都设置为1整秒,而且一定是规则发送,如果秒脉冲出现不规则的情况,一定是卫星接收机出现了问题,就不能用于组合导航, 由于惯导的计时精度有限,将判据有意识放宽,如果惯导的计时精度较高,可将判据加严至 0. 95s〜1. 05s甚至0. 99s〜1. Ols以上则效果更佳。[0105] ②等待和判断在卫星接收机秒脉冲到来后的950ms内卫星导航数据包是否到来。 如果是,则转入下一步;如果到950ms数据包一直未到,则按照惯导输出数据总线周期作时间更新,B = B+1,转到①;

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

[0107] ③判断是否满足闭环修正的卫星状态字有效和闭环修正周期秒数的条件。如果满

足,则转入下一步;如果不满足,则按照惯导输出数据总线周期作时间更新,B = B+1,转到 ①;

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

[0109] ④使得Z112与PL1的单位相同、Iz2I2与1¾的单位相同、...ZmI2与PLm的单位相同;

[0110] 说明:这是为了保证计算正确的前提条件。

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

[0112]是否 iz1i2zpl1s 1。如果是,则 r1 = r1。;

[0113]如果否,则 r1 = rltl* i z1i2zplp

[0114]是否 z21 Vpl2 ( 1。如果是,则 r2 = r20 ; [οι 15]如果否,则 r2 = r20* iz21 vpl2。

[0116] …

[0117]是否 i zm 12/plm 彡 1。如果是,则 rm = rm0 ;

[0118]如果否,则 rm = rm0* i zm 12/plm。

[0119] ⑥将量测噪声方差值r1a2,. . . rm置于卡尔曼滤波器中处理,获得本闭环修正周期内的闭环修正量sb1, s&,,闭环修正量状态字a = 1,同时闭环修正计时器b自动累加1 ;

[0120] 说明:闭环修正量sb1, sb2, . . . 是根据卡尔曼滤波器的标准计算获得的,其中只有部分量与开环修正有关。

[0121] ⑦重复①-⑥步骤。

[0122] 2)开环修正量更新

[0123] 在惯性/卫星组合导航系统中设定按秒脉冲更新的初始更新开环修正量是s” s2>…、,实际更新开环修正量ssi、ss2、,累积的开环修正总量是lsi、ls2、"4¾,初始值均为0,预先设置开环修正系数a,a大于0并小于惯导数据总线频率的倒数。

[0124] 说明:要用到闭环修正量状态字a和闭环修正计时器b的值,另外还要用到全部闭环修正量sb1, sb2, . . . 中的部分量sb1, sb2, . . . s〜。a —般是一个远小于1的正数。

[0125] 开环修正量更新的步骤如下:[0126] (1)、等待和判断是否卫星接收机秒脉冲在0.9s〜1. Is内到。如果是,则转入下一步;如果否,分为两种情况:如果秒脉冲未到,但还未结束0. 9s〜1. Is的时间遍历,则转到 (10),做开环修正量固定值的累加;如果已结束0.9s〜1. Is的时间遍历,秒脉冲仍未到,则卫星接收机出现故障,停止组合导航,转入纯惯性导航;

[0127] 说明:因为开环修正频率一般比闭环修正的频率高,在秒脉冲未到、还未结束 0. 9s〜1. Is的时间遍历、因此无法判定秒脉冲发送不正常的情况下,要继续做开环修正量固定值的累加。

[0128] (2)、锁定惯性/卫星组合导航系统上的卫星接收机秒脉冲到来时刻的惯导没有经过开环修正的相应数据Ins1, Ins2,. . . InsK,并存储到惯性/卫星组合导航系统上的内存单元中;

[0129] 说明:要注意存储的惯导数据Ls1,Ins2, . . . InsK没有经过开环修正,即为纯惯性导航数据或只经过闭环修正的数据。

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

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

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

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

[0134] (5)、在惯性/卫星组合导航系统中的卫星接收机秒脉冲对应卫星导航数据包到来时,从数据包中取出卫星导航的对应数据Sat1, Sat2, . . . SatK ;

[0135] (6)、根据取出的卫星导航对应数据和存储的惯导对应数据,计算初始开环修正量 S” S2、*** SK, S1 = Ins1-Sat1, S2 = Ins2-Sat2, ".Sk = InsK_SatK ;

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

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

[0138] (8)、如果是,就从开环修正量S1Ay…&先减去当前时刻的累积开环修正量IA、 LS2, "4¾,再减去即将使用的闭环修正量SB1;S4,. . . SBm中与开环修正量相对应的量SB1, SB2, ...S&,得到新的开环修正量SSp SS2、…化^计算出本时刻开始的实际开环修正量 a*SS”a*SS2、…、;用开环修正量 a+SSpa+SSy …、和闭环修正量 SBpSh,. . . SBk 同时做开环和闭环修正,实际上是把开环修正量a^SSpa^S^、…、分别累加到累积开环修正量IA、LS2、."IAi中,持续用累积开环修正量IA、LS2、…!^对惯导的相应输出数据分别作开环修正,并置A = 0,B = 0 ;

[0139] 说明:当前时刻的累积开环修正量LSp LS2、"4¾包括了从收到秒脉冲到当前时刻的新增的累积开环修正量,因为开环修正的累积量一直按照惯导数据总线的频率不停地累积,直到丢星、卫星导航数据被判定为不合格或人工选择纯惯性导航时才停止累积计算。 这里所说的同时做开环和闭环修正是指新增加的开环修正量a*SSi、a*SS2、…、和闭环修正量SB1, SB2,. . . 要同时起作用,对于开环修正实际上是把8*33^8*3$、…、 分别累加到累积开环修正量ΙΑ、LS2、"4¾中。

[0140] (9)、如果否,则只从量测数据中减去当前时刻的累积开环修正量LSp LS2、… LSK,得到新的开环修正量SSp SS2,…化^计算出本时刻开始的实际开环修正量a*SSi、 a*SS2、…、;只用开环修正量E^SSpE^SSy…、做开环修正,实际上是把开环修正量a^SSpa^S^、…、别累加到累积开环修正量IA、LS2、-LSk中,持续用累积开环修正量IA、LS2、"4¾对惯导的相应输出数据分别作开环修正,并置B = B+1 ;

[0141] (10)、按照惯导输出数据总线周期作时间更新,再次将开环修正量a*SSi、 a*SS2、…、分别累加到累积开环修正量ΙΑ、LS2, -LSk中,持续用累积开环修正量 LS1^LS2,…对惯导的相应输出数据分别作开环修正;

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

[0143] (11)回到(1)。

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

[0145] 实施例:

[0146] 分别可参照下列例子实施:

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

[0148] 可选取经度、纬度为量测量,m = 2,取Rtll = R02 = 202 (m2),闭环修正周期10s。

[0149]贝Ij :

[0150] ①判断是否卫星接收机秒脉冲在0. 9s〜1. Is内到,结果:是;

[0151] ②判断是否卫星数据包在950ms内到,结果:是;

[0152] ③判断是否满足卫星状态字有效和闭环修正周期秒数的条件,结果:是;

[0153] ④使得I Z112与PL1的单位相同、I Z212与PL2的单位相同的单位相同。

[0154]假设 IZ1I2 = (0.002° )2(纬度量测),1¾!2 = (0.003° )2(经度量测),

[0155] PL1 = (4. 712Xl(T6rad)2 PL1 = (6. 283 X l(T6rad)2

[0156]换算成同单位:|Zl I2 = (0.002° )2 = (3.491X10"5rad)2

[0157] Z2I2 = (0. 003° )2 = (5. 236X 10_5rad)2

[0158]⑤判断是否 IZ1IVpl1 彡 1,结果:否,则 R1 = R10X IZ1IVPL1 = 202X (3. 491 X IO-5)2/ (4. 712 Xl(Te)2 = 21955. 79 (m2)

[0159] 判断是否|Z2|7PL2< 1,结果:否,则

[0160] R2 = X I Z21VPL2 = 202 X (5. 236 X 1(Γ5) 7 (6. 283 X 1(Γ6)2 = 27779. 55 (m2)

[0161] ⑥将量测噪声方差值R1, &置于卡尔曼滤波器中处理,获得本闭环修正周期内的闭环修正值SB1Ah,. . . SBM, SA= LB = B+1。因为这是涉及到卡尔曼滤波器的标准计算步骤,且与本专利无直接关系,因此将这部分计算略去。

[0162] 2)开环修正量更新

[0163] 选取惯导和卫星导航的经度、纬度、东向速度、北向速度为量测量,开环修正周期 50ms, a = 0.002。设置按秒脉冲更新的初始更新开环修正量是S1. S2、&、S4,实际更新开环修正量SS1. SS2, SS3、SS4,累积的开环修正总量是IA、LS2, LS3、LS4,初始值均为0。已经计算出的对应闭环修正量为=SB1, SB2, SB3> SB40

[0164]贝Ij :

[0165] (1)、判断接收机秒脉冲是否在0. 9s〜1. Is内到,结果:是;

[0166] O)、在内存中存储惯导没有经过开环修正的与接收机秒脉冲同步的相应数据 Ins1, Ins2, Ins3, Ins4,分别是经度 E104. 3612°、纬度 N31. 2573、东向速度 Ve = 283. 2m/s、 北向速度 Vn = -192. 7m/s ;

[0167] (3)、卫星接收机数据包是否在950ms内到,结果:是;

[0168] 0)、卫星接收机数据包的状态字是否有效,结果:是;

[0169] (5)、从数据包中取出卫星导航的对应数据Mt1, Sat2, Sat3, Mt4,分别是经度 E104. 3635°、纬度 N31.2519。、东向速度 Ve = 282. lm/s、北向速度 Vn =-194. lm/s ;

[0170] (6)、计算初始开环修正量:

[0171] S1 = Ins1-Sar1 = -0. 0023°

[0172] S2 = Ins2-Sat2 = 0. 0054°

[0173] S3 = Ins3-Sat3 = 1. 13m/s

[0174] S4 = Ins4-Sat4 = 1. 48m/s

[0175] (7)、是否 A = 1 且8 < 600,实际 A = LB = 176,结果:是;

[0176] (8)、取出当前时刻的累积开环修正量:

[0177] 1^ = -0.0019°

[0178] I^2 = O. 0051°

[0179] LS3 = 1. 03m/s

[0180] LS4 = 1. 42m/s

[0181] 同时取出已经计算出的对应闭环修正量:

[0182] SB1 = -O-OOOr

[0183] SB2 = 0. 0002°

[0184] SB3 = 0. 06m/s

[0185] SB4 = 0.02m/s

[0186] 得到实际更新开环修正量

[0187] SS1 = S1-LS1-SB1 = -0. 0003°

[0188] SS2 = S2-LS2-SB2 = 0. 0001°

[0189] SS3 = S3-LS3-SB3 = 0. 04m/s

[0190] SS4 = S4-LS4-SB4 = 0. 04m/s

[0191] 计算实际开环修正量

[0192] a*Si =-6Χ1(Γ7(° ) = _1. 047 X 10_8 (rad)

[0193] a*S2 = 2Χ10_7(° ) = 3. 491 X IO-8 (rad)[0194] a*S3 = 8X10_5(m/s)

[0195] a*s4 = 8X10_5(m/s)

[0196] 用开环修正量a*SSi、a*SS2、a*SS3、a*SS4和闭环修正量SB1、SB2、SB3> SB4同时做开环和闭环修正,实际上是把开环修正量a*SS1、a*S&、a*S&、a*S、分别累加到累积开环修正量 LS1、LS2、LS3、LS4 中:

[0197] LS1 = LS1+a*SS1 = -0. 0019006(° )

[0198] LS2 = LS2+a*SS2 = 0. 0051002(° )

[0199] LS3 = LS3+a*SS3 = 1. 03008 (m/s)

[0200] LS4 = LS4+a*SS4 = 1. 42008 (m/s)

[0201] 持续用累积开环修正量ΙΑ、Ι^2、Ι^3、Ι^4对惯导的相应输出数据分别作开环修正;

[0202] 置A = 0,B = 0(因为已做过闭环修正);

[0203] (9)、按照惯导输出数据总线周期作时间更新,再次将开环修正量a^SSp a*SS2、 a*SS3、a*SS4分别累加到累积开环修正量LS1, L&、LS3> LS4中:

[0204] LS1 = LS1+a*SS1 = -0. 0019012(° )

[0205] LS2 = LS2+a*SS2 = 0. 0051004(° )

[0206] LS3 = LS3+a*SS3 = 1. 03016 (m/s)

[0207] LS4 = LS4+a*SS4 = 1. 42016 (m/s)

[0208] 持续用累积开环修正量ΙΑ、Ι^2、Ι^3、Ι^4对惯导的相应输出数据分别作开环修正。

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

Claims (1)

1. 一种惯性/卫星组合导航系统开闭环混合修正方法,其特征是,1)自适应获得卡尔曼滤波器量测噪声方差值在惯性/卫星组合导航系统中预先设定的量测噪声固定方差值为Rtll, R02, · · · R0m,卡尔曼滤波器已获得的量测值为Z1, Z2,... Zffl,卡尔曼滤波器已获得的与量测值对应的估计均方误差阵P中对应的对角线元素为PL1;PL2,... PLm,待计算的量测噪声方差值为R1A2,... Rffl, 一个闭环修正周期内的新闭环修正值为SB1, SB2, ... ,预置闭环修正量状态字A的初始值为0,预置闭环修正计时器B的初始值为0。获得量测噪声方差值的步骤如下:①等待和判断卫星接收机秒脉冲是否在0. 9s〜1. Is内到。如果是,则转入下一步;如果否,则卫星接收机出现故障,停止组合导航,转入纯惯性导航;②等待和判断在卫星接收机秒脉冲到来后的950m内卫星导航数据包是否到。如果是, 则转入下一步;如果到950ms数据包一直未到,则按照惯导输出数据总线周期作时间更新, B = B+1,转到①;③判断是否满足闭环修正的卫星状态字有效和闭环修正周期秒数的条件。如果满足, 则转入下一步;如果不满足,则按照惯导输出数据总线周期作时间更新,B = B+1,转到①;④使得I Z112与PL1的单位相同、I Z212与PL2的单位相同、...I Zj2与PLffl的单位相同;⑤分别判断量测值和估计均方误差阵P中对应的对角线元素,获得量测噪声方差值:是否 IZ11 VPL1 ( ι。如果是,则 R1 = R10 ;如果否,则 R1 = R10^IZ1IVpl10是否IZ21 VPL2彡ι。如果是,则R2 = R20 ;如果否,则 & = &q*|Z2|7pl2。是否I zm I Vpldi彡ι。如果是,则Rm = Rm0 ;如果否,则 Rm = Rmtl* IzmI 2/PLm。⑥将量测噪声方差值R1A2,... Rm置于卡尔曼滤波器中处理,获得本闭环修正周期内的闭环修正量SB1, SB2,. . . SBm,闭环修正量状态字A= 1,同时闭环修正计时器B自动累加1 ;⑦重复①-⑥步骤。2)开环修正量更新在惯性/卫星组合导航系统中设定按秒脉冲更新的初始更新开环修正量是Sp S2,… &,实际更新开环修正量SSi、S&、,累积的开环修正总量是LSpLS、"4¾,初始值均为0,预先设置开环修正系数a,a大于0并小于惯导数据总线频率的倒数。开环修正量更新的步骤如下:(1)、等待和判断卫星接收机秒脉冲是否在0.9s〜1. Is内到。如果是,则转入下一步;如果否,分为两种情况:如果秒脉冲未到,但还未结束0. 9s〜1. Is的时间遍历,则转到 (10),做开环修正量固定值的累加;如果已结束0.9s〜1. Is的时间遍历,秒脉冲仍未到,则卫星接收机出现故障,停止组合导航,转入纯惯性导航;(2)、锁定惯性/卫星组合导航系统上的卫星接收机秒脉冲到来时刻的惯导没有经过开环修正的相应数据Ins1, Ins2,. . . InsK,并存储到惯性/卫星组合导航系统上的内存单元中;(3)、等待和判断在卫星接收机秒脉冲到来后的950m内卫星导航数据包是否到。如果是,则转入下一步;如果到950ms数据包一直未到,则停止开环修正量累加,按照惯导输出数据总线周期作时间更新,B = B+1,转到(1);如果还未到950ms且数据包一直未到,则转到(10);G)、根据卫星导航数据包的状态字判断卫星导航数据是否可用。如果是,则进入下一步;如果否,则停止开环修正量累加,按照惯导输出数据总线周期作时间更新,B = B+1,转到⑴;(5)、在惯性/卫星组合导航系统中的卫星接收机秒脉冲对应卫星导航数据包到来时, 从数据包中取出卫星导航的对应数据Sat1, Sat2, . . . SatK ;(6)、根据取出的卫星导航对应数据和存储的惯导对应数据,计算初始开环修正量S” S2λ …SK,S1 = Ins1-Sat1, S2 = Ins2-Sat2, ".Sk = InsK_SatK ;(7)、判断状态字A是否为1且闭环修正计时器B是否小于300秒;(8)、如果是,就从开环修正量Sp S2,…、先减去当前时刻的累积开环修正量IA、 LS2, "4¾,再减去即将使用的闭环修正量SB1;S4,. . . SBm中与开环修正量相对应的量SB1, SB2, ...S&,得到新的开环修正量SSp SS2、…化^计算出本时刻开始的实际开环修正量 a*SS”a*SS2、…、;用开环修正量 a+SSpa+SSy …、和闭环修正量 SBpSh,. . . SBk 同时做开环和闭环修正,实际上是把开环修正量a^SSpa^S^、…、分别累加到累积开环修正量IA、LS2、."IAi中,持续用累积开环修正量IA、LS2、…!^对惯导的相应输出数据分别作开环修正,并置A = 0,B = 0 ;(9)、如果否,则只从量测数据中减去当前时刻的累积开环修正量LS”LS2、"ISk,得到新的开环修正量SS^ SS2,…化!^;计算出本时刻开始的实际开环修正量E^SSp a*SS2、…、 a*SSK ;只用开环修正量a*SS1、a*S&、…、做开环修正,实际上是把开环修正量a*SSp a*SS2、…、分别累加到累积开环修正量ΙΑ、LS2, -LSk中,持续用累积开环修正量 LS1^LS2, ."IAi对惯导的相应输出数据分别作开环修正,并置B = B+1 ;(10)、按照惯导输出数据总线周期作时间更新,再次将开环修正量a*SSi、a*SS2、…、 a*SSK分别累加到累积开环修正量IA、LS2, -LSk中,持续用累积开环修正量LS1, LS2,…对惯导的相应输出数据分别作开环修正;(11)、回到(1)。
CN2011102671897A 2011-09-01 2011-09-01 一种惯性/卫星组合导航系统开闭环混合修正方法 CN102426373A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN2011102671897A CN102426373A (zh) 2011-09-01 2011-09-01 一种惯性/卫星组合导航系统开闭环混合修正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN2011102671897A CN102426373A (zh) 2011-09-01 2011-09-01 一种惯性/卫星组合导航系统开闭环混合修正方法

Publications (1)

Publication Number Publication Date
CN102426373A true CN102426373A (zh) 2012-04-25

Family

ID=45960375

Family Applications (1)

Application Number Title Priority Date Filing Date
CN2011102671897A CN102426373A (zh) 2011-09-01 2011-09-01 一种惯性/卫星组合导航系统开闭环混合修正方法

Country Status (1)

Country Link
CN (1) CN102426373A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104135333A (zh) * 2014-07-24 2014-11-05 航天恒星科技有限公司 一种基于卡尔曼滤波器的tdma节点开环网络时间同步方法
CN107314768A (zh) * 2017-07-06 2017-11-03 上海海洋大学 水下地形匹配辅助惯性导航定位方法及其定位系统
CN107589441A (zh) * 2017-09-07 2018-01-16 成都理工大学 基于卡尔曼滤波器通道的脉冲堆积修正方法
CN108287354A (zh) * 2017-01-09 2018-07-17 北京四维图新科技股份有限公司 一种数据自动纠错方法和装置及导航设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361430A (zh) * 2000-12-23 2002-07-31 林清芳 增强的运动体定位和导航方法与系统
US20050060092A1 (en) * 2003-08-05 2005-03-17 The Boeing Company Laser range finder closed-loop pointing technology of relative navigation, attitude determination, pointing and tracking for spacecraft rendezvous
US20070156338A1 (en) * 2004-02-13 2007-07-05 Jacques Coatantiec Device for monitoring the integrity of information delivered by a hybrid ins/gnss system
CN101082493A (zh) * 2007-06-28 2007-12-05 华南农业大学 一种农业机械导航的组合定位方法
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361430A (zh) * 2000-12-23 2002-07-31 林清芳 增强的运动体定位和导航方法与系统
US20050060092A1 (en) * 2003-08-05 2005-03-17 The Boeing Company Laser range finder closed-loop pointing technology of relative navigation, attitude determination, pointing and tracking for spacecraft rendezvous
US20070156338A1 (en) * 2004-02-13 2007-07-05 Jacques Coatantiec Device for monitoring the integrity of information delivered by a hybrid ins/gnss system
CN101082493A (zh) * 2007-06-28 2007-12-05 华南农业大学 一种农业机械导航的组合定位方法
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李睿佳等: "《卫星/惯性组合导航事后高精度融合算法研究》", 《系统仿真学报》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104135333A (zh) * 2014-07-24 2014-11-05 航天恒星科技有限公司 一种基于卡尔曼滤波器的tdma节点开环网络时间同步方法
CN104135333B (zh) * 2014-07-24 2017-01-11 航天恒星科技有限公司 一种基于卡尔曼滤波器的tdma节点开环网络时间同步方法
CN108287354A (zh) * 2017-01-09 2018-07-17 北京四维图新科技股份有限公司 一种数据自动纠错方法和装置及导航设备
CN107314768A (zh) * 2017-07-06 2017-11-03 上海海洋大学 水下地形匹配辅助惯性导航定位方法及其定位系统
CN107314768B (zh) * 2017-07-06 2020-06-09 上海海洋大学 水下地形匹配辅助惯性导航定位方法及其定位系统
CN107589441A (zh) * 2017-09-07 2018-01-16 成都理工大学 基于卡尔曼滤波器通道的脉冲堆积修正方法
CN107589441B (zh) * 2017-09-07 2019-08-02 成都理工大学 基于卡尔曼滤波器通道的脉冲堆积修正方法

Similar Documents

Publication Publication Date Title
CN102221365B (zh) 用于确定惯性导航系统故障的系统和方法
US20150300827A1 (en) Telematics using personal mobile devices
CA2447809C (en) An inertial/gps navigation system
US6516021B1 (en) Global positioning systems and inertial measuring unit ultratight coupling method
CA2565787C (en) Gps navigation using successive differences of carrier-phase measurements
US5543804A (en) Navagation apparatus with improved attitude determination
CN1864078B (zh) 使用三个gps频率消除载波相位整数模糊度的方法
JP4780174B2 (ja) 角速度センサ補正装置、角速度算出装置、角速度センサ補正方法、角速度算出方法
CA2767684C (en) Method for selecting meteorological data for updating an aircraft trajectory
US7362263B2 (en) Keeping accurate time for a hybrid GPS receiver and mobile phone when powered off
CN103064097B (zh) 使用用于合成参考数据的电离层模型的gnss信号处理
US7911378B2 (en) System and method for applying code corrections for GNSS positioning
CN1794008B (zh) 测位用信号接收装置
US6453238B1 (en) Navigation system and method for tracking the position of an object
KR100740727B1 (ko) 측위 시스템, 정보 제공 장치, 정보 제공 장치의 제어프로그램을 기록한 컴퓨터 판독 가능한 기록매체,단말장치, 단말장치의 제어 프로그램을 기록한 컴퓨터 판독가능한 기록매체
EP1430272B1 (fr) Centrale de navigation inertielle hybride a integrite amelioree
EP1792201B1 (en) Improved gps accumulated delta range processing for navigation applications
EP1403654B1 (en) Position and velocity Kalman filter for use with global navigation satellite system receivers
CN103299205B (zh) 用于移动基站rtk测量的系统和方法
Geng et al. Adaptive estimation of multiple fading factors in Kalman filter for navigation applications
CN101680944B (zh) 在全球导航卫星系统中执行载波相位整数模糊度解算的方法及装置
US5893044A (en) Real time clock apparatus for fast acquisition or GPS signals
US8134499B2 (en) Ultra-tightly coupled GPS and inertial navigation system for agile platforms
Lu et al. Analysis of perishable-inventory systems with censored demand data
CN103424119B (zh) 导航装置

Legal Events

Date Code Title Description
PB01 Publication
C06 Publication
SE01 Entry into force of request for substantive examination
C10 Entry into substantive examination
AD01 Patent right deemed abandoned

Effective date of abandoning: 20120425

C20 Patent right or utility model deemed to be abandoned or is abandoned