CN103047999B - 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法 - Google Patents
一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法 Download PDFInfo
- Publication number
- CN103047999B CN103047999B CN201210552272.3A CN201210552272A CN103047999B CN 103047999 B CN103047999 B CN 103047999B CN 201210552272 A CN201210552272 A CN 201210552272A CN 103047999 B CN103047999 B CN 103047999B
- Authority
- CN
- China
- Prior art keywords
- navigation
- inertial navigation
- sub
- mtd
- mrow
- Prior art date
- Legal status (The legal status 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 status listed.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 238000012546 transfer Methods 0.000 title claims abstract description 46
- 238000004364 calculation method Methods 0.000 claims abstract description 71
- 230000004927 fusion Effects 0.000 claims abstract description 63
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 66
- 125000004122 cyclic group Chemical group 0.000 claims description 36
- 238000005259 measurement Methods 0.000 claims description 31
- 239000013598 vector Substances 0.000 claims description 17
- 238000004088 simulation Methods 0.000 claims description 14
- 238000003860 storage Methods 0.000 claims description 12
- 230000000694 effects Effects 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000007499 fusion processing Methods 0.000 claims description 6
- 238000007711 solidification Methods 0.000 claims description 6
- 230000008023 solidification Effects 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 3
- 238000013500 data storage Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 abstract 1
- 238000013459 approach Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005096 rolling process Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- HOWHQWFXSLOJEF-MGZLOUMQSA-N systemin Chemical compound NCCCC[C@H](N)C(=O)N[C@@H](CCSC)C(=O)N[C@@H](CCC(N)=O)C(=O)N[C@@H]([C@@H](C)O)C(=O)N[C@@H](CC(O)=O)C(=O)OC(=O)[C@@H]1CCCN1C(=O)[C@H]1N(C(=O)[C@H](CC(O)=O)NC(=O)[C@H](CCCN=C(N)N)NC(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H]2N(CCC2)C(=O)[C@H]2N(CCC2)C(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H](CCC(N)=O)NC(=O)[C@@H](NC(=O)[C@H](C)N)C(C)C)CCC1 HOWHQWFXSLOJEF-MGZLOUMQSA-N 0.000 description 1
- 108010050014 systemin Proteins 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法,针对舰船主/子组合中的传递对准,设计了一种适用于系泊/匀速直航条件下的陀螺误差快速估计方法,对子惯导陀螺常值误差进行快速估计,在实时多任务操作***的支持下,子惯导***对同一组数据进行循环导航解算与信息融合,充分利用计算机的高速性能,在传递对准过程中,快速完成对陀螺常值误差的估计。本发明不改变导航解算现有算法,不改变传递对准信息融合算法中的滤波器结构与信息匹配方式,充分利用现有导航计算机富余资源,加快传递对准过程中对陀螺误差的估计速度。
Description
技术领域
本发明涉及舰载主/子惯导快速传递对准方法,特别适用于舰船系泊或匀速直航条件下,陀螺误差的快速估计,为一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法。
背景技术
舰载环境下,主惯导(舰载平台式惯性导航***)的精度一般高于子惯导(如舰载导弹用捷联式惯性导航***)的精度数个量级。子惯导利用主惯导的导航信息完成初始化、失准角估计、器件误差估计的过程称为传递对准。
传递对准作为惯性技术领域的一种通用的、共性的技术,广泛应用于战车、战机、舰船、航天器等武器平台上所搭载子惯导***的初始对准。基于武器***快速反应的要求,传递对准的快速性一直是初始对准的目标。1989年,Kain和Cloutier首次提出“速度+姿态”匹配方式,并配合以载机的摇翼动作,即可在10s内达到1mrad的对准精度。此外,Spalding、Shortelle和Graham等人也验证了上述方法的有效性。
上述方法在强调了对准过程中失准角估计快速性时,弱化或忽略了对器件误差的估计。但是惯性器件存在以下问题:1)在长时间存放后,会存在一定量级的漂移,器件精度越低,漂移越严重;2)存在逐次启动误差,器件精度越低,误差越大。对于中低精度的子惯导***而言,在其初始对准时,若不对器件误差进行估计,必将导致器件误差在长时间导航解算过程中被积分放大,从而降低***导航精度。
事实上,传递对准Kalman滤波器中各状态量的可观测性依赖于信息匹配方式以及载体机动方式。在摇摆运动时,采用“速度+姿态”匹配方式,仪表误差的估计需要数分钟之久,在10s内无法估计。
发明内容
本发明要解决的问题是:舰载环境下,现有传递对准方法忽略了对惯性器件的误差估计,降低了导航精度,并且现有技术对误差的估计不够快速,影响导航的实时性。
本发明的技术方案为:一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法,在实时多任务操作***的支持下对陀螺常值误差进行快速估计,主/子惯导传递对准过程中,子惯导进行导航解算,子惯导利用主惯导的导航数据进行信息融合,设t0为陀螺常值误差估计的起始时刻,亦为初始对准的起始时刻,t0~t1为数据采集、数据存储、实时导航解算与信息融合以及姿态矩阵更新时间段,t1~t2为基于t0~t1间存储信息的循环导航解算与信息融合以及姿态矩阵更新时间段,上述各时间的确定如下:
T1)针对具体子惯导***,在主/子惯导传递对准中,通过子惯导陀螺仪误差收敛曲线确定完成整个对准所需要的时间,再根据子惯导测量与导航解算更新周期Δt,确定完成整个对准所需要的导航解算总更新次数,设定为ksum;根据主惯导导航信息更新周期ΔT,确定完成整个对准所需要的信息融合次数,ΔT即为信息融合周期;在确定的工作环境下,导航解算与信息融合两者时序关系确定,导航解算次数确定后,信息融合次数即可确定;
T2)根据子惯导仪表数据质量,主惯导量测信息质量,设定时刻t1及时间段t0~t1,并根据Δt计算得到该时间段内导航解算更新次数,设定为ksum1;
T3)从整个对准所需要的导航解算总更新次数ksum中,扣除步骤T2)中t0~t1时间段内导航解算更新次数ksum1,得到完成整个对准还需要完成的导航解算更新次数,设定为ksum2,ksum2=ksum-ksum1,从而得到t1~t2时间段的循环导航解算循环次数,设定为m,m=ksum2/ksum1,在m不为整数时,将m值向上圆整,并根据圆整后的m值调整ksum2;时刻t2由循环导航解算循环次数m和导航计算机的性能共同确定,在m确定时,导航计算机主频越高,t2与t1间差值越小;
根据所述确定的时间,在传递对准中完成子惯导陀螺误差估计,包括如下步骤:
1)在t0时刻,利用主惯导的位置、速度与姿态信息对子惯导的相应导航信息进行初始化,得到子惯导的位置、速度与姿态矩阵t为时间变量;瞬时凝固t0时刻的载体坐标系为b0,得到I为(3×3)单位矩阵;
2)在时间段t0~t1内,子惯导按周期Δt进行惯性仪表数据的采集、存储、导航解算,并利用式(6)更新子惯导载体坐标系b相对于b0系的姿态矩阵
式中,下标k1=1~ksum1表示导航解算更新的次数标号,为子惯导陀螺实时采集的数据,
同时子惯导按照周期ΔT接收来自主惯导的导航信息并存储,所述导航信息包括速度和航向,并记录主惯导导航信息与子惯导仪表数据间的时序关系;通过信息融合进行主、子惯导传递对准;
3)在步骤2)结束t1时刻,得到该时刻的实时载体矩阵瞬时凝固此时刻的载体坐标系为b1,有
4)在时间段t1~t2内,子惯导***进行如下工作:
41)对步骤2)中采集的子惯导惯性仪表数据进行m次循环导航解算和信息融合,t1~t2内进行m次循环导航解算,各子循环周期为:t10~t11、t20~t21、…、tj0~tj1、…tm0~tm1,j=1~m表示子循环周期的标号,在各子循环的导航解算与信息融合过程中,所用子惯导仪表数据与主惯导信息分别对应t0~t1内子惯导***采集的仪表数据与主惯导的导航信息,在每一个子循环周期内,导航解算更新次数与信息融合次数与t0~t1内的次数分别相等;循环解算过程中,舰船处于系泊或匀速直航状态,各子循环解算开始时刻的速度、位置直接取上一子循环周期结束时刻的速度与位置,但因船舶存在摇摆运动,各子循环解算开始时刻的姿态需要如下处理,在各子循环tj0~tj1开始时刻tj0的初始姿态矩阵:
式中,为在t(j-1)1时刻计算得到的载体坐标系b相对导航坐标系n的姿态矩阵,即第j-1个子循环解算结束时的姿态矩阵,
在每一个子循环过程中,按步骤2)中记录的时序关系进行导航解算与信息融合;
42)在进行上述循环导航解算与信息融合的同时,子惯导对惯性仪表数据进行实时采集,更新计算当前载体坐标系b系相对于b1系的姿态矩阵其离散形式为:
其中 k2为更新次数,步骤42)最终得到
5)在完成m个周期的循环导航解算以及对应的信息融合后,子惯导***根据tm1时刻载体坐标系b相对于导航坐标系n的姿态估计矩阵即t1时刻姿态矩阵的事后估计值,以及t2时刻载体坐标系b相对于b1的实时姿态矩阵得到t2时刻的实时姿态矩阵用于子惯导的导航;同时子惯导得到tm1时刻的陀螺误差估计值,即t1时刻陀螺误差的事后估计值,将tm1时刻的陀螺误差估计值作为最终的陀螺误差估计值;
T4)反复进行上述步骤T2)和步骤T3),通过离线仿真的形式,选择不同的时间点t1,得到不同的循环导航解算循环数m,根据步骤1)-5)对陀螺误差进行快速估计,选择最能接近T1)中估计效果的t1和m作为最佳时间点t1和循环导航计算循环数m,正式用于舰载主/子惯导的传递对准,所述估计效果指T1)中子惯导陀螺仪误差收敛曲线的收敛效果;在实际舰载主/子惯导传递对准过程中进行步骤1)-5),实现陀螺误差快速估计。
所述实时多任务操作***为多任务实时操作***VxWorks,完成以下四个任务,任务的调度由多任务实时操作***VxWorks按照任务的优先级别自动执行,在子惯导中运行以下四个任务:
1)任务一:子惯导仪表数据采集、存储与导航解算与更新解算,优先级别最高;
2)任务二:子惯导仪表数据采集、更新解算,优先级别第二;
3)任务三:子惯导对主惯导进行数据采集、存储与信息融合,优先级别第三;
4)任务四:子惯导基于存储数据的循环导航解算与信息融合,优先级别最低。传递对准过程中,子惯导的导航解算算法为:
式中,为子惯导载体坐标系b相对导航坐标系n的姿态矩阵,I为(3×3)单位矩阵,下标k=1~+∞表示导航解算的更新次数标号,n表示导航坐标系,b表示载体坐标系,i表示惯性坐标系,表示地球坐标系e相对于惯性坐标系i在导航坐标系n中的投影角速度,表示导航坐标系n相对于地球坐标系e在导航坐标系n中的投影角速度;式(1)中的 为子惯导陀螺采集数据, 分别为子惯导的东、北、天向速度,L、λ和h分别表示导航坐标系下的速度、纬度、经度和高度,fb为子惯导加速度计采集数据,gn为地球重力加速度在导航坐标系中的投影,RM和RN分别为导航所在地地球的子午圈和卯酉圈半径;运算符“×”表示向量反斜运算,舰载环境下,直接取子惯导天向速度与高度为零。
传递对准过程中,主、子惯导信息融合为:
采用Kalman滤波器作为信息融合滤波器,取子惯导东向/北向速度误差、失准角和陀螺误差作为***状态向量,即
X=[δVE δVN φE φN φU εx εy εz]T
其中,δVE/δVN为东向/北向速度误差;φE、φN、φU分别为纵摇、横摇、航向失准角;εx、εy、εz分别为三轴陀螺误差;
取***状态方程,
式中,A(t)为状态矩阵,W(t)为***噪声,根据***状态变量中速度、失准角误差方程,以及陀螺从载体坐标系b向导航坐标系n的投影关系,状态矩阵A(t)表示为:
其中,VE、VN为子惯导东向与北向速度,ωie为地球自转角速度,R为地球半径,L为当地地理纬度,fE、fN、fU分别为子惯导加速度测量值fb在导航坐标系n中的投影,Tpq,p,q=1,2,3为子惯导载体坐标系b相对导航坐标系n的姿态矩阵的各元素;
采用“速度+航向”信息匹配方式,取子、主惯导的速度、航向差值直接构造量测信息,即子/主惯导导航***量测向量为
Z=[VE-VME VN-VMN H-HM]T
其中,VE与VME分别为子、主惯导的东向速度;VN与VMN分别为子、主惯导的北向速度;H与HM分别为子、主惯导的航向信息,H从子惯导姿态矩阵中直接提取;
***量测方程为
Z(t)=H(t)X(t)+V(t) (5)
式中,H(t)为量测矩阵,V(t)为量测噪声,根据量测向量与状态向量的关系,有量测矩阵为
信息融合过程中,采用闭环修正方式,将信息融合估计得到的速度误差、失准角与陀螺误差估计值反馈回子惯导***参与导航解算。
针对舰船主/子组合中的传递对准,本发明设计了一种适用于系泊/匀速直航条件下的陀螺误差快速估计方法,对子惯导陀螺常值误差进行快速估计,在实时多任务操作***的支持下,子惯导***对同一组数据进行循环导航解算与信息融合,充分利用计算机的高速性能,在传递对准过程中,快速完成对陀螺常值误差的估计。本发明的方法具有如下优点:(1)不改变现有传递对准的信息匹配模式,不要求运载体做特定机动;(2)直接利用目前比较成熟的Kalman滤波技术进行估计;(3)利用导航计算机在姿态更新周期与信息融合周期内的富余计算机资源进行工作,不对计算机性能提出更高要求,虽然对准速度很大程度上仍依赖于计算机性能,但在与现有导航***同样计算机资源配置的情况下,可以取得更快的传递对准速度。
附图说明
图1为本发明沿时间轴对准过程步骤分解图;
图2为本发明使用的信息融合算法示意图;
图3为本发明循环计算过程图;
图4为本发明系泊条件下失准角估计误差图;
图5为本发明系泊条件下对仪表误差的估计曲线图;
图6为本发明匀速直航条件下失准角估计误差图;
图7为本发明匀速直航条件下对陀螺误差的估计曲线图。
具体实施方式
本发明在分析捷联式惯性导航***以“数学平台”代替平台式惯性导航***中“物理平台”的相关特点后认为,在捷联式惯性导航***中,可以利用同一数据进行导航解算,并不断调整数学平台,充分利用计算机的高速性能,在传递对准过程中,快速完成对仪表误差的估计。
下面结合附图对本发明实施方法做更详细地描述:
图1中,t0为陀螺常值误差估计的起始时刻,t0~t1为数据存储、导航解算与信息融合以及更新时间段,t1~t2为基于存储信息的循环导航解算与信息融合以及基于陀螺测量信息的更新时间段。Δt为子惯导的惯性仪表数据与导航解算更新周期;ΔT为主惯导信息更新周期与主子惯导信息融合周期,ΔT为Δt的整数倍,在本发明中,Δt为导航解算周期,ΔT为信息融合周期。对于确定的主/子惯导组合***而言,Δt与ΔT为确定值,且子惯导仪表数据与主惯导参考导航信息两者时序关系确定,也即导航解算与信息融合两者时序关系确定,导航解算次数确定后,信息融合次数即可确定。上述t1的选择以及循环次数m的确定需要根据具体***通过多次仿真确定。具体如下:
T1)针对具体子惯导***,在主/子惯导传递对准中,通过观察子惯导陀螺误差估计曲线的收敛情况,确定完成整个对准所需要的时间,根据子惯导测量与导航更新周期,确定完成整个对准所需要的导航解算总更新次数ksum;根据主惯导导航信息更新周期,确定完成整个对准所需要的信息融合次数;
T2)根据子惯导仪表数据质量,主惯导量测信息质量,选择时间t1,并根据Δt计算得到该时间段内导航解算更新次数,设定为ksum1,t0~t1是本发明中获取基础数据的时间段,后面的t1~t2时间段根据t0~t1时间段获得的数据进行进一步的误差估计,t0~t1的长短根据需要根据子惯导、主惯导的数据质量确定;
T3)从整个对准所需要的导航解算总更新次数ksum中,扣除步骤T2)中t0~t1时间段内导航解算更新次数ksum1,得到完成整个对准还需要完成的导航解算更新次数,设定为ksum2=ksum-ksum1,从而得到t1~t2时间段的循环导航解算循环次数,设定为m=ksum2/ksum1,在m不为正数时,将m值向上圆整,并根据圆整后的m值调整ksum2。时间t2由循环导航解算循环次数和导航计算机的性能共同确定,在m确定时,计算机主频越高,t2与t1间差值越小;
T4)根据上述步骤T2)和步骤T3),通过离线仿真的形式,比较选择最佳时间点t1和循环导航计算循环数m,实际用于主/子惯导的传递对准,所述最佳是指根据选定的时间点t1和循环数m得到的陀螺误差估计最贴近实际子惯导陀螺误差估计曲线的收敛情况。
本发明在实时多任务操作***的支持下进行陀螺误差估计:
图1中,(a)在t0~t1时间段,由于信息融合计算量相对较大,一般需要若干Δt方可完成,若采用单任务程序模式,即完成信息融合后才能进行仪表数据采集与导航解算,则程序设计复杂,很难满足导航解算实时性要求;(b)在t1~t2时间段,基于存储的t0~t1时间段的数据进行导航解算与信息融合,需要占用除更新外的所有计算机资源,若采用单任务模式,则程序设计复杂,很难满足导航解算实时性要求。
基于单任务模式不能满足上述要求,引入实时多任务操作***VxWorks,在VxWorks环境下,定义如下任务:
1)任务一:仪表数据采集、存储与导航解算与更新解算,优先级别最高;
2)任务二:仪表数据采集、更新解算,优先级别第二;
3)任务三:主惯导数据采集、存储与信息融合,优先级别第三;
4)任务四:基于存储数据的循环导航解算与信息融合,优先级别最低。
上述任务一、三在初始化后被发起,按照任务的优先级别占用导航计算机资源,在不进行信息融合时,任务一单独占用导航计算机资源;在进行信息融合时,若需要进行仪表数据采集、存储与导航解算,则任务三放弃对导航计算机资源的占用,任务一占用,在任务一完成后,任务三重新占用。
在t1时刻,当任务一完成后,任务一被删除,同时任务二被发起;在任务三完成后,任务三亦被删除,同时任务四被发起;在t1~t2时间段,任务二与任务四按照优先级别占用导航计算机资源,在任务二不需要执行时,任务四占用导航计算机资源进行循环导航解算。
本发明所采用的导航解算具体为:
式中,为子惯导载体坐标系b相对导航坐标系n的姿态矩阵,I为(3×3)单位矩阵,下标k=1~+∞表示导航解算的更新次数标号,n表示导航坐标系,b表示载体坐标系,i表示惯性坐标系,表示地球坐标系e相对于惯性坐标系i在导航坐标系n中的投影角速度,表示导航坐标系n相对于地球坐标系e在导航坐标系n中的投影角速度。式(4)中的 为子惯导陀螺采集数据, 分别为子惯导的东、北、天向速度,L、λ和h分别表示导航坐标系下的速度、纬度、经度和高度,fb为子惯导加速度计采集数据,gn为地球重力加速度在导航坐标系中的投影,RM和RN分别为导航所在地地球的子午圈和卯酉圈半径;运算符“×”表示向量反斜运算,舰载环境下,直接取子惯导天向速度与高度为零。本发明信息融合具体采用Kalman滤波器与“速度+航向”的匹配方式,如图2所示。
取东向/北向速度误差、失准角和陀螺误差作为***状态向量,即
X=[δVE δVN φE φN φU εxεyεz]T
其中,δVE/δVN为东向/北向速度误差;φE~U为纵摇、横摇、航向失准角;εx~z为三轴陀螺误差。
取***状态方程,
式中,A(t)为状态矩阵,W(t)为***噪声,根据***状态变量中速度、失准角误差方程,以及陀螺从载体坐标系b向导航坐标系n投影关系,状态矩阵A(t)表示为:
其中,VE、VN为东向与北向速度,ωie为地球自转角速度,R为地球半径,L为当地地理纬度,fE、fN、fU分别为加速度测量值fb在导航坐标系n中的投影,Tij(i,j=1,2,3)为子惯导载体坐标系b相对导航坐标系n的姿态矩阵的各元素;
采用速度+航向信息匹配方式。
取子、主惯导的速度、航向差值直接构造量测信息,即***量测向量为
Z=[VE-VME VN-VMN H-HM]T
其中,VE与VME分别为子、主惯导的东向速度;VN与VMN分别为子、主惯导的北向速度;H与HM分别为子、主惯导的航向信息,H可以从子惯导姿态矩阵中直接提取。
***量测方程为
Z(t)=H(t)X(t)+V(t) (5)
式中,H(t)为量测矩阵,V(t)为量测噪声,根据量测向量与状态向量的关系,有量测矩阵为
***采用闭环修正方式,如图2所示,信息融合过程中,得到的关于速度误差、失准角与陀螺误差被反馈回子惯导***参与运算。完成循环导航解算与信息融合后,得到的t1时刻陀螺误差估计值,即作为子惯导***陀螺误差的估计值。
下面具体说明本发明的陀螺误差估计过程:
1)在t0时刻,利用主惯导的位置、速度与姿态信息对子惯导的相应导航信息进行初始化,得到子惯导的位置、速度与姿态矩阵t为时间变量;瞬时凝固t0时刻的载体坐标系为b0,得到I为(3×3)单位矩阵;
2)在时间段t0~t1内,子惯导按周期Δt进行惯性仪表数据的采集、存储、导航解算,并利用式(6)更新子惯导载体坐标系b相对于b0系的姿态矩阵
式中,下标k1=1~ksum1表示导航解算更新的次数标号,更新次数k1与姿态矩阵的变量时间t是对应的,为子惯导陀螺实时采集的数据,
同时子惯导按周期ΔT接收来自主惯导的导航信息并存储,所述导航信息包括速度和航向,并记录主惯导导航信息与子惯导仪表数据间的时序关系;通过信息融合进行主、子惯导传递对准;
3)在步骤2)结束t1时刻,根据导航解算得到该时刻的实时载体矩阵瞬时凝固此时刻的载体坐标系为b1,有
4)在时间段t1~t2内,子惯导***进行如下工作:
41)对步骤2)中采集的子惯导惯性仪表数据进行m次循环导航解算和信息融合,t1~t2内进行m次循环导航解算,如图3所示,各子循环周期为:t10~t11、t20~t21、…、tj0~tj1、…tm0~tm1,j=1~m表示子循环周期的标号,在各子循环的导航解算与信息融合过程中,所用子惯导仪表数据与主惯导信息分别对应t0~t1内子惯导***采集的仪表数据与主惯导的导航信息,在每一个子循环周期内,导航解算更新次数与信息融合次数与t0~t1内的次数分别相等,在各个子循环过程tj0~tj1中,所用仪表数据与主惯导信息分别对应t0~t1中各采集点对应,其物理意义体现t0~t1时间段的实时性,但解算过程占用t1~t2时间的计算机资源,即每次子循环都是对t0~t1时间段的数据进行导航解算,在t1~t2内进行m次,循环解算过程中,舰船处于系泊或匀速直航状态,各子循环解算开始时刻的速度、位置直接取上一子循环周期结束时刻的速度与位置,但因船舶存在摇摆运动,各子循环解算开始时刻的姿态需要如下处理,在各子循环tj0~tj1开始时刻tj0的初始姿态矩阵:
式中,为在t(j-1)1时刻计算得到的载体坐标系b相对导航坐标系n的姿态矩阵,即第j-1个子循环解算结束时的姿态矩阵,
例如在t10~t11循环周期内,t10为起始时刻,其初始姿态矩阵可计算如下:
式中,为在t1时刻解算得到的载体坐标系b相对导航坐标系n的姿态矩阵。根据导航解算后得到再进行下一轮循环解算,直至得到
在每一个子循环过程中,按步骤2)中记录的时序关系进行导航解算与信息融合;循环解算结束时,即在t2时刻,因为是对t0~t1时间段的数据进行导航解算,上述循环导航解算得到姿态矩阵的实时性意义对应于t1时刻,由根据前述导航解算得到;
42)在进行上述循环导航解算与信息融合的同时,子惯导对惯性仪表数据进行实时采集,更新计算当前载体坐标系b系相对于b1系的姿态矩阵其离散形式为:
其中 k2为更次次数。步骤42)最终得到k2的取值上限由循环次数m以及计算机主频共同决定。时间段t1~t2内步骤41)与步骤42)同时进行,时间上步骤41)会先于步骤42)完成。
5)在完成m个周期的循环导航解算以及对应的信息融合后,子惯导***得到tm1时刻载体坐标系b相对于导航坐标系n的姿态估计矩阵即t1时刻姿态矩阵的事后估计值,以及t2时刻载体坐标系b相对于b1的实时姿态矩阵得到t2时刻的实时姿态矩阵 用于子惯导的导航;同时子惯导由第m次子循环的信息融合得到tm1时刻的陀螺误差估计值,即t1时刻陀螺误差的事后估计值,将tm1时刻的陀螺误差估计值作为最终的陀螺误差估计值;
T4)反复进行上述步骤T2)和步骤T3),通过离线仿真的形式,选择不同的时间点t1,得到不同的循环导航解算循环数m,根据步骤1)-5)对陀螺误差进行快速估计,选择最能接近T1)中估计效果的t1和m作为最佳时间点t1和循环导航计算循环数m,此处估计效果指T1)中子惯导陀螺仪误差收敛曲线的收敛效果,正式用于舰载主/子惯导的传递对准,在实际舰载主/子惯导传递对准过程中进行步骤1)-5),实现陀螺误差快速估计。
本发明的有益效果通过如下仿真得以验证:
1)Matlab模拟惯性仪表数据与主惯导数据
(1)系泊条件下模拟数据的产生
舰船在系泊条件下,以正弦规律绕三轴作摇摆运动,其纵摇、横向与航向摇摆数学模型为:
式中,θ、γ和ψ分别为纵摇、横摇与航向的角度变量;AP、AR、AY分别为纵摇、横摇与航向的摇摆幅值;ωP、ωP与ωY分别表示纵摇、横摇与航向的摇摆角频率;与分别纵摇、横摇与航向初始相位;ψ0表示初始航向;ωi=2π/Ti,i=P,R,Y,Ti表示相应的摇摆周期。
以上述仿真数据并叠加白噪声作为主惯导信息,主惯导对所述数据的采样周期为1s。
通过上述仿真数据模拟得到子惯导仪表理论数据,并在其上叠加相应的仪表误差作为仪表实际采集数据,子惯导对所述仪表实际采集数据进行采样,用于导航解算,采样周期为10ms。
仿真的相关参数:
舰船初始位置:东经118°、北纬32°;
舰船航速:0m/s;
舰船摇摆幅值:纵摇9°、横摇14°、航摇12°;
舰船摇摆周期:纵摇8s、横摇10s、航摇6s;
舰船摇摆初始相位:均为0;
舰船初始航向:0°;
赤道半径:6378165m;
地球椭球度:1/298.3;
地球表面重力加速度:9.8m/s2;
地球自转角速度:15.04088°/h;
陀螺常值误差:0.5°/h;
陀螺白噪声误差:0.5°/h;
加速度计零偏:500ug;
加速度计白噪声误差:500ug;
主惯导***量测白噪声参数:东/北向速度误差方差0.2m/s、航向误差方差0.15°/h。
(2)匀速直航条件下模拟数据的产生
匀速直航条件下相关参数与系泊条件相同,不同点为:舰船航速为10m/s。
2)导航计算机上数学验证
在x-86结构的导航计算机上分别采用两种方案进行验证,方案一:经典传递对准,即以时间为顺序进行仪表数据采集、导航解算,并在有主惯导量测信息时进行信息融合;方案二:采用本发明设计的快速对准算法,其中数据存储120s,即t1=120s。分别比较两种方案的对准精度与对准时间。
图4与图5的曲线表明,在系泊条件下,两种方案的对准精度相当,对陀螺误差的估计亦相当。图6与图7表明,在匀速直航条件下,两种方案的对准精度相当,对陀螺误差的估计亦相当。
但两种方案所用的对准时间不同。
对于方案一而言,图4~7中横坐标表示时间,在系泊与匀速直航条件下,方案一完成对失准角的估计需要200s左右,而对陀螺误差的估计大约需要600s。
对于方案二而言,图4~7中横坐标表示迭代次数,完成对失准角的估计需要迭代次数20000次,完成对陀螺误差的估计大约需要60000次。以完成迭代60000次分析计算机耗时,其中前12000次,需要存储惯性仪表数据与主惯导量测数据,耗时120s,该耗时与方案一相同;但在12001~60000(共计480000)次,所用时间由计算机速度决定。
在本仿真中,所用计算机主频333MHz。该型计算机完成一次导航解算以及或需要时间1ms;单独完成一次完成或需要约0.2ms;完成一次信息融合需要25ms。在48000次导航更新中,需要进行信息融合480次,导航更新耗时38.4s,信息融合耗时12s,共计耗时50.4s。在循环计算过程中,计算机需要更新以及***任务调度开销,计算机实际测量耗时59.2s。整个对准过程耗时120+59.2=179.2s,即t2=179.2s,对陀螺误差的估计时间远小于方案一的600s。
因此本发明设计的方法在对准精度上与经典对准方案相当,但是对陀螺误差的估计速度得到显著提高。
Claims (4)
1.一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法,其特征是在实时多任务操作***的支持下对陀螺常值误差进行快速估计,主/子惯导传递对准过程中,子惯导进行导航解算,子惯导利用主惯导的导航数据进行信息融合,设t0为陀螺常值误差估计的起始时刻,亦为初始对准的起始时刻,t0~t1为数据采集、数据存储、实时导航解算与信息融合以及姿态矩阵更新时间段,t1~t2为基于t0~t1间存储信息的循环导航解算与信息融合以及姿态矩阵更新时间段,上述各时间的确定如下:
T1)针对具体子惯导***,在主/子惯导传递对准中,通过子惯导陀螺仪误差收敛曲线确定完成整个对准所需要的时间,再根据子惯导测量与导航解算更新周期Δt,确定完成整个对准所需要的导航解算总更新次数,设定为ksum;根据主惯导导航信息更新周期ΔT,确定完成整个对准所需要的信息融合次数,ΔT即为信息融合周期;在确定的工作环境下,导航解算与信息融合两者时序关系确定,导航解算次数确定后,信息融合次数即可确定;
T2)根据子惯导仪表数据质量,主惯导量测信息质量,设定时刻t1及时间段t0~t1,并根据Δt计算得到该时间段内导航解算更新次数,设定为ksum1;
T3)从整个对准所需要的导航解算总更新次数ksum中,扣除步骤T2)中t0~t1时间段内导航解算更新次数ksum1,得到完成整个对准还需要完成的导航解算更新次数,设定为ksum2,ksum2=ksum-ksum1,从而得到t1~t2时间段的循环导航解算循环次数,设定为m,m=ksum2/ksum1,在m不为整数时,将m值向上圆整,并根据圆整后的m值调整ksum2;时刻t2由循环导航解算循环次数m和导航计算机的性能共同确定,在m确定时,导航计算机主频越高,t2与t1间差值越小;
根据所述确定的时间,在传递对准中完成子惯导陀螺误差估计,包括如下步骤:
1)在t0时刻,利用主惯导的位置、速度与姿态信息对子惯导的相应导航信息进行初始化,得到子惯导的位置、速度与姿态矩阵t为时间变量;瞬时凝固t0时刻的载体坐标系为b0,得到I为(3×3)单位矩阵;
2)在时间段t0~t1内,子惯导按周期Δt进行惯性仪表数据的采集、存储、导航解算,并利用式(1)更新子惯导载体坐标系b相对于b0系的姿态矩阵
式中,下标k1=1~ksum1表示导航解算更新的次数标号,为子惯导陀螺实时采集的数 据,运算符“×”表示向量反斜运算;
同时子惯导按照周期ΔT接收来自主惯导的导航信息并存储,所述导航信息包括速度和航向,并记录主惯导导航信息与子惯导仪表数据间的时序关系;通过信息融合进行主、子惯导传递对准;
3)在步骤2)结束t1时刻,得到该时刻的实时载体矩阵瞬时凝固此时刻的载体坐标系为b1,有
4)在时间段t1~t2内,子惯导***进行如下工作:
41)对步骤2)中采集的子惯导惯性仪表数据进行m次循环导航解算和信息融合,t1~t2内进行m次循环导航解算,各子循环周期为:t10~t11、t20~t21、tj0~tj1、tm0~tm1,j=1~m表示子循环周期的标号,在各子循环的导航解算与信息融合过程中,所用子惯导仪表数据与主惯导信息分别对应t0~t1内子惯导***采集的仪表数据与主惯导的导航信息,在每一个子循环周期内,导航解算更新次数与信息融合次数与t0~t1内的次数分别相等;循环解算过程中,舰船处于系泊或匀速直航状态,各子循环解算开始时刻的速度、位置直接取上一子循环周期结束时刻的速度与位置,但因船舶存在摇摆运动,各子循环解算开始时刻的姿态需要如下处理,在各子循环tj0~tj1开始时刻tj0的初始姿态矩阵:
式中,为在t(j-1)1时刻计算得到的载体坐标系b相对导航坐标系n的姿态矩阵,即第j-1个子循环解算结束时的姿态矩阵,
在每一个子循环过程中,按步骤2)中记录的时序关系进行导航解算与信息融合;
42)在进行上述循环导航解算与信息融合的同时,子惯导对惯性仪表数据进行实时采集,更新计算当前载体坐标系b系相对于b1系的姿态矩阵其离散形式为:
其中k2为更新次数,运算符“×”表示向量反斜运算,步骤42)最终得到
5)在完成m个周期的循环导航解算以及对应的信息融合后,子惯导***根据tm1时刻载体坐标系b相对于导航坐标系n的姿态估计矩阵即t1时刻姿态矩阵的事后估计值,以及t2时刻载体坐标系b相对于b1的实时姿态矩阵得到t2时刻的实时姿 态矩阵用于子惯导的导航;同时子惯导得到tm1时刻的陀螺误差估计值,即t1时刻陀螺误差的事后估计值,将tm1时刻的陀螺误差估计值作为最终的陀螺误差估计值;
T4)反复进行上述步骤T2)和步骤T3),通过离线仿真的形式,选择不同的时间点t1,得到不同的循环导航解算循环数m,根据步骤1)-5)对陀螺误差进行快速估计,选择最能接近T1)中估计效果的t1和m作为最佳时间点t1和循环导航计算循环数m,正式用于舰载主/子惯导的传递对准,所述估计效果指T1)中子惯导陀螺仪误差收敛曲线的收敛效果;在实际舰载主/子惯导传递对准过程中进行步骤1)-5),实现陀螺误差快速估计。
2.根据权利要求1所述的一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法,其特征是所述实时多任务操作***为多任务实时操作***VxWorks,完成以下四个任务,任务的调度由多任务实时操作***VxWorks按照任务的优先级别自动执行,在子惯导中运行以下四个任务:
1)任务一:子惯导仪表数据采集、存储与导航解算与更新解算,优先级别最高;
2)任务二:子惯导仪表数据采集、更新解算,优先级别第二;
3)任务三:子惯导对主惯导进行数据采集、存储与信息融合,优先级别第三;
任务四:子惯导基于存储数据的循环导航解算与信息融合,优先级别最低。
3.根据权利要求1或2所述的一种舰载主/子惯导传递对准过程中的陀螺误差
快速估计方法,其特征是传递对准过程中,子惯导的导航解算算法为:
式中,为子惯导载体坐标系b相对导航坐标系n的姿态矩阵,I为(3×3)单位矩阵,下标k=1~+∞表示导航解算的更新次数标号,n表示导航坐标系,b表示载体坐标系,i表示惯性坐标系,表示地球坐标系e相对于惯性坐标系i在导航坐标系n中的投影角速度,表示导航坐标系n相对于地球坐标系e在导航坐标系n中的投影角速度;式(4)中的 为子惯导陀螺采集数据, 分别为子惯导的东、北、天向速度,L、λ和h分别表示导航坐标系下的速度、纬度、经度和高度,fb为子惯导加速度计采集数据,gn为地球重力加速度在导航坐标系中的投影,RM和RN分别为导航所在地地球的子午圈和卯酉圈半径,运算符“×”表示向量反斜运算,舰载环境下,直接取子惯导天向速度与高度为零。
4.根据权利要求1或2所述的一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法,其特征是传递对准过程中,主、子惯导信息融合为:
采用Kalman滤波器作为信息融合滤波器,取子惯导东向/北向速度误差、失准角和陀螺误差作为***状态向量,即
X=[δVE δVN φE φN φU εx εy εz]T
其中,δVE/δVN为东向/北向速度误差;φE、φN、φU分别为纵摇、横摇、航向失准角;εx、εy、εz分别为三轴陀螺误差;
取***状态方程,
式中,A(t)为状态矩阵,W(t)为***噪声,根据***状态变量中速度、失准角误差方程,以及陀螺从载体坐标系b向导航坐标系n的投影关系,状态矩阵A(t)表示为:
其中,VE、VN为子惯导东向与北向速度,ωie为地球自转角速度,R为地球半径,L为当地地理纬度,fE、fN、fU分别为子惯导加速度测量值fb在导航坐标系n中的投影,Tpq,p,q=1,2,3为子惯导载体坐标系b相对导航坐标系n的姿态矩阵的各元素;
采用“速度+航向”信息匹配方式,取子、主惯导的速度、航向差值直接构造量测信息,即子/主惯导导航***量测向量为
Z=[VE-VME VN-VMN H-HM]T
其中,VE与VME分别为子、主惯导的东向速度;VN与VMN分别为子、主惯导的北向速度;H与HM分别为子、主惯导的航向信息,H从子惯导姿态矩阵中直接提取;
***量测方程为
Z(t)=H(t)X(t)+V(t) (8)
式中,H(t)为量测矩阵,V(t)为量测噪声,根据量测向量与状态向量的关系,有量测矩阵为
信息融合过程中,采用闭环修正方式,将信息融合估计得到的速度误差、失准角与陀螺误差估计值反馈回子惯导***参与导航解算。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210552272.3A CN103047999B (zh) | 2012-12-18 | 2012-12-18 | 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210552272.3A CN103047999B (zh) | 2012-12-18 | 2012-12-18 | 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103047999A CN103047999A (zh) | 2013-04-17 |
CN103047999B true CN103047999B (zh) | 2015-09-30 |
Family
ID=48060730
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201210552272.3A Expired - Fee Related CN103047999B (zh) | 2012-12-18 | 2012-12-18 | 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103047999B (zh) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105021212B (zh) * | 2015-07-06 | 2017-09-26 | 中国人民解放军国防科学技术大学 | 一种初始方位信息辅助下潜航器快速传递对准方法 |
CN105352529B (zh) * | 2015-11-16 | 2018-02-16 | 南京航空航天大学 | 多源组合导航***分布式惯性节点全误差在线标定方法 |
CN106767900B (zh) * | 2016-11-23 | 2020-01-03 | 东南大学 | 一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法 |
CN108981696B (zh) * | 2018-08-01 | 2022-02-18 | 西北工业大学 | 一种sins任意失准角无奇异快速传递对准方法 |
CN110044385B (zh) * | 2019-05-09 | 2020-12-08 | 北京壹氢科技有限公司 | 一种大失准角情况下的快速传递对准方法 |
CN111024065B (zh) * | 2019-12-02 | 2022-04-01 | 东南大学 | 一种用于最优估计精对准的严格逆向导航方法 |
CN111220182B (zh) * | 2020-03-23 | 2022-02-11 | 北京中科宇航技术有限公司 | 一种火箭传递对准方法及*** |
CN113012224B (zh) * | 2021-03-12 | 2022-06-03 | 浙江商汤科技开发有限公司 | 定位初始化方法和相关装置、设备、存储介质 |
CN116026370B (zh) * | 2023-03-30 | 2023-06-09 | 中国船舶集团有限公司第七〇七研究所 | 基于矩阵等价转换的光纤陀螺误差校准方法及*** |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101214861A (zh) * | 2007-12-26 | 2008-07-09 | 北京控制工程研究所 | 一种自主恢复轨控故障时的星敏感器定姿方法 |
CN101706281A (zh) * | 2009-11-13 | 2010-05-12 | 南京航空航天大学 | 惯性/天文/卫星高精度组合导航***及其导航方法 |
EP2264485A1 (en) * | 2009-06-04 | 2010-12-22 | Tracedge | Method for analysing moving object continuous trajectory based on sampled GPS position |
CN102508954A (zh) * | 2011-10-21 | 2012-06-20 | 天津大学 | Gps/sins组合导航全数字仿真方法及装置 |
CN102519470A (zh) * | 2011-12-09 | 2012-06-27 | 南京航空航天大学 | 多级嵌入式组合导航***及导航方法 |
-
2012
- 2012-12-18 CN CN201210552272.3A patent/CN103047999B/zh not_active Expired - Fee Related
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101214861A (zh) * | 2007-12-26 | 2008-07-09 | 北京控制工程研究所 | 一种自主恢复轨控故障时的星敏感器定姿方法 |
EP2264485A1 (en) * | 2009-06-04 | 2010-12-22 | Tracedge | Method for analysing moving object continuous trajectory based on sampled GPS position |
CN101706281A (zh) * | 2009-11-13 | 2010-05-12 | 南京航空航天大学 | 惯性/天文/卫星高精度组合导航***及其导航方法 |
CN102508954A (zh) * | 2011-10-21 | 2012-06-20 | 天津大学 | Gps/sins组合导航全数字仿真方法及装置 |
CN102519470A (zh) * | 2011-12-09 | 2012-06-27 | 南京航空航天大学 | 多级嵌入式组合导航***及导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN103047999A (zh) | 2013-04-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103047999B (zh) | 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法 | |
CN103743414B (zh) | 一种里程计辅助车载捷联惯导***行进间初始对准方法 | |
CN103471616B (zh) | 一种动基座sins大方位失准角条件下初始对准方法 | |
CN103245360B (zh) | 晃动基座下的舰载机旋转式捷联惯导***自对准方法 | |
CN106871928A (zh) | 基于李群滤波的捷联惯性导航初始对准方法 | |
CN101706281B (zh) | 惯性/天文/卫星高精度组合导航***及其导航方法 | |
CN101261130B (zh) | 一种船用光纤捷联惯导***传递对准精度评估方法 | |
CN109163735B (zh) | 一种晃动基座正向-正向回溯初始对准方法 | |
CN110398257A (zh) | Gps辅助的sins***快速动基座初始对准方法 | |
CN104655131A (zh) | 基于istssrckf的惯性导航初始对准方法 | |
CN105571578B (zh) | 一种利用伪观测取代精密转台的原地旋转调制寻北方法 | |
CN106595711A (zh) | 一种基于递推四元数的捷联惯性导航***粗对准方法 | |
CN105300404B (zh) | 一种舰船基准惯性导航***标校方法 | |
CN104236586B (zh) | 基于量测失准角的动基座传递对准方法 | |
CN110132269A (zh) | 一种导弹高精度垂直发射初始姿态获取方法 | |
CN106052686A (zh) | 基于dsptms320f28335的全自主捷联惯性导航*** | |
CN109059914B (zh) | 一种基于gps和最小二乘滤波的炮弹滚转角估计方法 | |
CN102305635B (zh) | 一种光纤捷联罗经***的对准方法 | |
CN102654406A (zh) | 基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法 | |
CN106197434A (zh) | 基于地面目标点位置的卫星姿态角的计算方法 | |
CN102519485A (zh) | 一种引入陀螺信息的二位置捷联惯性导航***初始对准方法 | |
CN104296780B (zh) | 一种基于重力视运动的sins自对准与纬度计算方法 | |
CN102768043A (zh) | 一种无外观测量的调制型捷联***组合姿态确定方法 | |
CN109211231B (zh) | 一种基于牛顿迭代法的炮弹姿态估计方法 | |
CN105300407B (zh) | 一种用于单轴调制激光陀螺惯导***的海上动态启动方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20150930 Termination date: 20171218 |