CN109765523A - 基于自适应akf的单应答器斜距水声定位方法及系统 - Google Patents
基于自适应akf的单应答器斜距水声定位方法及系统 Download PDFInfo
- Publication number
- CN109765523A CN109765523A CN201811574862.XA CN201811574862A CN109765523A CN 109765523 A CN109765523 A CN 109765523A CN 201811574862 A CN201811574862 A CN 201811574862A CN 109765523 A CN109765523 A CN 109765523A
- Authority
- CN
- China
- Prior art keywords
- matrix
- observation
- transponder
- positioning
- adaptive
- 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
- 238000000034 method Methods 0.000 title claims abstract description 52
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 50
- 238000005259 measurement Methods 0.000 claims abstract description 53
- 239000011159 matrix material Substances 0.000 claims description 115
- 238000001914 filtration Methods 0.000 claims description 44
- 238000012937 correction Methods 0.000 claims description 14
- 230000004044 response Effects 0.000 claims description 9
- 230000003595 spectral effect Effects 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 230000004927 fusion Effects 0.000 abstract description 5
- 238000004891 communication Methods 0.000 abstract description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 abstract description 2
- 230000004807 localization Effects 0.000 abstract 1
- 230000000644 propagated effect Effects 0.000 abstract 1
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000011084 recovery Methods 0.000 description 3
- -1 alkyl ketene dimer Chemical compound 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 230000009189 diving Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000001902 propagating effect Effects 0.000 description 1
- 238000004064 recycling Methods 0.000 description 1
- 239000013535 sea water Substances 0.000 description 1
- 230000005236 sound signal Effects 0.000 description 1
Landscapes
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明属于水声通信导航领域,涉及一种基于自适应AKF的单应答器斜距水声定位方法及系统。该系统由应答器、声信号收发器、多普勒流速计、姿态传感器、深度传感器、自适应卡尔曼滤波控制器组成。基于自适应卡尔曼滤波(AKF)算法,整合水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息,且考虑各个传感器的噪声、时间延迟、更新频率和多传感器信息融合,建立水下航行器的单一应答器单一斜距定位方法,计算出水下航行器的三维坐标,实现精确定位。本发明的方法可节省应答器布放与定位作业成本,能够克服受限于声信号在水下传播的速度与距离,斜距测量更新率较慢,与深度、姿态测量信息的更新不同步问题,从而提高定位效率和精度。
Description
技术领域
本发明属于水声通信导航领域,涉及一种基于自适应AKF的单应答器斜距水声定位方法及系统。
背景技术
水声定位系统中,常用的主要有长基线、短基线、超短基线等方式,虽然这三种方式能够提供可靠的水下航行器三维坐标,实现定位,但均需要三支以上的应答器,并且需要分别对每支应达器进行定位校正,需要耗费大量作业时间成本。
目前,国内学者进行了一些研究。例如:王熙赢等人提出了一种LBL/MINS组合导航系统及其导航信息融合方法(申请号201510518511.7),该系统包括长基线水下水声定位系统LBL和微惯性导航系统MINS,两系统之间通过扩展卡尔曼滤波器将系统的定位信息进行数据融合,输出最优的导航定位结果。但是长基线水下水声定位系统所需的应答器较多,至少3个以上,系统复杂操作繁琐。
孙晓颖(申请号201410406943.4)等人提出了一种基于近场源定位算法的多目标水声定位方法和系统,该系统将阵列信号处理领域的近场源定位算法应用于水声定位系统中,可以同时估计出角度和距离信息。但该方法距离信息很难提取,对各路接收信号有很严格的限制。
阳凡林(申请号:201510875555.5)等人提出了一种基于等效声速梯度的长基线水声定位方法,该方法不需要准确的声速剖面,有效消除了声速剖面代表性误差的影响。该方法同样属于长基线水下水声定位方法范畴,所需的应答器较多,至少3个以上,系统复杂操作繁琐。
杨明华(申请号:201520453130.0)提出了一种基于多种水声定位技术的水下目标定位系统,该系统包括定位装置、潜水装置和应答装置。该方法使得系统在长基线水声定位和超短基线水声定位两个定位模式中切换,同样需要布放多个海底应答器,系统复杂。
综上,目前公开的现有技术中所公布的技术方案无法降低应答器布放、校正、以及回收的作业成本。因此,亟需一种水声定位方法及系统能够实现降低应答器布放、校正、以及回收的作业成本,并同时达到精确水下航行器定位的目的。
发明内容
针对上述技术问题,本发明提供了一种基于自适应AKF的单应答器斜距水声定位方法及系统,通过多传感器信息融合和自适应卡尔曼滤波算法最优估计,只需一个应答器,降低应答器布放、校正、以及回收的作业成本,并同时达到精确水下航行器定位。
本发明是通过以下技术方案实现的:
基于自适应AKF的单应答器斜距水声定位方法及系统,通过单一应答器、声信号收发器、多普勒流速计、姿态传感器、深度传感器获得水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息,采用自适应卡尔曼滤波来融合测量信息;建立单一斜距定位模型,基于自适应卡尔曼滤波算法,通过迭代运算,根据传感器测量数据修正重新建立程序误差协方差矩阵Qk与观测误差协方差矩阵Rk;采用自适应卡尔曼滤波算法,在没有任何测量数据时,只进行定位预测,直到有任何观测值更新立即进行定位修正。
进一步地,所述方法包括以下步骤:
(1)将水下航行器在地球坐标系统下的位置、速度以及姿态角设为系统状态参数,将单一应答器的测量数据和多普勒流速计、深度计、姿态传感器的测量数据融合;
k时刻系统状态xk与观测值yk表示为:
其中,[X,Y,Z]T为水下航行器在地球坐标下的X,Y,Z方向的位置,[u,v,w]T为水下航行器在地球坐标下的X,Y,Z方向的速度;[ψ,φ,θ]T为水下航行器的艏向角、纵摇角、横摇角;
sr为水下航行器至海底单一应答器之间的斜距;[ud,vd,wd]T分别为水下航行器多普勒流速计所测量到的在参考坐标系Sd下的Xd,Yd,Zd三个方向的速度;
(2)构建单一斜距定位系统模型,系统状态xk的方程表示为:
xk=f(xk-1)+qk-1
其中,f(xk-1)为k-1时刻的系统状态函数,qk-1为k-1时刻的系统状态误差;根据前一时刻,即k-1时刻的系统状态,计算k时刻系统状态xk,表示为:
其中,Δt表示k-1时刻到k时刻的时间差;
根据坐标转换,k时刻观测值yk表示为:
其中,(xT yT zT)为海底单一应答器绝对位置;rk为k时刻观测误差;Tn是由参考坐标系Sd转换到地球坐标系的旋转矩阵;
系统状态xk的k-1时刻的过渡矩阵Ax(mk-1)与观测值yk的观测矩阵Hx(mk)分别表示为:
式中:0m×n代表维度为m×n的零矩阵;In代表维度n×n的单位矩阵,L为对[ψ,φ,θ]T微分的矩阵;
(3)k-1时刻的系统状态误差qk-1与观测误差rk的协方差矩阵分别为Qk-1、Rk;
将rk中的参数设置为应答器、声信号收发器、多普勒流速计、姿态传感器和压力传感器中各个传感器的测量误差;
qk中的参数对系统状态xk的影响以及rk中参数对观测值yk的影响分别独立,将qk的参数以及rk的参数分别称为程序误差协方差矩阵Qk、观测误差协方差矩阵Rk;其中Qk、Rk为对角矩阵;
(4)设定Qk、Rk的初始猜测值,基于自适应卡尔曼滤波算法,通过迭代运算,根据传感器测量数据修正重新建立Qk与Rk;
将系统状态xk微分可得到:
假设动态噪声误差wk的功率谱密度函数Wc为:
Wc=diag([w1,w2,w3,w4,w5,w6])
diag(·)表示(·)的1×n或n×1向量转化为n×n矩阵且对角元素为(·)的向量;
系统状态误差的协方差矩阵Qk为:
(5)观测值非同步更新的定位计算:采用自适应卡尔曼滤波的最佳线性估测性能,在没有任何测量数据时,只进行定位预测,直到有任何观测值更新立即进行定位修正;
依据系统观测矩阵Hx(mk),单一应答器、深度传感器、多普勒流速计、姿态传感器相对应的观测矩阵分别表示如下:
1)斜距观测矩阵
2)深度观测矩阵
Hdepth=[01×2,1,01×6]
3)速度观测矩阵
4)姿态角观测矩阵
Hangle=[03×6 I3]
假设在k时刻系统只收到斜距与深度数据,则将观测值表示为获得观测矩阵并且将观测误差改写为rk=[rrange,rdepth]k,其中rrange,rdepth分别为斜距与深度测量误差。
进一步地,步骤4)中,采用自适应卡尔曼滤波算法,利用前一刻的观测值通过迭代运算,迭代修正程序误差协方差矩阵Qk与观测误差协方差矩阵Rk,具体为:
(1)预估程序误差协方差矩阵Qk
式中:Φ为系统状态的动态矩阵;Γ为动态噪声分布矩阵;Δtk-1=tk-tk-1为时间间隔;Wc为wk-1的功率谱密度函数,表示各个参数随机干扰的程度;
(2)预估系统平均状态
式中:为系统平均状态估计值;mk-1为k-1时刻系统平均状态实际值;Ak-1为tk-1至tk的状态过渡矩阵;
(3)预估状态误差协方差矩阵
式中:为系统状态xk在tk时刻下的误差协方差矩阵的估计值;Pk-1为系统状态xk在tk-1时刻下的误差协方差矩阵,Pk为系统状态xk在tk时刻下的误差协方差矩阵的估计值;为k-1时刻的系统状态过渡矩阵;
(4)ρ为预估自由度:ρ的范围为(0<ρ≤1)
式中:vk代表tk时刻的自由度;n为正整数;
(5)预估尺度矩阵:
式中:为预估的尺度矩阵;
(6)计算卡尔曼增益Kk
(7)计算测量参数的残差
(8)修正预估系统平均状态
其中,代表系统状态观测值;
(9)更新状态误差的协方差矩阵Pk
其中I代表矩阵纬度;
(10)更新自由度vk
(11)更新观测误差协方差矩阵Rk
式中:ak=vk-n-1。
一种单应答器水声组合定位系统,采用所述基于自适应AKF的单应答器斜距水声定位方法及系统进行定位,所述定位系统包括:设置于海底的一个应答器、设置于水下航行器上的声信号收发器、多普勒流速计、姿态传感器、压力传感器、自适应卡尔曼滤波控制器;
所述声信号收发器向所述应答器发射声脉冲信号,所述应答器接收该声脉冲信号后随即回复一应答声脉冲信号,所述声信号收发器将接收到的应答声脉冲信号发送至所述自适应卡尔曼滤波控制器,获得水下航行器至海底单一应答器之间的斜距;并且所述多普勒流速计、所述姿态传感器以及所述压力传感器分别将测量获得的流速信息、水下航行器的姿态信息以及压力信息发送至所述自适应卡尔曼滤波控制器;
所述自适应卡尔曼滤波控制器基于自适应AKF的单应答器斜距水声定位方法及系统对水下航行器进行精确定位。
本发明的有益技术效果:
1)本发明所提供的基于自适应AKF的单应答器斜距水声定位方法及系统,整合水下航行器斜距信息、深度测量信息、速度信息、姿态信息,且考虑各个传感器的噪声、时间延迟、更新频率和多传感器信息融合,建立水下水下航行器的单一斜距定位方法,计算出水下航行器的三维坐标。
2)本发明所提供的单应答器单一斜距水声定位方法,只需一个应答器,降低应答器布放、校正、以及回收的作业成本,并同时达到精确水下航行器定位。
3)本发明所提供的单应答器单一斜距水声定位方法,所用的自适应卡尔曼滤波算法,可使程序误差共变异协方差矩阵Qk与观测误差共变异矩阵Rk可以在算法中利用前一刻的测量信息迭代修正。
4)本发明所提供的单应答器单一斜距水声定位方法,不需要提供精确的误差协方差矩阵Qk和Rk,而是通过迭代运算法更新模型,且计算方程会根据当前的测量数据而改变,进而提高单一斜距定位系统的定位更新效率。
5)本发明所提供的单应答器单一斜距水声定位方法,能够克服受限于声信号在水下传播的速度与距离,斜距测量的更新率较慢,与速度、深度、姿态测量信息的更新不同步问题,从而提高定位效率和精度。
附图说明
图1是本发明实施例中基于AKF的单应答器单一斜距水声定位方法流程图;
图2是本发明实施例中固定时间定位估算流程示意图;
图3是本发明实施例中单应答器斜距测量示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细描述。应当理解,此处所描述的具体实施例仅仅用于解释本发明,并不用于限定本发明。
相反,本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步,为了使公众对本发明有更好的了解,在下文对本发明的细节描述中,详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。
受限于声音在水下传播的速度与传播距离,斜距测量的更新率不快,与DVL、深度计、姿态传感器测量数据的更新也不同步。为降低这些测量因素造成的定位误差,本发明实施例采用自适应卡尔曼滤波来融合测量信息。传统的扩展式卡尔曼滤波(EKF)融合单一斜距与传感器或动态模式等信息来进行水下水下航行器的定位,都必须提供精确的协方差矩阵Qk、Rk,才能够达到良好的定位估算效果,但是实际应用过程中很难精确估算Qk和Rk。基于此,本发明实施例利用自适性卡尔曼滤波(AKF)算法建立单一斜距定位系统,不需要提供精确的Qk、Rk,而是通过迭代运算法更新系统,并且控制方程也会根据当前的测量信息进行修正,进而提升单一斜距定位系统的定位更新效率,并且能够节省应答器布放与定位作业成本、提高定位效率和精度。
本实施例提供一种基于自适应AKF的单应答器斜距水声定位方法及系统,通过单一应答器、声信号收发器、多普勒流速计、姿态传感器、深度传感器获得水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息,采用自适应卡尔曼滤波来融合测量信息;建立单一斜距定位模型,基于自适应卡尔曼滤波算法,通过迭代运算,根据传感器测量数据修正重新建立程序误差协方差矩阵Qk与观测误差协方差矩阵Rk;采用自适应卡尔曼滤波算法,在没有任何测量数据时,只进行定位预测,直到有任何观测值更新立即进行定位修正。
所述方法包括以下步骤:
(1)将水下航行器在地球坐标系统下的位置、速度以及姿态角设为系统状态参数,将单一应答器的测量数据和多普勒流速计、深度计、姿态传感器的测量数据融合;
k时刻系统状态xk与观测值yk表示为:
其中,[X,Y,Z]T为水下航行器在地球坐标下的X,Y,Z方向的位置,[u,v,w]T为水下航行器在地球坐标下的X,Y,Z方向的速度;[ψ,φ,θ]T为水下航行器的艏向角、纵摇角、横摇角;
sr为水下航行器至海底单一应答器之间的斜距;[ud,vd,wd]T分别为水下航行器多普勒流速计所测量到的在参考坐标系Sd下的Xd,Yd,Zd三个方向的速度;
(2)构建单一斜距定位系统模型,系统状态xk的方程表示为:
xk=f(xk-1)+qk-1
其中,f(xk-1)为k-1时刻的系统状态函数,qk-1为k-1时刻的系统状态误差;根据前一时刻,即k-1时刻的系统状态,计算k时刻系统状态xk,表示为:
其中,Δt表示k-1时刻到k时刻的时间差;
根据坐标转换,k时刻观测值yk表示为:
其中,(xT yT zT)为海底单一应答器绝对位置;rk为k时刻观测误差;Tn是由参考坐标系Sd转换到地球坐标系的旋转矩阵;
系统状态xk的k-1时刻的过渡矩阵Ax(mk-1)与观测值yk的观测矩阵Hx(mk)分别表示为:
式中:0m×n代表维度为m×n的零矩阵;In代表维度n×n的单位矩阵,L为对[ψ,φ,θ]T微分的矩阵;
(3)为实现卡尔曼滤波,传统的方式必须提供精确的qk-1与rk的协方差矩阵Qk-1、Rk。但绝大部分情况下难以准确估计其值,本实施例中做如下处理:
k-1时刻的系统状态误差qk-1与观测误差rk的协方差矩阵分别为Qk-1、Rk;
将rk中的参数设置为应答器、声信号收发器、多普勒流速计、姿态传感器和压力传感器中各个传感器的测量误差;
qk中的参数对系统状态xk的影响以及rk中参数对观测值yk的影响分别独立,将qk的参数以及rk的参数分别称为程序误差协方差矩阵Qk、观测误差协方差矩阵Rk,Qk、Rk为对角矩阵;
(4)由于自变量程序误差协方差矩阵Qk、自变量观测误差协方差矩阵Rk对卡尔曼滤波最佳化的结果有重大影响,因此,本发明实施例所述方法使用自适性卡尔曼滤波建立单一斜距定位系统,设定Qk、Rk的初始猜测值,基于自适应卡尔曼滤波算法,通过迭代运算,根据传感器测量数据修正重新建立Qk与Rk;
将系统状态xk微分可得到:
假设动态噪声误差wk的功率谱密度函数Wc为:
Wc=diag([w1,w2,w3,w4,w5,w6])
diag(·)表示(·)的1×n或n×1向量转化为n×n矩阵且对角元素为(·)的向量;
系统状态误差的的协方差矩阵Qk为:
(5)观测值非同步更新的定位计算:卡尔曼滤波融合单一应答器与DVL、深度计、姿态传感器的测量信息,建立的单一斜距系统,各个传感器测量的信息更新频率不同步,收到测量信息的时间不一致,且存在数据中断的问题。单一斜距定位系统必须等到所有的观测值全部更新之后才进行定位估算,必然会使得单一斜距定位系统运算延迟,导致无法快速有效地修正水下航行器状态。
为了提升单一斜距定位系统的修正效率,本实施例所述方法采用自适应卡尔曼滤波的最佳线性估测性能,在没有任何测量数据时,只进行定位预测,直到有任何观测值更新立即进行定位修正;
依据系统观测矩阵Hx(mk),单一应答器、深度传感器、多普勒流速计、姿态传感器相对应的观测矩阵分别表示如下:
1)斜距观测矩阵
2)深度观测矩阵
Hdepth=[01×2,1,01×6]
3)速度观测矩阵
4)姿态角观测矩阵
Hangle=[03×6 I3]
假设在k时刻系统只收到斜距与深度数据,则将观测值表示为获得观测矩阵并且将观测误差改写为rk=[rrange,rdepth]k,其中rrange,rdepth分别为斜距与深度测量误差。
因此单一斜距定位系统可以在任何给定的时间进行估算,只要取得观测值便可以进行定位修正。
其中,在本实施例,步骤4)中,采用自适应卡尔曼滤波算法,利用前一刻的观测值通过迭代运算,迭代修正程序误差协方差矩阵Qk与观测误差协方差矩阵Rk,如图1所示,具体为:
(1)预估程序误差的协方差矩阵Qk
式中:Φ为系统状态的动态矩阵;Γ为动态噪声分布矩阵;Δtk-1=tk-tk-1为时间间隔;Wc为wk-1的功率谱密度函数,表示各个参数随机干扰的程度;
(2)预估系统平均状态
式中:为系统平均状态估计值;mk-1为k-1时刻系统平均状态实际值;Ak-1为tk-1至tk的状态过渡矩阵;
(3)预估状态误差协方差矩阵
式中:为系统状态xk在tk时刻下的误差协方差矩阵的估计值;Pk-1为系统状态xk在tk-1时刻下的误差协方差矩阵,Pk为系统状态xk在tk时刻下的误差协方差矩阵的估计值;为k-1时刻的系统状态过渡矩阵;
(4)qk预估自由度:ρ的范围为(0<ρ≤1)
式中:vk代表tk时刻的自由度;n为正整数;
(5)预估尺度矩阵:
式中:为预估的尺度矩阵;
(6)计算卡尔曼增益Kk
(7)计算测量参数的残差
(8)修正预估系统平均状态
其中,代表系统状态观测值;
(9)更新状态误差的协方差矩阵Pk
其中,I代表矩阵纬度;
(10)更新自由度vk
(11)更新观测误差协方差矩阵Rk
式中:ak=vk-n-1
由上述计算步骤可看出,自适性卡尔曼滤波算法中与离散卡尔曼滤波算法的不同在于自适性卡尔曼滤波的程序误差共变异协方差矩阵Qk与观测误差共变异矩阵Rk可以在算法中利用前一刻的测量信息迭代修正。如图2所示,以每1秒进行定位估算为例,在第1秒内没有测量数据时,只进行定位估算,当在1.3s取得测量数据时,则通过时变的观测协方差矩阵即时的修正定位估算状态,到达第2秒时,数据已进行更新修正,提高定位精度。
本实施例所提供的方法由应答器、声信号收发器、多普勒流速计、姿态传感器、压力传感器、自适应卡尔曼滤波控制器组成。基于自适应卡尔曼滤波(AKF)算法,整合水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息,且考虑各个传感器的噪声、时间延迟、更新频率和多传感器信息融合,建立水下水下航行器的单一斜距定位方法,计算出水下航行器的三维坐标,实现精确定位。
受限于声音在水下传播的速度与距离,斜距测量的更新率不快,与速度、深度、姿态测量信息的更新也不同步,为降低这些测量因素造成的定位误差,本实施例所述方法采用自适应卡尔曼滤波融合测量资料,利用迭代与递归程序达到最佳估测。
本实施例所提供的基于自适性卡尔曼滤波(AKF)所建立的单一斜距定位方法,不需要提供精确的误差协方差矩阵Qk和Rk,而是通过迭代运算法更新模型,且测量方程会根据当前的测量数据而改变,进而提高单一斜距定位系统的定位更新效率。
本实施例还提供一种单应答器水声组合定位系统,采用上述基于自适应AKF的单应答器斜距水声定位方法及系统进行定位,所述定位系统包括:设置于海底的一个应答器、设置于水下航行器上的声信号收发器、多普勒流速计、姿态传感器、压力传感器、自适应卡尔曼滤波控制器;
所述声信号收发器向所述应答器发射声脉冲信号,所述应答器接收该声脉冲信号后随即回复一应答声脉冲信号,所述声信号收发器将接收到的应答声脉冲信号发送至所述自适应卡尔曼滤波控制器,获得水下航行器至海底单一应答器之间的斜距;并且所述多普勒流速计、所述姿态传感器以及所述压力传感器分别将测量获得的流速信息、水下航行器的姿态信息以及压力信息发送至所述自适应卡尔曼滤波控制器;
所述自适应卡尔曼滤波控制器基于自适应AKF的单应答器斜距水声定位方法及系统对水下航行器进行精确定位。
其中,单应答器斜距测量原理如图3所示,水下航行器发射机通过安置于底部的换能器向水下应答器(经高精度绝对校准且位置已知)发射声脉冲信号(即询问信号),应答器接收该信号后随即回复一应答声脉冲信号,水下航行器内部接收机记录发射询问信号和接收应答信号的时间间隔的表达式为
式中:C为声波在海水中的速度;t为声信号在水下航行器与海底应答器之间的双程传播往返时间总和。
通过该表达式即可算出水下航行器至海底应答器之间的斜距sr;由于应答器的深度z已知,因此,水下航行器至应答器之间的水平距离d可表示为:
应当理解的是,对本领域普通技术人员来说,可以根据上述说明加以改进或变换,而所有这些改进和变换都应属于本发明所附权利要求的保护范围。
Claims (4)
1.基于自适应AKF的单应答器斜距水声定位方法,其特征在于,通过单一应答器、声信号收发器、多普勒流速计、姿态传感器、深度传感器获得水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息,采用自适应卡尔曼滤波来融合测量信息;建立单一斜距定位模型,基于自适应卡尔曼滤波算法,通过迭代运算,根据传感器测量数据修正重新建立程序误差协方差矩阵Qk与观测误差协方差矩阵Rk;采用自适应卡尔曼滤波算法,在没有任何测量数据时,只进行定位预测,直到有任何观测值更新时立即进行定位修正。
2.根据权利要求1所述基于自适应AKF的单应答器斜距水声定位方法,其特征在于,所述方法包括以下步骤:
(1)将水下航行器在地球坐标系统下的位置、速度以及姿态角设为系统状态参数,将单一应答器测量的斜距信息数据和多普勒流速计、深度计、姿态传感器的测量数据融合;
k时刻系统状态xk与观测值yk表示为:
其中,[X,Y,Z]T为水下航行器在地球坐标下的X,Y,Z方向的位置,[u,v,w]T为水下航行器在地球坐标下的X,Y,Z方向的速度;[ψ,φ,θ]T为水下航行器的艏向角、纵摇角、横摇角;
sr为水下航行器至海底单一应答器之间的斜距;[ud,vd,wd]T分别为水下航行器多普勒流速计所测量到的在参考坐标系Sd下的Xd,Yd,Zd三个方向的速度;
(2)构建单一斜距定位系统模型,系统状态xk的方程表示为:
xk=f(xk-1)+qk-1
其中,f(xk-1)为k-1时刻的系统状态函数,qk-1为k-1时刻的系统状态误差;根据前一时刻,即k-1时刻的系统状态,计算k时刻系统状态xk,表示为:
其中,Δt表示k-1时刻到k时刻的时间差;
根据坐标转换,k时刻观测值yk表示为:
其中,(xT yT zT)为海底单一应答器绝对位置;rk为k时刻观测误差;Tn是由参考坐标系Sd转换到地球坐标系的旋转矩阵;
系统状态xk的k-1时刻的过渡矩阵Ax(mk-1)与观测值yk的观测矩阵Hx(mk)分别表示为:
式中:0m×n代表维度为m×n的零矩阵;In代表维度n×n的单位矩阵,L为对[ψ,φ,θ]T微分的矩阵;
(3)k-1时刻的系统状态误差qk-1与观测误差rk的协方差矩阵分别为Qk-1、Rk;
将rk中的参数设置为应答器、声信号收发器、多普勒流速计、姿态传感器和压力传感器中各个传感器的测量误差;
qk中的参数对系统状态xk的影响以及rk中参数对观测值yk的影响分别独立,将qk的参数以及rk的参数分别称为程序误差协方差矩阵Qk、观测误差协方差矩阵Rk;其中Qk、Rk为对角矩阵;
(4)设定Qk、Rk的初始猜测值,基于自适应卡尔曼滤波算法,通过迭代运算,根据传感器测量数据修正重新建立Qk与Rk;
将系统状态xk微分可得到:
假设动态噪声误差wk的功率谱密度函数Wc为:
Wc=diag([w1,w2,w3,w4,w5,w6])
diag(·)表示(·)的1×n或n×1向量转化为n×n矩阵且对角元素为(·)的向量;
系统状态误差的协方差矩阵Qk为:
(5)观测值非同步更新的定位计算:采用自适应卡尔曼滤波的最佳线性估测性能,在没有任何测量数据时,只进行定位预测,直到有任何观测值更新立即进行定位修正;
依据系统观测矩阵Hx(mk),单一应答器、深度传感器、多普勒流速计、姿态传感器相对应的观测矩阵分别表示如下:
1)斜距观测矩阵
2)深度观测矩阵
Hdepth=[01×2,1,01×6]
3)速度观测矩阵
4)姿态角观测矩阵
Hangle=[03×6 I3]
假设在k时刻系统只收到斜距与深度数据,则将观测值表示为获得观测矩阵并且将观测误差改写为rk=[rrange,rdepth]k,其中rrange,rdepth分别为斜距与深度测量误差。
3.根据权利要求1所述基于自适应AKF的单应答器斜距水声定位方法,其特征在于,
步骤4)中,采用自适应卡尔曼滤波算法,利用前一刻的观测值通过迭代运算,迭代修正程序误差协方差矩阵Qk与观测误差协方差矩阵Rk,具体为:
(1)预估程序误差协方差矩阵Qk
式中:Φ为系统状态的动态矩阵;Γ为动态噪声分布矩阵;Δtk-1=tk-tk-1为时间间隔;Wc为wk-1的功率谱密度函数,表示各个参数随机干扰的程度;
(2)预估系统平均状态
式中:为系统平均状态估计值;mk-1为k-1时刻系统平均状态实际值;Ak-1为tk-1至tk的状态过渡矩阵;
(3)预估状态误差协方差矩阵
式中:为系统状态xk在tk时刻下的误差协方差矩阵的估计值;Pk-1为系统状态xk在tk-1时刻下的误差协方差矩阵,Pk为系统状态xk在tk时刻下的误差协方差矩阵的估计值;为k-1时刻的系统状态过渡矩阵;
(4)ρ为预估自由度:ρ的范围为(0<ρ≤1)
式中:vk代表tk时刻的自由度;n为正整数;
(5)预估尺度矩阵:
式中:为预估的尺度矩阵;
(6)计算卡尔曼增益Kk
(7)计算测量参数的残差
(8)修正预估系统平均状态
其中,代表系统状态观测值;
(9)更新状态误差的协方差矩阵Pk
其中,I代表矩阵纬度;
(10)更新自由度vk
(11)更新观测误差协方差矩阵Rk
式中:ak=vk-n-1。
4.一种基于自适应AKF的单应答器斜距水声定位系统,采用权利要求1-3任一项所述基于自适应AKF的单应答器斜距水声定位方法进行定位,其特征在于,所述定位系统包括:设置于海底的一个应答器、设置于水下航行器上的声信号收发器、多普勒流速计、姿态传感器、压力传感器、自适应卡尔曼滤波控制器;
所述声信号收发器向所述应答器发射声脉冲信号,所述应答器接收该声脉冲信号后随即回复一应答声脉冲信号,所述声信号收发器将接收到的应答声脉冲信号发送至所述自适应卡尔曼滤波控制器,获得水下航行器至海底单一应答器之间的斜距;并且所述多普勒流速计、所述姿态传感器以及所述压力传感器分别将测量获得的流速信息、水下航行器的姿态信息以及压力信息发送至所述自适应卡尔曼滤波控制器;
所述自适应卡尔曼滤波控制器基于自适应AKF的单应答器斜距水声定位方法对水下航行器进行精确定位。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811574862.XA CN109765523B (zh) | 2018-12-21 | 2018-12-21 | 基于自适应akf的单应答器斜距水声定位方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811574862.XA CN109765523B (zh) | 2018-12-21 | 2018-12-21 | 基于自适应akf的单应答器斜距水声定位方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109765523A true CN109765523A (zh) | 2019-05-17 |
CN109765523B CN109765523B (zh) | 2022-11-01 |
Family
ID=66452064
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811574862.XA Active CN109765523B (zh) | 2018-12-21 | 2018-12-21 | 基于自适应akf的单应答器斜距水声定位方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109765523B (zh) |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110186454A (zh) * | 2019-06-12 | 2019-08-30 | 海南大学 | 一种水下巡检设备及其姿态传感器的滤波方法 |
CN110471096A (zh) * | 2019-09-11 | 2019-11-19 | 哈尔滨工程大学 | 一种分布式海底飞行节点群体定位方法 |
CN110554359A (zh) * | 2019-09-11 | 2019-12-10 | 哈尔滨工程大学 | 一种融合长基线与单信标定位的海底飞行节点定位方法 |
CN110873813A (zh) * | 2019-12-02 | 2020-03-10 | 中国人民解放军战略支援部队信息工程大学 | 一种水流速度估算方法、组合导航方法及装置 |
CN111323752A (zh) * | 2020-03-25 | 2020-06-23 | 哈尔滨工程大学 | 远近场过渡区间声源定位方法 |
CN111829512A (zh) * | 2020-06-08 | 2020-10-27 | 中国航天空气动力技术研究院 | 一种基于多传感器数据融合的auv导航定位方法及系统 |
CN111896962A (zh) * | 2020-07-25 | 2020-11-06 | 中国石油大学(华东) | 一种海底应答器定位方法、系统、存储介质及应用 |
CN112363169A (zh) * | 2020-10-27 | 2021-02-12 | 哈尔滨工程大学 | 全海深水下机器人及其定位方法 |
CN113156413A (zh) * | 2021-04-28 | 2021-07-23 | 哈尔滨工程大学 | 一种基于双程声路径的海底基准校准方法 |
CN113311388A (zh) * | 2021-05-27 | 2021-08-27 | 湖南国天电子科技有限公司 | 一种水下机器人的超短基线定位系统 |
CN116125380A (zh) * | 2023-04-19 | 2023-05-16 | 齐鲁工业大学(山东省科学院) | 一种基于卡尔曼滤波器的移动场景超分辨定位方法 |
CN116592896A (zh) * | 2023-07-17 | 2023-08-15 | 山东水发黄水东调工程有限公司 | 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
AU2000234653B2 (en) * | 2000-03-20 | 2004-10-21 | Francois Bernard | Apparatus for deploying a load to an underwater target position with enhanced accuracy and a method to control such apparatus |
US20070280051A1 (en) * | 2006-06-06 | 2007-12-06 | Novick Arnold W | Methods and systems for passive range and depth localization |
CN101441267A (zh) * | 2008-12-30 | 2009-05-27 | 哈尔滨工程大学 | 海底应答器绝对位置三测点高精度标校方法 |
CN101833081A (zh) * | 2010-04-23 | 2010-09-15 | 哈尔滨工程大学 | 深海海底应答器绝对位置的精确标校方法 |
CN104181505A (zh) * | 2014-08-18 | 2014-12-03 | 吉林大学 | 一种基于近场源定位算法的多目标水声定位方法和系统 |
CN104316045A (zh) * | 2014-11-06 | 2015-01-28 | 东南大学 | 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法 |
CN106017467A (zh) * | 2016-07-28 | 2016-10-12 | 中国船舶重工集团公司第七0七研究所 | 一种基于多水下应答器的惯性/水声组合导航方法 |
CN107289933A (zh) * | 2017-06-28 | 2017-10-24 | 东南大学 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
CN108759864A (zh) * | 2018-05-17 | 2018-11-06 | 中国人民解放军海军工程大学 | 基于单应答器的水下捷联惯导系统初始对准方法 |
CN108931799A (zh) * | 2018-07-18 | 2018-12-04 | 兰州交通大学 | 基于递归快速正交搜索的列车组合定位方法及系统 |
-
2018
- 2018-12-21 CN CN201811574862.XA patent/CN109765523B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
AU2000234653B2 (en) * | 2000-03-20 | 2004-10-21 | Francois Bernard | Apparatus for deploying a load to an underwater target position with enhanced accuracy and a method to control such apparatus |
US20070280051A1 (en) * | 2006-06-06 | 2007-12-06 | Novick Arnold W | Methods and systems for passive range and depth localization |
CN101441267A (zh) * | 2008-12-30 | 2009-05-27 | 哈尔滨工程大学 | 海底应答器绝对位置三测点高精度标校方法 |
CN101833081A (zh) * | 2010-04-23 | 2010-09-15 | 哈尔滨工程大学 | 深海海底应答器绝对位置的精确标校方法 |
CN104181505A (zh) * | 2014-08-18 | 2014-12-03 | 吉林大学 | 一种基于近场源定位算法的多目标水声定位方法和系统 |
CN104316045A (zh) * | 2014-11-06 | 2015-01-28 | 东南大学 | 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法 |
CN106017467A (zh) * | 2016-07-28 | 2016-10-12 | 中国船舶重工集团公司第七0七研究所 | 一种基于多水下应答器的惯性/水声组合导航方法 |
CN107289933A (zh) * | 2017-06-28 | 2017-10-24 | 东南大学 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
CN108759864A (zh) * | 2018-05-17 | 2018-11-06 | 中国人民解放军海军工程大学 | 基于单应答器的水下捷联惯导系统初始对准方法 |
CN108931799A (zh) * | 2018-07-18 | 2018-12-04 | 兰州交通大学 | 基于递归快速正交搜索的列车组合定位方法及系统 |
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110186454A (zh) * | 2019-06-12 | 2019-08-30 | 海南大学 | 一种水下巡检设备及其姿态传感器的滤波方法 |
CN110471096A (zh) * | 2019-09-11 | 2019-11-19 | 哈尔滨工程大学 | 一种分布式海底飞行节点群体定位方法 |
CN110554359A (zh) * | 2019-09-11 | 2019-12-10 | 哈尔滨工程大学 | 一种融合长基线与单信标定位的海底飞行节点定位方法 |
CN110873813A (zh) * | 2019-12-02 | 2020-03-10 | 中国人民解放军战略支援部队信息工程大学 | 一种水流速度估算方法、组合导航方法及装置 |
CN111323752A (zh) * | 2020-03-25 | 2020-06-23 | 哈尔滨工程大学 | 远近场过渡区间声源定位方法 |
CN111323752B (zh) * | 2020-03-25 | 2022-10-14 | 哈尔滨工程大学 | 远近场过渡区间声源定位方法 |
CN111829512A (zh) * | 2020-06-08 | 2020-10-27 | 中国航天空气动力技术研究院 | 一种基于多传感器数据融合的auv导航定位方法及系统 |
CN111829512B (zh) * | 2020-06-08 | 2024-04-09 | 中国航天空气动力技术研究院 | 一种基于多传感器数据融合的auv导航定位方法及系统 |
CN111896962B (zh) * | 2020-07-25 | 2022-10-04 | 中国石油大学(华东) | 一种海底应答器定位方法、系统、存储介质及应用 |
CN111896962A (zh) * | 2020-07-25 | 2020-11-06 | 中国石油大学(华东) | 一种海底应答器定位方法、系统、存储介质及应用 |
CN112363169B (zh) * | 2020-10-27 | 2022-12-13 | 哈尔滨工程大学 | 全海深水下机器人及其定位方法 |
CN112363169A (zh) * | 2020-10-27 | 2021-02-12 | 哈尔滨工程大学 | 全海深水下机器人及其定位方法 |
CN113156413A (zh) * | 2021-04-28 | 2021-07-23 | 哈尔滨工程大学 | 一种基于双程声路径的海底基准校准方法 |
CN113311388A (zh) * | 2021-05-27 | 2021-08-27 | 湖南国天电子科技有限公司 | 一种水下机器人的超短基线定位系统 |
CN113311388B (zh) * | 2021-05-27 | 2023-08-08 | 湖南国天电子科技有限公司 | 一种水下机器人的超短基线定位系统 |
CN116125380A (zh) * | 2023-04-19 | 2023-05-16 | 齐鲁工业大学(山东省科学院) | 一种基于卡尔曼滤波器的移动场景超分辨定位方法 |
CN116592896A (zh) * | 2023-07-17 | 2023-08-15 | 山东水发黄水东调工程有限公司 | 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法 |
CN116592896B (zh) * | 2023-07-17 | 2023-09-29 | 山东水发黄水东调工程有限公司 | 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109765523B (zh) | 2022-11-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109765523B (zh) | 基于自适应akf的单应答器斜距水声定位方法及系统 | |
CN109324330B (zh) | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 | |
CN109443379A (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN105319534B (zh) | 一种基于水声双程测距的多auv协同定位方法 | |
US20160124081A1 (en) | Metrology method and device for calibrating the geometry of a network of underwater acoustic beacons | |
CN110057365B (zh) | 一种大潜深auv下潜定位方法 | |
CN112284384B (zh) | 考虑量测异常的集群式多深海潜航器的协同定位方法 | |
CN111595348B (zh) | 一种自主水下航行器组合导航系统的主从式协同定位方法 | |
CN103323815B (zh) | 一种基于等效声速的水下声学定位方法 | |
CN112729291B (zh) | 一种深潜长航潜水器sins/dvl洋流速度估计方法 | |
KR101789188B1 (ko) | 수중 이동체의 위치 추적을 위한 수중 통합 항법 시스템 | |
CN116222578B (zh) | 基于自适应滤波和最优平滑的水下组合导航方法及系统 | |
CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
Xu et al. | Accurate two-step filtering for AUV navigation in large deep-sea environment | |
CN110132281B (zh) | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 | |
Choi et al. | Broadband acoustic communication aided underwater inertial navigation system | |
CN110389318B (zh) | 一种基于立体六元阵的水下移动平台定位系统及方法 | |
Guo et al. | A robust SINS/USBL integrated navigation algorithm based on earth frame and right group error definition | |
CN113156442A (zh) | 一种基于长基线水声系统辅助导航的auv水下定位方法 | |
NO339643B1 (no) | Fremgangsmåte for vinkelmessig kalibrering av antenne ved måling av relativ distanse | |
CN111708008B (zh) | 一种基于imu和tof的水下机器人单信标导航方法 | |
CN110873813B (zh) | 一种水流速度估算方法、组合导航方法及装置 | |
CN117146830B (zh) | 一种自适应多信标航位推算和长基线的紧组合导航方法 | |
CN113688562A (zh) | 一种基于ga算法的浅水多波束褶皱误差自动改正方法 | |
CN103697887B (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |