CN109029454A - 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法 - Google Patents
一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法 Download PDFInfo
- Publication number
- CN109029454A CN109029454A CN201810770789.7A CN201810770789A CN109029454A CN 109029454 A CN109029454 A CN 109029454A CN 201810770789 A CN201810770789 A CN 201810770789A CN 109029454 A CN109029454 A CN 109029454A
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- error
- abscissa
- horizontal
- navigation system
- 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.)
- Pending
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/20—Instruments for performing navigational calculations
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明提出了一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,属于极区导航技术领域,通过建立横坐标系下卡尔曼滤波误差模型,估计水平姿态误差角,利用反馈校正不断校正水平姿态误差角,从而抑制姿态、速度和位置误差中的舒勒振荡,实现卡尔曼滤波外水平阻尼。根据外速度有效性判据判断外速度信息的可靠性;并根据判断结果确定当前捷联惯导系统的运行状态,直到外速度信息有效时,系统从无阻尼切换至阻尼状态;最后待卡尔曼滤波稳定后,将估计的水平姿态误差角不断地进行反馈校正,从而抑制姿态、位置和速度误差中的舒勒振荡,实现极区卡尔曼滤波外水平阻尼。本发明不仅能实现极区外水平阻尼,还能抑制阻尼切换过程产生的超调误差。
Description
技术领域
本发明属于极区导航技术领域,涉及一种适用于极区的捷联惯性导航系统(SINS)阻尼算法,具体涉及一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法。
背景技术
由于极区特殊的地理环境,一般的导航方法如卫星导航、无线电导航、地磁导航都无法正常工作,捷联惯导系统以其具有自主性、连续性和全球性成为了极区导航的首选。随着纬度的升高,极区存在经线收敛过快和北向失去定义等问题,常用的指北方位惯导系统在极区无法正确解算出相关的导航信息。
为了解决这个问题,极区惯性导航系统,如游移方位惯导系统、自由方位惯导系统和横坐标系惯导系统等逐渐发展起来。其中游移方位惯导系统和自由方位惯导系统虽然能完成姿态和位置方向余弦矩阵计算,但是游移角和经度的提取仍然存在奇异值。横坐标系惯性导航系统通过重新定义横地球坐标和横地理坐标系,巧妙地解决了惯性导航系统在极区位置和姿态的解算问题。
捷联惯导系统在极区的惯导解算仍然包含三种周期性振荡:舒勒振荡、傅科振荡和地球振荡。对于长时间运行的载体而言,需要采用阻尼等方法来消除这些周期性振荡,以提高惯导解算精度。对于横坐标系捷联惯导系统,采用基于阻尼网络的全阻尼导航算法能够有效抑制舒勒振荡、傅科振荡和地球振荡。针对有外部信息定期进行校正的系统,采用基于阻尼网络的外水平阻尼算法来抑制舒勒周期振荡就能有效提高系统的导航精度。但是这种基于阻尼网络的算法一般存在两个主要问题:一是根据运动状态不同,阻尼网络参数难以确定,需要反复尝试;而是当外速度信息存在误差时,无阻尼切换到阻尼状态会打破系统平衡,带来较大的超调误差。
发明内容
本发明的目的在于提供一种适用于极区的基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法。
本发明的目的是这样实现的:
本发明提出了一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,通过以下步骤实现:
(1)建立横坐标系捷联惯导力学编排,获得横坐标系下的运动学方程:
其中,表示载体坐标系b到横导航坐标系的方向余弦矩阵;为载体的对地速度在横导航系下的投影;为载体在横坐标系下的位置矢量;为地球自转角速度在横导航系下的投影;为横导航系相对于横地球坐标系e的旋转角速率在横导航系下的投影;g为当地重力加速度大小;R为地球半径;Rc为位置方向余弦矩阵;为捷联惯导系统陀螺仪输出;fb为加速度计输出;
(2)根据运动学方程,针对舰船使用的横坐标系捷联惯导系统,建立横坐标系捷联惯导系统的状态方程和量测方程:
Z=HX+η
其中A为状态转移矩阵;状态变量为 为系统噪声;B为系统噪声驱动阵;H为观测阵;η为量测噪声;
(3)建立多普勒计程仪速度信息有效性判据,判断外速度信息有效性是否发生改变;其中,多普勒计程仪为Doppler velocity log,DVL;
(4)若步骤(3)判断外速度信息从有效变为无效,执行步骤(5);
若步骤(3)判断外速度信息从无效变为有效,执行步骤(6);
若步骤(3)判断外速度信息有效性状态不变,则系统继续执行当前状态;
(5)系统从阻尼切换至无阻尼状态,直接执行纯惯导下的姿态、速度和位置的解算;
(6)系统从无阻尼切换至阻尼状态,通过运行卡尔曼滤波器估计出水平姿态误差角,待滤波器稳定后,将估计的水平姿态误差角进行反馈校正,从而实现外水平阻尼。
对于一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,所述的步骤(1)中
根据船舶的运动特性,忽略天向通道的影响,设h=0,vz=0;通过获得捷联惯导系统陀螺仪输出和加速度计输出fb,完成横坐标系下捷联惯导系统的姿态、速度、位置的解算。
对于一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,步骤(2)主要通过以下步骤实现:
(2.1)对捷联惯导系统中陀螺仪和加速度计的标度因数误差和安装误差进行补偿,针对舰船使用的横坐标系捷联惯导系统建立系统的误差方程:
其中:
为姿态误差角;由于舰船不考虑天向通道,速度误差表示为 和分别为横纬度和横经度误差;陀螺漂移误差包括随机常值漂移εb和零均值的高斯白噪声εw两部分;加速度计误差δfb包括随机常值零偏和零均值的高斯白噪声
(2.2)获取DVL提供的有效外速度信息,根据舰船的运动特性,忽略天向通道ozt,建立横坐标系下捷联惯导系统的状态方程和量测方程:
Z=HX+η
其中,A、B、H可以分别表示为:
其中F7×7通过上面给出的捷联惯导系统误差方程获得,上述子矩阵中(·)m,n表示对矩阵(·)取前m行n列。
对于一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,所述的步骤(3)通过以下步骤实现:
(3.1)将DVL提供的外速度作为观测量,建立量测模型;
(3.2)判断DVL提供的外速度的信息有效性;收集横向捷联惯导解算的速度和计程仪提供的速度:
vDVL=vtrue+dvDVL
其中vtrue为舰船的真实速度,和dvDVL分别为横坐标系捷联惯导解算的速度和外参考速度vDVL的速度误差;
(3.3)取t1时刻和邻近t2时刻的滤波器观测值:
其中,和分别为t1和t2时刻滤波器观测值;
对t1和t2时刻的观测值做差:
其中,由两个时刻横坐标系捷联惯导速度误差和两个时刻的计程仪速度误差组成;
(3.4)根据的变化趋势判断DVL提供的外速度的有效性;
当发生大的改变时,判定DVL的速度误差发生较大变化,即DVL提供的外速度无效,系统进入无阻尼状态;
若由剧烈变化的向量向小幅值向量转变,则判定DVL提供的外速度有效,则系统由无阻尼状态进入阻尼状态。
对于一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,所述的步骤(6)通过以下步骤实现:
(6.1)对步骤(2)中建立的系统方程和量测方程进行离散化,通过离散卡尔曼滤波对系统误差进行估计;
(6.2)不断校正水平误差角,消除速度、位置、姿态中的舒勒振荡,实现水平阻尼。
本发明的有益效果在于:
本发明公开的一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,由于水平姿态误差角可观测性好,能够用卡尔曼滤波估计出来,只要不断地校正水平误差角就可以从根源上消除速度、位置、姿态中的舒勒振荡,实现横坐标系捷联惯导系统外水平阻尼;
此外,当横坐标系捷联惯导系统从无阻尼切换到阻尼状态时,基于阻尼网络的传统阻尼算法产生较大的超调误差与DVL提供的外速度误差紧密相关。因此,当采用低精度的DVL时,该算法很难从原理上减小超调误差。与基于阻尼网络的阻尼算法相比,当采用低精度的DVL时,由于水平姿态误差角的可观测性较好,通过建立的卡尔曼滤波误差模型仍然可以估计出水平姿态误差角。待卡尔曼滤波稳定后完成反馈校正,即可一定程度上减小阻尼切换过程中的超调误差。
附图说明
图1为本发明中横坐标系的定义图;
图2为本发明中横坐标系捷联惯导系统卡尔曼滤波外水平阻尼原理框图;
图3为本发明中横东向外部参考速度误差;
图4为本发明中横坐标系下姿态误差曲线;
图5为本发明中横坐标系下速度误差曲线;
图6为本发明中横坐标系下位置误差曲线;
图7为本发明中仿真初始参数指标表。
具体实施方式
下面结合附图对本发明做进一步描述。
结合图1至图7,本发明公开的一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,在本发明中,首先需要对多普勒计程仪提供的外速度信息进行有效性判断;然后根据外度度信息的可靠性决定能否从无阻尼切换至阻尼状态,直到外速度信有效时,横坐标系捷联惯导系统切换至卡尔曼滤波阻尼状态;最后待滤波器稳定后,将估计出的姿态误差角进行不断地反馈校正,实现卡尔曼滤波外水平阻尼。
图2为横坐标系捷联惯导系统的卡尔曼滤波阻尼原理框图。通常情况下,计程仪提供的外速度误差都是存在阶段性的常值误差的,使得估计出来速度误差与外速度误差耦合在一起,难以准确估计出速度误差,但是水平姿态误差角的可观测性很好,仍然准确地估计出来,因此可以通过估计的水平姿态误差角来校正系统的平台误差角。由于存在水平姿态误差角,使得水平方向引入重力加速度分量,构成二阶负反馈系统,从而表现为舒勒振荡,因此只要不断地校正水平误差角就可以从根源上消除速度、位置、姿态中的舒勒振荡,实现水平阻尼。本发明的具体实施步骤如下:
步骤一:建立横坐标系捷联惯导力学编排,获得横坐标系下的运动学方程:
根据船舶的运动特性,可以忽略天向通道的影响,即可以设h=0,vz=0。通过获得捷联惯导系统陀螺仪输出和加速度计输出fb,就可以完成横坐标系下捷联惯导系统的姿态、速度、位置的解算。
步骤二:根据运动学方程,针对舰船使用的横坐标系捷联惯导系统,建立横坐标系捷联惯导系统的状态方程和量测方程:
Z=HX+η
其中A为状态转移矩阵;状态变量为 为系统噪声;B为系统噪声驱动阵;H为观测阵;η为量测噪声。A、B、H可以分别表示为:
其中F7×7可以通过上面给出的捷联惯导系统误差方程获得,上述子矩阵中(·)m,n表示对矩阵(·)取前m行n列。
步骤三:建立DVL外速度信息有效性判据,判断外速度信息有效性是否发生改变;
则是由两个时刻横坐标系捷联惯导速度误差和两个时刻的计程仪速度误差组成,由于惯导系统的信息具有连续性的特点,在短时间内不会发生大的变化。但是当发生大的改变,可认为是DVL的速度误差发生大的变化,即DVL提供的外速度无效,则系统应该进入无阻尼状态。若由剧烈变化的向量向小幅值向量转变,即DVL提供的外速度有效,则系统由无阻尼状态向阻尼状态切换。
步骤四:当步骤三判断的外速度信息从有效变为无效时,执行步骤五,当步骤三判断外速度信息从无效变为有效时,执行步骤六,当步骤三中判断的外速度信息有效性状态不变时,系统继续执行当前状态;
步骤五:系统从阻尼切换至无阻尼状态,则根据步骤一中的运动方程直接执行纯惯导下的姿态、速度和位置的解算;
步骤六:系统从无阻尼切换至阻尼状态,通过运行卡尔曼滤波器估计出水平姿态误差角,待滤波器稳定后,将估计的水平姿态误差角进行反馈校正,从而实现外水平阻尼。以横东向水平回路为例,如图2所示。将卡尔曼滤波估计出来的东向姿态误差角反馈到系统,不断校正捷联惯导系统的水平姿态误差角,即可实现基于卡尔曼滤波的外水平阻尼,从而抑制舒勒振荡和被傅科周期振荡调制的舒勒振荡。
本发明还包括这样一些结构特征:
1.步骤一具体为:
横坐标系包括横地球坐标系和横地理坐标系它的定义与传统导航坐标系类似。以180°经线与赤道平面的交点为横北极点,以0°经线与赤道平面的交点为横南极点。如图1所示,横地球坐标系可以等效于将原地球坐标系oexeyeze经过两次旋转得到,其转动过程为:
将转动后得到的平面定义为横本初子午面,平面定义为横赤道平面。载体p点的位置可以由横经度横纬度和相对于水平面的高度h来描述,p点的横经度为经过p点的横子午面与横本初子午面之间的夹角,横纬度为p点和地心连线与横赤道平面之间的夹角。如图1所示,横地理坐标系以载体p的质心o为原点,为当地水平面,沿当地横经线方向指向横北极点,指向天向,轴满足右手定则。
本发明以横地理坐标系为导航坐标系,它的力学编排和误差方程与指北方位惯导系统相类似,主要区别是地球自转角度ωie在各自的导航系下的投影分量不同,因此可以获得载体在横坐标系下捷联惯导系统的运动学方程:
其中:
本发明中表示载体坐标系b到横导航坐标系的方向余弦矩阵;为载体的对地速度在横导航系下的投影;为载体在横坐标系下的位置矢量;为地球自转角速度在横导航系下的投影;为横导航系相对于横地球坐标系的旋转角速率在横导航系下的投影;g为当地重力加速度大小;R为地球半径;Rc为位置方向余弦矩阵。根据船舶的运动特性,可以忽略天向通道的影响,即可以设h=0,vz=0。通过获得捷联惯导系统陀螺仪输出和加速度计输出fb,就可以完成横坐标系下捷联惯导系统的姿态、速度、位置的解算。
2.步骤二具体为:
假设捷联惯导系统中陀螺仪和加速度计的标度因数误差和安装误差已经补偿,针对舰船使用的横坐标系捷联惯导系统可以建立系统的误差方程为:
其中:
为姿态误差角;由于舰船不考虑天向通道,速度误差可以表示为 和分别表示横纬度和横经度误差;陀螺漂移误差由随机常值漂移εb和零均值的高斯白噪声εw两部分组成;加速度计误差δfb由随机常值零偏和零均值的高斯白噪声组成。
当DVL提供外速度信息有效时,本发明针对舰船的运动特性,忽略天向通道建立横坐标系下捷联惯导系统的状态方程和量测方程为:
Z=HX+η
其中A为状态转移矩阵;状态变量为 为系统噪声;B为系统噪声驱动阵;H为观测阵;η为量测噪声。A、B、H可以分别表示为:
其中F7×7可以通过上面给出的捷联惯导系统误差方程获得,上述子矩阵中(·)m,n表示对矩阵(·)取前m行n列。
3.步骤三具体为:
本发明将DVL提供的外速度作为观测量,建立了量测模型。当船舶做复杂的机动运动或当船舶航行到海湾或海沟等复杂的海洋环境当中时,DVL的测速误差通常很大,此时认为计程仪测得的速度不可用。对于长时间航行的舰船捷联惯导系统,通常工作在无阻尼和阻尼两种状态,并根据DVL提供的外速度的是否有效进行状态切换。因此有必要建立DVL外速度信息有效性判据,横向捷联惯导解算的速度和计程仪提供的速度可以表示为:
vDVL=vtrue+dvDVL
其中vtrue为舰船的真实速度,和dvDVL分别为横坐标系捷联惯导解算的速度和外参考速度vDVL的速度误差。取t1时刻和邻近t2时刻的滤波器观测值如下:
上式中和分别为t1和t2时刻滤波器观测值。对t1和t2时刻的观测值做差得到:
则是由两个时刻横坐标系捷联惯导速度误差和两个时刻的计程仪速度误差组成,由于惯导系统的信息具有连续性的特点,在短时间内不会发生大的变化。但是当发生大的改变,可认为是DVL的速度误差发生大的变化,即DVL提供的外速度无效,则系统应该进入无阻尼状态。若由剧烈变化的向量向小幅值向量转变,即DVL提供的外速度有效,则系统由无阻尼状态向阻尼状态切换。
4.步骤六具体为:
将步骤二中建立的系统方程和量测方程进行离散化,可以通过离散卡尔曼滤波对系统误差进行估计。由于存在水平姿态误差角,使得水平方向引入重力加速度分量,构成二阶负反馈系统,从而表现为舒勒振荡,因此只要不断地校正水平误差角就可以从根源上消除速度、位置、姿态中的舒勒振荡,实现水平阻尼。以横东向水平回路为例,如图2所示。将卡尔曼滤波估计出来的东向姿态误差角反馈到系统,不断校正捷联惯导系统的水平姿态误差角,即可实现基于卡尔曼滤波的外水平阻尼,从而抑制舒勒振荡和被傅科周期振荡调制的舒勒振荡。
本实施例的实现条件与所得结果如下:
(1)实验仿真条件
为了验证所设计的横坐标系下捷联惯导系统的卡尔曼滤波阻尼算法性能,分别进行了10小时的无阻尼和有阻尼仿真测试。仿真的初始参数设置如图7所示。横东向参考速度误差如图3所示,当t<2.5h时,速度随机误差较大,DVL提供的外速度信息视为无效,当t≥2.5h时,DVL提供的外速度误差相对较小,可以作为外速度参考。当DVL测得的外速度信息可用时,系统由无阻尼状态切换到阻尼状态,仿真中在t=2.5h时,则可以进行阻尼切换。将横坐标系下捷联惯导系统在无阻尼、基于阻尼网络的传统阻尼和卡尔曼滤波阻尼下的导航误差进行对比,仿真结果如图4-6所示。
(2)实验仿真结果
当t=2.5h时,DVL提供的外速度信息有效,基于阻尼网络的阻尼算法直接由无阻尼状态切换至阻尼状态。卡尔曼滤波阻尼算法进入切换准备状态,待滤波器稳定后(约10min左右),将估计的水平姿态误差角不断地进行反馈校正,此时基于卡尔曼滤波的外水平阻尼算法正式进入阻尼状态。从图4看出,两种阻尼方法均能消除水平姿态角误差的舒勒周期振荡,只剩下由加速度计零偏引起的常值误差,而方位姿态角误差只存在地球周期振荡,因此两种阻尼算法的阻尼效果一样。从图5和图6看出,两种阻尼算法均能有效消除速度误差和位置误差中的舒勒振荡以及被傅科周期调制的舒勒振荡,但是地球周期振荡仍然存在。如图4-6所示,传统阻尼虽然能够对舒勒振荡进行阻尼,但在阻尼切换后存在较大的超调误差。采用卡尔曼滤波阻尼算法能够较平滑地从无阻尼切换到阻尼状态,实现水平阻尼。从仿真结果可以得出如下结论:(1)在高纬度地区的横坐标系捷联惯导系统中,基于阻尼网络的阻尼算法和本文设计的卡尔曼滤波阻尼算法均能够有效抑制舒勒周期振荡;(2)与基于阻尼网络的阻尼算法相比,所设计的卡尔曼滤波阻尼算法能够有效抑制从无阻尼切换到阻尼状态产生的超调误差。
综上,本发明公开了一种适用极区的横坐标系捷联惯导系统卡尔曼滤波阻尼算法。在该算法中,通过建立横坐标系下卡尔曼滤波误差模型,对水平姿态误差角进行估计,并利用反馈校正不断校正水平姿态误差角,从而抑制姿态、速度和位置误差中的舒勒振荡,实现卡尔曼滤波外水平阻尼。首先根据外速度有效性判据判断外速度信息的可靠性;然后根据外速度信息的判断结果确定当前捷联惯导系统的运行状态,直到外速度信息有效时,系统从无阻尼切换至阻尼状态;最后待卡尔曼滤波稳定后,将估计的水平姿态误差角不断地进行反馈校正,从而抑制姿态、位置和速度误差中的舒勒振荡,实现极区卡尔曼滤波外水平阻尼。与基于阻尼网络的传统阻尼方法相比,本发明提供的阻尼算法不仅能实现极区外水平阻尼,还能抑制阻尼切换过程产生的超调误差。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (5)
1.一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,其特征在于,通过以下步骤实现:
(1)建立横坐标系捷联惯导力学编排,获得横坐标系下的运动学方程:
其中,表示载体坐标系b到横导航坐标系的方向余弦矩阵;为载体的对地速度在横导航系下的投影;为载体在横坐标系下的位置矢量;为地球自转角速度在横导航系下的投影;为横导航系相对于横地球坐标系的旋转角速率在横导航系下的投影;g为当地重力加速度大小;R为地球半径;Rc为位置方向余弦矩阵;为捷联惯导系统陀螺仪输出;fb为加速度计输出;
(2)根据运动学方程,针对舰船使用的横坐标系捷联惯导系统,建立横坐标系捷联惯导系统的状态方程和量测方程:
Z=HX+η
其中A为状态转移矩阵;状态变量为 为系统噪声;B为系统噪声驱动阵;H为观测阵;η为量测噪声;
(3)建立多普勒计程仪速度信息有效性判据,判断外速度信息有效性是否发生改变;其中,多普勒计程仪为Doppler velocity log,DVL;
(4)若步骤(3)判断外速度信息从有效变为无效,执行步骤(5);
若步骤(3)判断外速度信息从无效变为有效,执行步骤(6);
若步骤(3)判断外速度信息有效性状态不变,则系统继续执行当前状态;
(5)系统从阻尼切换至无阻尼状态,直接执行纯惯导下的姿态、速度和位置的解算;
(6)系统从无阻尼切换至阻尼状态,通过运行卡尔曼滤波器估计出水平姿态误差角,待滤波器稳定后,将估计的水平姿态误差角进行反馈校正,从而实现外水平阻尼。
2.根据权利要求1所述的一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,其特征在于:所述的步骤(1)中
根据船舶的运动特性,忽略天向通道的影响,设h=0,vz=0;通过获得捷联惯导系统陀螺仪输出和加速度计输出fb,完成横坐标系下捷联惯导系统的姿态、速度、位置的解算。
3.根据权利要求1所述的一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,其特征在于,步骤(2)主要通过以下步骤实现:
(2.1)对捷联惯导系统中陀螺仪和加速度计的标度因数误差和安装误差进行补偿,针对舰船使用的横坐标系捷联惯导系统建立系统的误差方程:
其中:
为姿态误差角;由于舰船不考虑天向通道,速度误差表示为 和分别为横纬度和横经度误差;陀螺漂移误差包括随机常值漂移εb和零均值的高斯白噪声εw两部分;加速度计误差δfb包括随机常值零偏▽b和零均值的高斯白噪声▽w;
(2.2)获取DVL提供的有效外速度信息,根据舰船的运动特性,忽略天向通道ozt,建立横坐标系下捷联惯导系统的状态方程和量测方程:
Z=HX+η
其中,A、B、H可以分别表示为:
其中F7×7通过上面给出的捷联惯导系统误差方程获得,上述子矩阵中(·)m,n表示对矩阵(·)取前m行n列。
4.根据权利要求1所述的一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,其特征在于,所述的步骤(3)通过以下步骤实现:
(3.1)将DVL提供的外速度作为观测量,建立量测模型;
(3.2)判断DVL提供的外速度的信息有效性;收集横向捷联惯导解算的速度和计程仪提供的速度:
vDVL=vtrue+dvDVL
其中vtrue为舰船的真实速度,和dvDVL分别为横坐标系捷联惯导解算的速度和外参考速度vDVL的速度误差;
(3.3)取t1时刻和邻近t2时刻的滤波器观测值:
其中,和分别为t1和t2时刻滤波器观测值;
对t1和t2时刻的观测值做差:
其中,由两个时刻横坐标系捷联惯导速度误差和两个时刻的计程仪速度误差组成;
(3.4)根据的变化趋势判断DVL提供的外速度的有效性
当发生大的改变时,判定DVL的速度误差发生较大变化,即DVL提供的外速度无效,系统进入无阻尼状态;
若由剧烈变化的向量向小幅值向量转变,则判定DVL提供的外速度有效,则系统由无阻尼状态进入阻尼状态。
5.根据权利要求1所述的一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法,其特征在于,所述的步骤(6)通过以下步骤实现:
(6.1)对步骤(2)中建立的系统方程和量测方程进行离散化,通过离散卡尔曼滤波对系统误差进行估计;
(6.2)不断校正水平误差角,消除速度、位置、姿态中的舒勒振荡,实现水平阻尼。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810770789.7A CN109029454A (zh) | 2018-07-13 | 2018-07-13 | 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810770789.7A CN109029454A (zh) | 2018-07-13 | 2018-07-13 | 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109029454A true CN109029454A (zh) | 2018-12-18 |
Family
ID=64642173
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810770789.7A Pending CN109029454A (zh) | 2018-07-13 | 2018-07-13 | 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109029454A (zh) |
Cited By (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109945859A (zh) * | 2019-04-01 | 2019-06-28 | 东南大学 | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 |
CN110057383A (zh) * | 2019-05-05 | 2019-07-26 | 哈尔滨工程大学 | 一种auv推位导航系统杆臂误差标校方法 |
CN110926464A (zh) * | 2019-12-11 | 2020-03-27 | 中国人民解放军海军潜艇学院 | 一种基于双模式的惯性导航方法及系统 |
CN111238473A (zh) * | 2020-01-21 | 2020-06-05 | 西北工业大学 | 地心地固坐标系下惯导系统高度通道的二阶阻尼方法 |
CN112097763A (zh) * | 2020-08-28 | 2020-12-18 | 西北工业大学 | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 |
CN112129289A (zh) * | 2020-11-30 | 2020-12-25 | 中国人民解放军国防科技大学 | 一种基于输出校正的容错水平阻尼方法 |
CN113051757A (zh) * | 2021-03-23 | 2021-06-29 | 中国人民解放军海军工程大学 | 一种捷联惯导广义psi角误差模型及其构建方法 |
CN113939712A (zh) * | 2019-06-06 | 2022-01-14 | 赛峰电子与防务公司 | 基于运输设备取景器传送的信息重置运输设备惯性单元的方法和装置 |
CN115031727A (zh) * | 2022-03-31 | 2022-09-09 | 哈尔滨工程大学 | 一种基于状态变换的多普勒辅助捷联惯导系统初始对准方法 |
CN115235460A (zh) * | 2022-06-24 | 2022-10-25 | 中国人民解放军国防科技大学 | 基于法向量位置模型的船舶惯导容错阻尼方法及系统 |
CN116222618A (zh) * | 2023-03-11 | 2023-06-06 | 中国人民解放军国防科技大学 | 一种极地环境下的双惯导协同标定方法 |
CN116734887A (zh) * | 2023-03-11 | 2023-09-12 | 中国人民解放军国防科技大学 | 基于速度误差修正模型的极地双惯导协同标定方法 |
CN117214933A (zh) * | 2023-11-07 | 2023-12-12 | 中国船舶集团有限公司第七〇七研究所 | 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法 |
CN117470235A (zh) * | 2023-11-10 | 2024-01-30 | 中国人民解放军国防科技大学 | 一种优化的地球椭球模型下长航时跨极区阻尼切换方法 |
CN117516520A (zh) * | 2023-11-10 | 2024-02-06 | 中国人民解放军国防科技大学 | 一种优化的地球椭球模型下极区最优阻尼方法 |
CN117516519A (zh) * | 2023-11-10 | 2024-02-06 | 中国人民解放军国防科技大学 | 一种跨极区最优阻尼切换方法 |
CN118089716A (zh) * | 2024-04-24 | 2024-05-28 | 中国船舶集团有限公司第七〇七研究所 | 一种台体方位旋转惯导的方位陀螺误差估计及补偿方法 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103940429A (zh) * | 2014-03-21 | 2014-07-23 | 哈尔滨工程大学 | 一种惯性导航系统横坐标系下载体姿态的实时测量方法 |
-
2018
- 2018-07-13 CN CN201810770789.7A patent/CN109029454A/zh active Pending
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103940429A (zh) * | 2014-03-21 | 2014-07-23 | 哈尔滨工程大学 | 一种惯性导航系统横坐标系下载体姿态的实时测量方法 |
Non-Patent Citations (3)
Title |
---|
WEIQUAN HUANG等: "a damping grid strapdown inertial navigation system based on a kalman filter for ship in polar regions", 《SENSORS》 * |
刘飞等: "基于Kalman滤波技术的捷联惯导系统水平阻尼算法", 《中国惯性技术学报》 * |
张福斌等: "基于横坐标系的捷联惯性导航系统/多普勒速度仪极区组合导航算法", 《兵工学报》 * |
Cited By (28)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109945859A (zh) * | 2019-04-01 | 2019-06-28 | 东南大学 | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 |
CN110057383A (zh) * | 2019-05-05 | 2019-07-26 | 哈尔滨工程大学 | 一种auv推位导航系统杆臂误差标校方法 |
CN110057383B (zh) * | 2019-05-05 | 2023-01-03 | 哈尔滨工程大学 | 一种auv推位导航系统杆臂误差标校方法 |
CN113939712A (zh) * | 2019-06-06 | 2022-01-14 | 赛峰电子与防务公司 | 基于运输设备取景器传送的信息重置运输设备惯性单元的方法和装置 |
CN113939712B (zh) * | 2019-06-06 | 2023-11-28 | 赛峰电子与防务公司 | 基于运输设备取景器传送的信息重置运输设备惯性单元的方法和装置 |
CN110926464A (zh) * | 2019-12-11 | 2020-03-27 | 中国人民解放军海军潜艇学院 | 一种基于双模式的惯性导航方法及系统 |
CN111238473A (zh) * | 2020-01-21 | 2020-06-05 | 西北工业大学 | 地心地固坐标系下惯导系统高度通道的二阶阻尼方法 |
CN111238473B (zh) * | 2020-01-21 | 2022-11-22 | 西北工业大学 | 地心地固坐标系下惯导系统高度通道的二阶阻尼方法 |
CN112097763B (zh) * | 2020-08-28 | 2022-07-05 | 西北工业大学 | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 |
CN112097763A (zh) * | 2020-08-28 | 2020-12-18 | 西北工业大学 | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 |
CN112129289B (zh) * | 2020-11-30 | 2021-02-05 | 中国人民解放军国防科技大学 | 一种基于输出校正的容错水平阻尼方法 |
CN112129289A (zh) * | 2020-11-30 | 2020-12-25 | 中国人民解放军国防科技大学 | 一种基于输出校正的容错水平阻尼方法 |
CN113051757A (zh) * | 2021-03-23 | 2021-06-29 | 中国人民解放军海军工程大学 | 一种捷联惯导广义psi角误差模型及其构建方法 |
CN115031727A (zh) * | 2022-03-31 | 2022-09-09 | 哈尔滨工程大学 | 一种基于状态变换的多普勒辅助捷联惯导系统初始对准方法 |
CN115235460A (zh) * | 2022-06-24 | 2022-10-25 | 中国人民解放军国防科技大学 | 基于法向量位置模型的船舶惯导容错阻尼方法及系统 |
CN116222618A (zh) * | 2023-03-11 | 2023-06-06 | 中国人民解放军国防科技大学 | 一种极地环境下的双惯导协同标定方法 |
CN116734887A (zh) * | 2023-03-11 | 2023-09-12 | 中国人民解放军国防科技大学 | 基于速度误差修正模型的极地双惯导协同标定方法 |
CN116734887B (zh) * | 2023-03-11 | 2024-01-30 | 中国人民解放军国防科技大学 | 基于速度误差修正模型的极地双惯导协同标定方法 |
CN116222618B (zh) * | 2023-03-11 | 2024-02-13 | 中国人民解放军国防科技大学 | 一种极地环境下的双惯导协同标定方法 |
CN117214933A (zh) * | 2023-11-07 | 2023-12-12 | 中国船舶集团有限公司第七〇七研究所 | 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法 |
CN117214933B (zh) * | 2023-11-07 | 2024-02-06 | 中国船舶集团有限公司第七〇七研究所 | 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法 |
CN117470235A (zh) * | 2023-11-10 | 2024-01-30 | 中国人民解放军国防科技大学 | 一种优化的地球椭球模型下长航时跨极区阻尼切换方法 |
CN117516520A (zh) * | 2023-11-10 | 2024-02-06 | 中国人民解放军国防科技大学 | 一种优化的地球椭球模型下极区最优阻尼方法 |
CN117516519A (zh) * | 2023-11-10 | 2024-02-06 | 中国人民解放军国防科技大学 | 一种跨极区最优阻尼切换方法 |
CN117516519B (zh) * | 2023-11-10 | 2024-04-09 | 中国人民解放军国防科技大学 | 一种跨极区最优阻尼切换方法 |
CN117470235B (zh) * | 2023-11-10 | 2024-04-26 | 中国人民解放军国防科技大学 | 一种优化的地球椭球模型下长航时跨极区阻尼切换方法 |
CN117516520B (zh) * | 2023-11-10 | 2024-05-14 | 中国人民解放军国防科技大学 | 一种优化的地球椭球模型下极区最优阻尼方法 |
CN118089716A (zh) * | 2024-04-24 | 2024-05-28 | 中国船舶集团有限公司第七〇七研究所 | 一种台体方位旋转惯导的方位陀螺误差估计及补偿方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109029454A (zh) | 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法 | |
CN106123921B (zh) | 动态干扰条件下捷联惯导系统的纬度未知自对准方法 | |
CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
US11754400B2 (en) | Motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering | |
WO2020062791A1 (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN105606094B (zh) | 一种基于mems/gps组合系统的信息条件匹配滤波估计方法 | |
CN110487301A (zh) | 一种雷达辅助机载捷联惯性导航系统初始对准方法 | |
US7142983B2 (en) | Method for the processing of non-continuous atom interferometer intertial instrument measurements and continuous wide bandwidth instrument measurements with a gravity database | |
CN110221332A (zh) | 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 | |
CN106052686B (zh) | 基于dsptms320f28335的全自主捷联惯性导航系统 | |
CN104748722B (zh) | 利用卫星定位信息实时校准气压测高结果的高程定位方法 | |
CN109596018A (zh) | 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法 | |
CN103245360A (zh) | 晃动基座下的舰载机旋转式捷联惯导系统自对准方法 | |
CN110146075A (zh) | 一种增益补偿自适应滤波的sins/dvl组合定位方法 | |
CN104697520B (zh) | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 | |
CN104049269B (zh) | 一种基于激光测距和mems/gps组合导航系统的目标导航测绘方法 | |
CN108931791A (zh) | 卫惯紧组合钟差修正系统和方法 | |
CN101183004A (zh) | 一种在线实时消除光纤陀螺捷联惯导系统振荡误差的方法 | |
CN109916395A (zh) | 一种姿态自主冗余组合导航算法 | |
CN103674064B (zh) | 捷联惯性导航系统的初始标定方法 | |
CN107702712A (zh) | 基于惯性测量双层wlan指纹库的室内行人组合定位方法 | |
CN109931955A (zh) | 基于状态相关李群滤波的捷联惯性导航系统初始对准方法 | |
CN109828296A (zh) | 一种ins/usbl非线性紧组合综合校正方法 | |
Sokolovic et al. | Integration of INS, GPS, magnetometer and barometer for improving accuracy navigation of the vehicle | |
CN106123917B (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 | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20181218 |
|
RJ01 | Rejection of invention patent application after publication |