一种具有全局收敛性的水下航行器单信标定位方法技术

本发明专利技术涉及水下定位技术领域，特别涉及一种水下航行器的单信标定位方法。一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；本发明专利技术通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；在获得的随体坐标系下水下航行器与水相对速度以及航行器姿态航向时，进行Kalman滤波预测；在获得海流速度观测、深度计观测、水声信号传递时间时，分别通过Kalman滤波进行海流速度、深度、水声信号传递时间更新。在满足定位模型可观测的前提下，本方法具有全局指数收敛性。

A single beacon location method for underwater vehicles with global convergence

【技术实现步骤摘要】
一种具有全局收敛性的水下航行器单信标定位方法
本专利技术涉及水下定位
，特别涉及一种水下航行器的单信标定位方法。
技术介绍
精确的位置反馈是水下航行器完成既定水下任务的基础。由于水下电磁波信号衰减较快，广泛应用于陆地与天空定位的GNSS系统在水下无法应用。现有主流的水下定位方式包括以惯性导航为代表的航位推算方法以及以长基线定位为代表的水下声学定位方法。其中惯性导航设备往往会随时间增长产生较大累计误差，无法长时间用于水下定位，而高精度的惯性导航设备成本极高，限制了其在水下航行器中的应用。现有主流的水下声学定位方式包括长基线定位、超短基线定位、单信标定位等。长基线定位与超短基线定位发展均较为成熟，但其成本通常较高，且实时性通常较差，这限制了其在水下航行器中的应用。而新兴的水下单信标定位系统融合航位推算数据与单水声信标的测距信息，在定位成本和实时性方面均有较大的优势。目前的单信标定位系统均是以水声信号传递时间为观测，在水下声速已知的前提下，得到信标与水下航行器之间的地理斜距作为观测变量。但实际应用中，水声声速受到水下温度、盐度、密度等因素的影响，通常为时变未知的，精确的水声声速难以得到，这会导致水声测距误差，影响单信标定位系统的性能；此外，由于地理斜距的观测方程为非线性，目前的水下单信标定位系统通常采用非线性Kalman滤波或粒子滤波进行位置解算，这些非线性的滤波方法只具有局部收敛性，在水下航行器的初始位置误差或某一时刻的位置误差较大的情况下(例如：航行器在水下航行的过程中，在某一时间段内由于水下环境的恶劣性而失去了声学定位辅助，只依靠自身的航位推算而产生较大的累计误差，在恢复声学定位辅助之后会出现较大的位置误差)，难以保证后续滤波的收敛性，这也影响了现有水下单信标定位系统的实际应用。
技术实现思路
本专利技术的目的是：针对水下单信标定位当中水声声速的未知性、水下航行器初始位置误差或某一时刻的位置误差较大等问题，基于状态增广方法提出一种具有全局收敛性的水下航行器单信标定位方法。本专利技术的技术方案是：一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；所述方法包括以下步骤：A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；B.通过搭载的GPS获取该水下航行器在水下局部惯性坐标系当中的初始位置；C.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型；D.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；E.水下航行器接收到多普勒测速仪所获得的随体坐标系下水下航行器与水相对速度以及姿态航向参考系统所获得的航行器姿态航向时，进行Kalman滤波预测；F.水下航行器接收到多普勒测速仪所测得的绝对速度观测时，结合姿态航向参考系统所获得的水下航行器姿态航向角计算水下局部惯性坐标系下自身对地速度，进而获得海流速度观测，通过Kalman滤波进行海流速度更新；G.水下航行器接收到深度计观测时，通过Kalman滤波进行深度更新；H.水下航行器收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将水声信号传递时间的平方作为观测变量，通过Kalman滤波进行水声信号传递时间更新。在上述方案的基础上，具体的，所述步骤C中，运动学模型的建立方法为：定义位置向量为：p＝[xyz]T其中：x，y，z为水下航行器在水下局部惯性坐标系中的空间位置坐标；定义海流速度向量为：vc＝[vcxvcyvcz]T其中：vcx，vcy，vcz为水下局部惯性坐标系中x，y，z三个方向未知的海流速度；定义水下航行器对水速度向量为：vw＝[vwxvwyvwz]T其中：vwx，vwy，vwz分别为水下局部惯性坐标系中x，y，z三个方向水下航行器与水的相对速度，通过姿态航向参考系统以及多普勒测速仪所测数据计算得到，计算公式为：其中：为多普勒测速仪测得的随体坐标系下水下航行器与水的相对速度矢量，为随体坐标系向局部惯性坐标系旋转的旋转矩阵，其矩阵元素与姿态航向参考系统所测得的水下航行器姿态角、航向角相关；的计算公式为：其中：分别为水下航行器的横滚角、俯仰角及航向角，由姿态航向参考系统测得；记ve为水下未知的有效声速；求解未知p，vc及ve的时间导数，并考虑相应的不确定性，得到水下航行器的运动学模型：其中：为水下航行器在x，y，z方向的位置不确定性，ωc＝[ωcxωcyωcz]T为x，y，z方向的海流不确定性；ωe为有效声速不确定性。在上述方案的基础上，具体的，所述步骤C中，观测模型的建立方法为：S1.建立水声信号传递时间的观测模型；记水声信标发射水声信号的时刻为Te，不失一般性，记水声信标在水下局部惯性坐标系中的空间位置坐标为s＝[000]T，水下航行器接收到该水声信号的时刻为Ta，Te以及Ta均为已知量，观测方程为：其中：vt为对应的观测噪声；S2.建立海流流速观测模型；根据多普勒测速仪测得的随体坐标系下水下航行器的绝对速度结合姿态航向参考系统测得的水下航行器姿态和航向，计算得到水下航行器绝对速度在水下局部惯性坐标系下的表示：其中：vg＝[vgxvgyvgz]T为水下航行器绝对速度在局部惯性坐标系下x，y，z三个方向的分量；根据vg以及vw，计算得到海流速度观测量为：mc＝vg-vw其中：mc＝[mcxmcymcz]T表示三个方向的海流观测量；海流观测方程为线性，满足mc＝vc+vvc；其中：vvc为海流观测噪声向量，vvc＝[vvcxvvcyvvcz]T，其中vvcx为x方向的海流观测噪声；vvcy为y方向的海流观测噪声；vvcz为z方向的海流观测噪声；S3.建立深度观测模型；记水下航行器所搭载的深度计观测量为mz，则其观测方程为mz＝ap+vz其中：a＝[001]，vz为深度计观测噪声。在上述方案的基础上，具体的，所述步骤C中，运动学模型以及观测模型离散化方法为：S1.运动学模型离散化；以变量加上下标符号k为离散时间索引，以ΔT为离散间隔，运动学模型离散为：pk+1＝pk+ΔTvc,k+ΔTvw,k+ωp,kvc,k+1＝vc,k+ωc,kve,k+1＝ve,k+ωe,k其中：ωp,k,ωc,k,ωe,k表示离散状态下的过程噪声；S2.观测模型离散化；假设水下航行器在k时刻接收到水声信号，离散后的水声信号传递时间观测方程为：其中，vt,k为水声信号传递时间观测噪声；假设在每一个离散时间点处均可以得到海流速度观测，离散后观测方程为：本文档来自技高网...

