CN102830402B - 水下传感器网络目标跟踪***及方法 - Google Patents

Abstract

本发明公开了一种水下传感器网络目标跟踪***和方法。***包括陆上网络和水域网络，水域网络包括多个水下目标跟踪簇和一个水面处理中心，水下目标跟踪簇包括若干水下传感器节点和一个簇头节点。本发明在修正各个传感器节点所采集信号时延的基础上结合自适应UKF和PF对目标状态进行跟踪。修正时延降低了因传感器节点同一时刻采集的信号不同源所造成的跟踪误差，使跟踪更可靠；自适应UKF自动根据目标机动情况调整***噪声加权系数以更贴切的描述目标实际运动状态，克服了目标运动模型在目标机动时有失准确性的缺点，并为粒子滤波提供了包含更多后验信息的重要性函数，减轻了粒子退化，提高了跟踪精度。

Description

水下传感器网络目标跟踪***及方法

技术领域

本发明涉及一种传感器网络***，特别涉及一种水下传感器网络目标跟踪***及方法，属于水下传感器网络信息处理技术领域。

背景技术

海洋占了地球三分之二的表面积，对人类的生存和发展有着至关重要的意义。目前人类对海洋的了解和开发还只是初级阶段，怎样更深入地认识海洋，更合理的使用海洋资源是一件意义深远的事情。随着陆上无线传感器网络的不断成熟和发展，水下传感器网络作为陆上无线传感器网络向水下的延伸也不断发展。水下传感器网络提供了一个更全面和深刻认识和使用海洋资源的途径。水下被动目标跟踪无论在民事还是军事方面都有广阔的应用前景。被动跟踪只能利用目标的方位信息，传统的单站水下目标跟踪方法受到***可观测性的限制，要想保证滤波收敛，载体的运动阶数必须高于目标运动阶数。在跟踪时，往往需要载体机动，制定机动策略，使算法复杂度增加，因此水下传感器网络需要解决这一技术问题，提高跟踪精度和跟踪稳定性。

由于和陆上传感器网络所面临环境不同，从通信方式到网络协议、拓扑结构等，水下传感器网络有着自己的特点，已有的陆上传感器网络技术不能照搬到水下传感器网络。水声通信的低速率、高误码率、低带宽、高时延以及水下环境复杂可变带来的不可靠性限制了水下传感器网络技术的发展。目前，面向水下传感器网络的的应用还不够成熟，大多数都停留在理论阶段，离实际应用还有一定距离。基于水下传感器网络的被动目标跟踪更是如此。水下传感器网络的被动目标跟踪属于非线性状态估计问题，实际环境中的噪声也多不符合高斯假设，上世纪90年代发展起来粒子滤波算法作为非线性非高斯问题目前的最合理解法在目标跟踪方面得到广泛应用。但是粒子滤波存在粒子退化问题，为了解决这个问题往往采用优选重要性密度函数和重采样的方法。优选重要性密度函数使重要性函数包含了更多的后验信息可以提高滤波精度，更快的跟踪目标状态变化。采用EKF（扩展卡尔曼滤波）或者UKF（无迹卡尔曼滤波）产生重要性密度函数的建议分布是常用的优选重要性密度函数的方法，其中UKF计算相对于简单，且在同阶的情况下能达到更高的精度。现有技术中，各种改进的粒子滤波算法对这些问题有所考虑，但是没有结合水下长时延的环境。水下传感器网络采用声通信的方式带来了信号延时的问题，由于声波在水中的传播速度低，信号时延大，时延带来的误差使得多个传感器测的数据不是目标同一时刻的信息，增大了跟踪误差。并且大多水下传感器网络目标跟踪滤波算法多假设目标运动方程的***噪声为强度始终不变的高斯白噪声，不能很好地反映目标实际运动，尤其当目标机动时，这种噪声假设偏差更大，使滤波误差增大。大多数的算法研究也停留在二维平面范围内，没有考虑实际三维情况，和实际有一定差距。因此，针对水下特殊环境研究相适应的高精度的算法和相应的实用性强的应用***是水下传感器网络目标跟踪的发展方向。

发明内容

本发明的目的在于提供一种水下传感器网络目标跟踪***及方法，该***能耗较低，通信量较小，实用性较强。在跟踪方法上以修正时延自适应UKF_PF算法克服水下声信号长时延带来的问题，以适应了水下环境，且能够自动适应目标机动，提高跟踪精度。

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

一种水下传感器网络目标跟踪***，包括陆上网络部分和水域网络部分。

所述水域网络部分包括多个水下目标跟踪簇1和一个水面处理中心2；所述每个水下目标跟踪簇1包括多个水下传感器节点3和一个簇头节点4，所述水下传感器节点3采集水下目标的辐射噪声，得到目标方位信息并将这些信息传递给簇头节点4；所述簇头节点4，接收水下传感器节点3传送过来的目标方位信息，估计目标状态并将目标状态信息上传至水面处理中心2；所述簇头节点4还适时对簇内的水下传感器节点3的工作状态进行调整，并向其它簇头节点传递目标的状态信息以便用于下一时刻的状态估计。

所述水面处理中心2，融合水下各个簇头节点传送来的局部航迹信息，形成对目标状态的整体估计，并将目标状态有关信息发送给陆上网络部分。

所述陆上网络部分包括一个岸边基站5和多个远程指控中心6。

所述岸边基站5，用于接收水面处理中心2传递来的目标状态信息并进行显示，并将目标状态有关信息连接至网络。

所述远程指挥中心6，用于通过网络远程查看目标的各种状态信息，以服务于更高一层的应用。

一种水下传感器网络目标跟踪***的水下簇头节点对目标状态的估计方法，该方法为修正时延自适应UKF_PF算法，包括如下步骤：

（1）初始化，建立目标运动模型并给目标状态赋初值

