CN110285834A - 基于一点位置信息的双惯导系统快速自主重调方法 - Google Patents
基于一点位置信息的双惯导系统快速自主重调方法 Download PDFInfo
- Publication number
- CN110285834A CN110285834A CN201910609900.9A CN201910609900A CN110285834A CN 110285834 A CN110285834 A CN 110285834A CN 201910609900 A CN201910609900 A CN 201910609900A CN 110285834 A CN110285834 A CN 110285834A
- Authority
- CN
- China
- Prior art keywords
- navigation system
- inertial navigation
- error
- speed
- information
- 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
- 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
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
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
本发明提供的是一种基于一点位置信息的双惯导系统快速自主重调方法。一:根据获得的一点外部位置信息,对系统A进行位置重调,二:系统A实时解算的位置作为系统B的外部辅助信息进行kalman滤波,利用估计出来的误差对系统B的导航参数进行校正,系统B得到准确的速度、位置和姿态信息,三:系统B被校正之后,再利用系统B的位置和速度作为系统A的外部辅助信息进行滤波,对系统A的导航参数进行校正,系统A得到准确的位置、速度和姿态信息,最终实现两套系统快速自主重调。本发明无需连续外部测量设备辅助即可实现系统的快速自主重调,不仅缩短了重调时间,而且能够有效利用量测信息,适用于水下环境。
Description
技术领域
本发明涉及的是一种捷联惯性导航系统水下导航方法,具体地说是一种双惯导系统的自主重调方法。
背景技术
捷联惯性导航系统最大特点就是自主性,它可以不接收外界任何信息,利用自身的加速度计和陀螺仪敏感到运载体的线速度和角速度信息,从而提供速度、位置及姿态等多种导航信息。此外,它还具有全天候、抗干扰、隐蔽性高、体积小、价格低等诸多优点,可以满足多方面的需求。捷联惯性导航系统对水下航行器更是具有重要意义,其作用是为长期在水下潜航的水下航行器连续提供安全航行所需的导航参数和运动参数。
大中型水下航行器通常装备两套或者多套高精度惯性导航系统。采用两套或多套配置方案主要有两个优点,一是可提高水下航行器的任务可靠性,当一套惯导故障时,另一套惯导仍可保障任务的继续执行;二是如果多套惯导采取分区布置,可就近为水下航行器载荷系统提供更精确的姿态信息,减小了船体变形对载荷系统的影响。
但是如果惯导系统长时间工作不进行重调校正,会由于误差积累使系统导航精度下降。为了保证惯性导航系统能长期有效的工作,需要引入外部信息定期或者不定期的对系统进行重调,使其导航参数得到修正,提高惯导系统的精度。系统重调即综合校正,以往的文献中,例如在专利申请号为201310156535.3,名称为“一种惯性系下使用位置和航向信息的捷联惯导系统陀螺漂移校正方法”的专利文件中公开的校正方法,利用设备连续两次获得外部位置和航向信息,以平台漂移角为中间变量在惯性系下建立陀螺漂移与位置误差和航向误差的关系,经过一次位置和方位重调后,在第二次获得位置误差和航向误差后可以计算出陀螺漂移。但是该方法需要精确的航向信息基准,而在实际环境中,准确的航向信息通常难以获得,因此该方法有很大的局限性。又如在专利申请号为201310156551.2,名称为“一种惯性系下仅使用位置信息的捷联惯导系统陀螺漂移和航向误差校正方法”的专利文件中公开的校正方法,利用设备连续三次获得外部位置信息,以平台漂移角为中间变量在惯性系下建立陀螺漂移和位置误差的关系,经过两次位置重调后,在第三次获得位置误差后可以计算出陀螺漂移和航向误差。在以上方式中都需要花费几个小时的时间,并且需要多次利用位置信息,而对于一些水下载体而言,多点的位置信息难以获得,因此这种方式并不适用。再如专利申请号为201210015077.7,名称为“一种船用惯性导航系统单点海上校准方法”的专利文件中公开的方法,在船舶远航一段时间积累一定导航误差后,通过获得外部提供一次单点位置导航数据,即可对系统进行校准。而该单点校正算法只能对系统位置进行重调,不能对惯导系统的速度和姿态进行校正,因此无法满足高精度惯性导航系统精度的要求。
发明内容
本发明的目的在于提供一种无需连续外部测量设备辅助,能够提高导航精度的基于一点位置信息的双惯导系统快速自主重调方法。
本发明的目的是这样实现的:
步骤一:根据获得的一点外部位置信息,对惯导系统A进行位置重调,
步骤二:惯导系统A实时解算的位置作为惯导系统B的外部辅助信息进行kalman滤波,利用估计出来的误差对惯导系统B的导航参数进行校正,惯导系统B得到准确的速度、位置和姿态信息,
步骤三:惯导系统B被校正之后,再利用惯导系统B的位置和速度作为惯导系统A的外部辅助信息进行滤波,对惯导系统A的导航参数进行校正,惯导系统A得到准确的位置、速度和姿态信息,最终实现两套系统快速自主重调,
在滤波过程中根据惯导系统位置误差随时间发散的特性将量测噪声方差阵的对角元素设置成斜坡函数以调节量测信息的利用率。
本发明还可以包括:
1.步骤一具体包括:
1.1对惯导系统A和惯导系统B的惯性测量元件进行充分预热;
1.2初始化两套惯导系统的导航参数,实时采集加速度计输出的比力fb和光纤陀螺仪输出的角速度惯导系统A采集到加速度计输出的比力为光纤陀螺仪输出的角速度为惯导系统B采集到加速度计输出的比力为光纤陀螺仪输出的角速度为
1.3惯导系统A和惯导系统B进行初始对准,得到载体系b系到导航坐标系n系的初始捷联姿态矩阵和之后开始进入导航工作状态;
1.4水下航行器航行一段时间后,在t0时刻获得一点外部位置信息,利用该位置信息对此刻惯导系统A输出的位置数据进行重调,得到
其中,表示t0时刻获取的一点位置纬度,λ(t0)表示t0时刻获取的一点位置经度,表示t0时刻系统A解算的纬度,λA(t0)表示t0时刻系统A解算的经度;
1.5惯导系统A的位置被重调之后再继续进行惯导解算,实时输出位置信息,纬度为经度为λA(t),其中t表示当前时刻。
2.步骤二具体包括:
2.1选取惯导系统B的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移为kalman滤波的状态估计量;
2.2利用惯导系统A的位置被重调之后惯导系统A实时更新的导航纬度和导航经度λA(t)作为惯导系统B进行kalman滤波的外部辅助信息,与惯导系统B导航更新计算的位置信息纬度和经度λB(t)分别作差,将所得到的位置差值作为惯导系统B进行kalman滤波时的量测值;
2.3建立惯导系统B的kalman滤波器的状态方程和量测方程;
2.4根据惯导系统A位置误差随时间积累的特性,对惯导系统B滤波时的量测噪声方差阵RB(t)进行实时调节的设置;
2.5基于kalman滤波算法对惯导系统B的误差进行实时估计,得到惯导系统B的位置误差、速度误差和平台失准角的状态估计值;
2.6利用步骤2.5中估计出来的误差来校正惯导系统B的导航参数,完成惯导系统B的自主重调。
3.步骤三具体包括:
3.1选取惯导系统A的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移作为kalman滤波的状态估计量;
3.2惯导系统B的位置和速度都经过校正,将重调后惯导系统B实时输出的位置和λB(t)以及速度作为惯导系统A的外部辅助信息,与惯导系统A导航更新计算的位置信息和λA(t)以及速度信息分别作差,将所得到的位置和速度差值作为惯导系统A进行kalman滤波时的量测值;
3.3建立惯导系统A的kalman滤波器的状态方程和量测方程;
3.4基于kalman滤波算法对惯导系统A的误差进行实时估计,得到惯导系统A的位置误差、速度误差和平台失准角的状态估计值;
3.5利用步骤3.4估计出来的误差来校正惯导系统A的导航参数,完成惯导系统A的自主重调。
4.将量测噪声方差阵RB(t)中对角元素设置为斜率为0.01的斜坡函数,具体形式表示为:
RB(t)=diag{[0.01×(t-t0)+10]2,[0.01×(t-t0)+10]2}
其中,t表示当前时刻,t0表示获得一点外部位置信息的时刻。
5.惯导系统A进行kalman滤波时的量测值ZA(t)为:
其中,为惯导系统A解算的纬度、λA(t)为其解算的经度;为惯导系统B解算的纬度、λB(t)为其解算的经度;为惯导系统A解算的东向速度、为其北向速度、为其天向速度;为惯导系统B解算的东向速度、为其北向速度、为其天向速度。
本发明针对装备有两套高精度惯性导航系统的大中型水下航行器,提出了一种基于一点位置信息的高精度双惯导系统快速自主重调方法。该方法无需连续外部测量设备辅助(只需要从外部设备中获取一点位置信息即可),大大缩短了系统重调的时间。该方法不仅可以对两套系统的位置进行重调,而且可以对系统的速度和姿态进行校正,提高导航精度。
本发明有益效果在于:
第一,无需连续外部测量设备辅助(只需要从外部设备中获取一点位置信息即可),缩短了系统重调所需的时间,易于实施,适用于各种水下航行器。第二,本发明不仅可以对水下航行器上两套高精度惯性导航系统的位置进行重调,而且可以对其速度和姿态进行校正,提高导航精度。第三,本发明中系统B进行kalman滤波时会根据外观测量的特性对量测噪声方差阵进行实时调节,以提高估计精度,进一步提高了导航精度。
附图说明
图1本发明的实现流程图。
图2高精度捷联惯性导航系统B自主重调原理图。
图3高精度捷联惯性导航系统A自主重调原理图。
具体实施方式
下面举例对本发明做更详细的描述。
具体实施方式一:
步骤1、首先对两套高精度光纤陀螺捷联惯性导航系统A和B的惯性测量元件进行充分预热;
步骤2、初始化两套高精度惯导系统A和B的导航参数,实时采集加速度计输出的比力fb和光纤陀螺仪输出的角速度(惯导系统A采集到加速度计输出的比力为光纤陀螺仪输出的角速度为惯导系统B采集到加速度计输出的比力为光纤陀螺仪输出的角速度为);
步骤3、捷联惯性导航系统A和B进行初始对准,得到载体系(b系)到导航坐标系(n系,本发明中导航坐标系选取地理坐标系)的初始捷联姿态矩阵和之后开始进入导航工作状态;
步骤4、水下航行器航行一段时间后,在t0时刻获得一点外部位置信息,利用该位置信息对此刻惯性导航系统A输出的位置数据进行重调,得到
其中,表示t0时刻获取的一点位置纬度,λ(t0)表示t0时刻获取的一点位置经度,表示t0时刻系统A解算的纬度,λA(t0)表示t0时刻系统A解算的经度;
步骤5、系统A的位置被重调之后再继续进行惯导解算,实时输出位置信息,纬度为经度为λA(t),其中t表示当前时刻;
步骤6、选取系统B的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移为kalman滤波的状态估计量;
步骤7、利用步骤5中位置重调后惯导系统A实时更新的导航纬度和导航经度λA(t)作为惯导系统B进行kalman滤波的外部辅助信息,与惯导系统B导航更新计算的位置信息(纬度和经度λB(t))分别作差,将所得到的位置差值作为系统B进行kalman滤波时的量测值ZB(t):
步骤8、建立惯导系统B的kalman滤波器的状态方程和量测方程;
步骤9、系统B进行kalman滤波时是利用系统A的位置作为外部辅助信息的,因此根据系统A的位置随时间发散的特性对量测噪声方差阵RB(t)进行设置。本发明将量测噪声方差阵RB(t)中对角元素设置为斜率为0.01的斜坡函数,以便对其进行实时调节,具体形式如下:
RB(t)=diag{[0.01×(t-t0)+10]2,[0.01×(t-t0)+10]2} (3)
其中,t表示当前时刻,t0表示获得一点外部位置信息的时刻;
步骤10、基于kalman滤波算法对系统B的误差进行实时估计,得到系统B的位置误差、速度误差和平台失准角的状态估计值;
步骤11、利用步骤10中估计出来的误差来校正系统B的导航参数,完成系统B的自主重调;
步骤12、选取系统A的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移作为kalman滤波的状态估计量;
步骤13、在步骤11之后,系统B的位置和速度都经过了校正,此处利用重调后的系统B来辅助系统A再次进行重调,进一步来提高系统A的导航精度。当用系统B来辅助系统A时,除了系统B实时输出的位置信息λB(t)之外,还增加了速度信息来作为系统A的外部辅助信息,系统A进行kalman滤波时的量测值ZA(t)为:
其中,为系统A解算的纬度、λA(t)为其解算的经度;为系统B解算的纬度、λB(t)为其解算的经度;为系统A解算的东向速度、为其北向速度、为其天向速度;为系统B解算的东向速度、为其北向速度、为其天向速度;
步骤14、建立惯导系统A的kalman滤波器的状态方程和量测方程;
步骤15、基于kalman滤波算法对系统A的误差进行实时估计,得到系统A的位置误差、速度误差和平台失准角的状态估计值;
步骤16、利用步骤15中估计出来的误差来校正系统A的导航参数,完成系统A的自主重调。
具体实施方式二:
步骤1、对两套高精度光纤陀螺捷联惯性导航系统A和B的惯性测量元件进行充分预热;
步骤2、初始化两套高精度惯导系统A和B的导航参数,实时采集加速度计输出的比力fb和光纤陀螺仪输出的角速度(惯导系统A采集到加速度计输出的比力为光纤陀螺仪输出的角速度为惯导系统B采集到加速度计输出的比力为光纤陀螺仪输出的角速度为);
步骤3、捷联惯性导航系统A和B进行初始对准,分别得到载体系(b系)到导航坐标系(n系,本发明中导航坐标系选取地理坐标系)的初始捷联姿态矩阵和之后开始进入导航工作状态;
步骤4、根据步骤3中得到的光纤陀螺捷联惯性导航系统A的初始捷联姿态矩阵和步骤2中实时采集到角速度递推更新得到姿态四元数并对其进行归一化计算,归一化后的姿态四元数qA具体表达式为:
qA=[qA1 qA2 qA3 qA4]T (1)
其中,qA1、qA2、qA3和qA4是归一化后四元数qA的元素;
步骤5、根据步骤4中得到的归一化后的姿态四元数qA计算得到新的捷联姿态矩阵
步骤6、利用步骤5得到的捷联姿态矩阵将步骤2采集到的光纤陀螺捷联惯性导航系统A的加速度计输出的比力转换到导航坐标系下:
其中,表示加速度计输出比力在导航坐标系的投影;
步骤7、根据步骤6中系统A的加速度计输出比力在导航坐标系的投影去除有害加速度后得到系统A的加速度进一步更新计算得到速度,记为
其中,为系统A解算的东向速度、为其北向速度、为其天向速度,t表示当前时刻;
步骤8、利用步骤7更新计算后的速度更新计算捷联惯性导航系统A的导航纬度和经度λA(t);
步骤9、水下航行器航行一段时间后,在t0时刻获得一点外部位置信息,利用该位置信息对此刻惯性导航系统A输出的位置数据进行重调,得到
其中,表示t0时刻获取的一点位置纬度,λ(t0)表示t0时刻获取的一点位置经度,表示t0时刻系统A解算的纬度,λA(t0)表示t0时刻系统A解算的经度;
步骤10、系统A位置被重调之后再继续进行惯导解算,实时输出位置信息和λA(t);
步骤11、根据步骤3中得到的光纤陀螺捷联惯性导航系统B的初始捷联姿态矩阵和步骤2中实时采集到角速度递推得到姿态四元数并对其进行归一化计算,归一化后的姿态四元数qB为:
qB=[qB1 qB2 qB3 qB4]T (6)
其中,qB1、qB2、qB3和qB4是归一化后四元数qB的元素;
根据归一化后的姿态四元数qB计算得到新的捷联姿态矩阵
步骤12、根据步骤2的采集到的光纤陀螺捷联惯性导航系统B的加速度计输出的比力和步骤11更新计算后的新的捷联姿态矩阵进一步计算得到速度
其中,为系统B解算的东向速度、为其北向速度、为其天向速度。之后利用更新后的速度计算出捷联惯性导航系统B的导航纬度和经度λB(t);
步骤13、选取系统B的位置误差(纬度误差经度误差δλB和高度误差δhB)、东北天三个方向的速度误差δVB=[δVB_E δVB_N δVB_U]T、平台失准角误差φB=[φB_x φB_y φB_z]T、载体系三轴的加速度计的零位偏移ΔAB=[ΔAB_bx ΔAB_by ΔAB_bz]T和陀螺仪的常值漂移εB=[εB_bx εB_by εB_bz]T为kalman滤波的状态估计量XB,即
XB=[δrB δVB φB ΔAB εB]T
步骤14、选取系统B进行kalman滤波的系统噪声向量WB为:
其中,和为系统B在载体系中三轴加速度计的随机噪声,和为系统B在载体系中三轴陀螺仪的随机噪声,均为高斯白噪声;
步骤15、利用步骤10中位置重调后惯导系统A实时更新的导航纬度和导航经度λA(t)作为惯导系统B进行kalman滤波的外部辅助信息,与步骤12中惯导系统B导航更新计算的位置信息(纬度和经度λB(t))分别作差,将所得到的位置差值作为系统B进行kalman滤波时的量测值ZB(t):
步骤16、建立惯导系统B的kalman滤波器的状态方程和量测方程:
其中,FB(t)是15×15维的状态一步转移矩阵,GB(t)是15×6维的系统噪声驱动矩阵,两者是根据惯性导航系统误差方程得到的系统结构参数;HB(t)是2×15维的量测矩阵;WB(t)是6×1维的系统噪声向量,VB(t)是2×1维的量测噪声向量,两者都是零均值的高斯白噪声向量序列(服从正态分布),且它们之间互不相关,系统噪声方差阵为QB(t),量测噪声方差阵为RB(t);
步骤17、系统B进行kalman滤波时是利用系统A的位置作为外部辅助信息的,根据系统A的位置随时间发散的特性对量测噪声方差阵RB(t)进行设置。将量测噪声方差阵RB(t)中对角元素设置为斜率为0.01的斜坡函数,以便对其进行实时调节,具体形式如下:
RB(t)=diag{[0.01×(t-t0)+10]2,[0.01×(t-t0)+10]2} (11)
其中,t表示当前时刻,t0表示获得一点外部位置信息的时刻;
步骤18、将步骤16中的连续的状态方程和量测方程离散化:
其中,k代表kalman滤波时刻,是离散化后的状态一步转移矩阵;是离散化后系统噪声驱动矩阵;是离散化后的量测矩阵;是离散化后的系统噪声向量,离散化后的系统噪声方差阵为 是离散化后的量测噪声向量,离散化后的量测噪声方差阵为
步骤19、初始化系统B的kalman滤波器的滤波参数,即设置kalman滤波状态估计量的初值和状态估计均方误差的初值P0 B;
步骤20、基于kalman滤波算法对系统B的误差进行实时估计,滤波步骤如下:
(1)状态一步预测
(2)状态一步预测均方误差
(3)滤波增益
(4)状态估计
(5)状态估计均方误差
其中,I为15×15维的单位矩阵;
步骤21、根据步骤20中kalman滤波算法得到系统B误差进行实时估计,得到系统B的纬度误差估计值经度误差估计值东向速度误差估计值北向速度误差估计值天向速度误差估计值以及三轴平台失准角的估计值
步骤22、利用步骤21中实时估计出来的纬度误差经度误差和东北天三个方向的速度误差对系统B实时更新的纬度经度λB(t)及速度重调,即:
步骤23、利用步骤20中实时估计出来的平台失准角来构造补偿四元数对惯导系统B导航更新的当前时刻捷联姿态矩阵对应的姿态四元数qB=[qB1 qB2 qB3 qB4]T进行补偿得到修正后的四元数q′B=[q′B1 q′B2q′B3 q′B4]T:
其中,修正后的四元数q′B的元素为:
步骤24、将步骤23中得到的修正后的四元数q′B进行归一化处理:
得到归一化后的四元数为:
其中,和是归一化后四元数的元素;
步骤25、根据归一化后的姿态四元数计算得到校正后的捷联姿态矩阵
至此,完成系统B的自主重调过程;
步骤26、选取系统A的位置误差(纬度误差经度误差δλA和高度误差δhA)、东北天三个方向的速度误差δVA=[δVA_E δVA_N δVA_U]T、平台失准角误差φA=[φA_x φA_y φA_z]T、载体系三轴加速度计的零位偏移ΔAA=[ΔAA_bx ΔAA_by ΔAA_bz]T和陀螺仪的常值漂移εA=[εA_bx εA_by εA_bz]T为kalman滤波的状态估计量XA,即
XA=[δrA δVA φA ΔAA εA]T
步骤27、选取系统A进行kalman滤波的系统噪声向量为WA:
其中,和为系统A在载体系中三轴加速度计的随机噪声,和为系统A在载体系中三轴陀螺的随机噪声,均为高斯白噪声;
步骤28、在步骤25之后,系统B已经完成自主重调,将系统B实时输出的位置信息λB(t)和速度信息作为系统A的外部辅助信息,与惯导系统A导航更新计算的位置信息λA(t)和速度信息分别作差,并将所得到的位置和速度差值作为系统A进行kalman滤波时的量测值ZA(t):
步骤29、建立惯导系统A的kalman滤波器的状态方程和量测方程:
其中,FA(t)是15×15维的状态一步转移矩阵,GA(t)是15×6维的系统噪声驱动矩阵,两者是根据惯性导航系统误差方程得到的系统结构参数;HA(t)是5×15维的量测矩阵;WA(t)是6×1维的系统噪声向量,VA(t)是5×1维的量测噪声向量,两者都是零均值的高斯白噪声向量序列(服从正态分布),且它们之间互不相关,系统噪声方差阵为QA(t),量测噪声方差阵为RA(t);
步骤30、将步骤29中的连续的状态方程和量测方程离散化:
其中,是离散化后的状态一步转移矩阵;是离散化后系统噪声驱动矩阵;是离散化后的量测矩阵;是离散化后的系统噪声向量,离散化后的系统噪声方差阵为 是离散化后的量测噪声向量,离散化后的量测噪声方差阵为
步骤31、初始化系统A的kalman滤波器的滤波参数,即设置kalman滤波状态估计量的初值和状态估计均方误差的初值
步骤32、基于kalman滤波算法对系统A的误差进行实时估计,滤波步骤如下:
(1)状态一步预测
(2)状态一步预测均方误差
(3)滤波增益
(4)状态估计
(5)状态估计均方误差Pk A:
其中,I为15×15维的单位矩阵;
步骤33、根据步骤32中kalman滤波算法对系统A的误差进行实时估计,得到系统A的纬度误差估计值经度误差估计值东向速度误差估计值北向速度误差估计值天向速度误差估计值以及三轴平台失准角的估计值
步骤34、利用步骤33中实时估计出来的纬度误差经度误差和东北天三个方向的速度误差对系统A实时更新的纬度经度λA(t)及速度重调,即:
步骤35、利用步骤33中实时估计出来的平台失准角来构造补偿四元数对惯导系统A导航更新的当前时刻捷联姿态矩阵对应的姿态四元数qA=[qA1 qA2 qA3 qA4]T进行补偿得到修正后的四元数q′A=[q′A1 q′A2q′A3 q′A4]T:
其中修正后的四元数q′A的元素为:
步骤36、将步骤35中得到的修正后的四元数q′A进行归一化处理:
得到归一化后的四元数为:
其中,和是归一化后四元数的元素;
步骤37、根据归一化后的姿态四元数计算得到校正后的捷联姿态矩阵
完成系统A的自主重调过程;
至此,完成了水下航行器高精度双捷联惯导系统A和B的自主重调,为水下航行器上的两套高精度惯导系统的提供准确的导航参数。
Claims (6)
1.一种基于一点位置信息的双惯导系统快速自主重调方法,其特征是:
步骤一:根据获得的一点外部位置信息,对惯导系统A进行位置重调,
步骤二:惯导系统A实时解算的位置作为惯导系统B的外部辅助信息进行kalman滤波,利用估计出来的误差对惯导系统B的导航参数进行校正,惯导系统B得到准确的速度、位置和姿态信息,
步骤三:惯导系统B被校正之后,再利用惯导系统B的位置和速度作为惯导系统A的外部辅助信息进行滤波,对惯导系统A的导航参数进行校正,惯导系统A得到准确的位置、速度和姿态信息,最终实现两套系统快速自主重调,
在滤波过程中根据惯导系统位置误差随时间发散的特性将量测噪声方差阵的对角元素设置成斜坡函数以调节量测信息的利用率。
2.根据权利要求1所述的基于一点位置信息的双惯导系统快速自主重调方法,其特征是步骤一具体包括:
1.1对惯导系统A和惯导系统B的惯性测量元件进行充分预热;
1.2初始化两套惯导系统的导航参数,实时采集加速度计输出的比力fb和光纤陀螺仪输出的角速度惯导系统A采集到加速度计输出的比力为光纤陀螺仪输出的角速度为惯导系统B采集到加速度计输出的比力为光纤陀螺仪输出的角速度为
1.3惯导系统A和惯导系统B进行初始对准,得到载体系b系到导航坐标系n系的初始捷联姿态矩阵和之后开始进入导航工作状态;
1.4水下航行器航行一段时间后,在t0时刻获得一点外部位置信息,利用该位置信息对此刻惯导系统A输出的位置数据进行重调,得到
其中,表示t0时刻获取的一点位置纬度,λ(t0)表示t0时刻获取的一点位置经度,表示t0时刻系统A解算的纬度,λA(t0)表示t0时刻系统A解算的经度;
1.5惯导系统A的位置被重调之后再继续进行惯导解算,实时输出位置信息,纬度为经度为λA(t),其中t表示当前时刻。
3.根据权利要求2所述的基于一点位置信息的双惯导系统快速自主重调方法,其特征是步骤二具体包括:
2.1选取惯导系统B的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移为kalman滤波的状态估计量;
2.2利用惯导系统A的位置被重调之后惯导系统A实时更新的导航纬度和导航经度λA(t)作为惯导系统B进行kalman滤波的外部辅助信息,与惯导系统B导航更新计算的位置信息纬度和经度λB(t)分别作差,将所得到的位置差值作为惯导系统B进行kalman滤波时的量测值;
2.3建立惯导系统B的kalman滤波器的状态方程和量测方程;
2.4根据惯导系统A位置误差随时间积累的特性,对惯导系统B滤波时的量测噪声方差阵RB(t)进行实时调节的设置;
2.5基于kalman滤波算法对惯导系统B的误差进行实时估计,得到惯导系统B的位置误差、速度误差和平台失准角的状态估计值;
2.6利用步骤2.5中估计出来的误差来校正惯导系统B的导航参数,完成惯导系统B的自主重调。
4.根据权利要求3所述的基于一点位置信息的双惯导系统快速自主重调方法,其特征是步骤三具体包括:
3.1选取惯导系统A的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移作为kalman滤波的状态估计量;
3.2惯导系统B的位置和速度都经过校正,将重调后惯导系统B实时输出的位置和λB(t)以及速度作为惯导系统A的外部辅助信息,与惯导系统A导航更新计算的位置信息和λA(t)以及速度信息分别作差,将所得到的位置和速度差值作为惯导系统A进行kalman滤波时的量测值;
3.3建立惯导系统A的kalman滤波器的状态方程和量测方程;
3.4基于kalman滤波算法对惯导系统A的误差进行实时估计,得到惯导系统A的位置误差、速度误差和平台失准角的状态估计值;
3.5利用步骤3.4估计出来的误差来校正惯导系统A的导航参数,完成惯导系统A的自主重调。
5.根据权利要求4所述的基于一点位置信息的双惯导系统快速自主重调方法,其特征是将量测噪声方差阵RB(t)中对角元素设置为斜率为0.01的斜坡函数,具体形式表示为:
RB(t)=diag{[0.01×(t-t0)+10]2,[0.01×(t-t0)+10]2}
其中,t表示当前时刻,t0表示获得一点外部位置信息的时刻。
6.根据权利要求5所述的基于一点位置信息的双惯导系统快速自主重调方法,其特征是惯导系统A进行kalman滤波时的量测值ZA(t)为:
其中,为惯导系统A解算的纬度、λA(t)为其解算的经度;为惯导系统B解算的纬度、λB(t)为其解算的经度;为惯导系统A解算的东向速度、为其北向速度、为其天向速度;为惯导系统B解算的东向速度、为其北向速度、为其天向速度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910609900.9A CN110285834B (zh) | 2019-07-08 | 2019-07-08 | 基于一点位置信息的双惯导系统快速自主重调方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910609900.9A CN110285834B (zh) | 2019-07-08 | 2019-07-08 | 基于一点位置信息的双惯导系统快速自主重调方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110285834A true CN110285834A (zh) | 2019-09-27 |
CN110285834B CN110285834B (zh) | 2022-12-13 |
Family
ID=68020970
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910609900.9A Active CN110285834B (zh) | 2019-07-08 | 2019-07-08 | 基于一点位置信息的双惯导系统快速自主重调方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110285834B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110824583A (zh) * | 2019-11-21 | 2020-02-21 | 中国船舶重工集团公司第七0七研究所 | 一种航空重力仪用在线自主标定方法 |
CN112525216A (zh) * | 2020-09-28 | 2021-03-19 | 中国船舶重工集团公司第七0七研究所 | 一种惯导系统东向陀螺漂移及航向误差校准方法 |
CN114323065A (zh) * | 2021-11-24 | 2022-04-12 | 中国船舶重工集团公司第七0七研究所 | 基于多手段融合的水下自主导航系统误差监测与估计方法 |
CN116519011A (zh) * | 2023-03-11 | 2023-08-01 | 中国人民解放军国防科技大学 | 基于Psi角误差修正模型的长航时双惯导协同标定方法 |
Citations (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CA2447809A1 (en) * | 2001-06-04 | 2002-12-12 | Novatel Inc. | An inertial/gps navigation system |
WO2005071431A1 (en) * | 2004-01-23 | 2005-08-04 | Novatel Inc. | Inertial gps navigation system with modified kalman filter |
US20080114544A1 (en) * | 2006-06-17 | 2008-05-15 | Gang Kevin Liu | Estimate of relative position between navigation units |
CN101788296A (zh) * | 2010-01-26 | 2010-07-28 | 北京航空航天大学 | 一种sins/cns深组合导航系统及其实现方法 |
KR20120048335A (ko) * | 2010-11-05 | 2012-05-15 | 국방과학연구소 | 비행 중 정렬 장치 및 그 방법 |
CN102589570A (zh) * | 2012-01-17 | 2012-07-18 | 北京理工大学 | 一种船用惯性导航系统单点海上校准方法 |
CN103453903A (zh) * | 2013-08-26 | 2013-12-18 | 哈尔滨工程大学 | 一种基于惯性测量组件的管道探伤系统导航定位方法 |
CN103575299A (zh) * | 2013-11-13 | 2014-02-12 | 北京理工大学 | 利用外观测信息的双轴旋转惯导系统对准及误差修正方法 |
CN103968839A (zh) * | 2014-05-21 | 2014-08-06 | 哈尔滨工程大学 | 一种基于蜂群算法改进ckf的单点重力匹配方法 |
CN103983277A (zh) * | 2014-05-16 | 2014-08-13 | 哈尔滨工程大学 | 一种适用于极区的惯导系统综合校正方法 |
CN104019828A (zh) * | 2014-05-12 | 2014-09-03 | 南京航空航天大学 | 高动态环境下惯性导航系统杆臂效应误差在线标定方法 |
CN104776847A (zh) * | 2015-04-09 | 2015-07-15 | 哈尔滨工程大学 | 一种适用于水下导航系统单点估计陀螺漂移的方法 |
CN105371844A (zh) * | 2015-12-02 | 2016-03-02 | 南京航空航天大学 | 一种基于惯性/天文互助的惯性导航系统初始化方法 |
CN105806339A (zh) * | 2016-05-14 | 2016-07-27 | 中卫物联成都科技有限公司 | 一种基于gnss、ins和守时系统的组合导航方法和设备 |
CN105865488A (zh) * | 2016-05-19 | 2016-08-17 | 北京航空航天大学 | 一种基于自主量测信息的静基座动态快速精确对准方法 |
CN106123850A (zh) * | 2016-06-28 | 2016-11-16 | 哈尔滨工程大学 | Auv配载多波束声呐水下地形测绘修正方法 |
CN106441292A (zh) * | 2016-09-28 | 2017-02-22 | 哈尔滨工业大学 | 一种基于众包imu惯导数据的楼宇室内平面图建立方法 |
CN108180925A (zh) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | 一种里程计辅助车载动态对准方法 |
CN109186597A (zh) * | 2018-08-31 | 2019-01-11 | 武汉大学 | 一种基于双mems-imu的室内轮式机器人的定位方法 |
CN109211266A (zh) * | 2018-07-13 | 2019-01-15 | 哈尔滨工程大学 | 一种船用格网惯性导航系统综合校正方法 |
CN109781100A (zh) * | 2019-03-08 | 2019-05-21 | 哈尔滨工程大学 | 一种基于卡尔曼滤波估计的极区组合导航双通道误差校正方法 |
-
2019
- 2019-07-08 CN CN201910609900.9A patent/CN110285834B/zh active Active
Patent Citations (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CA2447809A1 (en) * | 2001-06-04 | 2002-12-12 | Novatel Inc. | An inertial/gps navigation system |
WO2005071431A1 (en) * | 2004-01-23 | 2005-08-04 | Novatel Inc. | Inertial gps navigation system with modified kalman filter |
US20080114544A1 (en) * | 2006-06-17 | 2008-05-15 | Gang Kevin Liu | Estimate of relative position between navigation units |
CN101788296A (zh) * | 2010-01-26 | 2010-07-28 | 北京航空航天大学 | 一种sins/cns深组合导航系统及其实现方法 |
KR20120048335A (ko) * | 2010-11-05 | 2012-05-15 | 국방과학연구소 | 비행 중 정렬 장치 및 그 방법 |
CN102589570A (zh) * | 2012-01-17 | 2012-07-18 | 北京理工大学 | 一种船用惯性导航系统单点海上校准方法 |
CN103453903A (zh) * | 2013-08-26 | 2013-12-18 | 哈尔滨工程大学 | 一种基于惯性测量组件的管道探伤系统导航定位方法 |
CN103575299A (zh) * | 2013-11-13 | 2014-02-12 | 北京理工大学 | 利用外观测信息的双轴旋转惯导系统对准及误差修正方法 |
CN104019828A (zh) * | 2014-05-12 | 2014-09-03 | 南京航空航天大学 | 高动态环境下惯性导航系统杆臂效应误差在线标定方法 |
CN103983277A (zh) * | 2014-05-16 | 2014-08-13 | 哈尔滨工程大学 | 一种适用于极区的惯导系统综合校正方法 |
CN103968839A (zh) * | 2014-05-21 | 2014-08-06 | 哈尔滨工程大学 | 一种基于蜂群算法改进ckf的单点重力匹配方法 |
CN104776847A (zh) * | 2015-04-09 | 2015-07-15 | 哈尔滨工程大学 | 一种适用于水下导航系统单点估计陀螺漂移的方法 |
CN105371844A (zh) * | 2015-12-02 | 2016-03-02 | 南京航空航天大学 | 一种基于惯性/天文互助的惯性导航系统初始化方法 |
CN105806339A (zh) * | 2016-05-14 | 2016-07-27 | 中卫物联成都科技有限公司 | 一种基于gnss、ins和守时系统的组合导航方法和设备 |
CN105865488A (zh) * | 2016-05-19 | 2016-08-17 | 北京航空航天大学 | 一种基于自主量测信息的静基座动态快速精确对准方法 |
CN106123850A (zh) * | 2016-06-28 | 2016-11-16 | 哈尔滨工程大学 | Auv配载多波束声呐水下地形测绘修正方法 |
CN106441292A (zh) * | 2016-09-28 | 2017-02-22 | 哈尔滨工业大学 | 一种基于众包imu惯导数据的楼宇室内平面图建立方法 |
CN108180925A (zh) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | 一种里程计辅助车载动态对准方法 |
CN109211266A (zh) * | 2018-07-13 | 2019-01-15 | 哈尔滨工程大学 | 一种船用格网惯性导航系统综合校正方法 |
CN109186597A (zh) * | 2018-08-31 | 2019-01-11 | 武汉大学 | 一种基于双mems-imu的室内轮式机器人的定位方法 |
CN109781100A (zh) * | 2019-03-08 | 2019-05-21 | 哈尔滨工程大学 | 一种基于卡尔曼滤波估计的极区组合导航双通道误差校正方法 |
Non-Patent Citations (7)
Title |
---|
FENG SUN等: "A Robust Self-Alignment Method for Ship’s Strapdown INS", 《SENSORS》 * |
JAMES H. KEPPER等: "MEMS IMU and one-way-travel-time navigation for autonomous underwater vehicles", 《OCEANS 2017 - ABERDEEN》 * |
刘为任等: "一种双惯导组合导航方法", 《中国惯性技术学报》 * |
唐鸿儒等: "基于ROS的遥控水下机器人监控系统研制", 《南京信息工程大学学报》 * |
奔粤阳等: "卫导辅助下的舰船捷联惯导航行间粗对准方法", 《系统工程与电子技术》 * |
王东升等: "基于方位旋转调制的平台式惯导系统"一点校"", 《中国惯性技术学报》 * |
赵立莎: "高精度惯性导航系统误差抑制研究", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110824583A (zh) * | 2019-11-21 | 2020-02-21 | 中国船舶重工集团公司第七0七研究所 | 一种航空重力仪用在线自主标定方法 |
CN112525216A (zh) * | 2020-09-28 | 2021-03-19 | 中国船舶重工集团公司第七0七研究所 | 一种惯导系统东向陀螺漂移及航向误差校准方法 |
CN114323065A (zh) * | 2021-11-24 | 2022-04-12 | 中国船舶重工集团公司第七0七研究所 | 基于多手段融合的水下自主导航系统误差监测与估计方法 |
CN114323065B (zh) * | 2021-11-24 | 2023-04-28 | 中国船舶重工集团公司第七0七研究所 | 基于多手段融合的水下自主导航系统误差监测与估计方法 |
CN116519011A (zh) * | 2023-03-11 | 2023-08-01 | 中国人民解放军国防科技大学 | 基于Psi角误差修正模型的长航时双惯导协同标定方法 |
CN116519011B (zh) * | 2023-03-11 | 2024-03-01 | 中国人民解放军国防科技大学 | 基于Psi角误差修正模型的长航时双惯导协同标定方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110285834B (zh) | 2022-12-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103994763B (zh) | 一种火星车的sins/cns深组合导航系统及其实现方法 | |
CN110285834A (zh) | 基于一点位置信息的双惯导系统快速自主重调方法 | |
CN101788296B (zh) | 一种sins/cns深组合导航系统及其实现方法 | |
CN104457754B (zh) | 一种基于sins/lbl紧组合的auv水下导航定位方法 | |
CN109443379A (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN109813311A (zh) | 一种无人机编队协同导航方法 | |
CN101881619B (zh) | 基于姿态测量的船用捷联惯导与天文定位方法 | |
CN110221332A (zh) | 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 | |
US20070006472A1 (en) | Independent personal underwater navigation system for scuba divers | |
CN106052686B (zh) | 基于dsptms320f28335的全自主捷联惯性导航系统 | |
JP3850796B2 (ja) | スレーブ慣性測定システムの姿勢アライメント | |
CN102829777A (zh) | 自主式水下机器人组合导航系统及方法 | |
CN107797125B (zh) | 一种减小深海探测型auv导航定位误差的方法 | |
CN104697520B (zh) | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 | |
CN105928515B (zh) | 一种无人机导航系统 | |
CN102879011A (zh) | 一种基于星敏感器辅助的月面惯导对准方法 | |
CN108931791A (zh) | 卫惯紧组合钟差修正系统和方法 | |
CN113503892B (zh) | 一种基于里程计和回溯导航的惯导系统动基座初始对准方法 | |
RU2592715C1 (ru) | Астронавигационная система | |
CN103245357A (zh) | 一种船用捷联惯导系统二次快速对准方法 | |
CN103278165A (zh) | 基于剩磁标定的磁测及星光备份的自主导航方法 | |
CN106767928A (zh) | 一种自适应快速传递对准方法 | |
CN109489661A (zh) | 一种卫星初始入轨时陀螺组合常值漂移估计方法 | |
CN104501809A (zh) | 一种基于姿态耦合的捷联惯导/星敏感器组合导航方法 | |
CN116222551A (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 |