【技术保护点】
1.一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；其特征在于，所述方法包括以下步骤：/nA.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；/nB.通过搭载的GPS获取该水下航行器在水下局部惯性坐标系当中的初始位置；/nC.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型；/nD.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；/nE.水下航行器接收到多普勒测速仪所获得的随体坐标系下水下航行器与水相对速度以及姿态航向参考系统所获得的航行器姿态航向时，进行Kalman滤波预测；/nF.水下航行器接收到多普勒测速仪所测得的绝对速度观测时，结合姿态航向参考系统所获得的水下航行器姿态航向角计算水下局部惯性坐标系下自身对地速度，进而获得海流速度观测，通过Kalman滤波进行海流速度更新；/nG.水下航行器接收到深度计观测时，通过Kalman滤波进行深度更新；/nH.水下航行器收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将水声信号传递时间的平方作为观测变量，通过Kalman滤波进行水声信号传递时间更新。/n...

【技术特征摘要】
1.一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；其特征在于，所述方法包括以下步骤：
A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
B.通过搭载的GPS获取该水下航行器在水下局部惯性坐标系当中的初始位置；
C.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型；
D.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；
E.水下航行器接收到多普勒测速仪所获得的随体坐标系下水下航行器与水相对速度以及姿态航向参考系统所获得的航行器姿态航向时，进行Kalman滤波预测；
F.水下航行器接收到多普勒测速仪所测得的绝对速度观测时，结合姿态航向参考系统所获得的水下航行器姿态航向角计算水下局部惯性坐标系下自身对地速度，进而获得海流速度观测，通过Kalman滤波进行海流速度更新；
G.水下航行器接收到深度计观测时，通过Kalman滤波进行深度更新；
H.水下航行器收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将水声信号传递时间的平方作为观测变量，通过Kalman滤波进行水声信号传递时间更新。


