CN110749891B - 一种可估计未知有效声速的自适应水下单信标定位方法 - Google Patents
一种可估计未知有效声速的自适应水下单信标定位方法 Download PDFInfo
- Publication number
- CN110749891B CN110749891B CN201910999922.0A CN201910999922A CN110749891B CN 110749891 B CN110749891 B CN 110749891B CN 201910999922 A CN201910999922 A CN 201910999922A CN 110749891 B CN110749891 B CN 110749891B
- Authority
- CN
- China
- Prior art keywords
- underwater
- observation
- sound velocity
- underwater vehicle
- velocity
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 84
- 230000005236 sound signal Effects 0.000 claims abstract description 52
- 230000005540 biological transmission Effects 0.000 claims abstract description 25
- 238000001914 filtration Methods 0.000 claims abstract description 24
- 239000011159 matrix material Substances 0.000 claims description 24
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 10
- 239000006185 dispersion Substances 0.000 claims description 9
- 238000012546 transfer Methods 0.000 claims description 7
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 238000013461 design Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000008054 signal transmission Effects 0.000 claims description 3
- 230000008859 change Effects 0.000 abstract description 3
- 230000000737 periodic effect Effects 0.000 abstract 1
- 238000007796 conventional method Methods 0.000 description 5
- 230000006872 improvement Effects 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002238 attenuated effect Effects 0.000 description 1
- BULVZWIRKLYCBC-UHFFFAOYSA-N phorate Chemical compound CCOP(=S)(OCC)SCSCC BULVZWIRKLYCBC-UHFFFAOYSA-N 0.000 description 1
- 230000036962 time dependent Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
- G01S15/50—Systems of measurement, based on relative movement of the target
- G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
- G01S15/60—Velocity or trajectory determination systems; Sense-of-movement determination systems wherein the transmitter and receiver are mounted on the moving object, e.g. for determining ground speed, drift angle, ground track
-
- 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/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)
- Acoustics & Sound (AREA)
- Computer Networks & Wireless Communication (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明属于水下定位技术领域,特别涉及一种水下航行器的单信标定位方法。水下航行器未接收到周期性水声信号时,通过配备的电子罗盘、深度计及读取螺旋桨转速进行航位推算,在接收到多普勒测速仪测得的绝对速度观测后构造海流速度观测量并通过Kalman滤波进行海流速度校正;水下航行器接收到水声信号后,考虑水下声速的未知性及声速不确定性噪声参数的未知性,将未知水声声速建模为均值及方差均未知的Gauss分布,基于扩展Kalman滤波及变分贝叶斯近似,以水声信号传递时间为观测变量,进行水下航行器的位置更新。相比于基于声速不确定性统计参数完全已知的水下单信标定位方法,本发明所提出方法可以更好的跟踪水声声速变化的趋势,进而得到更好的定位结果。
Description
技术领域
本发明属于水下定位技术领域,特别涉及一种水下航行器的单信标定位方法。
背景技术
精确的位置反馈是水下航行器完成既定水下任务的基础。由于水下电磁波信号衰减较快,广泛应用于陆地与天空定位的GNSS***在水下无法应用。现有主流的水下定位方式包括以惯性导航为代表的航位推算方法以及以长基线定位为代表的水下声学定位方法。其中惯性导航设备往往会随时间增长产生较大累计误差,无法长时间用于水下定位,而高精度的惯性导航设备成本极高,限制了其在水下航行器中的应用。现有主流的水下声学定位方式包括长基线定位、超短基线定位、单信标定位等。长基线定位与超短基线定位发展均较为成熟,但其成本通常较高,且实时性通常较差,这限制了其在水下航行器中的应用。而新兴的水下单信标定位***融合航位推算数据与单水声信标的测距信息,在定位成本和实时性方面均有较大的优势。水声测距是通过检测水声信号的传递时间乘以水声声速获得。目前的水下单信标定位方法通常假设水声声速完全已知,但实际的水声声速收到水域温度、盐度、密度、水深等因素的影响,通常为时变未知的。水声声速的设置误差会导致测距误差,进而会影响单信标定位***定位精度。而现有基于未知有效声速的水下单信标定位方法均将有效声速不确定性建模为均值和方差均已知的Gauss分布。但在实际的水下定位当中,受到水下多变环境的影响,水声声速不确定性的统计参数通常为时变的,且无法准确获得。而噪声统计参数设置不准确会恶化定位***的性能,甚至可能会导致滤波发散,在这种情况下,传统的基于已知噪声参数的Gauss分布对水声声速不确定性进行建模的单信标定位方法的定位性能会受到影响,这会影响其实际应用。
发明内容
本发明的目的是:针对水下单信标定位当中水声声速的未知性及其不确定性统计参数的未知性,结合扩展Kalman滤波与变分贝叶斯近似提出一种可估计未知有效声速的自适应水下信标定位方法。
本发明的技术方案是:一种可估计未知有效声速的自适应水下单信标定位方法,水下航行器搭载水听器、多普勒测速仪、深度计、电子罗盘及GPS;水声信标周期性广播水声信号,水下航行器通过所搭载的多普勒测速仪周期性的观测其绝对速度;水下航行器通过GPS获取初始位置,并且通过读取自身螺旋桨转速获得航行器与水的相对速度。水下航行器未接收到水声信号时通过自身携带的电子罗盘及读取自身螺旋桨转速信息进行航位推算;水下航行器在接收到多普勒测速仪提供的绝对速度观测时,通过结合螺旋桨转速信息及电子罗盘信息构造海流观测变量,基于Kalman滤波进行海流速度更新;水下航行器接收到水声信号后,考虑水下声速的未知性以及声速不确定性噪声参数的未知性,将未知水声声速建模为均值及方差均未知的Gauss分布,基于扩展Kalman滤波算法及变分贝叶斯近似,以水声信号传递时间为观测变量,进行水下航行器的位置更新。本发明的步骤包括:
A.以定位区域内任意点为原点,东、北、天三个方向分别设为x,y,z轴,建立水下局部惯性坐标系;
B.通过水下航行器所搭载的GPS***获取该所述水下航行器在水下局部惯性系当中的初始位置;
C.建立所述水下航行器的运动学模型以及观测模型并进行离散化;
D.建立有效声速及其不确定性统计参数的随机模型以及函数模型;
E.水声信标周期性广播水声信号,水声信号发射时间及水声信标位置已知;所述水下航行器在未接收到水声信号时,通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算,同时进行有效声速随机模型参数预测;所述水下航行器在接收到所搭载的多普勒测速仪测得的的绝对速度观测后,通过读取所述螺旋桨转速信息及所述电子罗盘信息,构造海流速度观测量并通过Kalman滤波进行海流速度校正;
F.所述水下航行器接收到水声信号后,记录接收时刻,根据已知的水声信号发射时刻及水声信标位置坐标,并考虑水下声速及其不确定性统计参数的未知性,以此基于扩展Kalman 滤波算法及变分贝叶斯近似,以水声信号传递时间为观测变量,进行所述水下航行器的位置更新。
进一步的,所述C步骤中,所述运动学模型的建立方法为:
定义状态向量为:
x=[x y vcx vcy]T
其中:x,y为所述水下航行器在所述水下局部惯性坐标系中的水平位置;vcx,vcy为未知的海流速度;
对x求导并加入所述水下航行器运动运动模型噪声影响,得到所述水下航行器的运动学模型:
其中:vwx为所述水下航行器x方向的对水速度;vwy为所述水下航行器y方向的对水速度;vwx及vwy通过读取所述螺旋桨转速与所述电子罗盘测得的航行器艏向角计算得出;ωx为所述水下航行器在x方向的位置不确定性;ωy为所述水下航行器在y方向的位置不确定性;ωcx为x方向的海流不确定性;ωcy为y方向的海流不确定性;
进一步的,所述C步骤中,所述观测模型的建立方法为:
S1.建立水声信号传递时间的观测模型;
设所述水下航行器获得所述水声信标发射水声信号的时刻为Te,所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为XTe,YTe,ZTe,所述水下航行器接收到该水声信号的时刻为Ta,观测方程为:
其中:νt为对应的观测噪声;z为所述水下航行器的深度,由深度计精确测得,为已知量;ve为有效声速;
将观测方程记作m=h(x,ve),其中:
S2.建立海流流速观测模型;
根据vgx,vgy以及vwx,vwy,计算得到海流速度观测分量为:
海流观测量为线性,满足mvc=Hx+νvc;
其中:观测向量mvc=[mcx mcy]T;mcx,mcy分别为x,y方向海流速度观测;νvc为海流观测噪声向量,νvc=[νv,cx νv,cy]T,其中νv,cx为x方向的海流不确定性;νv,cy为y方向的海流不确定性;H为海流观测矩阵,满足:
进一步的,所述C步骤中,所述运动学模型以及观测模型离散化方法为:
S1.运动学模型离散化;
以下标k为时间索引,以Δt=tk+1-tk为离散间隔,运动学模型离散为:
xk+1=Akxk+Bkuk+wk
其中:Ak为运动学方程,满足:
Bk为控制方程,满足:
uk为控制向量,满足uk=[vwx,k vwy,k]T,为已知量;
wk过程噪声向量,满足wk=[ωx,k ωy,k ωcx,k ωcy,k]T,对应各个状态变量的不确定性;将***状态xk,yk,vcx,k,vcy,k的过程噪声建模为零均值Gauss分布,其协方差矩阵满足:
其中,σw为所述水下航行器对水速度观测不确定性的标准差;σc为海流不确定性的标准差;
S2.观测模型离散化;
所述水下航行器在k-1至k间接收到该水声信号,将其假设为在k时刻接收到该水声信号,即离散后的水声信号传递时间观测方程为:
其中,νt,k为观测噪声,假设其满足方差为Rt,k的Gauss分布;考虑到有效声速ve,k的时变未知性,将ve,k也看作随机变量;离散形式的观测方程写作:
mk=hk(xk,ve,k),
由于海流观测采样频率较高,假设在每一个离散时间点k处均可以得到海流观测,故离散后的海流观测方程为:
mvc,k=Hkxk+νvc,k
其中,Hk为k时刻海流速度观测矩阵,满足:
νvc,k为k时刻海流观测噪声,为零均值Gauss分布,其观测噪声协方差矩阵满足:
其中,σvc,m为海流速度观测噪声的标准差。
进一步的,所述D步骤中,有效声速及其不确定性统计参数的随机模型以及函数模型建立方法为:
将有效声速初始先验分布建模为Gauss分布:
将k-1时刻有效声速的后验分布也建模为Gauss分布:
有效声速的动态扩散的函数模型记作:
ve,k=ve,k-1+ωe,k-1
其中:ωe,k-1为有效声速的不确定性,将其建模为均值及方差均未知的Gauss分布:
其中:μk-1与σe,k-1分别为声速不确定性的均值与标准差;
据此,可以将有效声速的概率扩散方程记作:
结合上式,获得有效声速的预测模型为:
通过在线估计Pe,k|k-1来补偿σe,k-1设置误差的影响;将μk-1与Pe,k|k-1的先验分布建模为其共轭先验,即Gauss-inverse Gama(GIG)分布:
GIG(a,A|τ,α,λ,ν)=N(a|τ,αA)IG(A|λ,ν)
其中:IG(A|λ,ν)表示以λ,ν为参数的逆Gama分布;
μk-1与Pe,k|k-1的先验分布可以被分解为:
μk-1与Pe,k|k-1的先验估计需要与其名义值匹配,即μk-1与Pe,k|k-1的先验统计参数满足:
可设计μk-1与Pe,k|k-1统计参数预测方程为:
其中:ρα与ρλ为调制参数。
进一步的,所述E步骤中,所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为:
根据Kalman滤波的预测环节,将***状态的先验分布近似为Gauss分布:
进一步的,所述E步骤中,所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后,进行海流速度校正的方法为:
Pk|k=Pk|k-1-KkHkPk|k-1
其中:Kk为海流更新Kalman增益。
进一步的,所述步骤F中,通过扩展Kalman滤波及变分贝叶斯近似进行水声信号传递时间校正的方法为:
S1.建立观测似然函数、有效声速后验分布及其不确定性统计参数后验概率密度分布模型;
根据离散形式的水声信号传递时间观测模型,得到水声信号传递时间的观测似然密度为:
p(mk|ve,k,xk)=N(mk|hk(xk,ve,k),Rk)
将有效声速后验分布建模为Gauss分布,即:
μk-1与Pe,k|k-1的后验分布同样建模为GIG分布,分解为:
S2.定义状态变量、有效声速及声速不确定性统计参数后验估计近似值;
通过变分贝叶斯近似,将状态变量、有效声速及声速不确定性统计参数的联合后验估计近似为:
p(xk,ve,k,μk-1,Pe,k|k-1|m1:k)≈q(xk)q(ve,k)q(μk-1)q(Pe,k|k-1)
以最小化近似前与近似后两个概率密度函数之间的Kullback-Leibler散度(KLD)最小化为目标,得到近似解为:
其中:log(·)表示对数运算;θ表示xk,ve,k,μk-1,Pe,k|k-1中的任意元素;Ex[·]表示相对于x的期望;上标(-θ)表示整个集合当中除了θ以外的其它元素;cθ表示与θ无关的常数;采用固定点迭代来求解q(θ);
S3.求解状态变量、有效声速及声速不确定性统计参数后验估计近似值;
S3.1求解联合概率密度对数值;
其对数形式表示为:
S3.2求解声速不确定性统计参数Pe,k|k-1估计值;
令θ=Pe,k|k-1,得到:
其中:
变量加上标(i)表示该变量在第i次迭代中的估计值;进而得到:
取Pe,k|k-1在第i+1次迭代中的估计值为:
S3.3求解声速不确定性统计参数μk-1估计值;
令θ=μk-1,得到:
进而得到:
取μk-1在第i+1次迭代中的估计值为其期望值,即:
S3.4求解有效声速ve,k估计值;
其中:
为观测方程的雅克比矩阵;
由此得到线性化后的水声信号传递时间观测方程为:
令θ=ve,k,得到:
其中:
根据Kalman滤波更新式,得到有效声速更新方程为:
S3.5求解***状态xk估计值;
其中:
为观测方程的雅克比矩阵;
由此得到线性化后的水声信号传递时间观测方程为:
令θ=xk,得到:
其中:
根据Kalman滤波更新式,得到***状态更新方程为:
记总迭代次数为N,则最终有效声速均值及方差、***状态均值及协方差矩阵的估计值分别为:
有益效果:本发明通过结合Kalman滤波、扩展Kalman滤波及变分贝叶斯近似,将未知有效声速不确定性建模为均值及方差均未知的Gauss分布,以水声信号传递时间为观测变量,进行水下航行器的位置更新。相比于基于已知定常有效声速以及基于声速不确定性统计参数完全已知的水下单信标定位方法,本发明所提出方法可以更好的跟踪水声声速变化的趋势,进而也可以得到更好的定位结果,增强水下单信标定位***的实际应用能力。
附图说明
图1为本发明的步骤流程图;
图2为水面船的真实运动轨迹、本发明与两种传统水下单信标定位方法所估计出的运动轨迹;
图3为本发明与两种传统水下单信标定位方法的水平定位误差随时间变化的曲线;
图4为真实声速值、基于传统方法1与本发明提出方法估计出的水声声速。
具体实施方式
实施例1,参见附图1,一种可估计未知有效声速的自适应水下单信标定位方法,包括以下步骤:
A.以定位区域内任意点为原点,东、北、天三个方向分别设为x,y,z轴,建立水下局部惯性坐标系;
B.通过水下航行器所搭载的GPS***获取该所述水下航行器在水下局部惯性系当中的初始位置;
C.建立所述水下航行器的运动学模型以及观测模型并进行离散化;
所述运动学模型的建立方法为:
定义状态向量为:
x=[x y vcx vcy]T
其中:x,y为所述水下航行器在所述水下局部惯性坐标系中的水平位置;vcx,vcy为未知的海流速度;
对x求导并加入所述水下航行器运动运动模型噪声影响,得到所述水下航行器的运动学模型:
其中:vwx为所述水下航行器x方向的对水速度;vwy为所述水下航行器y方向的对水速度;vwx及vwy通过读取所述螺旋桨转速与所述电子罗盘测得的航行器艏向角计算得出;ωx为所述水下航行器在x方向的位置不确定性;ωy为所述水下航行器在y方向的位置不确定性;ωcx为x方向的海流不确定性;ωcy为y方向的海流不确定性;
所述C步骤中,所述观测模型的建立方法为:
S1.建立水声信号传递时间的观测模型;
设所述水下航行器获得所述水声信标发射水声信号的时刻为Te,所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为XTe,YTe,ZTe,所述水下航行器接收到该水声信号的时刻为Ta,观测方程为:
其中:νt为对应的观测噪声;z为所述水下航行器的深度,由深度计精确测得,为已知量;ve为有效声速;
将观测方程记作m=h(x,ve),其中:
S2.建立海流流速观测模型;
根据vgx,vgy以及vwx,vwy,计算得到海流速度观测分量为:
海流观测量为线性,满足mvc=Hx+νvc;
其中:观测向量mvc=[mcx mcy]T;mcx,mcy分别为x,y方向海流速度观测;νvc为海流观测噪声向量,νvc=[νv,cx νv,cy]T,其中νv,cx为x方向的海流不确定性;νv,cy为y方向的海流不确定性;H为海流观测矩阵,满足:
所述运动学模型以及观测模型离散化方法为:
S1.运动学模型离散化;
以下标k为时间索引,以Δt=tk+1-tk为离散间隔,运动学模型离散为:
xk+1=Akxk+Bkuk+wk
其中:Ak为运动学方程,满足:
Bk为控制方程,满足:
uk为控制向量,满足uk=[vwx,k vwy,k]T,为已知量;
wk过程噪声向量,满足wk=[ωx,k ωy,k ωcx,k ωcy,k]T,对应各个状态变量的不确定性;将***状态xk,yk,vcx,k,vcy,k的过程噪声建模为零均值Gauss分布,其协方差矩阵满足:
其中,σw为所述水下航行器对水速度观测不确定性的标准差;σc为海流不确定性的标准差;
S2.观测模型离散化;
所述水下航行器在k-1至k间接收到该水声信号,将其假设为在k时刻接收到该水声信号,即离散后的水声信号传递时间观测方程为:
其中,νt,k为观测噪声,假设其满足方差为Rt,k的Gauss分布;考虑到有效声速ve,k的时变未知性,将ve,k也看作随机变量;离散形式的观测方程写作:
mk=hk(xk,ve,k),
由于海流观测采样频率较高,假设在每一个离散时间点k处均可以得到海流观测,故离散后的海流观测方程为:
mvc,k=Hkxk+νvc,k
其中,Hk为k时刻海流速度观测矩阵,满足:
νvc,k为k时刻海流观测噪声,为零均值Gauss分布,其观测噪声协方差矩阵满足:
其中,σvc,m为海流速度观测噪声的标准差;
D.建立有效声速及其不确定性统计参数的随机模型以及函数模型;
有效声速及其不确定性统计参数的随机模型以及函数模型建立方法为:
将有效声速初始先验分布建模为Gauss分布:
将k-1时刻有效声速的后验分布也建模为Gauss分布:
有效声速的动态扩散的函数模型记作:
ve,k=ve,k-1+ωe,k-1
其中:ωe,k-1为有效声速的不确定性,将其建模为均值及方差均未知的Gauss分布:
其中:μk-1与σe,k-1分别为声速不确定性的均值与标准差;
据此,可以将有效声速的概率扩散方程记作:
结合上式,获得有效声速的预测模型为:
通过在线估计Pe,k|k-1来补偿σe,k-1设置误差的影响;将μk-1与Pe,k|k-1的先验分布建模为其共轭先验,即Gauss-inverse Gama(GIG)分布:
GIG(a,A|τ,α,λ,ν)=N(a|τ,αA)IG(A|λ,ν)
其中:IG(A|λ,ν)表示以λ,ν为参数的逆Gama分布;
μk-1与Pe,k|k-1的先验分布可以被分解为:
μk-1与Pe,k|k-1的先验估计需要与其名义值匹配,即μk-1与Pe,k|k-1的先验统计参数满足:
可设计μk-1与Pe,k|k-1统计参数预测方程为:
其中:ρα与ρλ为调制参数;
E.水声信标周期性广播水声信号,水声信号发射时间及水声信标位置已知;所述水下航行器在未接收到水声信号时,通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算,同时进行有效声速随机模型参数预测;所述水下航行器在接收到所搭载的多普勒测速仪测得的的绝对速度观测后,通过读取所述螺旋桨转速信息及所述电子罗盘信息,构造海流速度观测量并通过Kalman滤波进行海流速度校正;
所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为:
根据Kalman滤波的预测环节,将***状态的先验分布近似为Gauss分布:
所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后,进行海流速度校正的方法为:
Pk|k=Pk|k-1-KkHkPk|k-1
其中:Kk为海流更新Kalman增益;
F.所述水下航行器接收到水声信号后,记录接收时刻,根据已知的水声信号发射时刻及水声信标位置坐标,并考虑水下声速及其不确定性统计参数的未知性,以此基于扩展Kalman 滤波算法及变分贝叶斯近似,以水声信号传递时间为观测变量,进行所述水下航行器的位置更新;
通过扩展Kalman滤波及变分贝叶斯近似进行水声信号传递时间校正的方法为:
S1.建立观测似然函数、有效声速后验分布及其不确定性统计参数后验概率密度分布模型;
根据离散形式的水声信号传递时间观测模型,得到水声信号传递时间的观测似然密度为:
p(mk|ve,k,xk)=N(mk|hk(xk,ve,k),Rk)
将有效声速后验分布建模为Gauss分布,即:
μk-1与Pe,k|k-1的后验分布同样建模为GIG分布,分解为:
S2.定义状态变量、有效声速及声速不确定性统计参数后验估计近似值;
通过变分贝叶斯近似,将状态变量、有效声速及声速不确定性统计参数的联合后验估计近似为:
p(xk,ve,k,μk-1,Pe,k|k-1|m1:k)≈q(xk)q(ve,k)q(μk-1)q(Pe,k|k-1)
以最小化近似前与近似后两个概率密度函数之间的Kullback-Leibler散度(KLD)最小化为目标,得到近似解为:
其中:log(·)表示对数运算;θ表示xk,ve,k,μk-1,Pe,k|k-1中的任意元素;Ex[·]表示相对于x的期望;上标(-θ)表示整个集合当中除了θ以外的其它元素;cθ表示与θ无关的常数;采用固定点迭代来求解q(θ);
S3.求解状态变量、有效声速及声速不确定性统计参数后验估计近似值;
S3.1求解联合概率密度对数值;
其对数形式表示为:
S3.2求解声速不确定性统计参数Pe,k|k-1估计值;
令θ=Pe,k|k-1,得到:
其中:
变量加上标(i)表示该变量在第i次迭代中的估计值;进而得到:
取Pe,k|k-1在第i+1次迭代中的估计值为:
S3.3求解声速不确定性统计参数μk-1估计值;
令θ=μk-1,得到:
进而得到:
取μk-1在第i+1次迭代中的估计值为其期望值,即:
S3.4求解有效声速ve,k估计值;
其中:
为观测方程的雅克比矩阵;
由此得到线性化后的水声信号传递时间观测方程为:
令θ=ve,k,得到:
其中:
根据Kalman滤波更新式,得到有效声速更新方程为:
S3.5求解***状态xk估计值;
其中:
为观测方程的雅克比矩阵;
由此得到线性化后的水声信号传递时间观测方程为:
令θ=xk,得到:
其中:
根据Kalman滤波更新式,得到***状态更新方程为:
记总迭代次数为N,则最终有效声速均值及方差、***状态均值及协方差矩阵的估计值分别为:
实施例2,利用实施例1所述的方法通过试验数据进行验证。
作为比较,本实施例同时展示了基于声速不确定性统计参数完全已知以及基于已知定常水声声速的水下单信标定位方法定位结果(分别记作传统方法1与传统方法2,传统方法1 参考文献Z.Zhu and S.L.J.Hu,“Model and algorithm improvement on singlebeacon underwater tracking,”IEEE Journal of Oceanic Engineering,vol.PP,no.99,pp.1–18,2017.)。试验数据的搜集方法为:水面船搭载GPS、水听器以及罗盘,在水面上进行二维运动。GPS观测到的水面船运动轨迹作为真实参考,水听器接收固定在水底的水声信标所发射的水声信号,并以此获得水声信号传递时间。由于水面船未安装多普勒测速仪,采用GPS轨迹进行差分结合电子罗盘测得的艏向角来仿真多普勒测速仪所观测的航行器对地速度。试验当中水声信号发射周期约为30秒(少数几个信号出现观测丢包),海流观测周期为1秒,离散时间间隔Δt同样设置为1秒。
在数值验证的过程中,各种方法调制参数设置为:(1)x与y两个方向位置初始误差均为10米;(2)x与y两个方向海流初始误差均为0.05米/秒;(3)名义声速为1540米/秒;(4)海流不确定性标准差σc为0.01米/秒;(5)航行器对水速度观测不确定性标准差σw为 0.1米/秒;(6)传统方法1与本发明方法采用的水声声速不确定性的标准差为1米/秒;(7) 水声信号传递时间观测噪声标准差σt,m为0.001秒;(8)海流观测噪声标准差σvc,m为0.01米 /秒;(9)传统方法2中斜距观测噪声标准差σr,m为5米;(10)本发明所提出方法的迭代次数N设置为15,调制参数ρα与ρλ分别为3与2,初始参数α0设置为2。
附图2为试验中水面船的真实运动轨迹、本发明的方法与两种传统水下单信标定位方法所估计出的运动轨迹。附图3表示三种方法的水平定位误差随时间变化的曲线,定位误差计算公式为附图4表示真实声速值、基于声速不确定性统计参数完全已知的水下单信标定位方法(传统方法1)与本发明提出方法的水声声速估计结果。两种传统方法与本发明所提出方法的平均均方定位误差分别为7.19米、13.09米与5.18米。而传统方法1与本发明方法的声速平均均方误差分别为6.87米/秒与3.03米/秒。平均均方定位误差与声速平均均方误差计算公式分别为:
其中T和K分别表示总离散间隔数及总水声信号传递时间采样数。根据图2、3、4以及三种方法的平均均方定位误差、声速平均均方误差,可以看出本发明所提出的方法可以获得比传统水下单信标定位方法更好的结果。相比于基于声速不确定性统计参数完全已知的水下单信标定位方法(传统方法1),本发明所提出方法可以更好的跟踪水声声速变化的趋势,进而也可以得到更好的定位结果。
实施例3,本发明的算法伪代码总结为:
虽然,上文中已经用一般性说明及具体实施例对本发明作了详尽的描述,但在本发明基础上,可以对之作一些修改或改进,这对本领域技术人员而言是显而易见的。因此,在不偏离本发明精神的基础上所做的这些修改或改进,均属于本发明要求保护的范围。
Claims (1)
1.一种可估计未知有效声速的自适应水下单信标定位方法,其特征在于,包括以下步骤:
A.以定位区域内任意点为原点,东、北、天三个方向分别设为x,y,z轴,建立水下局部惯性坐标系;
B.通过水下航行器所搭载的GPS***获取该所述水下航行器在水下局部惯性系当中的初始位置;
C.建立所述水下航行器的运动学模型以及观测模型并进行离散化;
所述运动学模型的建立方法为:
定义状态向量为:
x=[x y vcx vcy]T
其中:x,y为所述水下航行器在所述水下局部惯性坐标系中的水平位置;vcx,vcy为未知的海流速度;
对x求导并加入所述水下航行器运动模型噪声影响,得到所述水下航行器的运动学模型:
其中:vwx为所述水下航行器x方向的对水速度;vwy为所述水下航行器y方向的对水速度;vwx及vwy通过读取螺旋桨转速与电子罗盘测得的航行器艏向角计算得出;ωx为所述水下航行器在x方向的位置不确定性;ωy为所述水下航行器在y方向的位置不确定性;ωcx为x方向的海流不确定性;ωcy为y方向的海流不确定性;
所述观测模型的建立方法为:
S1.建立水声信号传递时间的观测模型;
设所述水下航行器获得水声信标发射水声信号的时刻为Te,所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为XTe,YTe,ZTe,所述水下航行器接收到该水声信号的时刻为Ta,观测方程为:
其中:νt为对应的观测噪声;z为所述水下航行器的深度,由深度计精确测得,为已知量;ve为有效声速;
将观测方程记作m=h(x,ve),其中:
S2.建立海流流速观测模型;
根据vgx,vgy以及vwx,vwy,计算得到海流速度观测分量为:
海流观测量为线性,满足mvc=Hx+νvc;
其中:观测向量mvc=[mcx mcy]T;mcx,mcy分别为x,y方向海流速度观测;νvc为海流观测噪声向量,νvc=[νv,cx νv,cy]T,其中νv,cx为x方向的海流不确定性;νv,cy为y方向的海流不确定性;H为海流观测矩阵,满足:
所述运动学模型以及所述观测模型离散化方法为:
S1.运动学模型离散化;
以下标k为时间索引,以Δt=tk+1-tk为离散间隔,运动学模型离散为:
xk+1=Akxk+Bkuk+wk
其中:Ak为运动学方程,满足:
Bk为控制方程,满足:
uk为控制向量,满足uk=[vwx,k vwy,k]T,为已知量;
wk过程噪声向量,满足wk=[ωx,k ωy,k ωcx,k ωcy,k]T,对应各个状态变量的不确定性;将***状态xk,yk,vcx,k,vcy,k的过程噪声建模为零均值Gauss分布,其协方差矩阵满足:
其中,σw为所述水下航行器对水速度观测不确定性的标准差;σc为海流不确定性的标准差;
S2.观测模型离散化;
所述水下航行器在k-1至k间接收到该水声信号,将其假设为在k时刻接收到该水声信号,即离散后的水声信号传递时间观测方程为:
其中,νt,k为观测噪声,假设其满足方差为Rt,k的Gauss分布;考虑到有效声速ve,k的时变未知性,将ve,k也看作随机变量;离散形式的观测方程写作:
mk=hk(xk,ve,k),
由于海流观测采样频率较高,假设在每一个离散时间点k处均可以得到海流观测,故离散后的海流观测方程为:
mvc,k=Hkxk+νvc,k
其中,Hk为k时刻海流速度观测矩阵,满足:
νvc,k为k时刻海流观测噪声,为零均值Gauss分布,其观测噪声协方差矩阵满足:
其中,σvc,m为海流速度观测噪声的标准差;
D.建立有效声速及其不确定性统计参数的随机模型以及函数模型;
将有效声速初始先验分布建模为Gauss分布:
将k-1时刻有效声速的后验分布也建模为Gauss分布:
有效声速的动态扩散的函数模型记作:
ve,k=ve,k-1+ωe,k-1
其中:ωe,k-1为有效声速的不确定性,将其建模为均值及方差均未知的Gauss分布:
其中:μk-1与σe,k-1分别为声速不确定性的均值与标准差;
据此,可以将有效声速的概率扩散方程记作:
结合上式,获得有效声速的预测模型为:
通过在线估计Pe,k|k-1来补偿σe,k-1设置误差的影响;将μk-1与Pe,k|k-1的先验分布建模为其共轭先验,即Gauss-inverse Gama,简写为GIG分布:
GIG(a,A|τ,α,λ,ν)=N(a|τ,αA)IG(A|λ,ν)
其中:IG(A|λ,ν)表示以λ,ν为参数的逆Gama分布;
μk-1与Pe,k|k-1的先验分布可以被分解为:
μk-1与Pe,k|k-1的先验估计需要与其名义值匹配,即μk-1与Pe,k|k-1的先验统计参数满足:
设计μk-1与Pe,k|k-1统计参数预测方程为:
其中:ρα与ρλ为调制参数;
E.水声信标周期性广播水声信号,水声信号发射时间及水声信标位置已知;所述水下航行器在未接收到水声信号时,通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算,同时进行有效声速随机模型参数预测;所述水下航行器在接收到所搭载的多普勒测速仪测得的绝对速度观测后,通过读取所述螺旋桨转速信息及电子罗盘信息,构造海流速度观测量并通过Kalman滤波进行海流速度校正;
所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为:
根据Kalman滤波的预测环节,将***状态的先验分布近似为Gauss分布:
所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后,进行海流速度校正的方法为:
Pk|k=Pk|k-1-KkHkPk|k-1
其中:Kk为海流更新Kalman增益;
F.所述水下航行器接收到水声信号后,记录接收时刻,根据已知的水声信号发射时刻及水声信标位置坐标,并考虑水下声速及其不确定性统计参数的未知性,以此基于扩展Kalman滤波算法及变分贝叶斯近似,以水声信号传递时间为观测变量,进行所述水下航行器的位置更新;
S1.建立观测似然函数、有效声速后验分布及其不确定性统计参数后验概率密度分布模型;
根据离散形式的水声信号传递时间观测模型,得到水声信号传递时间的观测似然密度为:
p(mk|ve,k,xk)=N(mk|hk(xk,ve,k),Rk)
将有效声速后验分布建模为Gauss分布,即:
μk-1与Pe,k|k-1的后验分布同样建模为GIG分布,分解为:
S2.定义状态变量、有效声速及声速不确定性统计参数后验估计近似值;
通过变分贝叶斯近似,将状态变量、有效声速及声速不确定性统计参数的联合后验估计近似为:
p(xk,ve,k,μk-1,Pe,k|k-1|m1:k)≈q(xk)q(ve,k)q(μk-1)q(Pe,k|k-1)
以最小化近似前与近似后两个概率密度函数之间的Kullback-Leibler散度(KLD)最小化为目标,得到近似解为:
其中:log(·)表示对数运算;θ表示xk,ve,k,μk-1,Pe,k|k-1中的任意元素;Ex[·]表示相对于x的期望;上标(-θ)表示整个集合当中除了θ以外的其它元素;cθ表示与θ无关的常数;采用固定点迭代来求解q(θ);
S3.求解状态变量、有效声速及声速不确定性统计参数后验估计近似值;
S3.1求解联合概率密度对数值;
其对数形式表示为:
S3.2求解声速不确定性统计参数Pe,k|k-1估计值;
令θ=Pe,k|k-1,得到:
其中:
变量加上标(i)表示该变量在第i次迭代中的估计值;进而得到:
取Pe,k|k-1在第i+1次迭代中的估计值为:
S3.3求解声速不确定性统计参数μk-1估计值;
令θ=μk-1,得到:
进而得到:
取μk-1在第i+1次迭代中的估计值为其期望值,即:
S3.4求解有效声速ve,k估计值;
其中:
为观测方程的雅克比矩阵;
由此得到线性化后的水声信号传递时间观测方程为:
令θ=ve,k,得到:
其中:
根据Kalman滤波更新式,得到有效声速更新方程为:
S3.5求解***状态xk估计值;
其中:
为观测方程的雅克比矩阵;
由此得到线性化后的水声信号传递时间观测方程为:
令θ=xk,得到:
其中:
根据Kalman滤波更新式,得到***状态更新方程为:
记总迭代次数为N,则最终有效声速均值及方差、***状态均值及协方差矩阵的估计值分别为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910999922.0A CN110749891B (zh) | 2019-10-21 | 2019-10-21 | 一种可估计未知有效声速的自适应水下单信标定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910999922.0A CN110749891B (zh) | 2019-10-21 | 2019-10-21 | 一种可估计未知有效声速的自适应水下单信标定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110749891A CN110749891A (zh) | 2020-02-04 |
CN110749891B true CN110749891B (zh) | 2021-08-24 |
Family
ID=69279108
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910999922.0A Expired - Fee Related CN110749891B (zh) | 2019-10-21 | 2019-10-21 | 一种可估计未知有效声速的自适应水下单信标定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110749891B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112526454B (zh) * | 2020-10-22 | 2022-04-26 | 自然资源部第一海洋研究所 | 一种顾及表层声速和坐标先验信息的水下控制点定位方法 |
CN112468116B (zh) * | 2020-12-01 | 2023-06-16 | 哈尔滨工程大学 | 一种基于Gibbs采样器的自适应平滑方法 |
CN113093092B (zh) * | 2021-04-01 | 2022-06-14 | 哈尔滨工程大学 | 一种水下鲁棒自适应单信标定位方法 |
CN114040325B (zh) * | 2021-11-05 | 2022-08-19 | 西北工业大学 | 惯导辅助下的单锚节点网络协同定位方法 |
CN116680500B (zh) * | 2023-06-12 | 2024-03-22 | 哈尔滨工程大学 | 水下航行器在非高斯噪声干扰下的位置估计方法及*** |
CN117146830B (zh) * | 2023-10-31 | 2024-01-26 | 山东科技大学 | 一种自适应多信标航位推算和长基线的紧组合导航方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP3563827B2 (ja) * | 1995-06-27 | 2004-09-08 | 日本無線株式会社 | 車載計測装置 |
WO2014070683A1 (en) * | 2012-10-29 | 2014-05-08 | Teledyne Rd Instruments, Inc. | System and method for water column aided navigation |
CN105676181B (zh) * | 2016-01-15 | 2018-06-01 | 浙江大学 | 基于分布式传感器能量比的水下运动目标扩展卡尔曼滤波跟踪方法 |
CN105823480B (zh) * | 2016-03-18 | 2018-07-06 | 中国海洋大学 | 基于单信标的水下移动目标定位算法 |
-
2019
- 2019-10-21 CN CN201910999922.0A patent/CN110749891B/zh not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
CN110749891A (zh) | 2020-02-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110749891B (zh) | 一种可估计未知有效声速的自适应水下单信标定位方法 | |
CN110794409B (zh) | 一种可估计未知有效声速的水下单信标定位方法 | |
CN109459040B (zh) | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 | |
CN110779518B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
CN110646783B (zh) | 一种水下航行器的水下信标定位方法 | |
CN110779519B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
CN109724599B (zh) | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 | |
Song et al. | Neural-network-based AUV navigation for fast-changing environments | |
CN110231636B (zh) | Gps与bds双模卫星导航***的自适应无迹卡尔曼滤波方法 | |
CN110823217A (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
CN112729291A (zh) | 一种深潜长航潜水器sins/dvl洋流速度估计方法 | |
Geng et al. | Hybrid derivative-free EKF for USBL/INS tightly-coupled integration in AUV | |
CN108871365A (zh) | 一种航向约束下的状态估计方法及*** | |
CN117146830B (zh) | 一种自适应多信标航位推算和长基线的紧组合导航方法 | |
Xu et al. | Accurate two-step filtering for AUV navigation in large deep-sea environment | |
CN112666519B (zh) | 一种基于广义二阶时延差的水下目标高精度定位方法 | |
Liu et al. | Navigation algorithm based on PSO-BP UKF of autonomous underwater vehicle | |
CN111307136B (zh) | 一种双智能水下机器人水下航行地形匹配导航方法 | |
CN110873813B (zh) | 一种水流速度估算方法、组合导航方法及装置 | |
CN113093092B (zh) | 一种水下鲁棒自适应单信标定位方法 | |
CN113219406B (zh) | 一种基于扩展卡尔曼滤波的直接跟踪方法 | |
Gade et al. | An aided navigation post processing filter for detailed seabed mapping UUVs | |
CN103940416B (zh) | 一种电磁计程仪辅助的auv多程序并行解算导航方法 | |
KR20220089392A (ko) | 무인 수상선의 지구물리적 내비게이션 장치 및 방법 | |
Jauffret et al. | Bearings-only TMA without observer maneuver |
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 | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20210824 |