一种基于超短基线和航位推算的水下导航定位方法技术

本发明专利技术涉及一种基于超短基线和航位推算的水下导航定位方法，包括以下步骤：S1、确定超短基线系统的定位原理；S2、确定超短基线系统的误差模型；S3、建立状态模型和观测模型；S4、建立考虑方向角量测偏差变化的扩展卡尔曼滤波算法。本发明专利技术能有效地适应方向角量测噪声变化的情形，在小方向角下也能保持较高的定位精度。

A method of underwater navigation and positioning based on ultra short baseline and dead reckoning

【技术实现步骤摘要】
一种基于超短基线和航位推算的水下导航定位方法
本专利技术涉及水下定位与导航
，更具体地说，涉及一种基于超短基线和航位推算的水下导航定位方法。
技术介绍
在水下航行器如ROV(RemoteOperatedVehicle)、AUV(AutonomousUnderwaterVehicle)作业过程中，对其进行高精度地导航定位是一项十分重要的操作。作为一种多源信息融合算法，卡尔曼滤波(KalmanFiltering)已被广泛应用于水下航行器的导航定位中。基于超短基线(UltrashortBaseLine，简称USBL)和航位推算(Deadreckoning，简称DR)的扩展卡尔曼滤波可以融合超短基线系统的观测数据和航位推算信息，得到水下目标位置的最优估计。精确的滤波模型和误差统计特性是卡尔曼滤波最优估计的基础。对于平面超短基线系统，其方向角量测偏差与方向角自身相关。而传统基于超短基线和航位推算的扩展卡尔曼滤波算法中，超短基线系统的方向角量测偏差常看作定值，这导致在小方向角下，其滤波定位精度较低。考虑到船舶摇晃或水下目标自主移动等导致方向角量测偏差发生变化的因素，本专利技术提出一种考虑方向角量测偏差变化、基于超短基线和航位推算的水下导航定位方法。
技术实现思路
(一)要解决的技术问题为了解决传统基于超短基线和航位推算的扩展卡尔曼滤波算法在小方向角下定位精度低的问题，本专利技术提供一种考虑方向角量测偏差变化、基于超短基线和航位推算的水下导航定位方法。(二)技术方案为了达到上述目的，本专利技术采用的主要技术方案包括：设计一种基于超短基线和航位推算的水下导航定位方法，包括以下步骤：S1、确定超短基线系统的定位原理；S2、确定超短基线系统的误差模型；S3、建立状态模型和观测模型；S4、建立考虑方向角量测偏差变化的扩展卡尔曼滤波算法。在上述方案中，在所述步骤S4中，建立考虑方向角量测偏差变化的卡尔曼滤波算法，包括以下步骤：S4-1、根据上一时刻后验估计位置以及其后验协方差，通过状态模型确定此时刻水下目标的先验估计位置以及其先验协方差；S4-2、根据上一时刻的后验估计位置，通过超短基线系统的误差模型确定此时刻的观测噪声矩阵；S4-3、根据此时刻观测噪声矩阵，确定增益；S4-4、根据量测方程确定新息；S4-5、根据增益及新息确定此时刻水下目标的后验估计位置以及其后验协方差；S4-6、重复上述步骤，获得水下航行器的实时位置。在上述方案中，在所述步骤S1中，换能器O布置在船体空间坐标系O-XYZ的原点位置，水听器S1、S2、S3、S4在船体空间坐标系XOY平面中心对称布置，其中，S1、S3阵元位于指向船艏的X轴上，S2、S4阵元位于垂直于船艏的Y轴上；阵元间距为d，即S1与S3间距、S2与S4间距；搭载声学应答器的水下目标位于T点，其在船体空间坐标系下坐标为(x,y,z)；超短基线水声定位系统利用水听器接收信号之间的时延差T和相位差△φx,△φy解算水下目标的距离r：r＝CT/2(1)和方位：θx＝arcos(λ△φx/2πd),θy＝arcos(λ△φy/2πd)(2)再根据公式(1)和公式(2)解算水下目标位置：其中，公式(1)～公式(3)中，C为水下声速，f为换能器收发频率。在上述方案中，在所述步骤S2中，对水下目标位置偏微分并求标准差：忽略声波长误差σλ、基阵安装误差σd，测向误差依据克拉美罗下界取最优，简化后的方向角θx,θy偏差模型为在上述方案中，在所述步骤S3中，规定水下目标的运动状态由对地航向、对地航速和升沉速度三个参数决定：公式(6)中，x(k),y(k),z(k)为k时刻水下目标相对换能器位置，COG(k)(CourseOfGround),SOG(k)(SpeedOfGround),△z(k)分别为k时刻水下目标的对地航向、对地航速以及升沉速度，εCOG(k),εSOG(k),ε△z(k)分别为k时刻水下目标的对地航向控制偏差、对地速度控制偏差以及升沉速度控制偏差，△t为k时刻与k+1时刻的时间间隔；k+1时刻水下目标的状态方程为：超短基线系统对水下目标的位置进行观测，公式(8)～(9)中，r(k)、θx(k)、θy(k)分别为k时刻超短基线系统的观测斜距、x方向上的方向角、y方向的方向角；δr(k)、分别为k时刻的观测斜距偏差、x方向上的方向角偏差、y方向的方向角偏差，(a,b,c)为换能器的绝对坐标，则量测方程为：公式(10)中，r(k)、θx(k)、θy(k)、dep(k)分别为k时刻超短基线系统的观测斜距、x方向上的方向角、y方向的方向角、深度计测量值；δr(k)、分别为k时刻的观测斜距偏差、x方向上的方向角偏差、y方向的方向角偏差。在上述方案中，在所述步骤4中，建立考虑方向角量测偏差变化的扩展卡尔曼滤波算法：F(k+1)为x(k+1)＝g(x(k),u(k),ε(k))关于k+1时刻的水下目标位置的雅可比矩阵，其表达式为：Q(k)为k时刻状态向量的噪声矩阵，将过程噪声看作定值：H(k+1)为观测方程z(k)＝h(x(k),δ(k))相对于水下目标位置坐标的雅可比矩阵：l,l1,l2为构造辅助变量：R(k+1)为k+1时刻超短基线系统观测向量的噪声矩阵；设定各观测向量相互独立，且观测斜距偏差为定值，将观测噪声矩阵分解为以下形式：公示(16)中，Σ(k+1)为观测噪声调节因子，反映水下目标相对声阵方位变化对观测偏差的影响；R为系统最小观测偏差；物体运动的连续性使得相邻两时刻的θx与θy相差不大，用上一时刻的滤波值作为此时刻的真值：根据下式取得：滤波增益K修正为：扩展卡尔曼滤波框架调整为：(三)有益效果本专利技术的有益效果是：本专利技术利用上一时刻的滤波所得的方向角近似构建下一时刻的方向角量测噪声，适应性地根据水下目标与水听器阵的相对位置变化，即方向角变化，调整超短基线系统的观测值在扩展卡尔曼滤波波值中所占权重。本专利技术能有效地适应方向角量测噪声变化的情形，在小方向角下也能保持较高的定位精度。附图说明图l为超短基线系统的定位原理示意图；图2为滤波框架示意图；图3为滤波效果示意图。具体实施方式为了更好的解释本专利技术，以便于理解，下面结合附图，通过具体实施方式，对本专利技术作详细描述。本专利技术提供一种考虑方向角量测偏差变化、基于超短基线和航位推算的水下导航定位方法，包括以下步骤：S1、确定超短基线系统的定位原理：如图1，换能器O布置在船体空间坐标系(O-XYZ)的原点位置。水听器S1、S2、S3、S本文档来自技高网...

