一种基于水声探测的目标运动定位方法与流程

本发明涉及目标定位技术领域，尤其涉及一种基于水声探测的目标运动定位方法。

背景技术：

定位技术是水声装备非常重要的技术之一。目前，随着分布式节点协同定位技术的发展，采用水声网络，利用网络内各个传感器目标信息，融合之后进行定位成为研究热点，受到关注。针对网络内的目标定位技术可以分为三类：基于到达时间差(timedifferenceofarrival,tdoa)；基于能量信息定位(receivedsignalstrength,rss)；基于到达角度定位(angleofarrival,aoa)。由于水声环境中，能量衰减较大，故基于rss的方法并不适用。而aoa需要单个节点的测向信息，而水声网络节点通常并不具备这个能力。因此，tdoa方法是目前水声网络中应用最为广泛的定位方法。

目前，应用于tdoa定位技术中的参数估计方法主要有两种，一种是利用多个tdoa量测值构建方程组，然后利用最小二乘的思想，求解出目标的可能解。另一种，利用量测信息构造测量方程，结合ekf思想，递推解算出目标的运动参数。考虑水声环境较为复杂，量测信息扰动比较大，所以第二种方法用的更多。但传统的ekf需要依赖于初值，而且当目标做强机动时，收敛效果不好。

技术实现要素：

鉴于上述的分析，本发明旨在提供一种基于水声探测的目标运动定位方法，用以解决现有tdoa定位技术中传统的扩展卡尔曼滤波算法依赖于初值，而且当目标做出强机动时，通常无法及时做出适应性的调整，最后导致估计精度下降或发散的问题。

本发明的目的主要是通过以下技术方案实现的：

本发明提供了一种基于水声探测的目标运动定位方法，包括以下步骤：

s1，通过多个水声探测节点获取多通道节点的时域波形数据；

s2，根据其他节点依次与第一个节点间时域波形的互谱函数计算得到独立的时延差量测值；

s3，根据所述时延差量测值和目标实时状态变化信息构建卡尔曼滤波算法框架；

s4，根据预设阈值对时延差量测值的统计量进行差异性统计判决改进卡尔曼滤波算法，得到目标运动参数估计结果后对目标进行定位。

进一步地，步骤s3中所述构建卡尔曼滤波算法框架基于时延差量测值得到的测量信息的方程和目标实时状态变化信息的状态参数方程，具体包括以下步骤：

s31，设定目标初始状态的参数估计量和初始状态的参数估计量方差其中，rx(t0)，ry(t0)，vx(t0)，vy(t0)分别为目标初始位置水平分量、目标初始位置垂直分量、目标初始速度水平分量和目标初始速度垂直分量，分别为相应初始状态的估计量方差；

s32，根据上一时刻的估计量预测下一时刻的估计量按照以下状态方程得到状态预估值：

根据上一时刻的协方差矩阵预测下一时刻的协方差矩阵按照以下预测方程得到预估协方差矩阵：

pk/k-1＝apk-1a^t+qk-1；

其中，qk-1为过程噪声方差矩阵；

s33，根据预估协方差矩阵并结合测量矩阵和量测噪声协方差矩阵按照以下公式计算得到最优滤波增益矩阵：

其中，rk为量测噪声协方差矩阵；

s34，根据量测信息和测量矩阵并结合增益矩阵按照以下公式调整状态的参数估计量：

s35，根据增益矩阵和测量矩阵按照以下公式更新滤波误差的协方差矩阵：

pk＝(i-kkhk)pk/k-1。

进一步地，所述目标实时状态变化信息的状态参数方程为：

x(tk)＝ax(tk-1)+wk-1；

其中，为转移矩阵，wk-1为过程噪声矢量，x(tk)＝[rx(tk)ry(tk)vx(tk)vy(tk)]^t为目标状态的参数估计量，rx(tk),ry(tk)分别为目标位置水平分量和目标位置垂直分量，vx(tk),vy(tk)分别为目标速度水平分量和目标速度垂直分量，k为时刻序列。

进一步地，根据所述时延差量测值得到测量信息的方程为：zk＝hkx(tk)+nk；

其中，为测量矩阵，zk为量测信息，τ12,τ13,τ14,...,τ1n分别为2,3,4,...到n号节点与1号节点的时延差量测值，各个量测值是相互独立的，x(tk)＝[rx(tk)ry(tk)vx(tk)vy(tk)]^t为目标状态的参数估计量，rx(tk),ry(tk)分别为目标位置水平分量和目标位置垂直分量，vx(tk),vy(tk)为目标速度水平分量和目标速度垂直分量，nk为测量噪声，k为时刻序列。

进一步地，步骤s2中所述其他节点依次与第一个节点间时域波形的互谱函数公式如下：

其中，y1(m)表示通道节点1时域波形所对应的频谱，表示通道节点n时域波形所对应的频谱的共轭，s1n(m)表示互谱函数，n表示序列长度。

进一步地，按照如下公式取s1n(m)的相位得到时延差量测值：

其中，fm表示第m个单频点，fs表示对时域波形的采样率。

进一步地，步骤s4中所述根据预设阈值对时延差量测值的统计量进行差异性统计判决改进卡尔曼滤波算法，得到目标运动参数估计结果包括以下步骤：

s41，通过多次试验统计结果得到统计量并设定阈值；

s42，通过比较统计量和设定阈值的大小判定目标是否发生强机动；

s43，若目标发生强机动结束，则修正卡尔曼滤波算法框架，得到目标运动参数估计结果；否则，不修正卡尔曼滤波算法框架，递推得到目标运动参数估计结果。

进一步地，步骤s41中通过多次试验统计结果得到统计量并设定阈值包括对n个水声探测节点的n-1个时延差量测值进行累计得到统计量，并根据多次试验统计结果设定阈值。

进一步地，步骤s42中通过比较统计量和设定阈值的大小判定目标是否发生强机动包括：

若满足统计量小于设定阈值，则判定目标强机动结束；

若不满足统计量小于设定阈值，则判定目标发生强机动。

进一步地，所述修正卡尔曼滤波算法框架包括：保持协方差矩阵pk/k-1对角线值不变，其余清零处理。

本发明技术方案的有益效果：本发明公开了一种基于水声探测的目标运动定位方法，利用多个水声探测节点，被动接收舰船的水下辐射噪声数据或主动声纳探测数据，通过其他节点依次与第一节点的互谱函数获取目标与节点间的时延差信息，然后利用改进之后的卡尔曼滤波实现强机动目标的实时定位。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分的从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明实施例的一种基于水声探测的目标运动定位方法流程图；

图2为本发明实施例的标准ekf算法的各状态参数估计结果图；

图3为本发明实施例的标准ekf算法的目标跟踪轨迹与真值的比对图；

图4为本发明实施例的改进算法的各状态参数的估计结果图；

图5为本发明实施例的改进算法的目标跟踪轨迹与真值的比对图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。

本发明的一个具体实施例，如图1所示，公开了一种基于水声探测的目标运动定位方法，包括以下步骤：

s1，通过多个水声探测节点获取多通道节点的时域波形数据；

s2，根据其他节点依次与第一个节点间时域波形的互谱函数计算得到独立的时延差量测值；

s3，根据所述时延差量测值和目标实时状态变化信息构建卡尔曼滤波算法框架；

s4，根据预设阈值对时延差量测值的统计量进行差异性统计判决改进卡尔曼滤波算法，得到目标运动参数估计结果后对目标进行定位。

与现有技术相比，本实施例利用tdoa量测信息，结合扩展卡尔曼滤波算法，通过实时获取目标状态信息，并利用该信息修正协方差矩阵，快速实现目标运动参数估计，更好地适应目标做强机动情况下的运动参数估计，且可以较好的避免滤波发射问题。

具体实际应用中，以4个水声探测节点为例，基于改进ekf算法的目标运动参数解算流程主要包括如下步骤：

a、利用四个水听器接收目标辐射噪声数据或主动声源发射的信号。

b、利用其他三个节点与第一个节点间的互谱函数，取互谱函数的相位得到时延差量测值如果是宽带信息，时延差量测值则是统计平均值。

c、利用时延差量测值，构造测量矩阵h。

d、设立多个缓存，用于存储针对多个目标的时延差数据，并实时感知目标的状态变化信息，得到多个统计量。

e、对得到的统计量进行判决，若满足条件，则对预估协方差矩阵进行修正。

f、保持ekf其他步骤不变，最后得到参数估计结果。

需要说明的是，4个水声探测节点的位置理论上是可以任意布放，但考虑到目标的定位性能，建议以方形结构布放。利用4个水声探测节点的时域接收数据，通过平滑滤波处理去除野值。假定目标按照<静止-运动-静止>方式做强机动。

本发明的一个具体实施例，步骤s3中所述构建卡尔曼滤波算法框架基于时延差量测值得到的测量信息的方程和目标实时状态变化信息的状态参数方程，具体包括以下步骤：

s31，设定目标初始状态的参数估计量和初始状态的参数估计量方差其中，rx(t0)，ry(t0)，vx(t0)，vy(t0)分别为目标初始位置水平分量、目标初始位置垂直分量、目标初始速度水平分量和目标初始速度垂直分量，分别为相应初始状态的估计量方差；

s32，根据上一时刻的估计量预测下一时刻的估计量按照以下状态方程得到状态预估值：

根据上一时刻的协方差矩阵预测下一时刻的协方差矩阵按照以下预测方程得到预估协方差矩阵：

pk/k-1＝apk-1a^t+qk-1；

其中，qk-1为过程噪声方差矩阵；

s33，根据预估协方差矩阵并结合测量矩阵和量测噪声协方差矩阵按照以下公式计算得到最优滤波增益矩阵：

其中，rk为量测噪声协方差矩阵；

s34，根据量测信息和测量矩阵并结合增益矩阵按照以下公式调整状态的参数估计量：

s35，根据增益矩阵和测量矩阵按照以下公式更新滤波误差的协方差矩阵：

pk＝(i-kkhk)pk/k-1。

本发明的一个具体实施例，所述目标实时状态变化信息的状态参数方程为：

x(tk)＝ax(tk-1)+wk-1；

其中，为转移矩阵，wk-1为过程噪声矢量，x(tk)＝[rx(tk)ry(tk)vx(tk)vy(tk)]^t为目标状态的参数估计量，rx(tk),ry(tk)分别为目标位置水平分量和目标位置垂直分量，vx(tk),vy(tk)分别为目标速度水平分量和目标速度垂直分量，k为时刻序列。

本发明的一个具体实施例，根据所述时延差量测值得到测量信息的方程为：zk＝hkx(tk)+nk；

其中，为测量矩阵，zk为量测信息，τ12,τ13,τ14,...,τ1n分别为2,3,4,...到n号节点与1号节点的时延差量测值，各个量测值是相互独立的，x(tk)＝[rx(tk)ry(tk)vx(tk)vy(tk)]^t为目标状态的参数估计量，rx(tk),ry(tk)分别为目标位置水平分量和目标位置垂直分量，vx(tk),vy(tk)为目标速度水平分量和目标速度垂直分量，nk为测量噪声，k为时刻序列。

具体实际应用中，利用互谱或互相关方法得到两两节点之间的时延差量测值τij，并进行存储。利用4个水声探测节点，采取3个独立的时延差量测值构建测量矩阵，计算得到测量信息。也就是说假设有n个水声探测节点，则有n-1个独立的时延差量测值。

本发明的一个具体实施例，步骤s2中所述其他节点依次与第一个节点间时域波形的互谱函数公式如下：

其中，y1(m)表示通道节点1时域波形所对应的频谱，表示通道节点n时域波形所对应的频谱的共轭，s1n(m)表示互谱函数，n表示序列长度。

本发明的一个具体实施例，按照如下公式取s1n(m)的相位得到时延差量测值：

其中，fm表示第m个单频点，fs表示对时域波形的采样率。

本发明的一个具体实施例，步骤s4中所述根据预设阈值对时延差量测值的统计量进行差异性统计判决改进卡尔曼滤波算法，得到目标运动参数估计结果包括以下步骤：

s41，通过多次试验统计结果得到统计量并设定阈值；

s42，通过比较统计量和设定阈值的大小判定目标是否发生强机动；

s43，若目标发生强机动结束，则修正卡尔曼滤波算法框架，得到目标运动参数估计结果；否则，不修正卡尔曼滤波算法框架，递推得到目标运动参数估计结果。

具体实际应用中，对4个水声观测节点独立的3个时延差分量τ12,τ13,τ14进行累计，当累计5个批次(根据多次试验统计结果)后，进行差异性统计判决，即判决阈值设定为th1＝0.005(多次试验统计结果)。当满足统计量小于这个阈值时，表示强机动结束，进入平稳阶段。此时需要修改协方差矩阵pk/k-1，主要是保持该矩阵对角线值不变，其余清零处理。一方面可以尽量保留前一时刻的目标协方差信息，另一方面通过对其余项清零操作，扰动项消失，可以加快参数收敛。若统计量不满足判决条件，则维持原处理框架不变。本发明通过累积多次量测判断目标是否发生机动的统计判决，修正协方差矩阵改善参数收敛结果。

本发明的一个具体实施例，步骤s41中通过多次试验统计结果得到统计量并设定阈值包括对n个水声探测节点的n-1个时延差量测值进行累计得到统计量，并根据多次试验统计结果设定阈值。

本发明的一个具体实施例，步骤s42中通过比较统计量和设定阈值的大小判定目标是否发生强机动包括：

若满足统计量小于设定阈值，则判定目标强机动结束；

若不满足统计量小于设定阈值，则判定目标发生强机动。

本发明的一个具体实施例，其特征在于，所述修正卡尔曼滤波算法框架包括：保持协方差矩阵pk/k-1对角线值不变，其余清零处理。

图2为传统标准ekf(扩展卡尔曼滤波)算法针对4个估计量rx，ry，vx，vy的估计结果仿真图(目标做由静止到运动再静止的强机动，目标初始位置是(0.2m,0m)，终点位置是(1.8m,0m)，平台运动速度是0.008m/s，共持续200s)。根据图2所示的运动参数估计结果可以绘制出传统标准ekf算法的参数估计的航迹与真实航迹的比对，参见图3，可知估计值的航迹与真值的轨迹相差较大。

图4为本发明改进卡尔曼滤波算法针对4个估计量rx，ry，vx，vy的估计结果仿真图(目标做由静止到运动再静止的强机动，目标初始位置是(0.2m,0m)，终点位置是(1.8m,0m)，平台运动速度是0.008m/s，共持续200s)。根据图4所示的运动参数估计结果可以绘制出改进卡尔曼滤波算法的参数估计的航迹与真实航迹的比对，参见图5，可知与图3相比，估计值的航迹与真值的轨迹的拟合度有明显的提高。

综上所述，本发明公开了一种基于水声探测的目标运动定位方法，包括以下步骤：s1，通过多个水声探测节点获取多通道节点的时域波形数据；s2，根据其他节点依次与第一个节点间时域波形的互谱函数计算得到时延差量测值；s3，根据所述时延差量测值和目标实时状态变化信息构建卡尔曼滤波算法框架；s4，根据预设阈值对时延差量测值的统计量进行差异性统计判决改进卡尔曼滤波算法，得到目标运动参数估计结果后对目标进行定位。与现有技术相比，本发明利用tdoa量测信息，结合扩展卡尔曼滤波算法，通过实时获取目标状态信息，并利用该信息修正协方差矩阵，快速实现目标运动参数估计，更好地适应目标做强机动情况下的运动参数估计，且可以较好的避免滤波发射问题。

本领域技术人员可以理解，实现上述实施例中方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。