CN111504324A - 一种噪声自适应滤波的水下组合导航方法 - Google Patents
一种噪声自适应滤波的水下组合导航方法 Download PDFInfo
- Publication number
- CN111504324A CN111504324A CN202010345972.XA CN202010345972A CN111504324A CN 111504324 A CN111504324 A CN 111504324A CN 202010345972 A CN202010345972 A CN 202010345972A CN 111504324 A CN111504324 A CN 111504324A
- Authority
- CN
- China
- Prior art keywords
- auv
- sins
- navigation
- error
- speed
- 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 44
- 238000000034 method Methods 0.000 title claims abstract description 33
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 9
- 230000002159 abnormal effect Effects 0.000 claims abstract description 15
- 238000005259 measurement Methods 0.000 claims description 51
- 239000013535 sea water Substances 0.000 claims description 20
- 230000008859 change Effects 0.000 claims description 19
- 239000011159 matrix material Substances 0.000 claims description 13
- 230000008569 process Effects 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 7
- 238000012937 correction Methods 0.000 claims description 6
- 238000005562 fading Methods 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 4
- 238000012883 sequential measurement Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 5
- 230000035772 mutation Effects 0.000 description 4
- 230000008033 biological extinction Effects 0.000 description 3
- 230000004927 fusion Effects 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 238000010521 absorption reaction Methods 0.000 description 1
- 125000004432 carbon atom Chemical group C* 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012850 discrimination method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 239000000463 material Substances 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/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- 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
-
- 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/18—Stabilised platforms, e.g. by gyroscope
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
本发明涉及一种噪声自适应滤波的水下组合导航方法,在AUV组合导航***中,采用深度传感器作为航行深度;对滤波中有关SINS的误差方程进行简化,基于残差的χ2检测法,利用SINS/DVL组合滤波过程中信息的统计特性,判断DVL输出的测量数据是否出现测量噪声过大等异常情况,利用SINS/转速计组合进行导航。有益效果:把AUV无刷直流推进电机的转动速度作为转速计使用,对AUV硬件成本增加很小,将转速计的信息用于在DVL异常状态下的SINS/转速计组合导航,抑制了AUV位置等误差的发散速度,提高了AUV的导航定位精度。
Description
技术领域
本发明属于多传感器组合导航领域,涉及针对SINS/DVL/转速计/深度传感器信息高效融合问题的一种噪声自适应滤波的水下组合导航方法。
背景技术
自主水下航行器(Autonomous Underwater Vehicle,AUV)是人类进行海洋探索的主要工具,而精确地水下导航定位是决定水下航行器是否能正常、稳定工作的保障,特别是对于在水下进行长航时工作的水下航行器更为至关重要。
对于AUV导航***而言,由于水下的复杂环境以及隐蔽性、自主性、可靠性等方面的要求,单一形式的导航方式很难较好地满足需要,所以通常使用几种导航方式组合增加***精度及可靠性。目前,可在AUV中使用的导航***主要有捷联式惯性导航***(SINS)、多普勒计程仪(DVL)、转速计、长基线***(LBL)、短基线***(SBL)、全球卫星定位***(GPS)、磁航向仪(MCP)等等。
水下组合导航***,因其自身特殊性,在水下工作时无法接收到来自水面的信号,所以GPS将失去用处,而基于声学的长基线、短基线导航设备需提前布置,应用范围有限。所以一般的AUV都采用捷联惯性导航***为主导航***,DVL传感器和深度传感器为辅助,利用DVL输出速度和深度传感器精度较高的特点来补偿SINS随时间累积的误差,以此大幅度提高SINS导航定位精度。
对于SINS/DVL/深度传感器组合导航***来说,存在几个问题:SINS进行自主航位推算时,会随时间的增加产生累积误差;其二,在复杂水文环境下或者AUV的运动在较大的俯仰角或者横滚角等情况下,声学的DVL提供的速度信息可能会出现精度瞬时下降,甚至无法提供有效数据等情况。
综上所述,解决SINS/DVL/深度传感器组合导航***存在的问题,提高组合导航***定位精度和可靠性,具有很强的理论意义与工程应用价值。
发明内容
要解决的技术问题
为了避免现有技术的不足之处,本发明提出一种噪声自适应滤波的水下组合导航方法,解决DVL输出的测量数据出现异常状况时AUV组合导航***导航精度快速下降问题,提高组合导航***的鲁棒性和导航定位精度,实现SINS/DVL/转速计/深度传感器测量数据的有效融合。
技术方案
一种噪声自适应滤波的水下组合导航方法,其特征在于步骤如下:
步骤1:在AUV组合导航***中,以深度传感器的测量值作为航行深度,对滤波中的SINS的误差方程进行简化,得到简化后的SINS的误差模型:
位置误差方程:
速度误差方程:
姿态误差方程:
式中:δL、δλ分别表示AUV的纬度误差、经度误差;δVE、δVN、δVU分别表示AUV的东向、北向、天向速度误差;φE、φN、φU分别表示AUV姿态角误差在东向、北向、天向的投影;L、λ、h分别表示AUV所在位置的纬度、经度和航行深度;RM、RN分别为AUV所处位置的地球子午圈和卯酉圈曲率半径;VE、VN、VU分别表示AUV在东向、北向、天向的航行速度;ωie为地球自转角速率;fE、fN、fU分别表示加速度计在东向、北向、天向测得的比力值;▽E、▽N、▽U分别表示加速度计的偏置误差在东向、北向、天向的投影;εE、εN、εU分别表示陀螺的随机漂移误差在东向、北向、天向的投影;为载体坐标系到导航坐标系的坐标转换矩阵;εbx、εby、εbz分别表示x轴、y轴、z轴的陀螺仪随机漂移误差;wgx、wgy、wgz分别三轴陀螺的零均值高斯白噪声;▽bx、▽by、▽bz分别表示x轴、y轴、z轴的加速度计偏置误差;wax、way、waz分别三轴加速度计的零均值高斯白噪声;
取状态变量为
X=[δL δλ δVE δVN δVU φE φN φU εbx εby εbz ▽bx ▽by ▽bz]T
w=[wgx wgx wgx wgx wgx wgx]T
***状态方程为
式中:F和G是关于时间参数t的确定性时变矩阵;
步骤2:建立SINS/多普勒速度仪组合滤波的量测方程:
多普勒速度仪测得AUV载体坐标系速度分别Vdx、Vdy、Vdz,利用坐标转换矩阵将其由载体坐标系转换到导航坐标系;利用SINS计算得到的导航系下的速度与转换到导航系下的多普勒速度仪的速度做差,得到SINS/多普勒速度仪组合的量测方程:
其中,HD为SINS/多普勒速度仪组合的量测阵,vD为其量测噪声,为零均值的高斯白噪声;
步骤3:根据SINS的计算周期T,对***状态方程和量测方程进行离散化得
其中:
量测噪声方差阵Rk为:
简记:
步骤4:利用SINS/多普勒速度仪组合滤波过程中信息的统计特性,通过故障函数λk,判断多普勒速度仪输出的测量数据是否异常:
根据给定的虚警概率α,由χ2分布得到门限值TD,判断多普勒速度仪工作状态的方法为:当λk≤TD时,多普勒速度仪工作正常;当λk>TD时,多普勒速度仪工作出现异常;
1、当多普勒速度仪工作正常时,采用SINS/多普勒速度仪进行组合导航,运用卡尔曼滤波进行状态估计,利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正,输出修正后的位置、速度、姿态等导航参数。
卡尔曼滤波公式
状态估计均方误差阵Pk=(I-KkHk)Pk,k-1
计算AUV周围海水相对于海底的流速Vwk。一个滤波周期T内,经过预处理的转速计输出的AUV相对于周围海水的在载体系y轴方向的k时刻的平均速度为Vpy_k,AUV周围海水k时刻的速度Vw_k计算公式如下:
用指数加权移动平均法计算AUV周围海水相对于海底的流速Vwk,具体如下
Vwk=βVw_k+(1-β)Vw_(k-1)
其中,β可根据AUV周围海流的流速变化速度设定,当变化速度较快时,β值取得大些,如β=0.6;当变化速度较慢时,β值取得小些,如β=0.2;
2、当多普勒速度仪工作异常时,采用SINS/转速计进行组合导航,其量测方程为
式中,Vpy为转速计输出的AUV相对于周围海水的速度在载体系y轴方向的投影,Vw为多普勒速度仪工作异常前AUV周围海水的流速估计值,HW为SINS/转速计组合的量测阵,vW为其量测噪声,其为零均值的高斯白噪声;
运用卡尔曼滤波对SINS/转速计组合导航进行状态估计,利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正,输出修正后的位置、速度、姿态导航参数。
有益效果
本发明提出的一种噪声自适应滤波的水下组合导航方法,在AUV组合导航***中,采用深度传感器作为航行深度;对滤波中有关SINS的误差方程进行简化,基于残差的χ2检测法,利用SINS/DVL组合滤波过程中信息的统计特性,判断DVL输出的测量数据是否出现测量噪声过大等异常情况,利用SINS/转速计组合进行导航。有益效果:把AUV无刷直流推进电机的转动速度作为转速计使用,对AUV硬件成本增加很小,将转速计的信息用于在DVL异常状态下的SINS/转速计组合导航,抑制了AUV位置等误差的发散速度,提高了AUV的导航定位精度。
附图说明
图1为本发明的方法流程示意图
图2为具体实施方式流程示意图
具体实施方式
现结合实施例、附图对本发明作进一步描述:
本发明的技术方案,包括以下步骤:
步骤1:在AUV组合导航***中,采用测量精度较高的深度传感器,将其测量值直接作为航行深度;
基于深度传感器测量精度较高这一特点,忽略其存在的测量误差,将其输出值作为深度真值,对滤波中有关SINS的误差方程进行简化;
步骤2:建立SINS/多普勒速度仪组合滤波的量测方程:
多普勒速度仪测得AUV载体坐标系速度分别Vdx、Vdy、Vdz,利用坐标转换矩阵将其由载体坐标系转换到导航坐标系;利用SINS计算得到的导航系下的速度与转换到导航系下的多普勒速度仪的速度做差,得到SINS/多普勒速度仪组合的量测方程:
其中,HD为SINS/多普勒速度仪组合的量测阵,vD为其量测噪声,为零均值的高斯白噪声。
步骤3:根据SINS的计算周期T,对***状态方程和量测方程进行离散化得
其中:
考虑到多普勒速度仪在工作过程中速度测量噪声可能发生变化,量测噪声方差阵Rk确定方法如下:
式中,取初值β0=1,而0<b<1称为渐消因子;随着k逐渐增大,近似有βk≈1-b,渐消因子b根据多普勒速度仪量测噪声等情况,取值范围为0.9~0.999;同时,采用序贯滤波方法对Rk对角线每个元素的大小进行限制,即采用序贯滤波在进行第i个标量序贯量测更新时,标量量测方程为:
简记:
步骤4:利用SINS/多普勒速度仪组合滤波过程中信息的统计特性,通过故障函数λk,判断多普勒速度仪输出的测量数据是否异常:
根据给定的虚警概率α,由χ2分布得到门限值TD,判断多普勒速度仪工作状态的方法为:当λk≤TD时,多普勒速度仪工作正常;当λk>TD时,多普勒速度仪工作出现异常。
步骤5:当多普勒速度仪工作正常时,采用SINS/多普勒速度仪进行组合导航,运用卡尔曼滤波进行状态估计,利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正,输出修正后的位置、速度、姿态等导航参数。
卡尔曼滤波公式
状态估计均方误差阵Pk=(I-KkHk)Pk,k-1
计算AUV周围海水相对于海底的流速Vwk。一个滤波周期T内,经过预处理的转速计输出的AUV相对于周围海水的在载体系y轴方向的k时刻的平均速度为Vpy_k,AUV周围海水k时刻的速度Vw_k计算公式如下:
基于AUV周围海流的流速变化相对缓慢这一特点,为了降低转速计的测量噪声影响,用指数加权移动平均法计算AUV周围海水相对于海底的流速Vwk,具体如下
Vwk=βVw_k+(1-β)Vw_(k-1)
其中,β可根据AUV周围海流的流速变化速度设定,当变化速度较快时,β值取得大些,如β=0.6;当变化速度较慢时,β值取得小些,如β=0.2。
步骤6:当多普勒速度仪工作异常时,采用SINS/转速计进行组合导航,其量测方程为
式中,Vpy为转速计输出的AUV相对于周围海水的速度在载体系y轴方向的投影,Vw为多普勒速度仪工作异常前AUV周围海水的流速估计值,HW为SINS/转速计组合的量测阵,vW为其量测噪声,其为零均值的高斯白噪声,主要包括转速计的量测噪声等。
运用卡尔曼滤波对SINS/转速计组合导航进行状态估计,利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正,输出修正后的位置、速度、姿态等导航参数。
SINS的误差模型:
滤波器的状态变量的选取:取AUV各状态的误差量,包括:定位误差(纬度误差、经度误差)δL、δλ,AUV的东向、北向、天向速度误差δVE、δVN、δVU,姿态角误差在东向、北向、天向的投影φE、φN、φU,x轴、y轴、z轴陀螺仪随机漂移误差εbx、εby、εbz,x轴、y轴、z轴加速度计的偏置误差▽bx、▽by、▽bz,假设姿态角误差为小角度并忽略重力场模型误差,可得SINS的近似线性误差模型:
位置误差方程:
速度误差方程:
姿态误差方程:
其中:L、λ、h分别表示AUV所在位置的纬度、经度和航行深度,RM、RN分别为AUV所处位置的地球子午圈和卯酉圈曲率半径,VE、VN、VU分别表示AUV在东向、北向、天向的航行速度,ωie为地球自转角速率,fE、fN、fU分别表示加速度计在东向、北向、天向测得的比力值,▽E、▽N、▽U分别表示加速度计的偏置误差在东向、北向、天向的投影,εE、εN、εU分别表示陀螺的随机漂移误差在东向、北向、天向的投影。
取状态变量为
X=[δL δλ δVE δVN δVU φE φN φU εbx εby εbz ▽bx ▽by ▽bz]T
w=[wgx wgx wgx wgx wgx wgx]T
***状态方程
F和G是关于时间参数t的确定性是时变矩阵,可根据SINS的位置误差方程、速度误差方程、姿态误差方程整理得到。
其中,HD为SINS/DVL组合的量测阵,vD为其量测噪声,为零均值的高斯白噪声,主要包括DVL的量测噪声等。
根据SINS的计算周期T,对***状态方程和量测方程进行离散化得
其中:
基于残差的DVL工作状态判别方法
DVL非正常工作状态可以分为两类:一类情况DVL速度测量精度下降,像AUV受到海流冲击或进行转弯等动作,主要表现为量测噪声增大,可被划分为噪声突变类型;二类情况主要表现为输出信息的变化,像遇到强吸声物质声波无法返回或处于俯仰角和横滚角较大情况下,输出速度信息可能为0,这类情况可被划分为信息突变类型。
基于残差的χ2检测法,利用信息的统计特性来判断DVL工作是否正常,以及非正常状态的类型。当DVL工作正常时,量测误差是零均值高斯白噪声;当DVL工作在非正常状态下时,量测误差不再是零均值高斯白噪声。
当DVL工作正常时,滤波过程中残差向量为零均值高斯白噪声过程,其协方差矩阵为:
设残差序列即新息为:
故障检测函数为:
由统计特性可知,λk服从自由度m(向量Zk维数)的χ2分布。设λk大于某一个门限值TD的概率为α,即:
其中,α为允许的虚警概率。
根据给定的虚警概率α,可由χ2分布得到门限值TD,此时检测过程为:
当λk≤TD时,DVL工作正常;
当λk>TD时,DVL工作出现异常。
常规基于残差的χ2检测法,虽然可以迅速判别***是否存在突变型故障,但无法区分故障类别。为了解决这一问题,将残差的均值作为判据引入到故障检测***中,噪声突变型其残差均值会在0附近震荡,接近于0;若为信息突变类型,则残差均值与0值会有一定偏差。残差均值计算方法为:
上式中,表示残差均值,m是滑动窗长度,λi是故障发生后i时刻SINS/DVL组合***残差。此时可以通过判断DVL产生了何种误差,当接近于0,为噪声突变类型误差,即DVL测量数据包含较大扰动噪声;反之则为信息突变类型误差,DVL短时失效。
当DVL工作正常或者工作在出现噪声突变类型误差的非正常状态时,采用自适应滤波,对SINS/DVL组合导航数据进行融合,量测预测误差公式为:
量测噪声方差阵Rk获取方法如下:
采用序贯滤波方法对Rk对角线每个元素的大小进行限制。假设Rk为对角线矩阵,采用序贯滤波在进行第i个标量序贯量测更新时,标量量测方程为:
简记:
在AUV采用SINS/DVL组合导航过程中,同时计算AUV周围海水相对于海底的流速Vwk。一个滤波周期T内,经过预处理的转速计输出的AUV相对于周围海水的在载体系y轴方向的k时刻的平均速度为Vpy_k,AUV周围海水k时刻的速度Vw_k计算公式如下:
基于AUV周围海流的流速变化相对缓慢这一特点,为了降低转速计的测量噪声影响,用指数加权移动平均法计算AUV周围海水相对于海底的流速Vwk,具体如下
Vwk=βVw_k+(1-β)Vw_(k-1)
其中,β可根据AUV周围海流的流速变化速度设定,当变化速度较快时,β值取得大些,如β=0.6;当变化速度较慢时,β值取得小些,如β=0.2。
当DVL数据误差类型为信息突变类型误差,则DVL输出的速度信息完全失效,组合导航***SINS/DVL无法正常工作,需切换到SINS/转速计组合导航。待DVL工作正常后,切换回SINS/DVL组合导航。
SINS/转速计组合导航的量测方程:
其中,Vw为根据DVL失效前最后一次测量的速度数据计算得到的AUV相对于水底的速度,HW为SINS/转速计组合的量测阵,vW为其量测噪声,为零均值的高斯白噪声,主要包括转速计的量测噪声等。
Claims (1)
1.一种噪声自适应滤波的水下组合导航方法,其特征在于步骤如下:
步骤1:在AUV组合导航***中,以深度传感器的测量值作为航行深度,对滤波中的SINS的误差方程进行简化,得到简化后的SINS的误差模型:
位置误差方程:
速度误差方程:
姿态误差方程:
式中:δL、δλ分别表示AUV的纬度误差、经度误差;δVE、δVN、δVU分别表示AUV的东向、北向、天向速度误差;φE、φN、φU分别表示AUV姿态角误差在东向、北向、天向的投影;L、λ、h分别表示AUV所在位置的纬度、经度和航行深度;RM、RN分别为AUV所处位置的地球子午圈和卯酉圈曲率半径;VE、VN、VU分别表示AUV在东向、北向、天向的航行速度;ωie为地球自转角速率;fE、fN、fU分别表示加速度计在东向、北向、天向测得的比力值;▽E、▽N、▽U分别表示加速度计的偏置误差在东向、北向、天向的投影;εE、εN、εU分别表示陀螺的随机漂移误差在东向、北向、天向的投影;为载体坐标系到导航坐标系的坐标转换矩阵;εbx、εby、εbz分别表示x轴、y轴、z轴的陀螺仪随机漂移误差;wgx、wgy、wgz分别三轴陀螺的零均值高斯白噪声;▽bx、▽by、▽bz分别表示x轴、y轴、z轴的加速度计偏置误差;wax、way、waz分别三轴加速度计的零均值高斯白噪声;
取状态变量为
X=[δL δλ δVE δVN δVU φE φN φU εbx εby εbz ▽bx ▽by ▽bz]T
w=[wgx wgx wgx wgx wgx wgx]T
***状态方程为
式中:F和G是关于时间参数t的确定性时变矩阵;
步骤2:建立SINS/多普勒速度仪组合滤波的量测方程:
多普勒速度仪测得AUV载体坐标系速度分别Vdx、Vdy、Vdz,利用坐标转换矩阵将其由载体坐标系转换到导航坐标系;利用SINS计算得到的导航系下的速度与转换到导航系下的多普勒速度仪的速度做差,得到SINS/多普勒速度仪组合的量测方程:
其中,HD为SINS/多普勒速度仪组合的量测阵,vD为其量测噪声,为零均值的高斯白噪声;
步骤3:根据SINS的计算周期T,对***状态方程和量测方程进行离散化得
其中:
量测噪声方差阵Rk为:
简记:
步骤4:利用SINS/多普勒速度仪组合滤波过程中信息的统计特性,通过故障函数λk,判断多普勒速度仪输出的测量数据是否异常:
根据给定的虚警概率α,由χ2分布得到门限值TD,判断多普勒速度仪工作状态的方法为:当λk≤TD时,多普勒速度仪工作正常;当λk>TD时,多普勒速度仪工作出现异常;
1、当多普勒速度仪工作正常时,采用SINS/多普勒速度仪进行组合导航,运用卡尔曼滤波进行状态估计,利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正,输出修正后的位置、速度、姿态导航参数;卡尔曼滤波公式
状态估计均方误差阵Pk=(I-KkHk)Pk,k-1
计算AUV周围海水相对于海底的流速Vwk,一个滤波周期T内,经过预处理的转速计输出的AUV相对于周围海水的在载体系y轴方向的k时刻的平均速度为Vpy_k,AUV周围海水k时刻的速度Vw_k计算公式如下:
用指数加权移动平均法计算AUV周围海水相对于海底的流速Vwk,具体如下
Vwk=βVw_k+(1-β)Vw_(k-1)
其中,β可根据AUV周围海流的流速变化速度设定,当变化速度较快时,β值取得大些,如β=0.6;当变化速度较慢时,β值取得小些,如β=0.2;
2、当多普勒速度仪工作异常时,采用SINS/转速计进行组合导航,其量测方程为
式中,Vpy为转速计输出的AUV相对于周围海水的速度在载体系y轴方向的投影,Vw为多普勒速度仪工作异常前AUV周围海水的流速估计值,HW为SINS/转速计组合的量测阵,vW为其量测噪声,其为零均值的高斯白噪声;
运用卡尔曼滤波对SINS/转速计组合导航进行状态估计,利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正,输出修正后的位置、速度、姿态导航参数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010345972.XA CN111504324B (zh) | 2020-04-27 | 2020-04-27 | 一种噪声自适应滤波的水下组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010345972.XA CN111504324B (zh) | 2020-04-27 | 2020-04-27 | 一种噪声自适应滤波的水下组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111504324A true CN111504324A (zh) | 2020-08-07 |
CN111504324B CN111504324B (zh) | 2022-07-26 |
Family
ID=71871456
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010345972.XA Active CN111504324B (zh) | 2020-04-27 | 2020-04-27 | 一种噪声自适应滤波的水下组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111504324B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112179347A (zh) * | 2020-09-18 | 2021-01-05 | 西北工业大学 | 一种基于光谱红移误差观测方程的组合导航方法 |
CN112710304A (zh) * | 2020-12-17 | 2021-04-27 | 西北工业大学 | 一种基于自适应滤波的水下自主航行器导航方法 |
CN112947117A (zh) * | 2021-02-22 | 2021-06-11 | 湖南大学 | 一种多rov水下协同作业仿真*** |
WO2022088797A1 (zh) * | 2020-10-26 | 2022-05-05 | 东南大学 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN110146075A (zh) * | 2019-06-06 | 2019-08-20 | 哈尔滨工业大学(威海) | 一种增益补偿自适应滤波的sins/dvl组合定位方法 |
CN110514203A (zh) * | 2019-08-30 | 2019-11-29 | 东南大学 | 一种基于isr-ukf的水下组合导航方法 |
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
-
2020
- 2020-04-27 CN CN202010345972.XA patent/CN111504324B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
WO2020062791A1 (zh) * | 2018-09-28 | 2020-04-02 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN110146075A (zh) * | 2019-06-06 | 2019-08-20 | 哈尔滨工业大学(威海) | 一种增益补偿自适应滤波的sins/dvl组合定位方法 |
CN110514203A (zh) * | 2019-08-30 | 2019-11-29 | 东南大学 | 一种基于isr-ukf的水下组合导航方法 |
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
Non-Patent Citations (2)
Title |
---|
查月等: "基于自适应滤波器的AUV组合导航***研究", 《舰船科学技术》 * |
袁东玉等: "基于SINS的水下组合导航***研究", 《网络新媒体技术》 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112179347A (zh) * | 2020-09-18 | 2021-01-05 | 西北工业大学 | 一种基于光谱红移误差观测方程的组合导航方法 |
WO2022088797A1 (zh) * | 2020-10-26 | 2022-05-05 | 东南大学 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
CN112710304A (zh) * | 2020-12-17 | 2021-04-27 | 西北工业大学 | 一种基于自适应滤波的水下自主航行器导航方法 |
CN112710304B (zh) * | 2020-12-17 | 2022-12-13 | 西北工业大学 | 一种基于自适应滤波的水下自主航行器导航方法 |
CN112947117A (zh) * | 2021-02-22 | 2021-06-11 | 湖南大学 | 一种多rov水下协同作业仿真*** |
Also Published As
Publication number | Publication date |
---|---|
CN111504324B (zh) | 2022-07-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111504324B (zh) | 一种噪声自适应滤波的水下组合导航方法 | |
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
WO2022028286A1 (zh) | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 | |
Whitcomb et al. | Advances in Doppler-based navigation of underwater robotic vehicles | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
US7778111B2 (en) | Methods and systems for underwater navigation | |
CN103744098B (zh) | 基于sins/dvl/gps的auv组合导航*** | |
Hegrenaes et al. | Doppler water-track aided inertial navigation for autonomous underwater vehicle | |
CN107797125B (zh) | 一种减小深海探测型auv导航定位误差的方法 | |
CN106679662A (zh) | 一种基于tma技术的水下机器人单信标组合导航方法 | |
Lee et al. | Underwater navigation system based on inertial sensor and doppler velocity log using indirect feedback kalman filter | |
CN110940340A (zh) | 基于小型uuv平台的多传感器信息融合方法 | |
Kinsey et al. | Towards in-situ calibration of gyro and doppler navigation sensors for precision underwater vehicle navigation | |
CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及*** | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
KR101763911B1 (ko) | 자기장 외란이 심한 환경에서 수중체의 선수각 추정 장치 및 그 방법 | |
Allotta et al. | Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements | |
Sun et al. | Fine resolution position estimation using Kalman filtering | |
Zhao et al. | The experimental study on GPS/INS/DVL integration for AUV | |
CN112697154A (zh) | 一种基于矢量分配的自适应多源融合导航方法 | |
Lager et al. | Underwater terrain navigation using standard sea charts and magnetic field maps | |
Bennamoun et al. | The development of an integrated GPS/INS/sonar navigation system for autonomous underwater vehicle navigation | |
CN110779549A (zh) | 一种水下组合导航***突变型故障诊断方法 | |
CN114459476B (zh) | 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法 | |
US20210318452A1 (en) | Method for identifying a static phase of a vehicle |
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 |