CN112254718A - 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 - Google Patents
一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 Download PDFInfo
- Publication number
- CN112254718A CN112254718A CN202010772026.3A CN202010772026A CN112254718A CN 112254718 A CN112254718 A CN 112254718A CN 202010772026 A CN202010772026 A CN 202010772026A CN 112254718 A CN112254718 A CN 112254718A
- Authority
- CN
- China
- Prior art keywords
- underwater
- filtering
- constraint
- error
- motion
- 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
- 238000001914 filtration Methods 0.000 title claims abstract description 90
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 38
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000005259 measurement Methods 0.000 claims abstract description 49
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 26
- 230000001133 acceleration Effects 0.000 claims abstract description 17
- 239000011159 matrix material Substances 0.000 claims description 41
- 230000007704 transition Effects 0.000 claims description 17
- 241000251468 Actinopterygii Species 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 4
- 230000005284 excitation Effects 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 239000013535 sea water Substances 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 2
- 238000012546 transfer Methods 0.000 claims description 2
- 230000009466 transformation Effects 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 6
- 238000013459 approach Methods 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Images
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
-
- 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
- G01C21/165—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 combined with non-inertial navigation instruments
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
本发明提供了一种运动约束辅助的基于改进Sage‑Husa自适应滤波的水下组合导航方法,包括:建立多普勒计程仪误差模型;构建基于卡尔曼滤波的水下组合导航***的状态方程;根据水下潜航器向心加速度和前向速度间的关系建立约束条件,构建完整运动约束模型;建立两个量测方程;建立滤波方程,在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算,当量测噪声发生变化时,采用改进的Sage‑Husa自适应滤波算法进行时间更新、量测更新和滤波更新。本发明能够提高水下组合导航***的滤波精度,较好地抑制量测干扰时的滤波发散,具有较好的鲁棒性和可靠性。
Description
技术领域
本发明属于导航制导和控制领域,涉及水下导航方法,具体涉及一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法。
背景技术
随着水下潜器技术愈渐成熟,水下潜航器作为可靠探测工具受到了国内外广泛关注,如何提高水下导航精度也成为各领域学者关注的焦点。目前应用最为成熟也最为广泛的是惯性/多普勒组合导航技术。惯性器件独立自主,不依靠外界信息的特性使其在水下环境依然具有较高可靠性,但惯性器件的误差会随着时间发散,从而影响定位的精度。而多普勒计程仪精度较高,误差不随着时间发散,因此可以很好的修正捷联惯导的速度信息,提高定位的精度。
但水下环境较为复杂,水下潜航器通常在海底以锯齿波滑翔,在滑翔过程中,若遇到海底沟渠、聚集性鱼群以及强机动性转弯时,多普勒计程仪提供的量测信息则会发生错误,并且预先设置的量测噪声矩阵将无法适应受到扰动的模型,从而影响定位的精度,而当水下潜航器以锯齿波滑翔或进行强机动性转弯时,所产生的向心加速度将会对水下潜航器的速度产生约束。
发明内容
为解决上述问题,本发明引入基于向心加速度约束以及改进的Sage-Husa自适应算法来辅助惯性/多普勒组合导航,提出一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,在水下潜航器因为海底沟渠、鱼群以及强机动性转弯时导致多普勒量测信息出现错误时,引入向心加速度对水下潜航器的速度进行约束,并且对Sage-Husa自适应滤波算法进行改进,从而减小定位误差,提高定位精度。
为了达到上述目的,本发明提供如下技术方案:
一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,包括如下步骤:
步骤1,根据多普勒计程仪的工作原理,建立多普勒计程仪误差模型;
步骤2,在惯性导航误差方程的基础上,引入多普勒误差模型中速度误差、偏流角误差以及比例系数误差作为水下组合导航的状态量构建基于卡尔曼滤波的水下组合导航***的状态方程;
步骤3,建立载体坐标系,将水下潜航器的运动分解到垂直于Z轴平面和垂直于X轴平面,并根据水下潜航器向心加速度和前向速度间的关系建立约束条件,构建完整运动约束模型;
步骤4,根据惯性器件以及多普勒计程仪的导航信息建立第一量测方程,并根据步骤2中完整运动约束模型建立第二量测方程;
步骤5,对状态方程以及量测方程进行离散化处理,结合步骤2和步骤4建立滤波方程,在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算,当量测噪声发生变化时,采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新。
进一步的,所述步骤1中多普勒计程仪的工作原理为:
水下潜航器底端安装前后左右各两对换能器,向四个方向发射波束,通过测量发射波束的频率以及反射后波束频率得到多普勒频移,从而得到载体坐标系三个方向上水下潜航器的速度如下:
fd13=fd1-fd3,fd24=fd2-fd4
所述多普勒计程仪误差模型为:
其中,vd为水下潜航器的前向速度,c为光速,vx、vy、vz为潜航器载体坐标系三个方向的速度;f0为发射波频率,fd1、fd2、fd3、fd4为安装在潜航器船底前后左右四个方向的发射器发射波束的多普勒频移;α为发射波束倾角;δvdU、δvdE、δvdN为东北天坐标系下潜航器三个方向的速度误差;δvd为多普勒测速误差;β为潜航器俯仰角;δC为比例因子误差;Kd、γ以及δΔ分别为潜航器考虑偏流角的航迹向、方位失准角以及偏流角误差。
进一步的,所述步骤2中基于卡尔曼滤波的水下组合导航***的状态方程为:
Xk=Ak,k-1Xk-1+Γk,k-1Wk-1
其中,Ak,k-1表示***从k-1时刻到k时刻的状态转移矩阵;Γk,k-1表示***的噪声驱动矩阵,Wk-1为***噪声激励序列;
相应的状态向量为:
其中,δvd、δΔ、δC由如下公式表示:
式中δL、δλ、δh表示载体经度、纬度以及高度误差;δvE、δvN、δvU为载体在东、北、天三个方向上的速度误差;α、β、γ为载体的姿态角误差;εx、εy、εy为陀螺仪零偏; 表示加计零偏;βd -1、βΔ -1为速度偏移误差的相关时间和偏流角误差的相关时间;ωd、ωΔ均为激励白噪声。
进一步的,所述步骤3具体包括如下过程:
假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0,可得约束条件:
潜航器的任意运动都分解到垂直于z轴平面与垂直于x轴平面,根据运动学公式,可得:
根据惯性器件可得:
由此可得完整的运动约束模型为:
从而得到完整运动约束的误差模型为:
其中,为潜航器在载体坐标系下的速度;arx、arz为潜航器载体坐标系中x和z方向的向心加速度值;为惯性器件x轴和z轴敏感角速度;fx b、fz b为加计x、z轴比力值;分别为地球自转角速度和由载体运动引起的角速度。
进一步的,所述步骤4中第一量测方程为:
第二量测方程为:
其中量测量Z1为惯导解算出的速度与多普勒计程仪测速之差,量测量Z2为潜航器两个方向的速度约束以及载体运动时的加速度约束。
进一步的,所述步骤5中状态空间模型包括:
Xk=Ak,k-1Xk-1+Γk,k-1Wk-1
Zk=HkXk+Vk
其中,Zk为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵,Vk为量测噪声序列;
***的状态转移矩阵A为:
***的量测矩阵为:
H1=[03×3 I3×3 S1 03×6 S2]
其中:
H2=[03×4 M1 M2 M3 M4 03×4]
假设:
则:
其中:
进一步的,所述步骤5中改进的Sage-Husa自适应滤波算法过程如下:
Xk=Xk,k-1+Kk(Zk-HkXk,k-1)
Pk=(I-KkHk)Pk,k-1
其中,Xk表示载体k时刻的状态变量,A表示***从k时刻到k+1时刻的状态转移矩阵;Zk为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵;Kk为k时刻的卡尔曼滤波增益;Q为***噪声协方差矩阵;R为观测协方差矩阵;P为误差协方差矩阵。
进一步的,所述βk通过如下过程推导:
在卡尔曼滤波中,量测预测误差公式为:
两边同时求方差可得:
利用指数减小记忆加权平均方法可得:
判断滤波是否发散,滤波正常,则按照一般滤波进行,若检测到滤波发散,则实时计算出最优βk,阻止滤波的发散;根据下式判断滤波是否发散:
当上式成立时,则代表滤波发散,其中γ为储备系数,当γ=1时是最严格的收敛判据,采用最严格的收敛判据可得:
进一步的,所述步骤5中,当量测噪声发生变化的情况包括以下情形中的至少一种:水下滑翔器遇到障碍物或强机动性转弯。
进一步的,所述障碍物包括水下沟渠、鱼群。
与现有技术相比,本发明具有如下优点和有益效果:
本发明方法引入水下潜航器拐弯或进行强机动动作时产生的向心加速度误差为量测量,相比于传统的运动约束更为完整,可以有效约束载体的前向速度,避免载体因为强机动运动导致的量测信息出现较大误差,有效提高了***的导航精度。本发明提出的改进的Sage-Husa算法,在传统的Sage-Husa自适应滤波基础上,增加了对量测信息的故障判断以及比例因子的最优估计,不仅可以减小量测信息正确时的滤波计算量,还可以在量测信息出现误差时减少***滤波的发散,具有较好的鲁棒性和可靠性,从而提高***的导航精度。本发明方法可以在多普勒量测信息发生错误时有效减小误差,可用于提高水下组合导航精度、定位精度以及导航***容错性,实现水下潜航器更精确的作业。
附图说明
图1为多普勒计程仪的工作原理图。
图2为多普勒计程仪误差模型示意图。
图3为水下潜航器运动分解示意图。
图4为运动约束辅助的基于改进Sage-Husa自适应滤波算法流程图。
图5为标准卡尔曼滤波算法下导航***的位置及速度误差图。
图6为运动约束辅助的基于改进Sage-Husa自适应滤波算法下导航***的位置及速度误差图。
图7为量测噪声受到干扰时,运动约束辅助的Sage-Husa自适应算法下导航***的位置及速度误差图。
图8为量测噪声受到干扰时,运动约束辅助的改进Sage-Husa自适应算法下导航***的位置及速度误差图。
具体实施方式
以下将结合具体实施例对本发明提供的技术方案进行详细说明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
本发明提出一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,实现原理及方法流程如图1-图4所示,具体步骤如下:
步骤1,根据多普勒计程仪的工作原理,建立多普勒计程仪误差模型:
多普勒计程仪的原理较为简单,如图1所示,水下潜航器底端安装前后左右各两对换能器,向四个方向发射波束,通过测量发射波束的频率以及反射后波束频率得到多普勒频移,从而得到载体坐标系三个方向上水下潜航器的速度如下:
fd13=fd1-fd3,fd24=fd2-fd4
相应的,得到多普勒计程仪的误差模型为:
其中,vd为水下潜航器的前向速度,c为光速,为潜航器在载体坐标系下的速度;f0为发射波频率,fd1、fd2、fd3、fd4为安装在潜航器船底前后左右四个方向的发射器发射波束的多普勒频移;α为发射波束倾角;δvdU、δvdE、δvdN为东北天坐标系下潜航器三个方向的速度误差;δvd为多普勒测速误差;β为潜航器俯仰角;δC为比例因子误差;Kd、γ以及δΔ分别为潜航器考虑偏流角的航迹向、方位失准角以及偏流角误差。
步骤2,在惯性导航误差方程的基础上,引入多普勒误差模型中速度误差δvd、偏流角误差δΔ以及比例系数误差δC作为水下组合导航的状态量,如图2所示,构建基于卡尔曼滤波的水下组合导航***的状态方程:
其中,运动约束辅助的基于改进Sage-Husa自适应滤波算法的水下组合导航***的状态方程为:
Xk=Ak,k-1Xk-1+Γk,k-1Wk-1
其中,Ak,k-1表示***从k-1时刻到k时刻的状态转移矩阵;Γk,k-1表示***的噪声驱动矩阵,Wk-1为***噪声激励序列。相应的状态向量为:
其中,δvd、δΔ、δC可由如下公式表示:
式中δL、δλ、δh表示载体经度、纬度以及高度误差;δvE、δvN、δvU为载体在东、北、天三个方向上的速度误差;α、β、γ为载体的姿态角误差;εx、εy、εy为陀螺仪零偏; 表示加计零偏;βd -1、βΔ -1为速度偏移误差的相关时间和偏流角误差的相关时间;ωd、ωΔ均为激励白噪声。
步骤3,水下潜航器的任意运动皆可以分解到相互垂直的两个平面上,建立载体坐标系,将水下潜航器的运动分解到垂直于Z轴平面和垂直于X轴平面,如图3所示,并根据水下潜航器向心加速度和前向速度间的关系建立约束条件,构建完整运动约束模型:
由于海底一定深度的水流速较为稳定,因此假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0,可得约束条件:
该约束条件只能约束潜航器两个方向的速度,而潜航器在拐弯或进行强机动运动时会产生向心加速度,根据运动学公式,可得:
根据惯性器件可得:
由此可得完整的运动约束模型为:
从而得到完整运动约束的误差模型为:
其中,为从导航坐标系到载体坐标系的转移矩阵,表示导航坐标系下的姿态角。vn为导航坐标系下潜航器的速度,gn为导航坐标系下的重力加速度,为载体坐标系下陀螺仪x轴和z轴的零偏,arx、arz为潜航器载体坐标系中x和z方向的向心加速度值; 为惯性器件x轴和z轴敏感角速度;fx b、fz b为加速度计x、z轴比力值;分别为地球自转角速度和由载体运动引起的角速度。
步骤4,根据惯性器件以及多普勒计程仪的导航信息建立量测方程(1),并根据完整运动约束模型建立量测方程(2):
式中,vE,vN、vU为惯导解算出的潜航器的东北天方向(本发明中选用东北天坐标系为导航坐标系)的速度。vdE、vdN、vdU为多普勒计程仪所测得经过坐标转换后东北天三个方向的速度。量测量Z1为惯导解算出的速度与多普勒计程仪测速之差,量测量Z2为潜航器两个方向的速度约束以及载体运动时的加速度约束,在潜航器正常工作时,量测量Z2的值应为0或为白噪声。
步骤5,对状态方程以及量测方程进行离散化处理,结合步骤2和步骤4建立滤波方程,在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算,当水下滑翔器遇到水下沟渠、鱼群或强机动性转弯等情况时,量测噪声发生变化时,采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新,具体流程如图4所示:
结合步骤1-步骤4,***的状态空间模型包括状态方程和量测方程,具体如下:
Xk=Ak,k-1Xk-1+Γk,k-1Wk-1
Zk=HkXk+Vk
其中,Zk为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵,Vk为量测噪声序列;
***的状态转移矩阵A为:
***的量测矩阵为:
H1=[03×3 I3×3 S1 03×6 S2]
其中:
H2=[03×4 M1 M2 M3 M4 03×4]
假设:
则:
其中:
在水下组合导航***中,通常***状态噪声是较为稳定的,因此本发明只对量测噪声进行自适应。在卡尔曼滤波中,量测预测误差公式为:
两边同时求方差可得:
利用指数减小记忆加权平均方法可得:
传统Sage-Husa滤波中认为:
其中b为渐消因子,但由于随着滤波次数k的增大,bk会趋近0,自适应滤波的权重会趋近于1-b,并将保持不变;同时,初始值对的分配权值逐渐衰减,并逐渐接近于常值0。上述原因使得噪声估计器的自适应程度降低,滤波的精度便会随之下降。
根据预报残差法,可以人为判断滤波是否发散,滤波正常,则按照一般滤波进行,若检测到滤波发散,则实时计算出最优βk,阻止滤波的发散。
滤波发散判据:
当上式成立时,则代表滤波发散,其中γ为储备系数,当γ=1时是最严格的收敛判据,采用最严格的收敛判据可得:
解得:
综上:
由此可得步骤5中基于改进Sage-Husa自适应滤波算法过程如下:
Xk=Xk,k-1+Kk(Zk-HkXk,k-1)
Pk=(I-KkHk)Pk,k-1
其中,Xk表示载体k时刻的状态变量,A表示***从k时刻到k+1时刻的状态转移矩阵;Zk为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵;Kk为k时刻的卡尔曼滤波增益;Q为***噪声协方差矩阵;R为观测协方差矩阵;P为误差协方差矩阵。
具体实施例
为了验证提出算法的正确性,这里提供了一个基于Matlab平台的仿真测试,仿真参数设置如下:
1.惯性元件指标及导航初始参数设置:
陀螺零偏稳定性:eb=0.2°/h
加计零偏稳定性:db=100ug
2.误差分析
结合上述参数,分别采用标准卡尔曼滤波算法和本发明提供的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法进行对比。图5给出了标准卡尔曼滤波算法下组合***的位置误差及速度误差,可以看出,标准卡尔曼算法在实验解算初期缺乏对***统计噪声特性的了解,滤波误差较大。附加运动约束的改进Sage-Husa自适应滤波算法对载体的三个方向速度进行约束,对量测噪声进行自适应,可以很好的抑制组合***误差的发散,也大大提高了***的导航精度,如图6所示。
为了验证滤波发散情况下改进自适应滤波的性能,在量测量600秒到610秒处加了十倍的量测噪声。图7和图8给出了Sage-Husa自适应滤波算法改进前后***的导航误差情况,经过改进的Sage-Husa自适应滤波算法可以很好的抑制滤波发散,提高导航***的精度及鲁棒性。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段,还包括由以上技术特征任意组合所组成的技术方案。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也视为本发明的保护范围。
Claims (10)
1.一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,包括如下步骤:
步骤1,根据多普勒计程仪的工作原理,建立多普勒计程仪误差模型;
步骤2,在惯性导航误差方程的基础上,引入多普勒误差模型中速度误差、偏流角误差以及比例系数误差作为水下组合导航的状态量构建基于卡尔曼滤波的水下组合导航***的状态方程;
步骤3,建立载体坐标系,将水下潜航器的运动分解到垂直于Z轴平面和垂直于X轴平面,并根据水下潜航器向心加速度和前向速度间的关系建立约束条件,构建完整运动约束模型;
步骤4,根据惯性器件以及多普勒计程仪的导航信息建立第一量测方程,并根据步骤2中完整运动约束模型建立第二量测方程;
步骤5,对状态方程以及量测方程进行离散化处理,结合步骤2和步骤4建立滤波方程,在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算,当量测噪声发生变化时,采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新。
2.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于:所述步骤1中多普勒计程仪的工作原理为:
水下潜航器底端安装前后左右各两对换能器,向四个方向发射波束,通过测量发射波束的频率以及反射后波束频率得到多普勒频移,从而得到载体坐标系三个方向上水下潜航器的速度如下:
fd13=fd1-fd3,fd24=fd2-fd4
所述多普勒计程仪误差模型为:
3.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述步骤2中基于卡尔曼滤波的水下组合导航***的状态方程为:
Xk=Ak,k-1Xk-1+Γk,k-1Wk-1
其中,Ak,k-1表示***从k-1时刻到k时刻的状态转移矩阵;Γk,k-1表示***的噪声驱动矩阵,Wk-1为***噪声激励序列;
相应的状态向量为:
其中,δvd、δΔ、δC由如下公式表示:
4.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述步骤3具体包括如下过程:
假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0,可得约束条件:
潜航器的任意运动都分解到垂直于z轴平面与垂直于x轴平面,根据运动学公式,可得:
根据惯性器件可得:
由此可得完整的运动约束模型为:
从而得到完整运动约束的误差模型为:
6.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述步骤5中状态空间模型包括:
Xk=Ak,k-1Xk-1+Γk,k-1Wk-1
Zk=HkXk+Vk
其中,Zk为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵,Vk为量测噪声序列;
***的状态转移矩阵A为:
***的量测矩阵为:
H1=[03×3 I3×3 S1 03×6 S2]
其中:
H2=[03×4 M1 M2 M3 M4 03×4]
假设:
则:
其中:
9.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述步骤5中,当量测噪声发生变化的情况包括以下情形中的至少一种:水下滑翔器遇到障碍物或强机动性转弯。
10.根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述障碍物包括水下沟渠、鱼群。
Priority Applications (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010772026.3A CN112254718B (zh) | 2020-08-04 | 2020-08-04 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
US17/779,167 US11754400B2 (en) | 2020-08-04 | 2021-07-28 | Motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering |
PCT/CN2021/108862 WO2022028286A1 (zh) | 2020-08-04 | 2021-07-28 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010772026.3A CN112254718B (zh) | 2020-08-04 | 2020-08-04 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112254718A true CN112254718A (zh) | 2021-01-22 |
CN112254718B CN112254718B (zh) | 2024-04-09 |
Family
ID=74223881
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010772026.3A Active CN112254718B (zh) | 2020-08-04 | 2020-08-04 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
Country Status (3)
Country | Link |
---|---|
US (1) | US11754400B2 (zh) |
CN (1) | CN112254718B (zh) |
WO (1) | WO2022028286A1 (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113984054A (zh) * | 2021-09-17 | 2022-01-28 | 兰州交通大学 | 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备 |
WO2022028286A1 (zh) * | 2020-08-04 | 2022-02-10 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
CN114166203A (zh) * | 2021-11-16 | 2022-03-11 | 哈尔滨工程大学 | 一种基于改进的s-h自适应联邦滤波的智能水下机器人多源组合导航方法 |
CN114637036A (zh) * | 2022-05-18 | 2022-06-17 | 深圳华大北斗科技股份有限公司 | 非完整性约束自适应量测噪声方法 |
CN116222578A (zh) * | 2023-05-04 | 2023-06-06 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20220326016A1 (en) * | 2021-09-23 | 2022-10-13 | Harbin Engineering University | Robust Filtering Method for Integrated Navigation Based on Statistical Similarity Measure |
CN114567401B (zh) * | 2022-04-14 | 2023-02-14 | 中国人民解放***箭军工程大学 | 一种基于感知通信一体化的无人机蜂群状态联合估计方法 |
CN114623822B (zh) * | 2022-05-12 | 2022-08-02 | 东南大学 | 基于惯性约束的多波束水下地形组合匹配方法 |
CN114993302B (zh) * | 2022-05-27 | 2024-05-28 | 中国人民解放军海军工程大学 | 基于多柔性节点的水下智能定位***和方法 |
CN115079227A (zh) * | 2022-07-26 | 2022-09-20 | 武汉优米捷光电子制造有限责任公司 | 基于改进无迹卡尔曼滤波的自旋弹组合导航方法 |
CN115112154B (zh) * | 2022-08-30 | 2022-11-18 | 南开大学 | 一种水下自主导航定位***的标定方法 |
CN115574816B (zh) * | 2022-11-24 | 2023-03-14 | 东南大学 | 仿生视觉多源信息智能感知无人平台 |
CN115855049B (zh) * | 2023-02-07 | 2023-05-12 | 河海大学 | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 |
CN116067370B (zh) * | 2023-04-03 | 2023-06-27 | 广东智能无人***研究院(南沙) | 一种imu姿态解算方法及设备、存储介质 |
CN117367410B (zh) * | 2023-12-04 | 2024-03-19 | 中南大学 | 状态估计方法、无人水下航行器及计算机可读存储介质 |
CN117553787B (zh) * | 2024-01-09 | 2024-03-26 | 湖南大学无锡智能控制研究院 | 水下无人航行器的协同导航方法、装置及*** |
CN117606491B (zh) * | 2024-01-24 | 2024-04-26 | 中国海洋大学 | 一种自主式水下航行器的组合定位导航方法及装置 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103616026A (zh) * | 2013-12-17 | 2014-03-05 | 哈尔滨工程大学 | 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 |
NO20130697A1 (no) * | 2013-05-16 | 2014-11-17 | Høgskolen I Ålesund | Undervannsfarkost |
CN109141436A (zh) * | 2018-09-30 | 2019-01-04 | 东南大学 | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 |
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN109579850A (zh) * | 2019-01-16 | 2019-04-05 | 中国海洋大学 | 基于对水速度辅助惯导的深水智能导航方法 |
CN110146076A (zh) * | 2019-06-06 | 2019-08-20 | 哈尔滨工业大学(威海) | 一种无逆矩阵自适应滤波的sins/dvl组合定位方法 |
WO2019242336A1 (zh) * | 2018-06-22 | 2019-12-26 | 东南大学 | 一种水下滑翔器导航定位***及上浮精度校正方法 |
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20090287414A1 (en) | 2007-05-14 | 2009-11-19 | Zupt, Llc | System and process for the precise positioning of subsea units |
CN109032178B (zh) * | 2018-08-06 | 2021-08-24 | 江苏科技大学 | 全驱动auv回收控制***及自主回收方法 |
CN110940340A (zh) | 2019-12-23 | 2020-03-31 | 中科探海(苏州)海洋科技有限责任公司 | 基于小型uuv平台的多传感器信息融合方法 |
US11726200B2 (en) * | 2020-01-21 | 2023-08-15 | The Regents Of The University Of Colorado, A Body Corporate | Adaptive hybrid tracking algorithms for radio signal parameters estimations |
CN112254718B (zh) | 2020-08-04 | 2024-04-09 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
-
2020
- 2020-08-04 CN CN202010772026.3A patent/CN112254718B/zh active Active
-
2021
- 2021-07-28 WO PCT/CN2021/108862 patent/WO2022028286A1/zh active Application Filing
- 2021-07-28 US US17/779,167 patent/US11754400B2/en active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
NO20130697A1 (no) * | 2013-05-16 | 2014-11-17 | Høgskolen I Ålesund | Undervannsfarkost |
CN103616026A (zh) * | 2013-12-17 | 2014-03-05 | 哈尔滨工程大学 | 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 |
WO2019242336A1 (zh) * | 2018-06-22 | 2019-12-26 | 东南大学 | 一种水下滑翔器导航定位***及上浮精度校正方法 |
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN109141436A (zh) * | 2018-09-30 | 2019-01-04 | 东南大学 | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 |
CN109579850A (zh) * | 2019-01-16 | 2019-04-05 | 中国海洋大学 | 基于对水速度辅助惯导的深水智能导航方法 |
CN110146076A (zh) * | 2019-06-06 | 2019-08-20 | 哈尔滨工业大学(威海) | 一种无逆矩阵自适应滤波的sins/dvl组合定位方法 |
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
Non-Patent Citations (3)
Title |
---|
ZANG YUNGE等: "Design and Algorithm Research of Indoor Underwater Target Positioning System", 2016 TENTH INTERNATIONAL CONFERENCE ON SENSING TECHNOLOGY * |
严浙平;宋金雪;: "模型辅助和洋流估计下的UUV导航定位方法", 水下无人***学报, no. 05 * |
黄浩乾等: "水下滑翔器位姿估计的改进高斯混合粒子滤波器设计与验证", 中国惯性技术学报 * |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2022028286A1 (zh) * | 2020-08-04 | 2022-02-10 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
US11754400B2 (en) | 2020-08-04 | 2023-09-12 | Southeast University | Motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering |
CN113984054A (zh) * | 2021-09-17 | 2022-01-28 | 兰州交通大学 | 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备 |
CN114166203A (zh) * | 2021-11-16 | 2022-03-11 | 哈尔滨工程大学 | 一种基于改进的s-h自适应联邦滤波的智能水下机器人多源组合导航方法 |
CN114166203B (zh) * | 2021-11-16 | 2024-02-09 | 哈尔滨工程大学 | 一种基于改进的s-h自适应联邦滤波的智能水下机器人多源组合导航方法 |
CN114637036A (zh) * | 2022-05-18 | 2022-06-17 | 深圳华大北斗科技股份有限公司 | 非完整性约束自适应量测噪声方法 |
CN114637036B (zh) * | 2022-05-18 | 2022-08-19 | 深圳华大北斗科技股份有限公司 | 非完整性约束自适应量测噪声方法 |
CN116222578A (zh) * | 2023-05-04 | 2023-06-06 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
CN116222578B (zh) * | 2023-05-04 | 2023-08-29 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
Also Published As
Publication number | Publication date |
---|---|
WO2022028286A1 (zh) | 2022-02-10 |
US11754400B2 (en) | 2023-09-12 |
US20220404152A1 (en) | 2022-12-22 |
CN112254718B (zh) | 2024-04-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112254718B (zh) | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 | |
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN109324330B (zh) | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 | |
CN109724599B (zh) | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 | |
AU2009200190B2 (en) | Methods and systems for underwater navigation | |
CN106643709B (zh) | 一种海上运载体的组合导航方法及装置 | |
CN110794409B (zh) | 一种可估计未知有效声速的水下单信标定位方法 | |
CN110146075A (zh) | 一种增益补偿自适应滤波的sins/dvl组合定位方法 | |
CN107797125B (zh) | 一种减小深海探测型auv导航定位误差的方法 | |
CN110057365A (zh) | 一种大潜深auv下潜定位方法 | |
CN108931791A (zh) | 卫惯紧组合钟差修正***和方法 | |
CN110779519B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
CN110779518A (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
CN110646783B (zh) | 一种水下航行器的水下信标定位方法 | |
CN104061930B (zh) | 一种基于捷联惯性制导和多普勒计程仪的导航方法 | |
CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及*** | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
CN115096302A (zh) | 捷联惯性基导航***信息滤波鲁棒对准方法、***及终端 | |
CN114459476B (zh) | 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法 | |
CN116224407B (zh) | 一种gnss和ins组合导航定位方法及*** | |
CN110873813B (zh) | 一种水流速度估算方法、组合导航方法及装置 | |
CN116222578B (zh) | 基于自适应滤波和最优平滑的水下组合导航方法及*** | |
CN103697887B (zh) | 一种基于捷联惯导***和多普勒计程仪的优化导航方法 | |
CN113670303B (zh) | 一种基于rbf神经网络的sins/dvl组合导航流速补偿方法 | |
Gade et al. | An aided navigation post processing filter for detailed seabed mapping UUVs |
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 |