CN110554359A - 一种融合长基线与单信标定位的海底飞行节点定位方法 - Google Patents

Abstract

本发明属于水声定位技术领域，涉及一种海底飞行节点定位方法。本发明利用4个位置已知且固定的水声信标按照固定周期同时发射水声信号，水声信标与海底飞行节点实现时钟同步；海底飞行节点接收水声信号并记录信号到达时间，计算出水声信号传递时间；海底飞行节点未接收到水声信号时通过自身携带的低成本惯性测量单元进行惯性导航；海底飞行节点接收到水声信号后通过扩展Kalman滤波进行单信标定位位置校准；同一时刻发射的多个水声信号均被接收后，海底飞行节点通过最小二乘法进行长基线定位位置校准。本发明通过融合水下长基线定位与单信标定位，克服了长基线定位实时性差及单信标定位精度低的缺点，满足了海底飞行节点高精度实时定位的需求。

Description

一种融合长基线与单信标定位的海底飞行节点定位方法

技术领域

本发明属于水声定位技术领域，涉及一种海底飞行节点定位方法。

背景技术

水下地震波检测飞行节点是自动执行工作的装置，主要用于海底地震波的检测。现有的 地震波检测方法一般是采用地震检测仪，但随着检测水域深度的增加，对用于水下地震波检 测的海底飞行节点的需求也越来越大。

海底飞行节点需要实现自主运动、坐底检测、上浮等功能，其自主运动往往需要高精度 的位置反馈，而其坐底时定位的精确性决定了所采集数据的价值，因此海底飞行节点的水下 精确定位至关重要。由于电磁波在水下衰减较快，常规的GPS无法用于精确水下定位。较为 常见的惯性导航设备往往会随时间增长产生较大累计误差，无法长时间用于水下定位，而高 精度的惯性导航设备成本极高，不适合于海底飞行节点群体。现有的水下定位方式主要基于 水声信号，包括长基线定位、超短基线定位、单信标定位等。长基线定位较为成熟，定位精 度相对较高，但其位置解算需要获得所有信标发射的水声信号后才可进行，而在实际应用中 水声信号发射频率往往较低，且信号传播过程中存在丢包现象，长基线定位位置解算条件较 难满足，因此其位置解算的频率较低，难以满足海底飞行节点实时定位需求。超短基线定位 的位置解算系统位于母船，无法实现海底飞行节点实时定位。而水下单信标定位会融合惯性 测量单元观测数据，位置解算的频率高，可以实现实时定位，但其定位精度有限，难以满足 海底飞行节点高精度定位需求。

发明内容

本发明的目的是：提供一种应用于海底飞行节点、实现实时高精度水下定位的方法。

本发明的技术方案是：一种融合长基线与单信标定位的海底飞行节点定位方法，本方法 采用4个空间不共面的水声信标为移动的海底飞行节点定位；水声信标均为固定式且其空间 坐标已知，配备高精度原子钟；海底飞行节点配备低成本惯性测量单元、GPS系统、电子罗 盘、深度计、水听器及高精度原子钟；海底飞行节点的垂向位置由深度计直接测量；海底飞 行节点水平方向速度由其螺旋桨稳态转速与电子罗盘测得的姿态角计算得到；水声信标与海 底飞行节点通过高精度原子钟实现时钟同步；本方法包含以下步骤：

A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；

B.通过海底飞行节点所搭载的GPS系统获取该海底飞行节点在水下局部惯性系当中的 初始位置；

C.建立海底飞行节点的运动学模型以及观测模型并进行离散化；

D.4个空间不共面的位置已知且固定的水声信标按照固定周期T同时发射水声信号，所 述海底飞行节点接收到第j个所述水声信标发射的水声信号后通过Kalman滤波及扩展 Kalman滤波进行单信标定位位置更新；

E.海底飞行节点接收到4个水声信标同时发射的水声信号后，通过线性最小二乘法进行 长基线定位位置更新。

上述C步骤中，运动学模型的建立方法为：

定义状态向量为：

x＝[x v_x a_x y v_y a_y v_e]^T

