CN106767900A - 一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法 - Google Patents
一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法 Download PDFInfo
- Publication number
- CN106767900A CN106767900A CN201611048832.6A CN201611048832A CN106767900A CN 106767900 A CN106767900 A CN 106767900A CN 201611048832 A CN201611048832 A CN 201611048832A CN 106767900 A CN106767900 A CN 106767900A
- Authority
- CN
- China
- Prior art keywords
- error
- optical fibre
- speed
- formula
- 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.)
- Granted
Links
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建立的状态方程的基础上,根据模型预测滤波计算模型误差从而修正一步预测状态量实现在线实时修正***模型;
步骤6:采用Sage-Husa自适应扩展卡尔曼滤波,通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正,得到估计状态的最优值;
步骤7:根据步骤5推导出的一步预测状态量结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波,实现时间更新和量测更新,对步骤3中的滤波器状态量进行估计,用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值,得到子惯导***中***状态各项参数值,实现误差的估计与校正。
所述步骤1中建立光纤陀螺、加速度计的输出误差模型具体为:
将光纤陀螺的安装误差、刻度系数误差和常值漂移建入光纤陀螺误差模型,得:
式中,n为导航坐标系;b为载体坐标系;i为惯性坐标系;εn为载体坐标系下常值漂移;为从载体坐标系至导航坐标系的变换矩阵;为陀螺的输出;[δKG]为陀螺的刻度系数误差,[δG]为安装误差,
将加速度计的安装误差、刻度系数误差和常值漂移建入加速度计误差模型,得:
式中,为导航坐标系下常值漂移;fb为加速度计的输出;[δKA]为加速度计的刻度系数误差,[δA]为安装误差,
所述步骤2中建立子惯导***的位置、速度、姿态误差方程的过程为:
建立姿态误差方程为:
式中,εn、δKGi和δGi(i=x,y,z)分别为光纤陀螺的常值漂移、刻度系数误差和安装误差;φ为姿态误差角矢量;为导航坐标系相对惯性坐标系的角速度在导航坐标系下的投影;为导航坐标系相对惯性坐标系转动角速度计算误差;
建立速度误差方程为:
式中,φn为导航坐标系下姿态误差角;fb为载体坐标系下加速度计的输出;fn为导航坐标系下加速度计的输出;δKAi和δAi(i=x,y,z)分别为加速度计的常值漂移、刻度系数误差和安装误差;
建立位置误差方程为:
式中,δVE、δVN为东北向速度误差;RN为沿卯酉圈的曲率半径;RM沿子午圈的曲率半径;λ为经度;L为纬度。
所述步骤3中通过子惯导***解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差这些惯性测量组件误差参数作为滤波器状态量,应用模型预测滤波和自适应扩展卡尔曼滤波组合滤波的方法建立状态方程具体为:
式中,
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是***噪声驱动阵。式中:
式中:
cij(i,j=1,2,3)——姿态矩阵对应元素
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——子惯导导航输出的航向角
则***的量测矩阵为:
所述步骤5中在步骤3建立的状态方程的基础上,根据模型预测滤波计算模型误差从而修正一步预测状态量实现在线实时修正***模型具体为:
模型误差向量估计:
式中,Λ(Δt)∈Rm×m是对角阵,m是量测量的维数,其对角元素为:
是灵敏度矩阵,表示为:
式中,pi(i=1,2,…,m)是D出现在H的第i个分量Hi的微分中的最低阶数;是k阶李导数;gi(i=1,2,…,q)是G1的第i列;
T∈Rn×n是模型误差加权矩阵;如果模型误差相对而言比较小,T的值应该要取大;如果误差比较大,那么T的值应该取小;
是m维列向量,表示为:
综上:
由此推得自适应卡尔曼滤波(AEKF)的一步预测为:
所述步骤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推导出的一步预测状态量结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波,实现时间更新和量测更新,对步骤3中的滤波器状态量进行估计,用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值,得到子惯导***中***状态各项参数值,实现误差的估计与校正具体为:
计算模型误差:
自适应扩展卡尔曼滤波滤波结合模型预测滤波得到***状态的一步预测状态量为:
***状态的状态转移阵为:
最优滤波值为:
偏差为:
一步预测误差方差阵为:
Pk+1/k=Φk+1,kPk/kΦk+1,k T+G2QkWT
新的信息为:
采用Sage-Husa自适应扩展卡尔曼滤波方法对量测噪声方差阵进行动态估计:
卡尔曼滤波增益阵为:
***的估计误差方差阵为:
有益效果:本发明根据船用主导航***的位置、速度和航向信息,采用“位置+速度+航向角”的匹配模式,对光纤捷联惯导***进行在线标定。本发明采用模型预测滤波修正状态一步预测值,估计惯性器件各项参数误差值,并采用简化的Sage-Husa自适应扩展卡尔曼滤波对量测噪声方差阵不断进行估计和修正,提高了滤波精度,实现了在线标定。相对于现有技术,本发明利用主导航***输出信息,使用相应的滤波方法在线估测出惯性测量组件的各标定参数,从而解决惯性测量组件的在线标定问题,避免了光纤陀螺捷联惯性导航***定期拆卸标定,有效提高光纤捷联惯导***精度,具有突出的应用价值。
附图说明
图1为本发明流程图。
具体实施方式
下面将结合附图,对本发明的实施案例进行详细的描述:
如图1所示,一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法,根据船用主导航***提供的位置、速度和航向信息对船用光纤捷联子惯导***的惯性测量组件误差进行在线标定,其包括如下具体步骤:
步骤1:建立光纤陀螺、加速度计的输出误差模型;
将光纤陀螺的安装误差、刻度系数误差和常值漂移建入光纤陀螺误差模型,得:
式中,n为导航坐标系;b为载体坐标系;i为惯性坐标系;εn为载体坐标系下常值漂移;为从载体坐标系至导航坐标系的变换矩阵;为陀螺的输出;[δKG]为陀螺的刻度系数误差,[δG]为安装误差,
将加速度计的安装误差、刻度系数误差和常值漂移建入加速度计误差模型,得:
式中,为导航坐标系下常值漂移;fb为加速度计的输出;[δKA]为加速度计的刻度系数误差,[δA]为安装误差,
步骤2:建立子惯导***的位置、速度、姿态误差方程;
建立姿态误差方程为:
式中,εn、δKGi和δGi(i=x,y,z)分别为光纤陀螺的常值漂移、刻度系数误差和安装误差;φ为姿态误差角矢量;为导航坐标系相对惯性坐标系的角速度在导航坐标系下的投影;为导航坐标系相对惯性坐标系转动角速度计算误差。
建立速度误差方程为:
式中,φn为导航坐标系下姿态误差角;fb为载体坐标系下加速度计的输出;fn为导航坐标系下加速度计的输出;δKAi和δAi(i=x,y,z)分别为加速度计的常值漂移、刻度系数误差和安装误差;
建立位置误差方程为:
式中,δVE、δVN为东北向速度误差;RN为沿卯酉圈的曲率半径;RM沿子午圈的曲率半径;λ为经度;L为纬度。
步骤3:通过子惯导***解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差等惯性测量组件误差参数作为滤波器状态量,应用模型预测滤波(Model Predictive Filter)和自适应扩展卡尔曼滤波(Adaptive Extended KalmanFilter)组合滤波的方法建立状态方程;
式中,
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是***噪声驱动阵。式中:
式中:
cij(i,j=1,2,3)——姿态矩阵对应元素
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——子惯导导航输出的航向角
则***的量测矩阵为:
步骤5:在步骤3建立的状态方程的基础上,根据模型预测滤波计算模型误差从而修正一步预测状态量实现在线实时修正***模型;
模型误差向量估计:
式中,Λ(Δt)∈Rm×m是对角阵,m是量测量的维数,其对角元素为:
是灵敏度矩阵,表示为:
式中,pi(i=1,2,…,m)是D出现在H的第i个分量Hi的微分中的最低阶数;是k阶李导数;gi(i=1,2,…,q)是G1的第i列;
T∈Rn×n是模型误差加权矩阵;如果模型误差相对而言比较小,T的值应该要取大;如果误差比较大,那么T的值应该取小;
是m维列向量,可表示为:
综上:
由此推得自适应卡尔曼滤波(AEKF)的一步预测为:
步骤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推导出的一步预测状态量结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波,实现时间更新和量测更新,对步骤3中的滤波器状态量进行估计,用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值,得到子惯导***中***状态各项参数值,实现误差的估计与校正。
计算模型误差:
自适应扩展卡尔曼滤波滤波结合模型预测滤波滤波可得到***状态的一步预测状态量为:
***状态的状态转移阵为:
最优滤波值为:
偏差为:
一步预测误差方差阵为:
Pk+1/k=Φk+1,kPk/kΦk+1,k T+G2QkWT
新息为:
采用Sage-Husa自适应扩展卡尔曼滤波方法对量测噪声方差阵进行动态估计:
卡尔曼滤波增益阵为:
***的估计误差方差阵为:
Claims (8)
1.一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法,其特征在于,根据船用的主导航***提供的位置、速度和航向信息对船用光纤捷联的子惯导***的惯性测量组件误差进行在线标定;其包括如下具体步骤:
步骤1:建立光纤陀螺、加速度计的输出误差模型;
步骤2:建立子惯导***的位置、速度、姿态误差方程;
步骤3:通过子惯导***解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差这些惯性测量组件误差参数作为滤波器状态量,应用模型预测滤波和自适应扩展卡尔曼滤波组合滤波的方法建立状态方程;
步骤4:以子惯导***解算的位置、速度、航向信息和主导航***提供的位置、速度、航向信息的差值作为滤波器的量测量,建立量测方程;
步骤5:在步骤3建立的状态方程的基础上,根据模型预测滤波计算模型误差从而修正一步预测状态量实现在线实时修正***模型;
步骤6:采用Sage-Husa自适应扩展卡尔曼滤波,通过步骤4建立的量测方程和观测量中的航向、速度和位置信息对设定的量测噪声方差阵R进行估计和修正,得到估计状态的最优值;
步骤7:根据步骤5推导出的一步预测状态量结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波,实现时间更新和量测更新,对步骤3中的滤波器状态量进行估计,用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值,得到子惯导***中***状态各项参数值,实现误差的估计与校正。
2.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导***的在线标定方法,其特征在于,所述步骤1中建立光纤陀螺、加速度计的输出误差模型具体为:
将光纤陀螺的安装误差、刻度系数误差和常值漂移建入光纤陀螺误差模型,得:
式中,n为导航坐标系;b为载体坐标系;i为惯性坐标系;εn为载体坐标系下常值漂移;为从载体坐标系至导航坐标系的变换矩阵;为陀螺的输出;[δKG]为陀螺的刻度系数误差,[δG]为安装误差,
将加速度计的安装误差、刻度系数误差和常值漂移建入加速度计误差模型,得:
式中,为导航坐标系下常值漂移;fb为加速度计的输出;[δKA]为加速度计的刻度系数误差,[δA]为安装误差,
3.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导***的在线标定方法,其特征在于,所述步骤2中建立子惯导***的位置、速度、姿态误差方程的过程为:
建立姿态误差方程为:
式中,εn、δKGi和δGi(i=x,y,z)分别为光纤陀螺的常值漂移、刻度系数误差和安装误差;φ为姿态误差角矢量;为导航坐标系相对惯性坐标系的角速度在导航坐标系下的投影;为导航坐标系相对惯性坐标系转动角速度计算误差;
建立速度误差方程为:
式中,φn为导航坐标系下姿态误差角;fb为载体坐标系下加速度计的输出;fn为导航坐标系下加速度计的输出;δKAi和δAi(i=x,y,z)分别为加速度计的常值漂移、刻度系数误差和安装误差;
建立位置误差方程为:
式中,δVE、δVN为东北向速度误差;RN为沿卯酉圈的曲率半径;RM沿子午圈的曲率半径;λ为经度;L为纬度。
4.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导***的在线标定方法,其特征在于,所述步骤3中通过子惯导***解算的位置、速度、姿态信息和陀螺加速度计常值漂移、刻度因数误差、安装误差这些惯性测量组件误差参数作为滤波器状态量,应用模型预测滤波和自适应扩展卡尔曼滤波组合滤波的方法建立状态方程具体为:
式中,
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是***噪声驱动阵。式中:
式中:
cij(i,j=1,2,3)——姿态矩阵对应元素
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为***量测噪声;
其中:
式中:
VME、VMN——主导航***输出的东向和北向速度
VSE、VSN——子惯导导航***输出的东向和北向速度
λM、LM——主导航***的纬度和经度
λS、LS——子惯导导航输出的经度和纬度
ΨM——主导航***的航向角
ΨS——子惯导导航输出的航向角
则***的量测矩阵为:
6.如权利要求1所述的基于组合导航技术的船用光纤捷联惯导***的在线标定方法,其特征在于,所述步骤5中在步骤3建立的状态方程的基础上,根据模型预测滤波计算模型误差从而修正一步预测状态量实现在线实时修正***模型具体为:
模型误差向量估计:
式中,Λ(Δt)∈Rm×m是对角阵,m是量测量的维数,其对角元素为:
是灵敏度矩阵,表示为:
式中,pi(i=1,2,…,m)是D出现在H的第i个分量Hi的微分中的最低阶数;是k阶李导数;gi(i=1,2,…,q)是G1的第i列;
T∈Rn×n是模型误差加权矩阵;如果模型误差相对而言比较小,T的值应该要取大;如果误差比较大,那么T的值应该取小;
是m维列向量,表示为:
综上:
由此推得自适应卡尔曼滤波(AEKF)的一步预测为:
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推导出的一步预测状态量结合步骤6中采用的Sage-Husa自适应扩展卡尔曼滤波,实现时间更新和量测更新,对步骤3中的滤波器状态量进行估计,用模型预测滤波修正自适应扩展卡尔曼滤波的状态一步预测值,得到子惯导***中***状态各项参数值,实现误差的估计与校正具体为:
计算模型误差:
自适应扩展卡尔曼滤波滤波结合模型预测滤波得到***状态的一步预测状态量为:
***状态的状态转移阵为:
最优滤波值为:
偏差为:
一步预测误差方差阵为:
Pk+1/k=Φk+1,kPk/kΦk+1,k T+G2QkWT
新的信息为:
采用Sage-Husa自适应扩展卡尔曼滤波方法对量测噪声方差阵进行动态估计:
卡尔曼滤波增益阵为:
***的估计误差方差阵为:
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 true CN106767900A (zh) | 2017-05-31 |
CN106767900B 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) |
Cited By (26)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107621264A (zh) * | 2017-09-30 | 2018-01-23 | 华南理工大学 | 车载微惯性/卫星组合导航***的自适应卡尔曼滤波方法 |
CN107655493A (zh) * | 2017-09-06 | 2018-02-02 | 东南大学 | 一种光纤陀螺sins六位置***级标定方法 |
CN108427080A (zh) * | 2018-06-20 | 2018-08-21 | 上海海事大学 | 混合动力船舶的动力电池组的荷电状态计算方法 |
CN109085554A (zh) * | 2018-08-30 | 2018-12-25 | 衡阳市衡山科学城科技创新研究院有限公司 | 一种主动雷达导引头视线角零位误差估计方法及装置 |
CN109164391A (zh) * | 2018-07-12 | 2019-01-08 | 杭州神驹科技有限公司 | 一种动力电池荷电状态在线估算方法及*** |
CN109405825A (zh) * | 2018-11-09 | 2019-03-01 | 北京航天时代激光导航技术有限责任公司 | 一种激光惯组惯性本体高精度拆装方法 |
CN109556633A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于自适应ekf的仿生偏振传感器多源误差标定方法 |
CN109827572A (zh) * | 2019-03-12 | 2019-05-31 | 北京星网宇达科技股份有限公司 | 一种检测车位置预测的方法及装置 |
CN109945859A (zh) * | 2019-04-01 | 2019-06-28 | 东南大学 | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 |
WO2019179882A1 (en) * | 2018-03-23 | 2019-09-26 | Fnv Ip B.V. | Automated tuning of an estimator |
CN110823213A (zh) * | 2018-08-14 | 2020-02-21 | 北京自动化控制设备研究所 | 一种提高sins/dr组合导航***相对航向角精度的方法 |
CN110926465A (zh) * | 2019-12-11 | 2020-03-27 | 哈尔滨工程大学 | 一种mems/gps松组合导航方法 |
CN111044075A (zh) * | 2019-12-10 | 2020-04-21 | 上海航天控制技术研究所 | 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法 |
CN111290008A (zh) * | 2020-03-23 | 2020-06-16 | 兰州交通大学 | 一种动态自适应扩展卡尔曼滤波容错算法 |
CN111323050A (zh) * | 2020-03-19 | 2020-06-23 | 哈尔滨工程大学 | 一种捷联惯导和多普勒组合***标定方法 |
CN111982151A (zh) * | 2020-07-17 | 2020-11-24 | 中科长城海洋信息***有限公司 | 一种光纤捷联惯导***的自标定方法 |
CN112146653A (zh) * | 2020-08-03 | 2020-12-29 | 河北汉光重工有限责任公司 | 一种提高组合导航解算频率的方法 |
CN112880704A (zh) * | 2021-01-19 | 2021-06-01 | 中国人民解放军海军工程大学 | 光纤陀螺捷联惯导***智能标定方法 |
CN113465628A (zh) * | 2021-06-17 | 2021-10-01 | 杭州鸿泉物联网技术股份有限公司 | 惯性测量单元数据补偿方法及*** |
CN113569653A (zh) * | 2021-06-30 | 2021-10-29 | 宁波春建电子科技有限公司 | 一种基于面部特征信息的三维头部姿态估计算法 |
CN113939712A (zh) * | 2019-06-06 | 2022-01-14 | 赛峰电子与防务公司 | 基于运输设备取景器传送的信息重置运输设备惯性单元的方法和装置 |
CN114061574A (zh) * | 2021-11-20 | 2022-02-18 | 北京唯实深蓝科技有限公司 | 一种基于位置不变约束及零速校正的采煤机定姿定向方法 |
CN114485727A (zh) * | 2022-01-04 | 2022-05-13 | 中国煤炭科工集团太原研究院有限公司 | 一种捷联惯导***精度自检测方法和装置 |
CN115574817A (zh) * | 2022-12-08 | 2023-01-06 | 中国人民解放军国防科技大学 | 一种基于三轴旋转式惯导***的导航方法及导航*** |
CN116242397A (zh) * | 2023-03-11 | 2023-06-09 | 中国人民解放军国防科技大学 | 一种速度误差修正模型下的双惯导协同标定方法 |
CN116481564A (zh) * | 2023-03-11 | 2023-07-25 | 中国人民解放军国防科技大学 | 基于Psi角误差修正模型的极地双惯导协同标定方法 |
Citations (4)
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 | 戴洪德 | 惯导***速度加部分角速度匹配抗干扰快速传递对准方法 |
-
2016
- 2016-11-23 CN CN201611048832.6A patent/CN106767900B/zh active Active
Patent Citations (4)
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)
Title |
---|
JOHN L.CRASSIDIS等: "Predictive Filtering for Nonlinear Systems", 《AMERICAN INSTITUTE OF AERONAUTICS AND ASTRONAUTICS》 * |
SANAZ MAHMOODI等: "Nonlinear model predictive control of a pH neutralization process based on WienerLaguerre model", 《CHEMICAL ENGINEERING JOURNAL》 * |
宫晓琳等: "模型预测滤波在机载SAR运动补偿POS***中的应用", 《航空学报》 * |
岳海文: "动基座传递对准误差分析与补偿方法研究", 《中国优秀硕士学位论文全文数据库工程科技II辑》 * |
高社生等: "抗差自适应模型预测滤波及其在组合导航中的应用", 《中国惯性技术学报》 * |
Cited By (36)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107655493A (zh) * | 2017-09-06 | 2018-02-02 | 东南大学 | 一种光纤陀螺sins六位置***级标定方法 |
CN107621264A (zh) * | 2017-09-30 | 2018-01-23 | 华南理工大学 | 车载微惯性/卫星组合导航***的自适应卡尔曼滤波方法 |
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 | 杭州神驹科技有限公司 | 一种动力电池荷电状态在线估算方法及*** |
CN109164391A (zh) * | 2018-07-12 | 2019-01-08 | 杭州神驹科技有限公司 | 一种动力电池荷电状态在线估算方法及*** |
CN110823213A (zh) * | 2018-08-14 | 2020-02-21 | 北京自动化控制设备研究所 | 一种提高sins/dr组合导航***相对航向角精度的方法 |
CN110823213B (zh) * | 2018-08-14 | 2022-07-08 | 北京自动化控制设备研究所 | 一种提高sins/dr组合导航***相对航向角精度的方法 |
CN109085554A (zh) * | 2018-08-30 | 2018-12-25 | 衡阳市衡山科学城科技创新研究院有限公司 | 一种主动雷达导引头视线角零位误差估计方法及装置 |
CN109085554B (zh) * | 2018-08-30 | 2021-03-30 | 衡阳市衡山科学城科技创新研究院有限公司 | 一种主动雷达导引头视线角零位误差估计方法及装置 |
CN109405825A (zh) * | 2018-11-09 | 2019-03-01 | 北京航天时代激光导航技术有限责任公司 | 一种激光惯组惯性本体高精度拆装方法 |
CN109556633A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于自适应ekf的仿生偏振传感器多源误差标定方法 |
CN109827572A (zh) * | 2019-03-12 | 2019-05-31 | 北京星网宇达科技股份有限公司 | 一种检测车位置预测的方法及装置 |
CN109945859A (zh) * | 2019-04-01 | 2019-06-28 | 东南大学 | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 |
CN113939712B (zh) * | 2019-06-06 | 2023-11-28 | 赛峰电子与防务公司 | 基于运输设备取景器传送的信息重置运输设备惯性单元的方法和装置 |
CN113939712A (zh) * | 2019-06-06 | 2022-01-14 | 赛峰电子与防务公司 | 基于运输设备取景器传送的信息重置运输设备惯性单元的方法和装置 |
CN111044075A (zh) * | 2019-12-10 | 2020-04-21 | 上海航天控制技术研究所 | 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法 |
CN111044075B (zh) * | 2019-12-10 | 2023-09-15 | 上海航天控制技术研究所 | 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法 |
CN110926465A (zh) * | 2019-12-11 | 2020-03-27 | 哈尔滨工程大学 | 一种mems/gps松组合导航方法 |
CN111323050A (zh) * | 2020-03-19 | 2020-06-23 | 哈尔滨工程大学 | 一种捷联惯导和多普勒组合***标定方法 |
CN111290008A (zh) * | 2020-03-23 | 2020-06-16 | 兰州交通大学 | 一种动态自适应扩展卡尔曼滤波容错算法 |
CN111982151B (zh) * | 2020-07-17 | 2022-07-22 | 中科长城海洋信息***有限公司 | 一种光纤捷联惯导***的自标定方法 |
CN111982151A (zh) * | 2020-07-17 | 2020-11-24 | 中科长城海洋信息***有限公司 | 一种光纤捷联惯导***的自标定方法 |
CN112146653A (zh) * | 2020-08-03 | 2020-12-29 | 河北汉光重工有限责任公司 | 一种提高组合导航解算频率的方法 |
CN112880704A (zh) * | 2021-01-19 | 2021-06-01 | 中国人民解放军海军工程大学 | 光纤陀螺捷联惯导***智能标定方法 |
CN113465628A (zh) * | 2021-06-17 | 2021-10-01 | 杭州鸿泉物联网技术股份有限公司 | 惯性测量单元数据补偿方法及*** |
CN113569653A (zh) * | 2021-06-30 | 2021-10-29 | 宁波春建电子科技有限公司 | 一种基于面部特征信息的三维头部姿态估计算法 |
CN114061574A (zh) * | 2021-11-20 | 2022-02-18 | 北京唯实深蓝科技有限公司 | 一种基于位置不变约束及零速校正的采煤机定姿定向方法 |
CN114061574B (zh) * | 2021-11-20 | 2024-04-05 | 北京唯实深蓝科技有限公司 | 一种基于位置不变约束及零速校正的采煤机定姿定向方法 |
CN114485727A (zh) * | 2022-01-04 | 2022-05-13 | 中国煤炭科工集团太原研究院有限公司 | 一种捷联惯导***精度自检测方法和装置 |
CN115574817A (zh) * | 2022-12-08 | 2023-01-06 | 中国人民解放军国防科技大学 | 一种基于三轴旋转式惯导***的导航方法及导航*** |
CN116242397A (zh) * | 2023-03-11 | 2023-06-09 | 中国人民解放军国防科技大学 | 一种速度误差修正模型下的双惯导协同标定方法 |
CN116481564A (zh) * | 2023-03-11 | 2023-07-25 | 中国人民解放军国防科技大学 | 基于Psi角误差修正模型的极地双惯导协同标定方法 |
CN116242397B (zh) * | 2023-03-11 | 2024-01-30 | 中国人民解放军国防科技大学 | 一种速度误差修正模型下的双惯导协同标定方法 |
CN116481564B (zh) * | 2023-03-11 | 2024-02-23 | 中国人民解放军国防科技大学 | 基于Psi角误差修正模型的极地双惯导协同标定方法 |
Also Published As
Publication number | Publication date |
---|---|
CN106767900B (zh) | 2020-01-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106767900A (zh) | 一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法 | |
CN110031882B (zh) | 一种基于sins/dvl组合导航***的外量测信息补偿方法 | |
CN109556632B (zh) | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 | |
CN104344836B (zh) | 一种基于姿态观测的冗余惯导***光纤陀螺***级标定方法 | |
CN104344837B (zh) | 一种基于速度观测的冗余惯导***加速度计***级标定方法 | |
CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
CN109163721A (zh) | 姿态测量方法及终端设备 | |
CN101706284B (zh) | 提高船用光纤陀螺捷联惯导***定位精度的方法 | |
CN103822633B (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN106052686B (zh) | 基于dsptms320f28335的全自主捷联惯性导航*** | |
CN109974697A (zh) | 一种基于惯性***的高精度测绘方法 | |
CN101571394A (zh) | 基于旋转机构的光纤捷联惯性导航***初始姿态确定方法 | |
CN108168574A (zh) | 一种基于速度观测的8位置捷联惯导***级标定方法 | |
CN103575299A (zh) | 利用外观测信息的双轴旋转惯导***对准及误差修正方法 | |
CN105180968A (zh) | 一种imu/磁强计安装失准角在线滤波标定方法 | |
CN106940193A (zh) | 一种基于Kalman滤波的船舶自适应摇摆标定方法 | |
CN109870173A (zh) | 一种基于校验点的海底管道惯性导航***的轨迹修正方法 | |
CN109000640A (zh) | 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法 | |
CN110243377B (zh) | 一种基于分层式结构的集群飞行器协同导航方法 | |
CN103674064B (zh) | 捷联惯性导航***的初始标定方法 | |
CN104374401A (zh) | 一种捷联惯导初始对准中重力扰动的补偿方法 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN109708663B (zh) | 基于空天飞机sins辅助的星敏感器在线标定方法 | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN113503892A (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 |