CN110646783A - 一种水下航行器的水下信标定位方法 - Google Patents

一种水下航行器的水下信标定位方法 Download PDF

Info

Publication number
CN110646783A
CN110646783A CN201910939017.6A CN201910939017A CN110646783A CN 110646783 A CN110646783 A CN 110646783A CN 201910939017 A CN201910939017 A CN 201910939017A CN 110646783 A CN110646783 A CN 110646783A
Authority
CN
China
Prior art keywords
underwater
underwater vehicle
observation
beacon
vehicle
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
Application number
CN201910939017.6A
Other languages
English (en)
Other versions
CN110646783B (zh
Inventor
秦洪德
余相
朱仲本
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201910939017.6A priority Critical patent/CN110646783B/zh
Publication of CN110646783A publication Critical patent/CN110646783A/zh
Application granted granted Critical
Publication of CN110646783B publication Critical patent/CN110646783B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/14—Systems for determining distance or velocity not using reflection or reradiation using ultrasonic, sonic, or infrasonic waves
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明属于水下定位技术领域，特别涉及一种水下航行器的定位方法。水声信标周期性广播水声信号；水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，并在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，构造海流速度观测量并通过Kalman滤波进行海流速度校正；水下航行器接收到水声信号后，考虑水下声速的未知性、水声信标位置误差及水声信号收发端的时钟漂移，以此基于扩展Kalman滤波算法及期望最大化算法，以水声信号传递时间为观测变量，进行水下航行器的位置更新。本发明可以保证水下航行器在存在时钟漂移、信标位置及声速设置误差的情况下仍然得到理想的定位性能。

Description

一种水下航行器的水下信标定位方法
技术领域
本发明属于水下定位技术领域，特别涉及一种水下航行器的定位方法。
背景技术
精确的位置反馈是水下航行器完成既定水下任务的基础。由于水下电磁波信号衰减较快，广泛应用于陆地与天空定位的GNSS系统在水下无法应用。现有主流的水下定位方式包括以惯性导航为代表的航位推算方法以及以长基线定位为代表的水下声学定位方法。其中惯性导航设备往往会随时间增长产生较大累计误差，无法长时间用于水下定位，而高精度的惯性导航设备成本极高，限制了其在水下航行器中的应用。现有主流的的水下声学定位方式包括长基线定位、超短基线定位、单信标定位等。长基线定位与超短基线定位发展均较为成熟，但其成本通常较高，且实时性通常较差，这限制了其在水下航行器中的应用。而新兴的水下单信标定位系统融合航位推算数据与单水声信标的测距信息，在定位成本和实时性方面均有较大的优势。但目前的单信标定位系统均基于水声信号收发端的精确时钟同步，且要求水声信标的位置坐标及水声声速完全已知。但实际应用中，水声信标的位置通常是通过在任务执行前利用水面船进行多点测距获得，信标位置通常会存在校正误差；水声声速收到水下温度、盐度、密度等因素的影响，通常为时变未知的，精确的水声声速难以得到；而纵使安装高精度的原子钟，水声信号收发端仍会存在随着时间累计的时钟漂移，对于在水下长时间执行任务的航行器而言，由时钟漂移造成的测距误差不可忽视。以上的实际三个问题均会引起水声测距偏差，进而导致定位误差，恶化单信标定位系统性能。
发明内容
本发明的目的是：针对水下单信标定位当中水声声速的未知性、水下信标位置偏差及水声信号收发端的时钟漂移的问题，基于期望最大化方法提出一种水下航行器的水下信标定位方法。
本发明的技术方案是：水下航行器搭载水听器、多普勒测速仪、深度计、电子罗盘及GPS；水声信标周期性广播水声信号，水下航行器通过所搭载的多普勒测速仪周期性的观测其绝对速度；水下航行器通过GPS获取初始位置，并且通过读取自身螺旋桨转速获得航行器与水的相对速度。水下航行器未接收到水声信号时通过自身携带的电子罗盘及读取自身螺旋桨转速信息进行航位推算；水下航行器在接收到多普勒测速仪提供的绝对速度观测时，通过结合螺旋桨转速信息及电子罗盘信息构造海流观测变量，基于Kalman滤波进行海流速度更新；水下航行器接收水声信号后记录信号到达时间，并通过期望最大化方法及扩展Kalman滤波进行定位位置校准。本发明的步骤包括：
一种水下航行器的水下信标定位方法，包括以下步骤：
A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
B.通过水下航行器所搭载的GPS系统获取该水下航行器在水下局部惯性系当中的初始位置；
C.建立水下航行器的运动学模型以及观测模型并进行离散化；
D.水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知，但存在一定偏差；水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，并在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，通过读取螺旋桨转速信息及电子罗盘信息，构造海流速度观测量并通过Kalman滤波进行海流速度校正；
E.水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速的未知性、水声信标位置误差及水声信号收发端的时钟漂移，以此基于扩展Kalman滤波算法及期望最大化算法，以水声信号传递时间为观测变量，进行水下航行器的位置更新。
具体的，上述C步骤中，运动学模型的建立方法为：
定义状态向量为：
x＝[x y vcx vcy]T
其中：x，y为水下航行器在水下局部惯性坐标系中的水平位置；vcx，vcy为未知的海流速度；
对x求导并加入水下航行器运动运动模型噪声影响，得到水下航行器的运动学模型：
Figure BDA0002222375740000031
其中：vwx为水下航行器x方向的对水速度；vwy为水下航行器y方向的对水速度；vwx及vwy通过读取螺旋桨转速与电子罗盘测得的航行器艏向角计算得出；ωx为水下航行器在x方向的位置不确定性；ωy为水下航行器在y方向的速度不确定性；ωcx为x方向的海流不确定性；ωcy为x方向的海流不确定性；
vwx及vwy的计算公式为：
Figure BDA0002222375740000032
式中vw为根据螺旋桨转速得出的水下航行器对水速度，
Figure BDA0002222375740000033
为电子罗盘测得的艏向角。
具体的，上述C步骤中，观测模型的建立方法为：
S1.建立水声信号传递时间的观测模型；
设水下航行器获得水声信标发射水声信号的时刻为Te，水声信标在水下局部惯性坐标系中的空间位置坐标为XTe，YTe，ZTe，水下航行器接收到该水声信号的时刻为Ta，观测方程为：
其中：vt为对应的观测噪声；z为水下航行器的深度，ZTe为水声信标的深度，z与ZTe均由深度计精确测得为已知量；ΔTt为水声信号收发端的时钟漂移，ve为有效声速，在水声声速、水声信标位置坐标及水声信号收发端时钟漂移未知时，XTe，YTe，ΔTt，ve均为未知量；将XTe，YTe，ΔTt，ve看作未知的系统参数，定义参数集为θ＝[XTe YTe ΔTt ve]T，则观测方程记作Tt＝h(x,θ)，其中，
Figure BDA0002222375740000035
S2.建立海流流速观测模型；
根据多普勒测速仪测得的水下航行器的绝对速度vg，结合电子罗盘测得的艏向角
Figure BDA0002222375740000037
计算得到水下航行器绝对速度在局部惯性坐标系下的分量vgx,vgy；
根据vgx,vgy以及vwx,vwy，计算得到海流速度分量为：
Figure BDA0002222375740000036
海流观测量为线性，满足m＝Hx+vvc；
其中：观测向量m＝[mcx mcy]T；mcx，mcy分别为x，y方向海流速度观测；vvc为海流观测噪声向量，vvc＝[vcx vcy]T，其中vcx为x方向的海流不确定性；vcy为y方向的海流不确定性；H为海流观测矩阵，满足：
具体的，上述C步骤中，运动学模型以及观测模型离散化方法为：
S1.运动学模型离散化；
以符号k为时间索引，以Δt＝tk+1-tk为离散间隔，运动学模型离散为：
xk+1＝Akxk+Bkuk+wk
其中：Ak为运动学方程，满足：
Bk为控制方程，满足：
Figure BDA0002222375740000043
uk为控制向量，满足uk＝[vwx,k vwy,k]T，为已知量；
wk过程噪声向量，满足wk＝[ωx,k ωy,k ωcx,k ωcy,k]T；
对应各个状态变量的不确定性，过程噪声协方差矩阵满足：
Figure BDA0002222375740000044
其中，σw为水下航行器对水速度观测不确定性的标准差；σc为海流不确定性的标准差；
S2.观测模型离散化；
水下航行器在k-1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：
Figure BDA0002222375740000051
其中，vt,k为观测噪声，假设其满足方差为Rt,k的Gauss分布(标准差记作σt,m)；考虑到XTe YTe ΔTt,k ve,k的时变未知性，记未知参数集θk＝[XTe YTe ΔTt,k ve,k]T，观测方程可以记作mt,k＝hk(xk,θk)，
Figure BDA0002222375740000052
由于多普勒测速仪、电子罗盘、航行器螺旋桨转速采样频率较高，假设在每一个离散时间点k处均可以得到观测，故离散后观测方程为：
mvc，k＝Hkxk+vvc,k
其中，Hk为k时刻速度与加速度观测矩阵，满足：
Figure BDA0002222375740000053
vvc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：
Figure BDA0002222375740000054
其中，σvc,m 2为海流速度观测噪声的标准差。
具体的，上述D步骤中，水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：
Figure BDA0002222375740000055
Figure BDA0002222375740000056
其中：
Figure BDA0002222375740000057
与Pk-1|k-1分别为k-1时刻的后验状态和后验方差(航行器进行更新后的导航状态参数)；
Figure BDA0002222375740000058
与Pk|k-1分别为k时刻的先验状态和先验方差(航行器未进行更新时的导航状态参数)。
具体的，上述D步骤中，水下航行器接收到多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：
Figure BDA0002222375740000061
Figure BDA0002222375740000063
其中：Kk为Kalman增益。
具体的，上述步骤E中，通过扩展Kalman滤波及期望最大化算法进行水声信号传递时间校正的方法为：
S1.期望最大化算法求期望；
S1.1定义目标函数；
Figure BDA0002222375740000064
其中：
Figure BDA0002222375740000065
表示θk在第l次迭代中的近似值；表示相对于x的期望；
Figure BDA00022223757400000613
表示随机变量a的概率密度函数，函数与
Figure BDA00022223757400000614
相关；m1:k表示从1到k时刻的观测量；p(x|y)表示随机变量x以y为条件的条件期望；
S1.2定义参数估计值解；
在已知
Figure BDA0002222375740000066
与m1:k的前提下，定义下一次迭代的参数估计值为：
Figure BDA0002222375740000067
假设共进行N次迭代，则最终参数估计解为：
Figure BDA0002222375740000068
S1.3求解联合概率密度；
由于系统观测噪声及过程噪声均为Gauss白噪声，系统联合概率密度可以分解为：由于运动学模型为线性，当k-1时刻系统后验为Gauss分布时，k时刻的先验分布也满足Gauss分布，即：
Figure BDA00022223757400000610
其中，N(x；μ,Σ)表示以μ为均值向量，以Σ为方差矩阵，满足Gauss分布的随机变量x；先验参数
Figure BDA00022223757400000611
与Pk|k-1由所述步骤5当中的航位推算流程得出；
S1.4求解后验状态分布；
对非线性的水声信号传递时间观测方程mt,k＝hk(xk,θk)进行线性化，只保留一阶项，得到：
其中：
Figure BDA0002222375740000072
为观测方程的雅克比矩阵；
由此得到线性化后的水声信号传递时间观测方程为：
在已知
Figure BDA0002222375740000074
与m1:k的前提下，记状态后验估计为：
Figure BDA0002222375740000075
选择线性化展开点相应的雅克比矩阵为：
Figure BDA0002222375740000077
其中：
Figure BDA0002222375740000078
根据扩展Kalman滤波流程，可以得到后验估计参数为：
Figure BDA0002222375740000079
Figure BDA00022223757400000710
Figure BDA00022223757400000711
S1.5求解目标函数；
选择
Figure BDA00022223757400000712
作为线性展开点，根据
Figure BDA00022223757400000713
以及
Figure BDA00022223757400000714
得到：
其中：
Figure BDA00022223757400000716
表示与θk无关的常数；
Figure BDA0002222375740000081
Figure BDA0002222375740000082
Figure BDA0002222375740000084
根据以上关系，将Fk简化为：
Figure BDA0002222375740000085
其中：
Figure BDA0002222375740000086
S2.期望最大化算法求极值步骤；
Figure BDA0002222375740000087
为
Figure BDA0002222375740000088
的极大值的必要条件为：
Figure BDA0002222375740000089
计算得到：
Figure BDA00022223757400000810
当
Figure BDA00022223757400000811
时，以上极值条件满足；
令：
Figure BDA00022223757400000812
其中：
Figure BDA0002222375740000091
表示水声信标位置坐标与水声声速的名义值，在迭代过程中一直保持不变，通过更新水声信号收发端的时钟漂移ΔTt,k来间接补偿信标位置坐标与水声声速设置误差带来的影响。
通过以上参数更新方案，可以保证期望最大化方法的极值条件满足。
有益效果：本发明通过结合Kalman滤波、扩展Kalman滤波与期望最大化方法，将未知的水声声速、水声信标位置坐标与水声信号收发端的时钟漂移看作未知系统参数，实现了导航状态与系统参数的同时估计，可以保证水下航行器在存在时钟漂移、信标位置及声速设置误差的情况下仍然得到理想的定位性能。
附图说明
图1为本发明的步骤流程图；
图2、图3为利用本发明进行实测数据数值验证的结果。
具体实施方式
实施例1，参见附图1：一种水下航行器的水下信标定位方法，包括以下步骤：
A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
B.通过水下航行器所搭载的GPS系统获取该水下航行器在水下局部惯性系当中的初始位置；
C.建立水下航行器的运动学模型以及观测模型并进行离散化；
运动学模型的建立方法为：
定义状态向量为：
x＝[x y vcx vcy]T
其中：x，y为水下航行器在水下局部惯性坐标系中的水平位置；vcx，vcy为未知的海流速度；
对x求导并加入水下航行器运动运动模型噪声影响，得到水下航行器的运动学模型：
Figure BDA0002222375740000101
其中：vwx为水下航行器x方向的对水速度；vwy为水下航行器y方向的对水速度；vwx及vwy通过读取螺旋桨转速与电子罗盘测得的航行器艏向角计算得出；ωx为水下航行器在x方向的位置不确定性；ωy为水下航行器在y方向的速度不确定性；ωcx为x方向的海流不确定性；ωcy为x方向的海流不确定性；
vwx及vwy的计算公式为：
Figure BDA0002222375740000102
式中vw为根据螺旋桨转速得出的水下航行器对水速度，
Figure BDA0002222375740000103
为电子罗盘测得的艏向角；
观测模型的建立方法为：
S1.建立水声信号传递时间的观测模型；
设水下航行器获得水声信标发射水声信号的时刻为Te，水声信标在水下局部惯性坐标系中的空间位置坐标为XTe，YTe，ZTe，水下航行器接收到该水声信号的时刻为Ta，观测方程为：
Figure BDA0002222375740000104
其中：vt为对应的观测噪声；z为水下航行器的深度，ZTe为水声信标的深度，z与ZTe均由深度计精确测得为已知量；ΔTt为水声信号收发端的时钟漂移，ve为有效声速，在水声声速、水声信标位置坐标及水声信号收发端时钟漂移未知时，XTe，YTe，ΔTt，ve均为未知量；将XTe，YTe，ΔTt，ve看作未知的系统参数，定义参数集为θ＝[XTe YTe ΔTt ve]T，则观测方程记作Tt＝h(x,θ)，其中，
S2.建立海流流速观测模型；
根据多普勒测速仪测得的水下航行器的绝对速度vg，结合电子罗盘测得的艏向角
Figure BDA0002222375740000106
计算得到水下航行器绝对速度在局部惯性坐标系下的分量vgx,vgy；
根据vgx,vgy以及vwx,vwy，计算得到海流速度分量为：
Figure BDA0002222375740000107
海流观测量为线性，满足m＝Hx+vvc；
其中：观测向量m＝[mcx mcy]T；mcx，mcy分别为x，y方向海流速度观测；vvc为海流观测噪声向量，vvc＝[vcx vcy]T，其中vcx为x方向的海流不确定性；vcy为y方向的海流不确定性；H为海流观测矩阵，满足：
Figure BDA0002222375740000111
运动学模型以及观测模型离散化方法为：
S1.运动学模型离散化；
以符号k为时间索引，以Δt＝tk+1-tk为离散间隔，运动学模型离散为：
xk+1＝Akxk+Bkuk+wk
其中：Ak为运动学方程，满足：
Bk为控制方程，满足：
Figure BDA0002222375740000113
uk为控制向量，满足uk＝[vwx,k vwy,k]T，为已知量；
wk过程噪声向量，满足wk＝[ωx,k ωy,k ωcx,k ωcy,k]T；
对应各个状态变量的不确定性，过程噪声协方差矩阵满足：
Figure BDA0002222375740000114
其中，σw为水下航行器对水速度观测不确定性的标准差；σc为海流不确定性的标准差；
S2.观测模型离散化；
水下航行器在k-1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：
Figure BDA0002222375740000121
其中，vt,k为观测噪声，假设其满足方差为Rt,k的Gauss分布(标准差记作σt,m)；考虑到XTeYTeΔTt,kve,k的时变未知性，记未知参数集θk＝[XTe YTe ΔTt,k ve,k]T，观测方程可以记作mt,k＝hk(xk,θk)，
Figure BDA0002222375740000122
由于多普勒测速仪、电子罗盘、航行器螺旋桨转速采样频率较高，假设在每一个离散时间点k处均可以得到观测，故离散后观测方程为：
mvc，k＝Hkxk+vvc,k
其中，Hk为k时刻速度与加速度观测矩阵，满足：
Figure BDA0002222375740000123
vvc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：
Figure BDA0002222375740000124
其中，σvc,m 2为海流速度观测噪声的标准差；
D.水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知，但存在一定偏差；水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，并在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，通过读取螺旋桨转速信息及电子罗盘信息，构造海流速度观测量并通过Kalman滤波进行海流速度校正；
水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：
Figure BDA0002222375740000125
Figure BDA0002222375740000126
其中：
Figure BDA0002222375740000127
与Pk-1|k-1分别为k-1时刻的后验状态和后验方差(航行器进行更新后的导航状态参数)；
Figure BDA0002222375740000131
与Pk|k-1分别为k时刻的先验状态和先验方差(航行器未进行更新时的导航状态参数)；
水下航行器接收到多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：
Figure BDA0002222375740000132
Figure BDA0002222375740000133
Pk|k＝Pk|k-1-KkHkPk|k-1
其中：Kk为Kalman增益。
E.水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速的未知性、水声信标位置误差及水声信号收发端的时钟漂移，以此基于扩展Kalman滤波算法及期望最大化算法，以水声信号传递时间为观测变量，进行水下航行器的位置更新；
通过扩展Kalman滤波及期望最大化算法进行水声信号传递时间校正的方法为：
S1.期望最大化算法求期望；
S1.1定义目标函数；
Figure BDA0002222375740000134
其中：
Figure BDA0002222375740000135
表示θk在第l次迭代中的近似值；表示相对于x的期望；
Figure BDA00022223757400001310
表示随机变量a的概率密度函数，函数与
Figure BDA00022223757400001311
相关；m1:k表示从1到k时刻的观测量；p(x|y)表示随机变量x以y为条件的条件期望；
S1.2定义参数估计值解；
在已知
Figure BDA0002222375740000136
与m1:k的前提下，定义下一次迭代的参数估计值为：
假设共进行N次迭代，则最终参数估计解为：
Figure BDA0002222375740000138
S1.3求解联合概率密度；
由于系统观测噪声及过程噪声均为Gauss白噪声，系统联合概率密度可以分解为：
Figure BDA00022223757400001312
由于运动学模型为线性，当k-1时刻系统后验为Gauss分布时，k时刻的先验分布也满足Gauss分布，即：
Figure BDA0002222375740000141
其中，N(x；μ,Σ)表示以μ为均值向量，以Σ为方差矩阵，满足Gauss分布的随机变量x；先验参数
Figure BDA0002222375740000142
与Pk|k-1由所述步骤5当中的航位推算流程得出；
S1.4求解后验状态分布；
对非线性的水声信号传递时间观测方程mt,k＝hk(xk,θk)进行线性化，只保留一阶项，得到：
Figure BDA0002222375740000143
其中：
Figure BDA0002222375740000144
为观测方程的雅克比矩阵；
由此得到线性化后的水声信号传递时间观测方程为：
Figure BDA0002222375740000145
在已知
Figure BDA0002222375740000146
与m1:k的前提下，记状态后验估计为：
选择线性化展开点
Figure BDA0002222375740000148
相应的雅克比矩阵为：
Figure BDA0002222375740000149
其中：
Figure BDA00022223757400001410
根据扩展Kalman滤波流程，可以得到后验估计参数为：
Figure BDA00022223757400001411
Figure BDA00022223757400001412
Figure BDA00022223757400001413
S1.5求解目标函数；
选择
Figure BDA00022223757400001414
作为线性展开点，根据
Figure BDA00022223757400001415
以及
Figure BDA00022223757400001416
得到：
Figure BDA00022223757400001417
其中：
Figure BDA0002222375740000151
表示与θk无关的常数；
Figure BDA0002222375740000152
Figure BDA0002222375740000153
根据以上关系，将Fk简化为：
Figure BDA0002222375740000156
其中：
S2.期望最大化算法求极值步骤；
为的极大值的必要条件为：
Figure BDA00022223757400001510
计算得到：
Figure BDA00022223757400001511
当
Figure BDA00022223757400001512
时，以上极值条件满足；
令：
Figure BDA00022223757400001513
其中：
Figure BDA0002222375740000161
表示水声信标位置坐标与水声声速的名义值，在迭代过程中一直保持不变，通过更新水声信号收发端的时钟漂移ΔTt,k来间接补偿信标位置坐标与水声声速设置误差带来的影响。
通过以上参数更新方案，可以保证期望最大化方法的极值条件满足。
实施例2，本发明的算法伪代码总结为：
Figure BDA0002222375740000162
实施例3，利用实施例1所述的方法通过实验数据进行验证。
作为比较，本实施例同时展示了传统的水下单信标定位方法的定位结果。实验数据的搜集方法为：水面船搭载GPS、水听器以及罗盘，在水面上进行二维运动。GPS观测到的水面船运动轨迹作为真实参考，水听器接收固定在水底的水声信标所发射的水声信号，并以此获得水声信号传递时间。由于水面船未安装多普勒测速仪，采用GPS轨迹进行差分结合电子罗盘测得的艏向角来仿真多普勒测速仪所观测的航行器对地速度。试验当中水声信号发射周期约为30秒(少数几个信号出现观测丢包)，海流观测周期为1秒，离散时间间隔Δt同样设置为1秒。为了验证本发明所提出的方法，在试验数据中人为加入信标的位置误差以及水声信号收发端的时钟漂移，其中x与y两个方向信标位置误差均未20米，时钟漂移规律设置为ΔTt,k＝0.005kΔt/3600，相当于时钟漂移为5毫秒/小时。
在数值验证的过程中，滤波器初始参数设置为：(1)x与y两个方向海流初始误差均为0.5米/秒；(2)名义声速
Figure BDA0002222375740000171
为1520米/秒；(3)名义时钟漂移为0秒；(4)海流不确定性标准差σc为0.01米/秒；(5)航行器对水速度观测不确定性标准差σw为0.1米/秒；(6)水声信号传递时间观测噪声标准差σt,m为0.001秒；(7)海流观测噪声标准差σvc,m为0.01米/秒；(8)传统水下单信标定位应用中斜距观测噪声标准差σr,m为5米；(9)本发明所提出方法的迭代次数N设置为10。
附图2为试验中水面船的真实运动轨迹、本发明的方法与传统水下单信标定位方法所估计出的运动轨迹、真实以及名义水声信标位置。附图3表示两种方法的水平定位误差随时间变化的曲线，定位误差计算公式为
Figure BDA0002222375740000172
本发明所提出方法与传统水下单信标定位方法的平均均方定位误差分别为0.2497米与20.4466米。根据图2、3以及两种方法的平均均方定位误差，可以看出在存在时钟漂移、信标位置偏差及水声声速设置误差时，本发明所提出的方法可以获得比传统水下单信标定位方法更好的结果。时钟漂移、信标位置偏差及水声声速设置误差对本发明所提出方法的定位性能几乎没有影响。
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。

Claims (7)

1.一种水下航行器的水下信标定位方法，其特征在于，包括以下步骤：
A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
B.通过所述水下航行器所搭载的GPS系统获取该所述水下航行器在水下局部惯性系当中的初始位置；
C.建立所述水下航行器的运动学模型以及观测模型并进行离散化；
D.所述水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知，但存在一定偏差；所述水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，并在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，通过读取所述螺旋桨转速信息及所述电子罗盘信息，构造海流速度观测量并通过Kalman滤波进行海流速度校正；
E.所述水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速的未知性、水声信标位置误差及水声信号收发端的时钟漂移，以此基于扩展Kalman滤波算法及期望最大化算法，以水声信号传递时间为观测变量，进行所述水下航行器的位置更新。
2.如权利要求1所述的一种水下航行器的水下信标定位方法，其特征在于，所述C步骤中，所述运动学模型的建立方法为：
定义状态向量为：
x＝[x y vcx vcy]T
其中：x，y为所述水下航行器在所述水下局部惯性坐标系中的水平位置；vcx，vcy为未知的海流速度；
对x求导并加入所述水下航行器运动运动模型噪声影响，得到所述水下航行器的运动学模型：
Figure FDA0002222375730000011
其中：vwx为所述水下航行器x方向的对水速度；vwy为所述水下航行器y方向的对水速度；vwx及vwy通过读取所述螺旋桨转速与所述电子罗盘测得的航行器艏向角计算得出；ωx为所述水下航行器在x方向的位置不确定性；ωy为所述水下航行器在y方向的速度不确定性；ωcx为x方向的海流不确定性；ωcy为x方向的海流不确定性；
vwx及vwy的计算公式为：式中vw为根据所述螺旋桨转速得出的所述水下航行器对水速度，
Figure FDA0002222375730000022
为所述电子罗盘测得的艏向角。
3.如权利要求2所述的一种水下航行器的水下信标定位方法，其特征在于，所述C步骤中，所述观测模型的建立方法为：
S1.建立水声信号传递时间的观测模型；
设所述水下航行器获得所述水声信标发射水声信号的时刻为Te，所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为XTe，YTe，ZTe，所述水下航行器接收到该水声信号的时刻为Ta，观测方程为：
其中：vt为对应的观测噪声；z为所述水下航行器的深度，ZTe为所述水声信标的深度，z与ZTe均由深度计精确测得为已知量；ΔTt为水声信号收发端的时钟漂移，ve为有效声速，在水声声速、所述水声信标位置坐标及水声信号收发端时钟漂移未知时，XTe，YTe，ΔTt，ve均为未知量；将XTe，YTe，ΔTt，ve看作未知的系统参数，定义参数集为θ＝[XTe YTe ΔTt ve]T，则观测方程记作Tt＝h(x,θ)，其中，
S2.建立海流流速观测模型；
根据所述多普勒测速仪测得的所述水下航行器的绝对速度vg，结合所述电子罗盘测得的艏向角
Figure FDA0002222375730000025
计算得到所述水下航行器绝对速度在局部惯性坐标系下的分量vgx,vgy；
根据vgx,vgy以及vwx,vwy，计算得到海流速度分量为：
Figure FDA0002222375730000026
海流观测量为线性，满足m＝Hx+vvc；
其中：观测向量m＝[mcx mcy]T；mcx，mcy分别为x，y方向海流速度观测；vvc为海流观测噪声向量，vvc＝[vcx vcy]T，其中vcx为x方向的海流不确定性；vcy为y方向的海流不确定性；H为海流观测矩阵，满足：
Figure FDA0002222375730000031
4.如权利要求3所述的一种水下航行器的水下信标定位方法，其特征在于，所述C步骤中，所述运动学模型以及观测模型离散化方法为：
S1.运动学模型离散化；
以符号k为时间索引，以Δt＝tk+1-tk为离散间隔，运动学模型离散为：
xk+1＝Akxk+Bkuk+wk
其中：Ak为运动学方程，满足：
Figure FDA0002222375730000032
Bk为控制方程，满足：
Figure FDA0002222375730000033
uk为控制向量，满足uk＝[vwx,k vwy,k]T，为已知量；
wk过程噪声向量，满足wk＝[ωx,k ωy,k ωcx,k ωcy,k]T；
对应各个状态变量的不确定性，过程噪声协方差矩阵满足：
Figure FDA0002222375730000034
其中，σw为所述水下航行器对水速度观测不确定性的标准差；σc为海流不确定性的标准差；
S2.观测模型离散化；
所述水下航行器在k-1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：
Figure FDA0002222375730000041
其中，vt,k为观测噪声，假设其满足方差为Rt,k的Gauss分布；考虑到XTe YTe ΔTt,k ve,k的时变未知性，记未知参数集θk＝[XTe YTe ΔTt,k ve,k]T，观测方程可以记作：
mt,k＝hk(xk,θk)，
Figure FDA0002222375730000042
假设在每一个离散时间点k处均可以得到观测，故离散后观测方程为：
mvc，k＝Hkxk+vvc,k
其中，Hk为k时刻速度与加速度观测矩阵，满足：
Figure FDA0002222375730000043
vvc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：
Figure FDA0002222375730000044
其中，σvc,m 2为海流速度观测噪声的标准差。
5.如权利要求4所述的一种水下航行器的水下信标定位方法，其特征在于，所述D步骤中，所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：
Figure FDA0002222375730000045
其中：与Pk-1|k-1分别为k-1时刻的后验状态和后验方差；
Figure FDA0002222375730000048
与Pk|k-1分别为k时刻的先验状态和先验方差。
6.如权利要求5所述的一种水下航行器的水下信标定位方法，其特征在于，所述D步骤中，所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：
Figure FDA00022223757300000511
Figure FDA0002222375730000051
Pk|k＝Pk|k-1-KkHkPk|k-1
其中：Kk为Kalman增益。
7.如权利要求6所述的一种水下航行器的水下信标定位方法，其特征在于，所述步骤E中，通过扩展Kalman滤波及期望最大化算法进行水声信号传递时间校正的方法为：
S1.期望最大化算法求期望；
S1.1 定义目标函数；
Figure FDA0002222375730000052
其中：
Figure FDA0002222375730000053
表示θk在第l次迭代中的近似值；
Figure FDA00022223757300000512
表示相对于x的期望；
Figure FDA0002222375730000054
表示随机变量a的概率密度函数，函数与
Figure FDA0002222375730000055
相关；m1:k表示从1到k时刻的观测量；p(x|y)表示随机变量x以y为条件的条件期望；
S1.2 定义参数估计值解；
在已知与m1:k的前提下，定义下一次迭代的参数估计值为：
Figure FDA0002222375730000057
假设共进行N次迭代，则最终参数估计解为：
Figure FDA0002222375730000058
S1.3 求解联合概率密度；
由于系统观测噪声及过程噪声均为Gauss白噪声，系统联合概率密度可以分解为：
Figure FDA0002222375730000059
由于运动学模型为线性，当k-1时刻系统后验为Gauss分布时，k时刻的先验分布也满足Gauss分布，即：
Figure FDA00022223757300000510
其中，N(x；μ,Σ)表示以μ为均值向量，以Σ为方差矩阵，满足Gauss分布的随机变量x；
S1.4 求解后验状态分布；
对非线性的水声信号传递时间观测方程mt,k＝hk(xk,θk)进行线性化，只保留一阶项，得到：
其中：
Figure FDA0002222375730000062
为观测方程的雅克比矩阵；
由此得到线性化后的水声信号传递时间观测方程为：
Figure FDA0002222375730000063
在已知
Figure FDA0002222375730000064
与m1:k的前提下，记状态后验估计为：
Figure FDA0002222375730000065
选择线性化展开点
Figure FDA0002222375730000066
相应的雅克比矩阵为：
Figure FDA0002222375730000067
其中：
Figure FDA0002222375730000068
根据扩展Kalman滤波流程，可以得到后验估计参数为：
Figure FDA0002222375730000069
Figure FDA00022223757300000610
Figure FDA00022223757300000611
S1.5 求解目标函数；
选择作为线性展开点，根据
Figure FDA00022223757300000613
以及得到：
Figure FDA00022223757300000615
其中：
Figure FDA00022223757300000616
表示与θk无关的常数；
Figure FDA00022223757300000617
Figure FDA00022223757300000618
Figure FDA00022223757300000619
Figure FDA0002222375730000071
根据以上关系，将Fk简化为：
Figure FDA0002222375730000072
其中：
S2.期望最大化算法求极值步骤；
Figure FDA0002222375730000074
为
Figure FDA0002222375730000075
的极大值的必要条件为：
Figure FDA0002222375730000076
计算得到：
Figure FDA0002222375730000077
当时，以上极值条件满足；
令：
Figure FDA0002222375730000079
其中：
Figure FDA00022223757300000710
表示所述水声信标位置坐标与水声声速的名义值，在迭代过程中一直保持不变，通过更新水声信号收发端的时钟漂移ΔTt,k来间接补偿信标位置坐标与水声声速设置误差带来的影响。
CN201910939017.6A 2019-09-30 2019-09-30 一种水下航行器的水下信标定位方法 Expired - Fee Related CN110646783B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910939017.6A CN110646783B (zh) 2019-09-30 2019-09-30 一种水下航行器的水下信标定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910939017.6A CN110646783B (zh) 2019-09-30 2019-09-30 一种水下航行器的水下信标定位方法

Publications (2)

Publication Number Publication Date
CN110646783A true CN110646783A (zh) 2020-01-03
CN110646783B CN110646783B (zh) 2021-07-09

Family

ID=68993345

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910939017.6A Expired - Fee Related CN110646783B (zh) 2019-09-30 2019-09-30 一种水下航行器的水下信标定位方法

Country Status (1)

Country Link
CN (1) CN110646783B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111928850A (zh) * 2020-03-20 2020-11-13 中国科学院沈阳自动化研究所 适用于极地冰架下环境的自主水下机器人的组合导航方法
CN113093092A (zh) * 2021-04-01 2021-07-09 哈尔滨工程大学 一种水下鲁棒自适应单信标定位方法
RU2800186C1 (ru) * 2023-02-17 2023-07-19 Акционерное общество "Концерн "Центральный научно-исследовательский институт "Электроприбор" Способ калибровки лага, установленного на подводном аппарате
CN116609815A (zh) * 2023-07-17 2023-08-18 天津水动力科技有限公司 一种海底航行器智能定位系统及方法
CN117092588A (zh) * 2023-10-20 2023-11-21 中国科学院深海科学与工程研究所 一种估算水声定位系统时钟偏差方法
CN117146830A (zh) * 2023-10-31 2023-12-01 山东科技大学 一种自适应多信标航位推算和长基线的紧组合导航方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103476110A (zh) * 2013-08-21 2013-12-25 中国石油大学(华东) 节点自定位与目标跟踪同时进行的分布式算法
CN103697910A (zh) * 2013-12-14 2014-04-02 浙江大学 自主水下航行器多普勒计程仪安装误差的校正方法
CN104713560A (zh) * 2015-03-31 2015-06-17 西安交通大学 基于期望最大化的多源测距传感器空间配准方法
CN106017467A (zh) * 2016-07-28 2016-10-12 中国船舶重工集团公司第七0七研究所 一种基于多水下应答器的惯性/水声组合导航方法
CN109946972A (zh) * 2019-04-08 2019-06-28 哈尔滨工程大学 基于在线学习模型技术的水下机器人预测控制系统及方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103476110A (zh) * 2013-08-21 2013-12-25 中国石油大学(华东) 节点自定位与目标跟踪同时进行的分布式算法
CN103697910A (zh) * 2013-12-14 2014-04-02 浙江大学 自主水下航行器多普勒计程仪安装误差的校正方法
CN104713560A (zh) * 2015-03-31 2015-06-17 西安交通大学 基于期望最大化的多源测距传感器空间配准方法
CN106017467A (zh) * 2016-07-28 2016-10-12 中国船舶重工集团公司第七0七研究所 一种基于多水下应答器的惯性/水声组合导航方法
CN109946972A (zh) * 2019-04-08 2019-06-28 哈尔滨工程大学 基于在线学习模型技术的水下机器人预测控制系统及方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
HONG-DE QIN等: "Unscented Kalman Filter Based Single Beacon Underwater Localization with Unknown Effective Sound Velocity", 《IEEE XPLORE》 *
JAMES H. KEPPER IV等: "A Navigation Solution Using a MEMS IMU,Model-Based Dead-Reckoning, and One-Way-Travel-Time Acoustic Range Measurements for Autonomous Underwater Vehicles", 《IEEE JOURNAL OF OCEANIC ENGINEERING》 *
YULONG HUANG等: "A New Adaptive Extended Kalman Filter for Cooperative Localization", 《IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS》 *
ZHONGCHAO DENG等: "Adaptive Kalman Filter Based Single Beacon Underwater Tracking With Unknown Effective Sound Velocity", 《IEEE XPLORE》 *
易清明等: "GPS /INS 组合导航中两步自适应滤波方法", 《航天控制》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111928850A (zh) * 2020-03-20 2020-11-13 中国科学院沈阳自动化研究所 适用于极地冰架下环境的自主水下机器人的组合导航方法
CN111928850B (zh) * 2020-03-20 2023-12-29 中国科学院沈阳自动化研究所 适用于极地冰架下环境的自主水下机器人的组合导航方法
CN113093092A (zh) * 2021-04-01 2021-07-09 哈尔滨工程大学 一种水下鲁棒自适应单信标定位方法
RU2800186C1 (ru) * 2023-02-17 2023-07-19 Акционерное общество "Концерн "Центральный научно-исследовательский институт "Электроприбор" Способ калибровки лага, установленного на подводном аппарате
CN116609815A (zh) * 2023-07-17 2023-08-18 天津水动力科技有限公司 一种海底航行器智能定位系统及方法
CN117092588A (zh) * 2023-10-20 2023-11-21 中国科学院深海科学与工程研究所 一种估算水声定位系统时钟偏差方法
CN117092588B (zh) * 2023-10-20 2024-01-09 中国科学院深海科学与工程研究所 一种估算水声定位系统时钟偏差方法
CN117146830A (zh) * 2023-10-31 2023-12-01 山东科技大学 一种自适应多信标航位推算和长基线的紧组合导航方法
CN117146830B (zh) * 2023-10-31 2024-01-26 山东科技大学 一种自适应多信标航位推算和长基线的紧组合导航方法

Also Published As

Publication number Publication date
CN110646783B (zh) 2021-07-09

Similar Documents

Publication Publication Date Title
CN110646783B (zh) 一种水下航行器的水下信标定位方法
CN110794409B (zh) 一种可估计未知有效声速的水下单信标定位方法
CN110749891B (zh) 一种可估计未知有效声速的自适应水下单信标定位方法
CN110779518B (zh) 一种具有全局收敛性的水下航行器单信标定位方法
CN103630137B (zh) 一种用于导航系统的姿态及航向角的校正方法
CN102508278B (zh) 一种基于观测噪声方差阵估计的自适应滤波方法
CN110779519B (zh) 一种具有全局收敛性的水下航行器单信标定位方法
CN110554359B (zh) 一种融合长基线与单信标定位的海底飞行节点定位方法
CN109782289B (zh) 一种基于基线几何结构约束的水下航行器定位方法
US8816896B2 (en) On-board INS quadratic correction method using maximum likelihood motion estimation of ground scatterers from radar data
CN107289932B (zh) 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法
CN104807479A (zh) 一种基于主惯导姿态变化量辅助的惯导对准性能评估方法
CN113433553B (zh) 一种水下机器人多源声学信息融合精确导航方法
CN114777812B (zh) 一种水下组合导航系统行进间对准与姿态估计方法
CN112729291A (zh) 一种深潜长航潜水器sins/dvl洋流速度估计方法
CN108983216A (zh) 一种基于坐标校准的机载雷达微弱目标检测前跟踪方法
CN117146830B (zh) 一种自适应多信标航位推算和长基线的紧组合导航方法
Zorina et al. Enhancement of INS/GNSS integration capabilities for aviation-related applications
CN113155134B (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
CN110471096A (zh) 一种分布式海底飞行节点群体定位方法
CN111708008B (zh) 一种基于imu和tof的水下机器人单信标导航方法
CN109471102B (zh) 一种惯组误差修正方法
CN111829511A (zh) 一种基于m估计的auv组合导航方法及系统
CN113093092B (zh) 一种水下鲁棒自适应单信标定位方法
Siebler et al. Bounding ins positioning errors with magnetic-field-signatures in railway environments

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
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20210709