其中：x，y为海底飞行节点在水下局部惯性坐标系中的水平位置；v_x，v_y为海底飞行节点相对于水下局部惯性坐标系的水平速度；a_x，a_y为海底飞行节点相对于水下局部惯性坐标系的水平加速度；v_e为有效声速；

对x求导并加入海底飞行节点运动模型噪声影响，得到海底飞行节点的运动学模型：

其中：ω_x为海底飞行节点在x方向的位置不确定性；ω_vx为海底飞行节点在x方向的速 度不确定性；ω_ax为海底飞行节点在x方向的加速度不确定性；ω_y为海底飞行节点在y方向 的位置不确定性；ω_vy为海底飞行节点在y方向的速度不确定性；ω_ay为海底飞行节点在y方 向的加速度不确定性；ω_ve为声速不确定性。

上述C步骤中，观测模型的建立方法为：

S1.建立水声信号传递时间的观测模型；

设第j个水声信标的空间坐标为X_j,Y_j,Z_j，水声信标与海底飞行节点通过高精度原子钟 实现时钟同步，海底飞行节点接收到第j个水声信标发射信号时，其对应的观测方程为：

其中：T_a,j为海底飞行节点接收到第j个水声信标发射信号的时刻，T_e,j为第j个水声信标 发射水声信号的时刻，z为海底飞行节点的深度，由深度计直接测得，v为对应的观测噪声；

S2.建立速度、加速度观测模型；

海底飞行节点水平方向速度可由其螺旋桨稳态转速与电子罗盘测得的姿态角联合计算得 到；海底飞行节点水平方向加速度可由惯性测量单元检测的局部坐标系下加速度与电子罗盘 测得的姿态角联合计算得到；海底飞行节点水平方向速度与加速度两组观测量均为线性，满 足m＝Hx+v；

其中：观测向量m＝[m_vx m_ax m_vy m_ay]^T；m_vx，m_vy为x，y方向速度观测，m_ax，m_ay为x， y方向加速度观测；

H为速度与加速度观测矩阵，满足：

上述C步骤中，运动学模型以及观测模型离散化方法为：

S1.运动学模型离散化；

以符号k为时间索引，以Δt＝t_k+1-t_k为离散间隔，运动学模型离散为：

x_k+1＝A_kx_k+w_k

其中：A_k为运动学方程，满足：

过程噪声向量w_k＝[ω_x,k ω_vx,k ω_ax,k ω_y,k ω_vy,k ω_ay,k ω_ve,k]^T，对应各个状态变量的不确定性，过程噪声协方差矩阵满足：

其中，q为比例系数，σ_e为声速不确定性的标准差；

S2.观测模型离散化；

海底飞行节点在k-1至k间接收到第j个水声信标信号，将其假设为在k时刻接收到该水 声信号，即离散后的水声信号传递时间观测方程为：

其中，v_ve,k为观测噪声，假设其满足方差为R_ve,k的Gauss分布；

由于海底飞行节点速度与加速度的观测采样频率较高，假设在每一个离散时间点k处均 可以得到速度与加速度观测；故离散后观测方程为：

m_av，k＝H_kx_k+v_k

其中，H_k为k时刻速度与加速度观测矩阵，满足：

v_k为k时刻观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：

其中，ω_vel与ω_acc分别为速度与加速度观测噪声的标准差。

上述步骤D中，基于Kalman滤波及扩展Kalman滤波的单信标定位位置更新方法为：

S1.预测；

其中：与分别为k-1时刻的后验状态和后验方差，与分别为k时刻的先验 状态和先验方差；

S2.速度、加速度更新；

其中，K_k为Kalman增益；

S3.水声信号到达时间更新；

S3.1水声信号传递时间观测方程线性化；

对水声信号传递时间观测方程在先验估计处进行Taylor展开并保留第一项，得到k时刻 海底飞行节点接收到第j个水声信标信号的观测矩阵：

其中：为k时刻海底飞行节点在水下局部惯性坐标系下x，y方向的位置先验估 计；为k时刻的水下声速先验估计；为k时刻海底飞行节点与第j个水声信标的距离 估计，满足：

S3.2水声信号传递时间位置更新；

上述E步骤中，基于线性最小二乘法进行长基线定位位置更新方法为：