2.如权利要求1所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤C中，运动学模型的建立方法为：
定义位置向量为：
p＝[xyz]T
其中：x，y，z为水下航行器在水下局部惯性坐标系中的空间位置坐标；
定义海流速度向量为：
vc＝[vcxvcyvcz]T
其中：vcx，vcy，vcz为水下局部惯性坐标系中x，y，z三个方向未知的海流速度；
定义水下航行器对水速度向量为：
vw＝[vwxvwyvwz]T
其中：vwx，vwy，vwz分别为水下局部惯性坐标系中x，y，z三个方向水下航行器与水的相对速度，通过姿态航向参考系统以及多普勒测速仪所测数据计算得到，计算公式为：



其中：为多普勒测速仪测得的随体坐标系下水下航行器与水的相对速度矢量，为随体坐标系向局部惯性坐标系旋转的旋转矩阵，其矩阵元素与姿态航向参考系统所测得的水下航行器姿态角、航向角相关；的计算公式为：



其中：θ,ψ分别为水下航行器的横滚角、俯仰角及航向角，由姿态航向参考系统测得；
记ve为水下未知的有效声速；
求解未知p，vc及ve的时间导数，并考虑相应的不确定性，得到水下航行器的运动学模型：









其中：为水下航行器在x，y，z方向的位置不确定性，ωc＝[ωcxωcyωcz]T为x，y，z方向的海流不确定性；ωe为有效声速不确定性。


3.如权利要求2所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤C中，观测模型的建立方法为：
S1.建立水声信号传递时间的观测模型；
记水声信标发射水声信号的时刻为Te，不失一般性，记水声信标在水下局部惯性坐标系中的空间位置坐标为s＝[000]T，水下航行器接收到该水声信号的时刻为Ta，Te以及Ta均为已知量，观测方程为：



其中：vt为对应的观测噪声；
S2.建立海流流速观测模型；
根据多普勒测速仪测得的随体坐标系下水下航行器的绝对速度结合姿态航向参考系统测得的水下航行器姿态和航向，计算得到水下航行器绝对速度在水下局部惯性坐标系下的表示：



其中：vg＝[vgxvgyvgz]T为水下航行器绝对速度在局部惯性坐标系下x，y，z三个方向的分量；
根据vg以及vw，计算得到海流速度观测量为：
mc＝vg-vw
其中：mc＝[mcxmcymcz]T表示三个方向的海流观测量；
海流观测方程为线性，满足mc＝vc+vvc；
其中：vvc为海流观测噪声向量，vvc＝[vvcxvvcyvvcz]T，其中vvcx为x方向的海流观测噪声；vvcy为y方向的海流观测噪声；vvcz为z方向的海流观测噪声；
S3.建立深度观测模型；
记水下航行器所搭载的深度计观测量为mz，则其观测方程为
mz＝ap+vz
其中：a＝[001]，vz为深度计观测噪声。


4.如权利要求3的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤C中，运动学模型以及观测模型离散化方法为：
S1.运动学模型离散化；
以变量加上下标符号k为离散时间索引，以ΔT为离散间隔，运动学模型离散为：
pk+1＝pk+ΔTvc,k+ΔTvw,k+ωp,k
vc,k+1＝vc,k+ωc,k
ve,k+1＝ve,k+ωe,k
其中：ωp,k,ωc,k,ωe,k表示离散状态下的过程噪声；
S2.观测模型离散化；
假设水下航行器在k时刻接收到水声信号，离散后的水声信号传递时间观测方程为：



其中，vt,k为水声信号传递时间观测噪声；
假设在每一个离散时间点处均可以得到海流速度观测，离散后观测方程为：
mvc，k＝vc,k+vvc,k
其中，vvc,k为k时刻海流速度观测噪声；
同样，假设在每一个离散时间点处均可以得到深度计观测，离散后观测方程为：
mz,k＝apk+vz,k
其中，vz,k为k时刻深度计观测噪声。


5.如权利要求4的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤D包括：
S1.运动学模型处理；
定义离散状态变量：



根据离散时间的水下航行器运动学模型，可以得到：
x1,k+1＝x1,k+ΔTx3,kvw,k+ΔTx2,k+ω1,k
x2,k+1＝x2...

【专利技术属性】
技术研发人员：朱仲本，余相，秦洪德，邓忠超，万磊，田瑞菊，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙;23