【技术保护点】
1.一种基于超短基线和航位推算的水下导航定位方法，其特征在于，包括以下步骤：/nS1、确定超短基线系统的定位原理；/nS2、确定超短基线系统的误差模型；/nS3、建立状态模型和观测模型；/nS4、建立考虑方向角量测偏差变化的扩展卡尔曼滤波算法。/n

【技术特征摘要】
1.一种基于超短基线和航位推算的水下导航定位方法，其特征在于，包括以下步骤：
S1、确定超短基线系统的定位原理；
S2、确定超短基线系统的误差模型；
S3、建立状态模型和观测模型；
S4、建立考虑方向角量测偏差变化的扩展卡尔曼滤波算法。


2.如权利要求1所述的一种基于超短基线和航位推算的水下导航定位方法，其特征在于：在所述步骤S4中，建立考虑方向角量测偏差变化的卡尔曼滤波算法，包括以下步骤：
S4-1、根据上一时刻后验估计位置以及其后验协方差，通过状态模型确定此时刻水下目标的先验估计位置以及其先验协方差；
S4-2、根据上一时刻的后验估计位置，通过超短基线系统的误差模型确定此时刻的观测噪声矩阵；
S4-3、根据此时刻观测噪声矩阵，确定增益；
S4-4、根据量测方程确定新息；
S4-5、根据增益及新息确定此时刻水下目标的后验估计位置以及其后验协方差；
S4-6、重复上述步骤，获得水下航行器的实时位置。


3.如权利要求1所述的一种基于超短基线和航位推算的水下导航定位方法，其特征在于：在所述步骤S1中，换能器O布置在船体空间坐标系O-XYZ的原点位置，水听器S1、S2、S3、S4在船体空间坐标系XOY平面中心对称布置，其中，S1、S3阵元位于指向船艏的X轴上，S2、S4阵元位于垂直于船艏的Y轴上；阵元间距为d，即S1与S3间距、S2与S4间距；搭载声学应答器的水下目标位于T点，其在船体空间坐标系下坐标为(x,y,z)；超短基线水声定位系统利用水听器接收信号之间的时延差T和相位差△φx,△φy解算水下目标的距离r：
r＝CT/2(1)
和方位：
θx＝arcos(λ△φx/2πd),θy＝arcos(λ△φy/2πd)(2)
再根据公式(1)和公式(2)解算水下目标位置：



其中，公式(1)～公式(3)中，C为水下声速，f为换能器收发频率。


4.如权利要求1所述的一种基于超短基线和航位推算的水下导航定位方法，...

【专利技术属性】
技术研发人员：马杰，张煜，亓强强，彭智浩，李文楷，
申请(专利权)人：武汉理工大学，
类型：发明
国别省市：湖北;42