忽略海底飞行节点在接收到各个水声信号间隔的运动，设海底飞行节点在k时刻同时接 收到4个水声信标的水声信号，海底飞行节点在k时刻位置为x_k,y_k,z_k，此时观测方程为：

其中：r_k,1,r_k,2,r_k,3,r_k,4分别为4个水声信标的测距观测，由所得测的水声信号传递时间 m_t1,k，m_t2,k，m_t3,k，m_t4,k与单水声信标定位系统所估算出的实时水下声速相乘得到；

令R_k＝x_k ²+y_k ²，观测方程表示为线性：m_k＝A_kp_k；

其中：

p_k＝[x_k y_k R_k]^T

通过线性最小二乘进行位置解算可得长基线定位的位置解算坐标为：

以长基线定位解算出的x_k，y_k对k时刻单信标定位的位置解算结果进行校正，可以进一 步提高系统定位精度。

有益效果：本方法通过4个位置固定的水声信标按照一定周期发射水声信号，海底飞行 节点在未接收到水声信号时根据自身惯性测量单元进行惯性导航、接收到水声信号时利用扩 展Kalman滤波进行单信标定位位置校准、同一时刻发射的四个水声信号全部接收到后利用 最小二乘法进行长基线定位位置校准。此定位方式可以兼具单信标定位的实时性及长基线定 位的高精度，同时具备低成本、体积小、易于向群体扩展等优点，适合应用于海底飞行节点。

附图说明

图1为本发明原理示意图；

图2为本发明A步骤中建立的水下局部惯性坐标系示意图；

图3为本发明E步骤中基于线性最小二乘法进行长基线定位位置更新方法示意图；

图4为本发明的定位信号处理时序示意图；

图5为本发明实施例2仿真实验过程示意图；

图6、图7为本发明实施例2对多种定位方法仿真实验结果的对比示意图。

具体实施方式

实施例1，参见附图1，一种融合长基线与单信标定位的海底飞行节点定位方法，本方法 采用4个空间不共面的水声信标为移动的海底飞行节点定位；水声信标均为固定式且其空间 坐标已知，配备高精度原子钟；海底飞行节点配备低成本惯性测量单元、GPS系统、电子罗 盘、深度计、水听器及高精度原子钟；海底飞行节点的垂向位置由深度计直接测量；海底飞 行节点水平方向速度由其螺旋桨稳态转速与电子罗盘测得的姿态角计算得到；水声信标与海 底飞行节点通过高精度原子钟实现时钟同步；本方法包含以下步骤：

参见附图2，A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；

B.通过海底飞行节点所搭载的GPS系统获取该海底飞行节点在水下局部惯性系当中的 初始位置；

C.建立海底飞行节点的运动学模型以及观测模型并进行离散化；

C1.运动学模型的建立方法为：

定义状态向量为：

x＝[x v_x a_x y v_y a_y v_e]^T

其中：x，y为海底飞行节点在水下局部惯性坐标系中的水平位置；v_x，v_y为海底飞行节点相对于水下局部惯性坐标系的水平速度；a_x，a_y为海底飞行节点相对于水下局部惯性坐标系的水平加速度；v_e为有效声速；

对x求导并加入海底飞行节点运动模型噪声影响，得到海底飞行节点的运动学模型：

其中：ω_x为海底飞行节点在x方向的位置不确定性；ω_vx为海底飞行节点在x方向的速 度不确定性；ω_ax为海底飞行节点在x方向的加速度不确定性；ω_y为海底飞行节点在y方向 的位置不确定性；ω_vy为海底飞行节点在y方向的速度不确定性；ω_ay为海底飞行节点在y方 向的加速度不确定性；ω_ve为声速不确定性。

C2.观测模型的建立方法为：

S1.建立水声信号传递时间的观测模型；

设第j个水声信标的空间坐标为X_j,Y_j,Z_j，水声信标与海底飞行节点通过高精度原子钟 实现时钟同步，海底飞行节点接收到第j个水声信标发射信号时，其对应的观测方程为：

其中：T_a,j为海底飞行节点接收到第j个水声信标发射信号的时刻，T_e,j为第j个水声信标 发射水声信号的时刻，z为海底飞行节点的深度，由深度计直接测得；v为对应的观测噪声；

S2.建立速度、加速度观测模型；

海底飞行节点水平方向速度可由其螺旋桨稳态转速与电子罗盘测得的姿态角联合计算得 到；海底飞行节点水平方向加速度可由惯性测量单元检测的局部坐标系下加速度与电子罗盘 测得的姿态角联合计算得到；海底飞行节点水平方向速度与加速度两组观测量均为线性，满 足m＝Hx+v；

其中：观测向量m＝[m_vx m_ax m_vy m_ay]^T；m_vx，m_vy为x，y方向速度观测，m_ax，m_ay为x， y方向加速度观测；

H为速度与加速度观测矩阵，满足：

C3.运动学模型以及观测模型离散化方法为：

S1.运动学模型离散化；

以符号k为时间索引，以Δt＝t_k+1-t_k为离散间隔，运动学模型离散为：

x_k+1＝A_kx_k+w_k

其中：A_k为运动学方程，满足：

过程噪声向量w_k＝[ω_x,k ω_vx,k ω_ax,k ω_y,k ω_vy,k ω_ay,k ω_ve,k]^T，对应各个状态变量的不确定性，过程噪声协方差矩阵满足：

其中，q为比例系数，σ_e为声速不确定性的标准差；

S2.观测模型离散化；

海底飞行节点在k-1至k间接收到第j个水声信标信号，将其假设为在k时刻接收到该水 声信号，即离散后的水声信号传递时间观测方程为：

其中，v_ve,k为观测噪声，假设其满足方差为R_ve,k的Gauss分布；

由于海底飞行节点速度与加速度的观测采样频率较高，假设在每一个离散时间点k处均 可以得到速度与加速度观测，故离散后观测方程为：

m_av，k＝H_kx_k+v_k

其中，H_k为k时刻速度与加速度观测矩阵，满足：

v_k为k时刻观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：

其中，ω_vel与ω_acc分别为速度与加速度观测噪声的标准差。

D.4个空间不共面的位置已知且固定的水声信标按照固定周期T同时发射水声信号，所 述海底飞行节点接收到第j个所述水声信标发射的水声信号后通过Kalman滤波及扩展 Kalman滤波进行单信标定位位置更新；

基于Kalman滤波及扩展Kalman滤波的单信标定位位置更新方法为：

S1.预测；

其中：与分别为k-1时刻的后验状态和后验方差，与分别为k时刻的先验 状态和先验方差；

S2.速度、加速度更新；

其中，K_k为Kalman增益；

S3.水声信号到达时间更新；

S3.1水声信号传递时间观测方程线性化；

对水声信号传递时间观测方程在先验估计处进行Taylor展开并保留第一项，得到k时刻 海底飞行节点接收到第j个水声信标信号的观测矩阵：

其中：为k时刻海底飞行节点在水下局部惯性坐标系下x，y方向的位置先验估 计；为k时刻的水下声速先验估计；为k时刻海底飞行节点与第j个水声信标的距离 估计，满足：

S3.2水声信号传递时间位置更新；

参见附图3，E.海底飞行节点接收到4个水声信标同时发射的水声信号后，通过线性最 小二乘法进行长基线定位位置更新；

基于线性最小二乘法进行长基线定位位置更新方法为：

忽略海底飞行节点在接收到各个水声信号间隔的运动，设海底飞行节点在k时刻同时接 收到4个水声信标的水声信号，海底飞行节点在k时刻位置为x_k,y_k,z_k，此时观测方程为：

其中：r_k,1,r_k,2,r_k,3,r_k,4分别为4个水声信标的测距观测，由所得测的水声信号传递时间 m_t1,k，m_t2,k，m_t3,k，m_t4,k与单水声信标定位系统所估算出的实时水下声速相乘得到；

令R_k＝x_k ²+y_k ²，观测方程表示为线性：m_k＝A_kp_k；

其中：

p_k＝[x_k y_k R_k]^T

通过线性最小二乘进行位置解算可得长基线定位的位置解算坐标为：

以长基线定位解算出的x_k，y_k对k时刻单信标定位的位置解算结果进行校正，可以进一 步提高系统定位精度。

实施例2，本例为如实施例1所述的方法的仿真实验结果：

如图4所示为海底飞行节点的运动轨迹，四个水声信标分别布放在上，位置坐标为X1＝(-1000,7000,0)，X2＝(2000,-2000,0)，X3＝(3000,5000,0)，X4＝(-5000,5000,0)，水声信号 发射周期为10s，信号在传播过程中的丢包率为10％，水下声速假设为恒定的1530m/s，加上 均值为0，标准差为25m/s的Gauss白噪声。整个仿真时长为3000s，海底飞行节点运动航速 约为0.5kn。海底飞行节点配备的传感器及其采样频率、观测噪声等如表。此外，为更加贴合 实际情况，加速度传感器在x和y两个方向上均增加100ug的观测漂移，海底飞行节点在x 和y两个方向上的初始位置偏差为10m。

传感器	观测变量	采样频率	观测噪声
				速度传感器	v<sub>x</sub> v<sub>y</sub>	50Hz	4m/s
加速度传感器	a<sub>x</sub> a<sub>y</sub>	50Hz	9.76mg
				深度计	z	50Hz	1mm
水声通信机	T<sub>t</sub>		30us

结合图6与图7，根据本发明的方法与常规惯性导航、长基线定位、单信标定位的导航 轨迹对比，可得出惯性导航定位误差极大，且误差随时间不断增加，有发散趋势；单信标定 位精度相对惯性导航有所改进，但仍然难以满足实际海底飞行节点的高精度定位需求，且其 误差波动较大；长基线定位轨迹虽与真实轨迹最为接近，但其结果较为离散，实时性太差。 而本发明所提出的融合长基线与单信标定位的海底飞行节点定位方法可以同时获得较好的定 位精度及定位实时性。四种定位方式的平均误差、平均采样周期及特点总结如下表。

定位方式	平均误差	平均采样周期	特点
				IMU	14.8561m	0.02s	精度极低
单信标	2，3490m	0.02s	精度低，实时性好
				长基线	0.6884m	15s	精度高，实时性差
单信标+长基线	0.8930m	0.02s	精度高，实时性好

虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基 础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏 离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。

Claims (6)

1.一种融合长基线与单信标定位的海底飞行节点定位方法，其特征在于，包括以下步骤：

A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；

B.通过所述海底飞行节点所搭载的GPS系统获取该所述海底飞行节点在水下局部惯性系当中的初始位置；

C.建立所述海底飞行节点的运动学模型以及观测模型并进行离散化；

D.4个空间不共面的位置已知且固定的水声信标按照固定周期T同时发射水声信号，所述海底飞行节点接收到第j个所述水声信标发射的水声信号后通过Kalman滤波及扩展Kalman滤波进行单信标定位位置更新；

E.所述海底飞行节点接收到所述4个水声信标同时发射的水声信号后，通过线性最小二乘法进行长基线定位位置更新。

2.如权利要求1所述的一种融合长基线与单信标定位的海底飞行节点定位方法，其特征在于，所述C步骤中，所述运动学模型的建立方法为：

定义状态向量为：

x＝[x v_x a_x y v_y a_y v_e]^T

其中：x，y为所述海底飞行节点在所述水下局部惯性坐标系中的水平位置；v_x，v_y为所述海底飞行节点相对于所述水下局部惯性坐标系的水平速度；a_x，a_y为所述海底飞行节点相对于所述水下局部惯性坐标系的水平加速度；v_e为有效声速；

对x求导并加入所述海底飞行节点运动模型噪声影响，得到所述海底飞行节点的运动学模型：

其中：ω_x为海底飞行节点在x方向的位置不确定性；ω_vx为海底飞行节点在x方向的速度不确定性；ω_ax为海底飞行节点在x方向的加速度不确定性；ω_y为海底飞行节点在y方向的位置不确定性；ω_vy为海底飞行节点在y方向的速度不确定性；ω_ay为海底飞行节点在y方向的加速度不确定性；ω_ve为声速不确定性。

3.如权利要求2所述的一种融合长基线与单信标定位的海底飞行节点定位方法，其特征在于，所述C步骤中，所述观测模型的建立方法为：

S1.建立水声信号传递时间的观测模型；

设第j个所述水声信标的空间坐标为X_j,Y_j,Z_j，所述水声信标与所述海底飞行节点通过高精度原子钟实现时钟同步，所述海底飞行节点接收到第j个所述水声信标发射信号时，其对应的观测方程为：

其中：T_a,j为所述海底飞行节点接收到第j个所述水声信标发射信号的时刻，T_e,j为第j个所述水声信标发射水声信号的时刻，z为所述海底飞行节点的深度；v为对应的观测噪声；

S2.建立速度、加速度观测模型；

所述海底飞行节点水平方向速度与加速度两组观测量均为线性，满足m＝Hx+v；

其中：观测向量m＝[m_vx m_ax m_vy m_ay]^T；m_vx，m_vy为x，y方向速度观测，m_ax，m_ay为x，y方向加速度观测；

H为速度与加速度观测矩阵，满足：

4.如权利要求3所述的一种融合长基线与单信标定位的海底飞行节点定位方法，其特征在于，所述C步骤中，所述运动学模型以及观测模型离散化方法为：

S1.运动学模型离散化；

以符号k为时间索引，以Δt＝t_k+1-t_k为离散间隔，运动学模型离散为：

x_k+1＝A_kx_k+w_k

其中：A_k为运动学方程，满足：

过程噪声向量w_k＝[ω_x,k ω_vx,k ω_ax,k ω_y,k ω_vy,k ω_ay,k ω_ve,k]^T，对应各个状态变量的不确定性，过程噪声协方差矩阵满足：

其中，q为比例系数，σ_e为声速不确定性的标准差；

S2.观测模型离散化；

所述海底飞行节点在k-1至k间接收到第j个所述水声信标信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：

其中，v_ve,k为观测噪声，假设其满足方差为R_ve,k的Gauss分布；

假设在每一个离散时间点k处均可以得到速度与加速度观测，故离散后观测方程为：

m_av，k＝H_kx_k+v_k

其中，H_k为k时刻速度与加速度观测矩阵，满足：

v_k为k时刻观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：

其中，ω_vel与ω_acc分别为速度与加速度观测噪声的标准差。

5.如权利要求4所述的一种融合长基线与单信标定位的海底飞行节点定位方法，其特征在于，所述步骤D中，基于Kalman滤波及扩展Kalman滤波的单信标定位位置更新方法为：

S1.预测；

其中：与分别为k-1时刻的后验状态和后验方差，与分别为k时刻的先验状态和先验方差；

S2.速度、加速度更新；

其中，K_k为Kalman增益；

S3.水声信号到达时间更新；

S3.1水声信号传递时间观测方程线性化；

对水声信号传递时间观测方程在先验估计处进行Taylor展开并保留第一项，得到k时刻所述海底飞行节点接收到第j个所述水声信标信号的观测矩阵：

其中：为k时刻所述海底飞行节点在所述水下局部惯性坐标系下x，y方向的位置先验估计；为k时刻的水下声速先验估计；为k时刻所述海底飞行节点与第j个所述水声信标的距离估计，满足：

S3.2水声信号传递时间位置更新；

6.如权利要求5所述的一种融合长基线与单信标定位的海底飞行节点定位方法，其特征在于，所述E步骤中，所述基于线性最小二乘法进行长基线定位位置更新方法为：

设所述海底飞行节点在k时刻同时接收到4个所述水声信标的水声信号，所述海底飞行节点在k时刻位置为x_k,y_k,z_k，此时观测方程为：

其中：r_k,1,r_k,2,r_k,3,r_k,4分别为4个所述水声信标的测距观测，由所得测的水声信号传递时间m_t1,k，m_t4,k与单水声信标定位系统所估算出的实时水下声速相乘得到；

令R_k＝x_k ²+y_k ²，观测方程表示为线性：m_k＝A_kp_k；

其中：

p_k＝[x_k y_k R_k]^T

通过线性最小二乘进行位置解算可得长基线定位的位置解算坐标为：