建立目标的运动模型为匀速模型，将目标的加速度假设为过程噪声，并引入噪声权系数来反映目标机动或环境变化带来的过程噪声强度的变化，设目标加速度为零均值高斯分布，目标运动方程为X(k+1)=AX(k)+Γ(k)Bw(k)，观测方程为，Z(k)=H(X(k))+v(k)，其中X(k)=[x(k)y(k)z(k)v_x(k)v_y(k)v_z(k)]^T为目标k时刻的状态，x(k)、y(k)、z(k)为目标k时刻的位置坐标，v_x(k)、v_y(k)、v_z(k)为k时刻目标在x、y、z轴的速度；Z(k)=[Θ₁(k)，…，Θ_N(k)]为k时刻簇内n个传感器节点的测量值i＝1，...，n组成的矩阵，φ_i(k)为方位角，为俯仰角，w(k)为k时刻的过程噪声，v(k)为k时刻的测量噪声， $A=\left[\begin{array}{cccccc}1& 0& 0& \mathrm{Ts}& 0& 0\\ 0& 1& 0& 0& \mathrm{Ts}& 0\\ 0& 0& 1& 0& 0& \mathrm{Ts}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$ 为***矩阵， $B=\left[\begin{array}{ccc}{\mathrm{Ts}}^2/2& 0& 0\\ 0& {\mathrm{Ts}}^2/2& 0\\ 0& 0& {\mathrm{Ts}}^2/2\\ \mathrm{Ts}& 0& 0\\ 0& \mathrm{Ts}& 0\\ 0& 0& \mathrm{Ts}\end{array}\right]$ 为控制矩阵，Ts是目标跟踪间隔时间， $H=\left[\begin{array}{ccc}\arctan \left(\frac{y\left(k\right)-y_1}{x\left(k\right)-x_1}\right)& ...& \arctan \left(\frac{y\left(k\right)-y_n}{x\left(k\right)-x_n}\right)\\ \arctan \left(\frac{z\left(k\right)-z_1}{\sqrt{{(x\left(k\right)-x_1)}^2+{(y\left(k\right)-y_1)}^2}}\right)& ...& \arctan \left(\frac{z\left(k\right)-z_n}{\sqrt{{(x\left(k\right)-x_n)}^2+{(y\left(k\right)-y_n)}^2}}\right)\end{array}\right]$ 为测量方程矩阵，x_i、y_i、z_i为簇内第i个传感器位置坐标，Γ(k)=diag(q_x(k)q_x(k)q_y(k)q_y(k)q_z(k)q_z(k)}，是过程噪声的权系数矩阵，可根据目标机动程度自动调整以改变***噪声的强度，q_x(k)、q_y(k)、q_z(k)分别是x、y、z方向的权系数，可取不同值以反映各个方向上目标机动的不同，给出目标初始状态估计X₀，初始状态分布p(X₀)，产生初始粒子集，其中 $X_0^i~p\left(X_0\right),$ 权值 $w_0^i=1/N,$ i=1，…，N；

（2）进行目标状态估计

步骤（2）进一步包括如下步骤：

（21）修正时延

根据上一时刻目标位置 ${\mathrm{\τ}}_i\left(k\right)=\sqrt{{(x(k-1)-x_i)}^2+{(y(k-1)-y_1)}^2+{(z(k-1)-y_i)}^2}/c,$ 其中τ_i(k)是k时刻目标辐射噪声传递到簇内第i个传感器节点所需时间，x(k-1)、y(k-1)、z(k-1)为估计得出的目标上一时刻在x、y、z轴的位置，c为声波在水下的传播速度，以簇头节点为参考，取出当前时刻用于目标状态估计的各个节点方位信息，{Θ_i(k+(τ_i(k)-τ₀(k))/Ts)}，其中τ₀(k)为k时刻目标到簇头节点的时延，计算出当前目标状态估计的时间间隔，Ts-(τ₀(k)-τ₀(k-1))，用其代替原来目标运动模型中的时间间隔Ts；

（22）根据量测残差自动调整权系数

对每个采样得到的粒子引入中间参量， ${\mathrm{\λ}}_x^i={\mathrm{\αq}}_x^2\left(k\right)+\mathrm{\β}(v_x^{\′2}\left(k\right)-s_{\mathrm{xx}}^{\′})/Q_{\mathrm{xx}},$ ${\mathrm{\λ}}_y^i={\mathrm{\αq}}_y^2\left(k\right)+\mathrm{\β}(v_y^{\′2}\left(k\right)-s_{\mathrm{yy}}^{\′})/Q_{\mathrm{yy}},$ ${\mathrm{\λ}}_z^i={\mathrm{\αq}}_z^2\left(k\right)+\mathrm{\β}(v_z^{\′2}\left(k\right)-s_{\mathrm{zz}}^{\′})/Q_{\mathrm{zz}},$ 其中 $v^{\′}\left(k\right)=H^{-1}\left(X_k^i\right)-Z\left(k\right),$ s′_xx、s′_yy、s′_zz为 $S^{\′}\left(k\right)=H^{-1}\left(P_{{\hat{y}}_k{\hat{y}}_k}^i\right)$ 的对角线元素，表示残差在三维空间x、y、z方向上的方差，是第i个粒子用标准UKF计算得到的量测残差协方差矩阵；Q_xx、Q_yy、Q_zz为过程噪声协方差阵对角线上的值，表示x、y、z方向上的方差，α+β=1，β越大，对目标机动的自适应能力越强；若则否则同理可得 既得到了第i个粒子所对应下一时刻噪声权系数矩阵 ${\mathrm{\Γ}}_{k+1}^i=\mathrm{diag}\left\{\begin{array}{cccccc}{q}_x^i(k+1)& q_x^i(k+1)& q_y^i(k+1)& q_y^i(k+1)& q_z^i(k+1)& q_z^i(k+1)\end{array}\right\};$

（23）对于每个粒子用自适应UKF算法计算估计值和方差状态预测协方差矩阵在标准UKF算法所得结果的基础上加上其它预测更新过程同标准UKF滤波算法；

（24）对于每个粒子，采样得到新的粒子并计算权值 $w_k^i=w_{k-1}^i\frac{p\left(Z\left(k\right)\right|{\tilde{X}}_k^i)p\left({\tilde{X}}_k^i\right|X_{k-1}^i)}{q\left({\tilde{X}}_k^i\right|X_{0:k-1}^i,Z_{1:k})},$ 其中重要性密度函数 $q\left({\tilde{X}}_k^i\right|X_{0:k-1}^i,Z_{1:k})=N({\tilde{X}}_k^i,{\mathrm{\Σ}}_{{\hat{X}}_k^i}),$ Z_1：k表示从开始到第k时刻观测值的集合；

（25）判断是否需要重采样，若小于事先确定的阈值则进入步骤（26），否则令 $\{X_k^i,i=1,\·\·\·N\}=\{{\tilde{X}}_k^i,i=1,\·\·\·N\},$ 进入步骤（27）；

（26）重采样，生成新的粒子集合且满足式中Pr()表示概率，重新定义权值

（27）为解决重采样引起的粒子耗尽问题，引入马尔科夫链蒙特卡洛移动（MCMC）步骤；

（28）目标状态估计和估计方差更新： $p\left(k\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i(X_k^i-\hat{X}\left(k\right)){(X_k^i-\hat{X}\left(k\right))}^T;$

（29）进入下一时刻，重复步骤（21）—（28）。

与现有技术相比，本发明的有益效果是：

1．水下传感器网络簇内每个节点在不同位置对目标进行观测，相当于单站测量时载体机动到不同位置，克服了单站测量中目标状态不可测问题，避免了机动策略的制定，同时多传感器同时测量也提高了跟踪精度和对数据丢失的鲁棒性。

2．在目标运动过程中每一阶段只有一个簇内的节点对其辐射噪声进行采集并计算得到方位角信息，其他簇内节点都处于休眠状态，节省了节点能耗，提高了节点寿命。

3．传感器节点只向簇头节点传递目标方位角信息，各个簇头节点之间只传递后验概率密度函数和粒子集，簇头节点只向水面基站传递目标状态信息和协方差，整体通信量小，适应水下通信带宽低的环境特点。

4．针对水下声信号通信速度慢，辐射噪声从目标到各个测量节点的传输时延不同的问题，本发明以簇头节点为时间参考节点对齐了各个节点的测量值，减小了因为时延带来的跟踪误差，适应通信延时大的环境。

5．本发明利用自适应UKF算法预测目标状态及方差，重要性密度函数的生成充分利用了当前观测信息，提高了粒子集的精度，并且能够自动调整***噪声权系数以适应目标运动实际运动状态，减小了目标机动时的跟踪误差，且重采样只在满足一定条件才进行，减少了粒子数，提高了计算速度。

附图说明

图1是本发明水下传感器网路目标跟踪***图；

图2是本发明水下传感器节点硬件框图；

图3是本发明水下传感器网络功能层次划分图；

图4是本发明水下目标跟踪簇工作流程图；

图5是本发明水面基站的航迹融合示意图；

图6是本发明修正时延自适应UKF-PF算法流程图；

图7是本发明通信时延示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

如图1所示，一种水下传感器网络目标跟踪***，包括陆上网络部分和水域网络部分。

所述水域网络部分包括多个水下目标跟踪簇1和一个水面处理中心2；所述每个水下目标跟踪簇1包括多个水下传感器节点3和一个簇头节点4，所述水下传感器节点3采集水下目标的辐射噪声，得到目标方位信息并将这些信息传递给簇头节点4；所述簇头节点4，接收水下传感器节点3传送过来的目标方位信息，利用修正时延自适应UKF-PF算法估计目标状态并将目标状态信息上传至水面处理中心2；所述簇头节点4还适时对簇内的水下传感器节点3的工作状态进行调整，并向其它簇头节点传递目标的状态信息以便用于下一时刻的状态估计。

所述水面处理中心2，融合水下各个簇头节点传送来的局部航迹信息，形成对目标状态的整体估计，并将目标状态有关信息发送给陆上网络部分。

所述陆上网络部分包括一个岸边基站5和多个远程指控中心6。

所述岸边基站5，用于接收水面处理中心2传递来的目标状态信息并进行显示，并将目标状态有关信息连接至网络。

所述远程指挥中心6，用于通过网络远程查看目标的各种状态信息，以服务于更高一层的应用。

水域网络通过水声无线通信方式组网和通信，陆上网络通过无线电波通信。陆上基站和远程指挥中心可以实时产看目标运动状态，同时水面上的船只还可以通过无线电波方式和水面处理中心通信，得到实时的目标运动信息，以便各种应用。

水下传感器节点布放在海底，并且装备有一个浮标，浮标可以拉着水下传感器节点移向海洋表面。水下传感器节点的深度可以利用水下传感器节点自带的电子设备调节浮标拉着的水下传感器节点绳子的长度来控制。各个传感器节点在三维海洋空间中组成立体传感器网络。传感器节点硬件组成框图如图2所示，换能器和调制解调器模块负责组网相关和数据传输相关的通信工作；传感器和数据采集接口负责采集目标辐射噪声并得到目标的方位角信息；存储模块可以存储采集到目标信和和其它关键数据；控制器协调控制整个传感器节点的工作；电源负责给传感器节点的各个部分提供能量。

水域网络层次结构如图3所示。物理层负责基本的调制解调工作，数据链路层解决了节点接入网络的问题，路由层选择从源节点到目的节点的路由途径，传输层保证数据传输的可靠性，节点时间同步和定位为上层应用提供支撑，目标跟踪属于具体应用。

本实施例按照本发明提供的水下传感器网络目标跟踪工作步骤，对水下航行器进行目标跟踪。

水域网络部分中的水下目标跟踪簇的工作流程如图4所示，步骤如下：

（1）完成网络配置并进行网络初始化。水面处理中心可通过无线方式和卫星通信，用GPS方式得到自己的地理位置，并为***提供基准时间标准。按照最小ID分簇方法划分簇并选择簇头节点，簇头节点以水面处理中心为参照对准自己的时间和位置坐标，簇内传感器节点以簇头节点为基准对准自己的时间和位置坐标。

（2）簇头节点根据是否在本簇范围内检测到目标或者是否有相邻簇头节点传送来的相关目标状态信息来决定是否启动簇内节点进行目标跟踪。如果检测到目标或者有传送来的信息，则进入步骤（3），否则继续等待。

（3）簇头节点唤醒簇内休眠节点，簇内传感器节点采集目标辐射噪声，得到目标方位信息，并将这些信息发送给簇头节点。

（4）簇头节点依据传感器节点传送来的目标方位信息用修正时延自适应UKF-PF算法估计目标状态，将当前时刻目标状态传送给水面基站。

（5）簇头节点监测目标是否超出的本簇的跟踪范围，若没有超出本簇跟踪范围则重复步骤（2）—（4），若超出则进入步骤（6）。

（6）簇头节点选择距离自己最近的簇头节点为下一个簇头节点，将目标当前时刻状态估计相关信息、后验概率密度函数和粒子集传递给下一个簇头节点，并将本簇内节点状态调整到休眠。

（7）重复步骤（2）—（6）。

水域网络部分中的水面基站将接收到目标当前时刻状态将其作为初始状态（刚开始跟踪）或者将其和已有目标过去时刻状态进行融合（已经跟踪一段时间）得到目标从开始到当前时刻的全部状态，包括航迹、速度等，并将这些信息实时传送给岸边基站。水面基站的航迹融合示意图如图5所示。

陆上网络部分中的岸边基站实时接收水面基站传送过来的目标状态信息，并将这些信息连入已有的陆上网络，远程指控中心可以实时产看目标状态信息，并根据这些信息做进一步的高层应用，如态势评估等。

步骤（4）所用到的自适应UKF_PF算法流程如图6所示，进一步包括：

（1）初始化，建立目标运动模型并给目标状态赋初值：

建立目标的运动模型为匀速模型，将目标的加速度假设为过程噪声，并引入噪声权系数来反映目标机动或环境变化带来的过程噪声强度的变化。设目标加速度为零均值高斯分布，目标运动方程为X(k+1)=AX(k)+Γ(k)Bw(k)，观测方程为，Z(k)=H(X(k))+v(k)。其中X(k)=[x(k)y(k)z(k)v_x(k)v_y(k)v_z(k)]^T为目标k时刻的状态，x(k)、y(k)、z(k)为目标k时刻的位置坐标，v_x(k)、v_y(k)、v_z(k)为k时刻目标在x、y、z轴的速度；Z(k)=[Θ₁(k)，…，Θ_N(k)]为k时刻簇内n个传感器节点的测量值i＝1，...，n组成的矩阵，φ_i(k)为方位角，为俯仰角；w(k)为k时刻的过程噪声，v(k)为k时刻的测量噪声； $A=\left[\begin{array}{cccccc}1& 0& 0& \mathrm{Ts}& 0& 0\\ 0& 1& 0& 0& \mathrm{Ts}& 0\\ 0& 0& 1& 0& 0& \mathrm{Ts}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$ 为***矩阵， $B=\left[\begin{array}{ccc}{\mathrm{Ts}}^2/2& 0& 0\\ 0& {\mathrm{Ts}}^2/2& 0\\ 0& 0& {\mathrm{Ts}}^2/2\\ \mathrm{Ts}& 0& 0\\ 0& \mathrm{Ts}& 0\\ 0& 0& \mathrm{Ts}\end{array}\right]$ 为控制矩阵，Ts是目标跟踪间隔时间； $H=\left[\begin{array}{ccc}\arctan \left(\frac{y\left(k\right)-y_1}{x\left(k\right)-x_1}\right)& ...& \arctan \left(\frac{y\left(k\right)-y_n}{x\left(k\right)-x_n}\right)\\ \arctan \left(\frac{z\left(k\right)-z_1}{\sqrt{{(x\left(k\right)-x_1)}^2+{(y\left(k\right)-y_1)}^2}}\right)& ...& \arctan \left(\frac{z\left(k\right)-z_n}{\sqrt{{(x\left(k\right)-x_n)}^2+{(y\left(k\right)-y_n)}^2}}\right)\end{array}\right]$ 为测量方程矩阵，x_i、y_i、z_i为簇内第i个传感器位置坐标；Γ(k)=diag{q_x(k)q_x(k)q_y(k)q_y(k)q_z(k)q_z(k)}，是过程噪声的权系数矩阵，可根据目标机动程度自动调整以改变***噪声的强度，q_x(k)、q_y(k)、q_z(k)分别是x、y、z方向的权系数，可取不同值以反映各个方向上目标机动的不同。给出目标初始状态估计X₀，初始状态分布p(X₀)，产生初始粒子集，其中 $X_0^i~p\left(X_0\right),$ 权值 $w_0^i=1/N,$ i=1，…，N。

（2）进行目标状态估计。

步骤（2）进一步包括如下步骤：

（21）修正时延：

根据上一时刻目标位置 ${\mathrm{\τ}}_i\left(k\right)=\sqrt{{(x(k-1)-x_i)}^2+{(y(k-1)-y_1)}^2+{(z(k-1)-y_i)}^2}/c,$ 其中τ_i(k)是k时刻目标辐射噪声传递到簇内第i个传感器节点所需时间，x(k-1)、y(k-1)、z(k-1)为估计得出的目标上一时刻在x、y、z轴的位置，c为声波在水下的传播速度。目标到各传感器的时延如图7所示。以簇头节点为参考，取出当前时刻用于目标状态估计的各个节点方位信息，{Θ_i(k+(τ_i(k)-τ₀(k))/Ts)}，其中τ₀(k)为k时刻目标到簇头节点的时延。计算出当前目标状态估计的时间间隔，Ts-(τ₀(k)-τ₀(k-1))，用其代替原来目标运动模型中的时间间隔Ts。

（22）根据量测残差自动调整权系数：

对每个采样得到的粒子引入中间参量， ${\mathrm{\λ}}_x^i={\mathrm{\αq}}_x^2\left(k\right)+\mathrm{\β}(v_x^{\′2}\left(k\right)-s_{\mathrm{xx}}^{\′})/Q_{\mathrm{xx}},$ ${\mathrm{\λ}}_y^i={\mathrm{\αq}}_y^2\left(k\right)+\mathrm{\β}(v_y^{\′2}\left(k\right)-s_{\mathrm{yy}}^{\′})/Q_{\mathrm{yy}},$ ${\mathrm{\λ}}_z^i={\mathrm{\αq}}_z^2\left(k\right)+\mathrm{\β}(v_z^{\′2}\left(k\right)-s_{\mathrm{zz}}^{\′})/Q_{\mathrm{zz}},$ 其中 $v^{\′}\left(k\right)=H^{-1}\left(X_k^i\right)-Z\left(k\right),$ s′_xx、s′_yy、s′_zz为（是第i个粒子用标准UKF计算得到的量测残差协方差矩阵）的对角线元素，表示残差在三维空间x、y、z方向上的方差，Q_xx、Q_yy、Q_zz为过程噪声协方差阵对角线上的值，表示x、y、z方向上的方差，α+β=1，β越大，对目标机动的自适应能力越强。若则否则同理可得 既得到了第i个粒子所对应下一时刻噪声权系数矩阵 ${\mathrm{\Γ}}_{k+1}^i=\mathrm{diag}\left\{\begin{array}{cccccc}{q}_x^i(k+1)& q_x^i(k+1)& q_y^i(k+1)& q_y^i(k+1)& q_z^i(k+1)& q_z^i(k+1)\end{array}\right\}.$

（23）对于每个粒子用自适应UKF算法计算估计值和方差状态预测协方差矩阵在标准UKF算法所得结果的基础上加上其它预测更新过程同标准UKF滤波算法。

（24）对于每个粒子，采样得到新的粒子并计算权值 $w_k^i=w_{k-1}^i\frac{p\left(Z\left(k\right)\right|{\tilde{X}}_k^i)p\left({\tilde{X}}_k^i\right|X_{k-1}^i)}{q\left({\tilde{X}}_k^i\right|X_{0:k-1}^i,Z_{1:k})},$ 其中重要性密度函数 $q\left({\tilde{X}}_k^i\right|X_{0:k-1}^i,Z_{1:k})=N({\tilde{X}}_k^i,{\mathrm{\Σ}}_{{\hat{X}}_k^i}),$ Z_1k表示从开始到第k时刻的观测值的集合。

（25）判断是否需要重采样，若小于总粒子数的三分之一则进入步骤（26），否则令 $\{X_k^i,i=1,\·\·\·N\}=\{{\tilde{X}}_k^i,i=1,\·\·\·N\},$ 进入步骤（27）。

（26）用残差重采样法进行重采样，生成新的粒子集合且满足（Pr()表示概率，），重新定义权值

（27）为解决重采样引起的粒子耗尽问题，引入马尔科夫链蒙特卡洛移动（MCMC）步骤。

（28）目标状态估计和方差更新： $p\left(k\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i(X_k^i-\hat{X}\left(k\right)){(X_k^i-\hat{X}\left(k\right))}^T.$

（29）进入下一时刻，重复步骤（21）—（28）。

除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。

Claims (1)

1.一种水下传感器网络目标跟踪***的水下簇头节点对目标状态的估计方法，水下传感器网络目标跟踪***包括陆上网络部分和水域网络部分；所述水域网络部分包括多个水下目标跟踪簇(1)和一个水面处理中心(2)；每个水下目标跟踪簇(1)包括多个水下传感器节点(3)和一个簇头节点(4)，所述水下传感器节点(3)采集水下目标的辐射噪声，得到目标方位信息并将这些信息传递给簇头节点(4)；所述簇头节点(4)，接收水下传感器节点(3)传送过来的目标方位信息，估计目标状态并将目标状态信息上传至水面处理中心(2)；所述簇头节点(4)还对簇内的水下传感器节点(3)的工作状态进行调整，并向其它簇头节点传递目标的状态信息以便用于下一时刻的状态估计；所述水面处理中心(2)，融合水下各个簇头节点传送来的局部航迹信息，形成对目标状态的整体估计，并将目标状态有关信息发送给陆上网络部分；所述陆上网络部分包括一个岸边基站(5)和多个远程指控中心(6)；所述岸边基站(5)，用于接收水面处理中心(2)传递来的目标状态信息并进行显示，并将目标状态有关信息连接至网络；所述远程指挥中心(6)，用于通过网络远程查看目标的各种状态信息，以服务于更高一层的应用；所述的水下传感器网络目标跟踪***的水下簇头节点对目标状态的估计方法为修正时延自适应UKF_PF算法，其特征在于，包括如下步骤：

(1)初始化，建立目标运动模型并给目标状态赋初值

建立目标的运动模型为匀速模型，将目标的加速度假设为过程噪声，并引入噪声权系数来反映目标机动或环境变化带来的过程噪声强度的变化，设目标加速度为零均值高斯分布，目标运动方程为X(k+1)＝AX(k)+Γ(k)Bw(k)，观测方程为，Z(k)＝H(X(k))+v(k)，其中X(k)＝[x(k) y(k) z(k) v_x(k) v_y(k) v_z(k)]^T为目标k时刻的状态，x(k)、y(k)、z(k)为目标k时刻的位置坐标，v_x(k)、v_y(k)、v_z(k)为k时刻目标在x、y、z轴的速度；Z(k)＝[Θ₁(k)，…，Θ_n(k)]为k时刻簇内n个传感器节点的测量值j＝1，...，n组成的矩阵，φ_j(k)为方位角，为俯仰角，w(k)为k时刻的过程噪声，v(k)为k时刻的测量噪声， $A=\left[\begin{array}{cccccc}1& 0& 0& \mathrm{Ts}& 0& 0\\ 0& 1& 0& 0& \mathrm{Ts}& 0\\ 0& 0& 1& 0& 0& \mathrm{Ts}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$ 为***矩阵， $B=\left[\begin{array}{ccc}{\mathrm{Ts}}^2/2& 0& 0\\ 0& {\mathrm{Ts}}^2/2& 0\\ 0& 0& {\mathrm{Ts}}^2/2\\ \mathrm{Ts}& 0& 0\\ 0& \mathrm{Ts}& 0\\ 0& 0& \mathrm{Ts}\end{array}\right]$ 为控制矩阵，Ts是目标跟踪间隔时间， $H=\left[\begin{array}{ccc}\arctan \left(\frac{y\left(k\right)-y_1}{x\left(k\right)-x_1}\right)& ...& \arctan \left(\frac{y\left(k\right)-y_n}{x\left(k\right)-x_n}\right)\\ \arctan \left(\frac{z\left(k\right)-z_1}{\sqrt{{(x\left(k\right)-x_1)}^2+{(y\left(k\right)-y_1)}^2}}\right)& ...& \arctan \left(\frac{z\left(k\right)-z_n}{\sqrt{{(x\left(k\right)-x_n)}^2+{(y\left(k\right)-y_n)}^2}}\right)\end{array}\right]$ 为测量方程矩阵，x_j、y_j、z_j为簇内第j个传感器位置坐标，Γ(k)＝diag{q_x(k) q_x(k) q_y(k) q_y(k) q_z(k) q_z(k)}，是过程噪声的权系数矩阵，可根据目标机动程度自动调整以改变***噪声的强度，q_x(k)、q_y(k)、q_z(k)分别是x、y、z方向的权系数，可取不同值以反映各个方向上目标机动的不同，给出目标初始状态估计X₀，初始状态分布p(X₀)，产生初始粒子集，其中权值 $w_0^i=1/N,i=1,...,N;$

(2)进行目标状态估计

步骤(2)进一步包括如下步骤：

(21)修正时延

根据上一时刻目标位置计算 ${\mathrm{\τ}}_j\left(k\right)=\sqrt{{(x(k-1)-x_j)}^2+{(y(k-1)-y_j)}^2+{(z(k-1)-z_j)}^2}/c,$ 其中τ_j(k)是k时刻目标辐射噪声传递到簇内第j个传感器节点所需时间，x(k-1)、y(k-1)、z(k-1)为估计得出的目标上一时刻在x、y、z轴的位置，c为声波在水下的传播速度，以簇头节点为参考，取出当前时刻用于目标状态估计的各个节点方位信息{Θ_j(k+(τ_j(k)-τ₀(k))/Ts)}，其中τ₀(k)为k时刻目标到簇头节点的时延，计算出当前目标状态估计的时间间隔Ts-(τ₀(k)-τ₀(k-1))，用其代替原来目标运动模型中的时间间隔Ts；

(22)根据量测残差自动调整权系数

对每个采样得到的粒子引入中间参量， ${\mathrm{\λ}}_y^i={\mathrm{\αq}}_y^2\left(k\right)+\mathrm{\β}({v^{\′}}_y^2\left(k\right)-s_{\mathrm{yy}}^{\′})/Q_{\mathrm{yy}},$ ${\mathrm{\λ}}_z^i={\mathrm{\αq}}_z^2\left(k\right)+\mathrm{\β}({v^{\′}}_z^2\left(k\right)-s_{\mathrm{zz}}^{\′})/Q_{\mathrm{zz}},$ 其中 $v^{\′}\left(k\right)=H^{-1}\left(X_k^i\right)-Z\left(k\right),$ s′_xx、s′_yy、s′_zz为 $S^{\′}\left(k\right)=H^{-1}\left(P_{{\hat{y}}_k{\hat{y}}_k}^i\right)$ 的对角线元素，表示残差在三维空间x、y、z方向上的方差，是第i个粒子用标准UKF计算得到的量测残差协方差矩阵；Q_xx、Q_yy、Q_zz为过程噪声协方差阵对角线上的值，表示x、y、z方向上的方差，α+β＝1，β越大，对目标机动的自适应能力越强；若则 $q_x^i(k+1)={\mathrm{\λ}}_x^i,$ 否则 $q_x^i(k+1)=0,$ 同理可得即得到了第i个粒子所对应下一时刻噪声权系数矩阵 ${\mathrm{\Γ}}_{k+1}^i=\mathrm{diag}\left\{\begin{array}{cccccc}{q}_x^i(k+1)& q_x^i(k+1)& q_y^i(k+1)& q_y^i(k+1)& q_z^i(k+1)& q_z^i(k+1)\end{array}\right\};$

(23)对于每个粒子用自适应UKF算法计算估计值和方差状态预测协方差矩阵在标准UKF算法所得结果的基础上加上其它预测更新过程同标准UKF滤波算法；

(24)对于每个粒子，采样得到新的粒子并计算权值 $w_k^i=w_{k-1}^i\frac{p\left(Z\left(k\right)\right|{\tilde{X}}_k^i)p\left({\tilde{X}}_k^i\right|X_{k-1}^i)}{q\left({\tilde{X}}_k^i\right|X_{0:k-1}^i,Z_{1:k})},$ 其中重要性密度函数 $q\left({\tilde{X}}_k^i\right|X_{0:k-1}^i,Z_{1:k})=N({\hat{X}}_k^i,{\mathrm{\Σ}}_{{\hat{X}}_k^i}),$ Z_1：k表示从开始到第k时刻观测值的集合；

(25)判断是否需要重采样，若小于事先确定的阈值则进入步骤(26)，否则令 $\{X_k^i,i=1,...N\}=\{{\tilde{X}}_k^i,i=1,...N\},$ 进入步骤(27)；

(26)重采样，生成新的粒子集合 $\{X_k^i,i=1,...N\}$ ，且满足 $\mathrm{Pr}(X_k^i={\tilde{X}}_k^i)=w_k^i,$ 式中Pr( )表示概率，重新定义权值

(27)为解决重采样引起的粒子耗尽问题，引入马尔科夫链蒙特卡洛移动(MCMC)步骤；

(28)目标状态估计和估计方差更新： $P\left(k\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i(X_k^i-\hat{X}\left(k\right)){(X_k^i-\hat{X}\left(k\right))}^T;$

(29)进入下一时刻，重复步骤(21)-(28)。