CN106767900B - 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法 - Google Patents

一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法 Download PDF

Info

Publication number
CN106767900B
CN106767900B CN201611048832.6A CN201611048832A CN106767900B CN 106767900 B CN106767900 B CN 106767900B CN 201611048832 A CN201611048832 A CN 201611048832A CN 106767900 B CN106767900 B CN 106767900B
Authority
CN
China
Prior art keywords
error
navigation system
measurement
inertial navigation
accelerometer
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.)
Active
Application number
CN201611048832.6A
Other languages
English (en)
Other versions
CN106767900A (zh
Inventor
徐晓苏
吴梅
王捍兵
金博楠
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201611048832.6A priority Critical patent/CN106767900B/zh
Publication of CN106767900A publication Critical patent/CN106767900A/zh
Application granted granted Critical
Publication of CN106767900B publication Critical patent/CN106767900B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，根据船用主导航系统的位置、速度和航向信息，采用“位置+速度+航向角”的匹配模式，对光纤捷联惯导系统进行在线标定。采用模型预测滤波修正状态一步预测值，估计惯性器件各项参数误差值，并采用Sage‑Husa自适应扩展卡尔曼滤波对量测噪声方差阵不断进行估计和修正，提高了滤波精度，实现了在线标定。本发明利用主导航系统输出信息，使用相应的滤波方法在线估测出惯性测量组件的各标定参数，从而解决惯性测量组件的在线标定问题，避免了光纤陀螺捷联惯性导航系统定期拆卸标定，有效提高光纤捷联惯导系统精度，具有突出的应用价值。

Description

一种基于组合导航技术的船用光纤捷联惯导系统的在线标定 方法
技术领域
本发明属于导航技术领域，尤其涉及一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法。
背景技术
近年来，海洋事业发展迅速，对船用捷联惯导系统的性能的要求越来越高。惯性测量组件误差约占系统误差的90％左右，即只有保证惯性测量组件的高精度，才能保证惯导系统的高精度。惯性系统的性能指标下降，导致不能满足导航的精度要求，因此必须通过各种方法提高精度。
目前，提高惯性器件精度主要从硬件和软件两个方面入手。(1)硬件层面：一是对现有的陀螺仪和加速度计进行深加工；二是当科学技术更加成熟的时候，有新型的材料能够取代现有的材料，并且新材料的性能非常卓越。(2)软件层面：深入研究惯性导航系统的算法，建立更加精准的系统模型，通过软件对系统进行标定，得到精度较高的误差数据，补偿进入惯性导航系统中，从而提高系统的输出精度。在惯性器件的精度达到一定程度的时候，通过软件的补偿继续提高精度成为首要选择，也能够有效的降低投入成本。
当前针对光纤陀螺等惯性测量组件的误差，一般都在实验室通过角增量实验、位置实验和零位误差修正实验进行分立式标定。但是惯性组件安装完成之后，环境的改变和时间的迁移会使误差参数与分立式标定的结果不一致，这会导致捷联惯导系统的精度降低。而频繁拆卸捷联惯导系统进行分立式标定会带来巨大的经济损失和工程实用负担。
发明内容
发明目的：为解决现有技术存在的问题，本发明提出一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，避免了光纤陀螺捷联惯性导航系统定期拆卸标定，有效提高光纤捷联惯导系统精度。
技术方案：一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，根据船用的主导航系统提供的位置、速度和航向信息对船用光纤捷联的子惯导系统的惯性测量组件误差进行在线标定；其包括如下具体步骤：
步骤1：建立光纤陀螺、加速度计的输出误差模型；
步骤2：建立子惯导系统的位置、速度、姿态误差方程；
步骤3：通过子惯导系统解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差这些惯性测量组件误差参数作为滤波器状态量，应用模型预测滤波和自适应扩展卡尔曼滤波组合滤波的方法建立状态方程；
步骤4：以子惯导系统解算的位置、速度、航向信息和主导航系统提供的位置、速度、航向信息的差值作为滤波器的量测量，建立量测方程；
步骤5：在步骤3建立的状态方程的基础上，根据模型预测滤波计算模型误差
Figure BDA0001160211920000021
从而修正一步预测状态量
Figure BDA0001160211920000022
实现在线实时修正系统模型；
步骤6：采用Sage-Husa自适应扩展卡尔曼滤波，通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正，得到估计状态的最优值；
步骤7：根据步骤5推导出的一步预测状态量
Figure BDA0001160211920000023
结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波，实现时间更新和量测更新，对步骤3中的滤波器状态量进行估计，用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值，得到子惯导系统中系统状态各项参数值，实现误差的估计与校正。
所述步骤1中建立光纤陀螺、加速度计的输出误差模型具体为：
将光纤陀螺的安装误差、刻度系数误差和常值漂移建入光纤陀螺误差模型，得：
Figure BDA0001160211920000024
式中，n为导航坐标系；b为载体坐标系；i为惯性坐标系；εn为载体坐标系下常值漂移；
Figure BDA0001160211920000025
为从载体坐标系至导航坐标系的变换矩阵；为陀螺的输出；[δKG]为陀螺的刻度系数误差，
Figure BDA0001160211920000027
[δG]为安装误差，
Figure BDA0001160211920000028
将加速度计的安装误差、刻度系数误差和常值漂移建入加速度计误差模型，得：
Figure BDA0001160211920000029
式中，
Figure BDA00011602119200000210
为导航坐标系下常值漂移；fb为加速度计的输出；[δKA]为加速度计的刻度系数误差，
Figure BDA00011602119200000211
[δA]为安装误差，
Figure BDA0001160211920000031
所述步骤2中建立子惯导系统的位置、速度、姿态误差方程的过程为：
建立姿态误差方程为：
式中，εn、δKGi和δGi(i＝x,y,z)分别为光纤陀螺的常值漂移、刻度系数误差和安装误差；φ为姿态误差角矢量；
Figure BDA0001160211920000033
为导航坐标系相对惯性坐标系的角速度在导航坐标系下的投影；
Figure BDA0001160211920000034
为导航坐标系相对惯性坐标系转动角速度计算误差；
建立速度误差方程为：
Figure BDA0001160211920000035
式中，φn为导航坐标系下姿态误差角；fb为载体坐标系下加速度计的输出；fn为导航坐标系下加速度计的输出；
Figure BDA0001160211920000036
δKAi和δAi(i＝x,y,z)分别为加速度计的常值漂移、刻度系数误差和安装误差；
建立位置误差方程为：
Figure BDA0001160211920000037
Figure BDA0001160211920000038
式中，δVE、δVN为东北向速度误差；RN为沿卯酉圈的曲率半径；RM沿子午圈的曲率半径；λ为经度；L为纬度。
所述步骤3中通过子惯导系统解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差这些惯性测量组件误差参数作为滤波器状态量，应用模型预测滤波和自适应扩展卡尔曼滤波组合滤波的方法建立状态方程具体为：
Figure BDA0001160211920000039
式中，
Figure BDA00011602119200000310
D＝[δGx δGy δGz δKGx δKGy δKGz δAx δAy δKAx δKAy]。
式中：
φE、φN、φU——东北天失准角
δVE、δVN——东北向速度误差
δL、δλ——经纬度误差
εi(i＝x,y,z)——光纤陀螺的陀螺零偏
Figure BDA0001160211920000041
——X、Y的加速度计常值漂移
δKGi(i＝x,y,z)——光纤陀螺的刻度系数误差
δGi(i＝x,y,z)——光纤陀螺的安装误差
δKAi(i＝x,y)——加速度计的刻度系数误差
δAi(i＝x,y)——加速度计的安装误差
式中，A为系统向量矩阵；W为系统噪声矩阵；G1是模型误差分布阵；G2是系统噪声驱动阵。式中：
Figure BDA0001160211920000043
Figure BDA0001160211920000045
Figure BDA0001160211920000048
Figure BDA0001160211920000051
Figure BDA0001160211920000052
Figure BDA0001160211920000053
Figure BDA0001160211920000056
Figure BDA0001160211920000057
Figure BDA0001160211920000058
式中：
cij(i,j＝1,2,3)——姿态矩阵
Figure BDA0001160211920000059
对应元素
VE、VN、VU——东北天向速度
L、λ——经纬度
R——WGS-84坐标系中的地球半径
ωie——地球自转角速度
ωi(i＝x,y,z)——载体运动角速度
fi(i＝x,y)——加速度计输出值。
在所述步骤4中应用以子惯导系统解算的位置、速度、航向信息和主导航系统提供的位置、速度、航向信息的差值作为滤波器的量测量，建立量测方程具体为：
Z＝HX+V
式中，Z为量测量；H为系统量测矩阵；V为系统量测噪声；
其中：
Figure BDA0001160211920000061
式中：
VME、VMN——主导航系统输出的东向和北向速度
VSE、VSN——子惯导导航系统输出的东向和北向速度
λM、LM——主导航系统的纬度和经度
λS、LS——子惯导导航输出的经度和纬度
ΨM——主导航系统的航向角
ΨS——子惯导导航输出的航向角
则系统的量测矩阵为：
Figure BDA0001160211920000062
所述步骤5中在步骤3建立的状态方程的基础上，根据模型预测滤波计算模型误差
Figure BDA0001160211920000063
从而修正一步预测状态量
Figure BDA0001160211920000064
实现在线实时修正系统模型具体为：
模型误差向量估计：
Figure BDA0001160211920000065
式中，Λ(Δt)∈Rm×m是对角阵，m是量测量的维数，其对角元素为：
Figure BDA0001160211920000066
Figure BDA0001160211920000067
是灵敏度矩阵，表示为：
Figure BDA0001160211920000068
式中，pi(i＝1,2,…,m)是D出现在H的第i个分量Hi的微分中的最低阶数；
Figure BDA0001160211920000069
是k阶李导数；gi(i＝1,2,…,q)是G1的第i列；
T∈Rn×n是模型误差加权矩阵；如果模型误差相对而言比较小，T的值应该要取大；如果误差比较大，那么T的值应该取小；
Figure BDA00011602119200000610
是m维列向量，表示为：
Figure BDA0001160211920000071
综上：
Figure BDA0001160211920000072
由此推得自适应卡尔曼滤波(AEKF)的一步预测为：
Figure BDA0001160211920000073
所述步骤6中采用Sage-Husa自适应扩展卡尔曼滤波，通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正，得到估计状态的最优值具体为：
利用观测量带来的信息，在估计子惯导系统的系统状态的同时估计量测噪声方差阵；
mk+1＝Zk+1-Hk+1Xk+1/k
Rk+1＝(1-dk+1)Rk+dk+1[mk+1mk+1 T-Hk+1Pk/kHk+1 T]
式中，dk+1＝(1-b)(1-bk+1)，b为遗忘因子，在[0.95,0.995]间取值；当b较小时，Rk+1对当前时刻量测信息依赖的权重较大；当b较大时，Rk+1对历史的量测信息依赖的权重较大。
所述步骤7根据步骤5推导出的一步预测状态量
Figure BDA0001160211920000074
结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波，实现时间更新和量测更新，对步骤3中的滤波器状态量进行估计，用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值，得到子惯导系统中系统状态各项参数值，实现误差的估计与校正具体为：
计算模型误差：
Figure BDA0001160211920000075
自适应扩展卡尔曼滤波滤波结合模型预测滤波得到系统状态的一步预测状态量为：
Figure BDA0001160211920000076
系统状态的状态转移阵为：
Figure BDA0001160211920000081
最优滤波值为：
Figure BDA0001160211920000082
偏差为：
Figure BDA0001160211920000083
一步预测误差方差阵为：
Pk+1/k＝Φk+1,kPk/kΦk+1,k T+G2QkWT
新的信息为：
采用Sage-Husa自适应扩展卡尔曼滤波方法对量测噪声方差阵进行动态估计：
Figure BDA0001160211920000085
卡尔曼滤波增益阵为：
系统的估计误差方差阵为：
Figure BDA0001160211920000087
有益效果：本发明根据船用主导航系统的位置、速度和航向信息，采用“位置+速度+航向角”的匹配模式，对光纤捷联惯导系统进行在线标定。本发明采用模型预测滤波修正状态一步预测值，估计惯性器件各项参数误差值，并采用简化的Sage-Husa自适应扩展卡尔曼滤波对量测噪声方差阵不断进行估计和修正，提高了滤波精度，实现了在线标定。相对于现有技术，本发明利用主导航系统输出信息，使用相应的滤波方法在线估测出惯性测量组件的各标定参数，从而解决惯性测量组件的在线标定问题，避免了光纤陀螺捷联惯性导航系统定期拆卸标定，有效提高光纤捷联惯导系统精度，具有突出的应用价值。
附图说明
图1为本发明流程图。
具体实施方式
下面将结合附图，对本发明的实施案例进行详细的描述：
如图1所示，一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，根据船用主导航系统提供的位置、速度和航向信息对船用光纤捷联子惯导系统的惯性测量组件误差进行在线标定，其包括如下具体步骤：
步骤1：建立光纤陀螺、加速度计的输出误差模型；
将光纤陀螺的安装误差、刻度系数误差和常值漂移建入光纤陀螺误差模型，得：
Figure BDA0001160211920000091
式中，n为导航坐标系；b为载体坐标系；i为惯性坐标系；εn为载体坐标系下常值漂移；
Figure BDA0001160211920000092
为从载体坐标系至导航坐标系的变换矩阵；
Figure BDA0001160211920000093
为陀螺的输出；[δKG]为陀螺的刻度系数误差，
Figure BDA0001160211920000094
[δG]为安装误差，
Figure BDA0001160211920000095
将加速度计的安装误差、刻度系数误差和常值漂移建入加速度计误差模型，得：
Figure BDA0001160211920000096
式中，
Figure BDA0001160211920000097
为导航坐标系下常值漂移；fb为加速度计的输出；[δKA]为加速度计的刻度系数误差，
Figure BDA0001160211920000098
[δA]为安装误差，
Figure BDA0001160211920000099
步骤2：建立子惯导系统的位置、速度、姿态误差方程；
建立姿态误差方程为：
Figure BDA00011602119200000910
式中，εn、δKGi和δGi(i＝x,y,z)分别为光纤陀螺的常值漂移、刻度系数误差和安装误差；φ为姿态误差角矢量；
Figure BDA00011602119200000911
为导航坐标系相对惯性坐标系的角速度在导航坐标系下的投影；
Figure BDA00011602119200000912
为导航坐标系相对惯性坐标系转动角速度计算误差。
建立速度误差方程为：
式中，φn为导航坐标系下姿态误差角；fb为载体坐标系下加速度计的输出；fn为导航坐标系下加速度计的输出；
Figure BDA0001160211920000101
δKAi和δAi(i＝x,y,z)分别为加速度计的常值漂移、刻度系数误差和安装误差；
建立位置误差方程为：
Figure BDA0001160211920000103
式中，δVE、δVN为东北向速度误差；RN为沿卯酉圈的曲率半径；RM沿子午圈的曲率半径；λ为经度；L为纬度。
步骤3：通过子惯导系统解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差等惯性测量组件误差参数作为滤波器状态量，应用模型预测滤波(Model Predictive Filter)和自适应扩展卡尔曼滤波(Adaptive Extended KalmanFilter)组合滤波的方法建立状态方程；
Figure BDA0001160211920000104
式中，
Figure BDA0001160211920000105
D＝[δGx δGy δGz δKGx δKGy δKGz δAx δAy δKAx δKAy]。
式中：
φE、φN、φU——东北天失准角
δVE、δVN——东北向速度误差
δL、δλ——经纬度误差
εi(i＝x,y,z)——光纤陀螺的陀螺零偏
——X、Y的加速度计常值漂移
δKGi(i＝x,y,z)——光纤陀螺的刻度系数误差
δGi(i＝x,y,z)——光纤陀螺的安装误差
δKAi(i＝x,y)——加速度计的刻度系数误差
δAi(i＝x,y)——加速度计的安装误差
式中，A为系统向量矩阵；W为系统噪声矩阵；G1是模型误差分布阵；G2是系统噪声驱动阵。式中：
Figure BDA0001160211920000112
Figure BDA0001160211920000113
Figure BDA0001160211920000114
Figure BDA0001160211920000118
Figure BDA0001160211920000119
Figure BDA00011602119200001110
Figure BDA00011602119200001111
Figure BDA00011602119200001112
Figure BDA0001160211920000121
Figure BDA0001160211920000122
Figure BDA0001160211920000123
Figure BDA0001160211920000124
式中：
cij(i,j＝1,2,3)——姿态矩阵
Figure BDA0001160211920000126
对应元素
VE、VN、VU——东北天向速度
L、λ——经纬度
R——WGS-84坐标系中的地球半径
ωie——地球自转角速度
ωi(i＝x,y,z)——载体运动角速度
fi(i＝x,y)——加速度计输出值。
步骤4：以子惯导系统解算的位置、速度、航向信息和主导航系统提供的位置、速度、航向信息的差值作为滤波器的量测量，建立量测方程；
Z＝HX+V
式中，Z为量测量；H为系统量测矩阵；V为系统量测噪声。
其中：
式中：
VME、VMN——主导航系统输出的东向和北向速度
VSE、VSN——子惯导导航系统输出的东向和北向速度
λM、LM——主导航系统的纬度和经度
λS、LS——子惯导导航输出的经度和纬度
ΨM——主导航系统的航向角
Ψs——子惯导导航输出的航向角
则系统的量测矩阵为：
Figure BDA0001160211920000131
步骤5：在步骤3建立的状态方程的基础上，根据模型预测滤波计算模型误差
Figure BDA0001160211920000132
从而修正一步预测状态量
Figure BDA0001160211920000133
实现在线实时修正系统模型；
模型误差向量估计：
Figure BDA0001160211920000134
式中，Λ(Δt)∈Rm×m是对角阵，m是量测量的维数，其对角元素为：
Figure BDA0001160211920000135
是灵敏度矩阵，表示为：
Figure BDA0001160211920000137
式中，pi(i＝1,2,…,m)是D出现在H的第i个分量Hi的微分中的最低阶数；
Figure BDA0001160211920000138
是k阶李导数；gi(i＝1,2,…,q)是G1的第i列；
T∈Rn×n是模型误差加权矩阵；如果模型误差相对而言比较小，T的值应该要取大；如果误差比较大，那么T的值应该取小；
是m维列向量，可表示为：
Figure BDA00011602119200001310
综上：
Figure BDA00011602119200001311
由此推得自适应卡尔曼滤波(AEKF)的一步预测为：
Figure BDA00011602119200001312
步骤6：采用改进的Sage-Husa自适应扩展卡尔曼滤波，通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正，得到估计状态的最优值；
利用观测量带来的信息，在估计子惯导系统的系统状态的同时估计量测噪声方差阵：
mk+1＝Zk+1-Hk+1Xk+1/k
Rk+1＝(1-dk+1)Rk+dk+1[mk+1mk+1 T-Hk+1Pk/kHk+1 T]
式中，dk+1＝(1-b)(1-bk+1)，b为遗忘因子，在[0.95,0.995]间取值；当b较小时，Rk+1对当前时刻量测信息依赖的权重较大；当b较大时，Rk+1对历史的量测信息依赖的权重较大。
步骤7：根据步骤5推导出的一步预测状态量
Figure BDA0001160211920000141
结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波，实现时间更新和量测更新，对步骤3中的滤波器状态量进行估计，用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值，得到子惯导系统中系统状态各项参数值，实现误差的估计与校正。
计算模型误差：
Figure BDA0001160211920000142
自适应扩展卡尔曼滤波滤波结合模型预测滤波滤波可得到系统状态的一步预测状态量为：
Figure BDA0001160211920000143
系统状态的状态转移阵为：
Figure BDA0001160211920000144
最优滤波值为：
Figure BDA0001160211920000145
偏差为：
Figure BDA0001160211920000146
一步预测误差方差阵为：
Pk+1/k＝Φk+1,kPk/kΦk+1,k T+G2QkWT
新息为：
采用Sage-Husa自适应扩展卡尔曼滤波方法对量测噪声方差阵进行动态估计：
Figure BDA0001160211920000152
卡尔曼滤波增益阵为：
Figure BDA0001160211920000153
系统的估计误差方差阵为：
Figure BDA0001160211920000154

Claims (8)

1.一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，其特征在于，根据船用的主导航系统提供的位置、速度和航向信息对船用光纤捷联的子惯导系统的惯性测量组件误差进行在线标定；其包括如下具体步骤：
步骤1：建立光纤陀螺、加速度计的输出误差模型；
步骤2：建立子惯导系统的位置、速度、姿态误差方程；
步骤3：通过子惯导系统解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差这些惯性测量组件误差参数作为滤波器状态量，应用模型预测滤波和自适应扩展卡尔曼滤波组合滤波的方法建立状态方程；
步骤4：以子惯导系统解算的位置、速度、航向信息和主导航系统提供的位置、速度、航向信息的差值作为滤波器的量测量，建立量测方程；
步骤5：在步骤3建立的状态方程的基础上，根据模型预测滤波计算模型误差从而修正一步预测状态量实现在线实时修正系统模型；
步骤6：采用Sage-Husa自适应扩展卡尔曼滤波，通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正，得到估计状态的最优值；
步骤7：根据步骤5推导出的一步预测状态量
Figure FDA0001160211910000014
结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波，实现时间更新和量测更新，对步骤3中的滤波器状态量进行估计，用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值，得到子惯导系统中系统状态各项参数值，实现误差的估计与校正。
2.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，其特征在于，所述步骤1中建立光纤陀螺、加速度计的输出误差模型具体为：
将光纤陀螺的安装误差、刻度系数误差和常值漂移建入光纤陀螺误差模型，得：
式中，n为导航坐标系；b为载体坐标系；i为惯性坐标系；εn为载体坐标系下常值漂移；
Figure FDA0001160211910000016
为从载体坐标系至导航坐标系的变换矩阵；
Figure FDA0001160211910000017
为陀螺的输出；[δKG]为陀螺的刻度系数误差，
Figure FDA0001160211910000011
[δG]为安装误差，
Figure FDA0001160211910000021
将加速度计的安装误差、刻度系数误差和常值漂移建入加速度计误差模型，得：
Figure FDA00011602119100000213
式中，
Figure FDA00011602119100000212
为导航坐标系下常值漂移；fb为加速度计的输出；[δKA]为加速度计的刻度系数误差，
Figure FDA0001160211910000023
[δA]为安装误差，
Figure FDA0001160211910000024
3.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，其特征在于，所述步骤2中建立子惯导系统的位置、速度、姿态误差方程的过程为：
建立姿态误差方程为：
Figure FDA0001160211910000025
式中，εn、δKGi和δGi(i＝x,y,z)分别为光纤陀螺的常值漂移、刻度系数误差和安装误差；φ为姿态误差角矢量；为导航坐标系相对惯性坐标系的角速度在导航坐标系下的投影；为导航坐标系相对惯性坐标系转动角速度计算误差；
建立速度误差方程为：
Figure FDA0001160211910000028
式中，φn为导航坐标系下姿态误差角；fb为载体坐标系下加速度计的输出；fn为导航坐标系下加速度计的输出；δKAi和δAi(i＝x,y,z)分别为加速度计的常值漂移、刻度系数误差和安装误差；
建立位置误差方程为：
Figure FDA0001160211910000026
Figure FDA0001160211910000027
式中，δVE、δVN为东北向速度误差；RN为沿卯酉圈的曲率半径；RM沿子午圈的曲率半径；λ为经度；L为纬度。
4.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，其特征在于，所述步骤3中通过子惯导系统解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差这些惯性测量组件误差参数作为滤波器状态量，应用模型预测滤波和自适应扩展卡尔曼滤波组合滤波的方法建立状态方程具体为：
Figure FDA0001160211910000031
式中，
Figure FDA0001160211910000035
D＝[δGx δGy δGz δKGx δKGy δKGz δAx δAy δKAx δKAy]。
式中：
φE、φN、φU——东北天失准角
δVE、δVN——东北向速度误差
δL、δλ——经纬度误差
εi(i＝x,y,z)——光纤陀螺的陀螺零偏
——X、Y的加速度计常值漂移
δKGi(i＝x,y,z)——光纤陀螺的刻度系数误差
δGi(i＝x,y,z)——光纤陀螺的安装误差
δKAi(i＝x,y)——加速度计的刻度系数误差
δAi(i＝x,y)——加速度计的安装误差
式中，A为系统向量矩阵；W为系统噪声矩阵；G1是模型误差分布阵；G2是系统噪声驱动阵。式中：
Figure FDA0001160211910000032
Figure FDA0001160211910000033
Figure FDA0001160211910000034
Figure FDA0001160211910000041
Figure FDA0001160211910000042
Figure FDA0001160211910000043
Figure FDA0001160211910000044
Figure FDA0001160211910000045
Figure FDA0001160211910000046
Figure FDA0001160211910000047
Figure FDA0001160211910000048
Figure FDA0001160211910000049
Figure FDA00011602119100000411
Figure FDA00011602119100000413
式中：
cij(i,j＝1,2,3)——姿态矩阵
Figure FDA00011602119100000414
对应元素
VE、VN、VU——东北天向速度
L、λ——经纬度
R——WGS-84坐标系中的地球半径
ωie——地球自转角速度
ωi(i＝x,y,z)——载体运动角速度
fi(i＝x,y)——加速度计输出值。
5.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，其特征在于，在所述步骤4中应用以子惯导系统解算的位置、速度、航向信息和主导航系统提供的位置、速度、航向信息的差值作为滤波器的量测量，建立量测方程具体为：
Z＝HX+V
式中，Z为量测量；H为系统量测矩阵；V为系统量测噪声；
其中：
Figure FDA0001160211910000051
式中：
VME、VMN——主导航系统输出的东向和北向速度
VSE、VSN——子惯导导航系统输出的东向和北向速度
λM、LM——主导航系统的纬度和经度
λS、LS——子惯导导航输出的经度和纬度
ΨM——主导航系统的航向角
ΨS——子惯导导航输出的航向角
则系统的量测矩阵为：
Figure FDA0001160211910000055
6.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，其特征在于，所述步骤5中在步骤3建立的状态方程的基础上，根据模型预测滤波计算模型误差
Figure FDA0001160211910000053
从而修正一步预测状态量
Figure FDA0001160211910000054
实现在线实时修正系统模型具体为：
模型误差向量估计：
Figure FDA0001160211910000061
式中，Λ(Δt)∈Rm×m是对角阵，m是量测量的维数，其对角元素为：
Figure FDA0001160211910000062
Figure FDA0001160211910000068
是灵敏度矩阵，表示为：
Figure FDA0001160211910000063
式中，pi(i＝1,2,…,m)是D出现在H的第i个分量Hi的微分中的最低阶数；
Figure FDA0001160211910000067
是k阶李导数；gi(i＝1,2,…,q)是G1的第i列；
T∈Rn×n是模型误差加权矩阵；如果模型误差相对而言比较小，T的值应该要取大；如果误差比较大，那么T的值应该取小；
Figure FDA0001160211910000069
是m维列向量，表示为：
Figure FDA0001160211910000064
综上：
Figure FDA0001160211910000065
由此推得自适应卡尔曼滤波(AEKF)的一步预测为：
Figure FDA0001160211910000066
7.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，其特征在于，所述步骤6中采用Sage-Husa自适应扩展卡尔曼滤波，通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正，得到估计状态的最优值具体为：
利用观测量带来的信息，在估计子惯导系统的系统状态的同时估计量测噪声方差阵；
mk+1＝Zk+1-Hk+1Xk+1/k
Rk+1＝(1-dk+1)Rk+dk+1[mk+1mk+1 T-Hk+1Pk/kHk+1 T]
式中，dk+1＝(1-b)(1-bk+1)，b为遗忘因子，在[0.95,0.995]间取值；当b较小时，Rk+1对当前时刻量测信息依赖的权重较大；当b较大时，Rk+1对历史的量测信息依赖的权重较大。
8.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导系统的在线标定方法，其特征在于，所述步骤7根据步骤5推导出的一步预测状态量
Figure FDA0001160211910000077
结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波，实现时间更新和量测更新，对步骤3中的滤波器状态量进行估计，用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值，得到子惯导系统中系统状态各项参数值，实现误差的估计与校正具体为：
计算模型误差：
Figure FDA0001160211910000071
自适应扩展卡尔曼滤波滤波结合模型预测滤波得到系统状态的一步预测状态量为：
Figure FDA0001160211910000072
系统状态的状态转移阵为：
Figure FDA0001160211910000073
最优滤波值为：
Figure FDA0001160211910000074
偏差为：
Figure FDA0001160211910000075
一步预测误差方差阵为：
Pk+1/k＝Φk+1,kPk/kΦk+1,k T+G2QkWT
新的信息为：
Figure FDA0001160211910000076
采用Sage-Husa自适应扩展卡尔曼滤波方法对量测噪声方差阵进行动态估计：
卡尔曼滤波增益阵为：
Figure FDA0001160211910000082
系统的估计误差方差阵为：
Figure FDA0001160211910000083
CN201611048832.6A 2016-11-23 2016-11-23 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法 Active CN106767900B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611048832.6A CN106767900B (zh) 2016-11-23 2016-11-23 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611048832.6A CN106767900B (zh) 2016-11-23 2016-11-23 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法

Publications (2)

Publication Number Publication Date
CN106767900A CN106767900A (zh) 2017-05-31
CN106767900B true CN106767900B (zh) 2020-01-03

Family

ID=58910470

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611048832.6A Active CN106767900B (zh) 2016-11-23 2016-11-23 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法

Country Status (1)

Country Link
CN (1) CN106767900B (zh)

Families Citing this family (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107655493B (zh) * 2017-09-06 2021-04-06 东南大学 一种光纤陀螺sins六位置系统级标定方法
CN107621264B (zh) * 2017-09-30 2021-01-19 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
WO2019179882A1 (en) * 2018-03-23 2019-09-26 Fnv Ip B.V. Automated tuning of an estimator
CN108427080A (zh) * 2018-06-20 2018-08-21 上海海事大学 混合动力船舶的动力电池组的荷电状态计算方法
CN109164391B (zh) * 2018-07-12 2021-03-23 杭州神驹科技有限公司 一种动力电池荷电状态在线估算方法及系统
CN110823213B (zh) * 2018-08-14 2022-07-08 北京自动化控制设备研究所 一种提高sins/dr组合导航系统相对航向角精度的方法
CN109085554B (zh) * 2018-08-30 2021-03-30 衡阳市衡山科学城科技创新研究院有限公司 一种主动雷达导引头视线角零位误差估计方法及装置
CN109405825B (zh) * 2018-11-09 2021-02-09 北京航天时代激光导航技术有限责任公司 一种激光惯组惯性本体高精度拆装方法
CN109556633B (zh) * 2018-11-26 2020-11-20 北方工业大学 一种基于自适应ekf的仿生偏振传感器多源误差标定方法
CN109827572B (zh) * 2019-03-12 2021-05-28 北京星网宇达科技股份有限公司 一种检测车位置预测的方法及装置
CN109945859B (zh) * 2019-04-01 2022-12-06 东南大学 一种自适应h∞滤波的运动学约束捷联惯性导航方法
FR3097045B1 (fr) * 2019-06-06 2021-05-14 Safran Electronics & Defense Procédé et dispositif de recalage d’une centrale inertielle d’un moyen de transport à partir d’informations délivrées par un viseur du moyen de transport
CN111044075B (zh) * 2019-12-10 2023-09-15 上海航天控制技术研究所 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法
CN110926465A (zh) * 2019-12-11 2020-03-27 哈尔滨工程大学 一种mems/gps松组合导航方法
CN111323050B (zh) * 2020-03-19 2021-06-18 哈尔滨工程大学 一种捷联惯导和多普勒组合系统标定方法
CN111290008A (zh) * 2020-03-23 2020-06-16 兰州交通大学 一种动态自适应扩展卡尔曼滤波容错算法
CN111982151B (zh) * 2020-07-17 2022-07-22 中科长城海洋信息系统有限公司 一种光纤捷联惯导系统的自标定方法
CN112146653A (zh) * 2020-08-03 2020-12-29 河北汉光重工有限责任公司 一种提高组合导航解算频率的方法
CN112880704A (zh) * 2021-01-19 2021-06-01 中国人民解放军海军工程大学 光纤陀螺捷联惯导系统智能标定方法
CN113569653A (zh) * 2021-06-30 2021-10-29 宁波春建电子科技有限公司 一种基于面部特征信息的三维头部姿态估计算法
CN114061574B (zh) * 2021-11-20 2024-04-05 北京唯实深蓝科技有限公司 一种基于位置不变约束及零速校正的采煤机定姿定向方法
CN114485727A (zh) * 2022-01-04 2022-05-13 中国煤炭科工集团太原研究院有限公司 一种捷联惯导系统精度自检测方法和装置
CN115574817B (zh) * 2022-12-08 2023-03-31 中国人民解放军国防科技大学 一种基于三轴旋转式惯导系统的导航方法及导航系统
CN116242397B (zh) * 2023-03-11 2024-01-30 中国人民解放军国防科技大学 一种速度误差修正模型下的双惯导协同标定方法
CN116481564B (zh) * 2023-03-11 2024-02-23 中国人民解放军国防科技大学 基于Psi角误差修正模型的极地双惯导协同标定方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101131311A (zh) * 2007-10-15 2008-02-27 北京航空航天大学 一种智能化机载导弹动基座对准及标定方法
CN101963513A (zh) * 2010-09-03 2011-02-02 哈尔滨工程大学 消除水下运载体捷联惯导系统杆臂效应误差的对准方法
CN103047999A (zh) * 2012-12-18 2013-04-17 东南大学 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法
CN103175545A (zh) * 2013-03-15 2013-06-26 戴洪德 惯导系统速度加部分角速度匹配抗干扰快速传递对准方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101131311A (zh) * 2007-10-15 2008-02-27 北京航空航天大学 一种智能化机载导弹动基座对准及标定方法
CN101963513A (zh) * 2010-09-03 2011-02-02 哈尔滨工程大学 消除水下运载体捷联惯导系统杆臂效应误差的对准方法
CN103047999A (zh) * 2012-12-18 2013-04-17 东南大学 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法
CN103175545A (zh) * 2013-03-15 2013-06-26 戴洪德 惯导系统速度加部分角速度匹配抗干扰快速传递对准方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Nonlinear model predictive control of a pH neutralization process based on WienerLaguerre model;Sanaz Mahmoodi等;《Chemical Engineering Journal》;20091231;328-337 *
Predictive Filtering for Nonlinear Systems;John L.Crassidis等;《American Institute of Aeronautics and Astronautics》;19971231;566-572 *
动基座传递对准误差分析与补偿方法研究;岳海文;《中国优秀硕士学位论文全文数据库工程科技II辑》;20120715(第7期);正文第2页第17-30行 *
抗差自适应模型预测滤波及其在组合导航中的应用;高社生等;《中国惯性技术学报》;20111231;第19卷(第6期);701-705 *
模型预测滤波在机载SAR运动补偿POS系统中的应用;宫晓琳等;《航空学报》;20080131;第29卷(第1期);第103页第1栏第25-44行，第2栏，第104页第1-2栏，第105页第1-2栏，第106页第一栏第1-12行 *

Also Published As

Publication number Publication date
CN106767900A (zh) 2017-05-31

Similar Documents

Publication Publication Date Title
CN106767900B (zh) 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN109000640B (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN112629538A (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN110631574B (zh) 一种惯性/里程计/rtk多信息融合方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN110851776B (zh) 一种高动态变转速载体的姿态解算方法
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN113503892A (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN110388942B (zh) 一种基于角度和速度增量的车载姿态精对准系统
CN110595434A (zh) 基于mems传感器的四元数融合姿态估计方法
CN114935345A (zh) 一种基于模式识别的车载惯导安装角误差补偿方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN115574817B (zh) 一种基于三轴旋转式惯导系统的导航方法及导航系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant