一种基于遗传粒子滤波算法的水下目标跟踪定位方法与流程

本发明属于目标跟踪定位导航领域，具体涉及一种水下目标跟踪定位方法。

背景技术：

粒子滤波(PF:Particle Filter)的思想基于蒙特卡洛方法(Monte Carlo methods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法。尽管算法中的概率分布只是真实分布的一种近似，但由于非参数化的特点，它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约，能表达比高斯模型更广泛的分布，也对变量参数的非线性特性有更强的建模能力。

水下目标跟踪定位系统多采用水声定位的方法，即基线定位导航形式。以一定阵列形式在水域内分布一定数量的水听器，超声波换能器和水压深度计安装在定位目标上，利用这些水听器接收超声波换能器发出的脉冲，将接收的脉冲信号进行处理后，再按照预定的数学模型对其计算，声源的位置就可得到。为提高精度及简化计算量，也可用深度传感器测量目标的深度值，结合目标与已知参照点的距离信息跟踪目标位置。

由于水下定位的数学模型是非线性的，故一般的最小二乘、卡尔曼滤波等方法不能有效的进行解算。应用加权最小二乘法或其他经典迭代法进行解算，对定位精度的提升效果也不大。

技术实现要素：

发明目的：针对现有技术中存在的问题，本发明公开了一种基于遗传优化粒子滤波算法的水下目标跟踪定位方法以提高定位精度。

技术方案：一种基于遗传粒子滤波算法的水下目标跟踪定位方法，包括以下步骤：

(1)部署水下目标跟踪定位系统：在水域内安装m个水听器，在跟踪定位目标上安装超声波换能器和水压深度计；

(2)在水域内建立三维坐标系，获取水听器坐标(xj,yj,zj)、跟踪定位目标与m个水听器之间的测量距离sj、定位目标的深度h，其中j∈(1,2,...,m)，表示水听器序号；记跟踪定位目标在k时刻的坐标值为Xk＝(xk,yk,h)；

(3)根据跟踪定位目标与水听器之间的测量距离建立k时刻测量方程：

其中表示k时刻目标与第j个水听器之间的测量距离，为k时刻定位目标与第j个水听器之间的实际距离，为k时刻测量时引入的噪声，是方差为σ²的高斯白噪声；

(4)应用遗传粒子滤波算法对水下目标进行跟踪定位，即预测跟踪定位目标在k时刻的坐标值。

优选地，水听器以一定阵列的形式分布在水域内。

优选地，三维坐标系以任意一个水听器位置为坐标原点。

具体地，步骤(4)具体包括如下步骤：

(41)算法参数初始化：定义k时刻适应度函数：

其中S⁰k|k-1为预测测量距离；

定义S1,k＝{S1,S2,…,Sk}，X1,k＝{X1,X2,…,Xk}；以前一时刻的预测结果作为初始状态，根据先验概率在取值范围内采样，得到一个初始的N点目标坐标粒子集其中每个粒子表示跟踪定位目标的一个可能坐标值，i为粒子序号；令每个粒子权值的初始值为1/N，重采样门限为Nth＝N/2；

(42)在k时刻进行重要性采样，得到k时刻的目标坐标粒子集其中表示k时刻目标坐标粒子集中的第i个粒子，即k时刻跟踪定位目标的一个可能坐标值；为此粒子权值；

(43)判断是否要重采样：如果则进行重采样；否则粒子集即为预测粒子集；

(44)如果需要重采样，对目标坐标粒子集重采样获得预测粒子集其中表示k时刻目标坐标粒子集中的第i个粒子的预测值，为此粒子权值；

(45)根据预测粒子集计算跟踪定位目标坐标在k时刻的预测坐标值

(46)若跟踪定位未完成则进行状态更新，跳转到步骤(42)预测下一时刻的坐标值。

具体地，步骤(42)具体包括如下步骤：

(421)对重要性函数进行采样：从重要性函数中采样得到粒子集其中表示k时刻目标坐标粒子集中的第i个粒子，代表粒子权值；优选地，重要性函数选取概率密度函数p(Xk|Xk-1)；

(422)权值更新：计算每个粒子的权值

其中i∈(1,...,N)；

(423)权值归一化：

具体地，步骤(44)具体包括如下步骤：

(441)选择新粒子集：采用轮盘赌算法，以粒子权值为选择概率选择N个新粒子构成新粒子集

(442)交叉操作：新旧粒子集中分别随机取对应粒子进行交叉，产生两个新粒子，任选其一代替重复操作产生新粒子集其中交叉概率为：

k1为防止种群停止进化设置的偏移量，0<k1<1；

(443)变异操作：在粒子集中随机选取粒子进行变异操作，即以变异概率改变粒子某一些基因值为其他值，产生预测粒子构成预测粒子集变异概率为：

其中k2为偏移量，0<k2<1。

有益效果：与现有技术相比，本发明公开的水下目标跟踪定位方法通过采用基线原理结合遗传粒子滤波算法对水下目标进行跟踪定位求解，极大的提高了目标定位精度，且其具有有效性、鲁棒性和实时性。

附图说明

图1为本发明应用遗传粒子滤波算法对水下目标跟踪定位的流程图；

图2实施例中水下目标跟踪定位系统部署示意图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明。

图1为本发明公开的基于遗传粒子滤波算法对水下目标跟踪定位的流程图，包括以下步骤：

(1)部署水下目标跟踪定位系统：在水域内安装m个水听器，在跟踪定位目标上安装超声波换能器和水压深度计；

本实施例中采用3个水听器以三角阵列形式分布在水域内。

(2)在水域内建立三维坐标系，获取水听器坐标(xj,yj,zj)、跟踪定位目标与m个水听器之间的测量距离sj、定位目标的深度h，其中j∈(1,2,...,m)，表示水听器序号；记跟踪定位目标在k时刻的坐标值为Xk＝(xk,yk,h)；

水域内水听器有3个，以水听器1位置为原点建立三维坐标系，取东为x轴，北为y轴，深度方向为z轴，目标即换能器与水听器的距离通过水声测距原理得到，深度信息由水压计测得。水听器的坐标记为(xj,yj,zj),j＝1,2,3，目标与水听器之间的距离记为sj,j＝1,2,3，目标的深度为h，目标的xy平面坐标为(x,y)；其中有：

上式中c为声波传播速度，dj是时延估计测量值，为定位目标与第j个水听器之间的实际距离，vj为测量时引入的噪声，可认为是独立同分布的方差为σ²的高斯白噪声。

(3)根据跟踪定位目标与水听器之间的测量距离建立k时刻测量方程；

目标位于水域内，根据几何定律，目标坐标与水听器坐标之间存在以下定位关系：

记，S＝[s1,s2,s3]，k时刻目标的XY平面坐标即状态量为Xk＝(xk,yk)，可得k时刻系统测量方程为：

即：

其表示k时刻目标与第j个水听器之间的测量距离，为定位目标与第j个水听器之间的实际距离，为k时刻测量时引入的噪声，是方差为σ²的高斯白噪声；

k时刻的状态方程为：

Xk＝AXk-1+Buk

上式中uk的每一分量均为服从高斯分布N(0,Q)的白噪声，A为状态转移矩阵，根据目标运动状态确定；矩阵B为

其中，T是系统观察周期。

(4)应用遗传粒子滤波算法对水下目标进行跟踪定位，即预测跟踪定位目标在k时刻的坐标值，具体包括如下步骤：

(41)算法参数初始化：获取测量值，定义k时刻适应度函数：

其中S⁰k|k-1为预测测量距离；

定义S1,k＝{S1,S2,…,Sk}，X1,k＝{X1,X2,…,Xk}；以前一时刻的预测结果作为初始状态，根据先验概率在取值范围内采样，得到一个初始的N点目标坐标粒子集其中每个粒子表示跟踪定位目标的一个可能坐标值，i为粒子序号；令每个粒子权值的初始值为1/N，重采样门限为Nth＝N/2；

(42)在k时刻进行重要性采样，得到k时刻的目标坐标粒子集其中表示k时刻目标坐标粒子集中的第i个粒子，即k时刻跟踪定位目标的一个可能坐标值；为此粒子权值；

重要性采样包括如下步骤：

(421)对重要性函数进行采样：从重要性函数中采样得到粒子集其中表示k时刻目标坐标粒子集中的第i个粒子，代表粒子权值；重要性函数选取为概率密度函数p(Xk|Xk-1)；

(422)权值更新：计算每个粒子的权值

其中i∈(1,...,N)；

(423)权值归一化：

(43)判断是否要重采样：

如果则进行重采样；否则粒子集即为预测粒子集；

(44)如果需要重采样，对目标坐标粒子集重采样获得预测粒子集其中表示k时刻目标坐标粒子集中的第i个粒子的预测值，为此粒子权值；

重采样获得预测粒子集包括如下步骤：

(441)选择新粒子集：采用轮盘赌算法，以粒子权值为选择概率选择N个新粒子构成新粒子集

(442)交叉操作：新旧粒子集中分别随机取对应粒子进行交叉，产生两个新粒子，任选其一代替重复操作产生新粒子集其中交叉概率为：

k1为防止种群停止进化设置的偏移量，0<k1<1；

(443)变异操作：在粒子集中随机选取粒子进行变异操作，即以变异概率改变粒子某一些基因值为其他值，产生预测粒子构成预测粒子集变异概率为：

其中k2为偏移量，0<k2<1。

(45)根据预测粒子集计算跟踪定位目标坐标在k时刻的预测坐标值

(46)若跟踪定位未完成则进行状态更新，跳转到步骤(41)预测下一时刻的坐标值，直至目标跟踪结束。