CN101644758A - 一种目标定位跟踪***及方法 - Google Patents

Abstract

本发明涉及一种目标定位跟踪***及方法。所述***包括：多个簇定位模块及指控模块；所述簇定位模块包括：多个传感器节点及簇头节点；所述簇头节点包括：初始化模块、粒子滤波模块、粒子权值计算模块、重采样判断模块及估计目标状态模块。本发明采用一种结合卡尔曼和粒子滤波的方法实现目标的被动定位，其在实现高精度定位的基础上，运算速度远远低于粒子滤波方法；采用多传感器基于对目标方位的观测最终实现对目标的定位跟踪，可以克服传统机载或舰载单站***在观测期间必须进行机动的约束，不需要观测平台的机动，提高了目标定位的灵活性，大大增加目标监测定位的区域面积，避免存在定位盲区的不足；具有很高的有效性、精确性及可行性。

Description

一种目标定位跟踪***及方法

技术领域

本发明涉及信息技术领域，特别涉及一种目标定位跟踪***及方法。

背景技术

由于现代信息技术、网络技术及无线通信技术的发展，推动了无线传感器网络技术的迅速发展。运动目标的定位与跟踪技术具有广阔的应用背景，涉及到军事和民用领域。传统的目标被动定位技术中，多采用基于纯方位测量的目标被动定位技术，其主要解决的问题是如何利用观测的目标方位信息来估计目标的运动参数进而实现目标的定位跟踪。***配置多为机载或舰载单站测量***，该***配置由于受到运动目标可观测性原理的限制，要实现对目标的定位，观测平台需要在观测期内进行机动，可有时候由于观测平台无法机动，近而不能实现目标的定位；无线传感器网络通过部署的多个传感器节点组成，通过无线通信方式形成的一个多跳的自组织的网络***。其目的是协作地感知、采集和处理网络覆盖区域中感知对象的信息。

从技术方面讲，基于纯方位目标运动分析问题本质上是一个非线性Bayes滤波问题。由于非线性的原因，其精确解通常并不解析，所以工程上多采用基于扩展卡尔曼滤波(EKF)的次优解，它是利用泰勒级数展开，取其一阶近似使模型线性化，再利用卡尔曼滤波(KF)算法进行求解。而且EKF滤波在模型非线性较强以及***噪声非高斯时估计的精度严重降低，并可能造成滤波器的发散。而无味卡尔曼(UKF)采用UT(Unscented Transformation)变化，虽然较EKF有更好的滤波效果，但都是在基于模型线性化和Gauss假设的条件下。因此人们一直在寻找适用于非线性非高斯Bayes滤波方法来提高估计精度。近年来国际上在该方向的研究取得了令人瞩目的成就，其中尤以粒子滤波(PF)受到更为广泛的关注，所以近年来又出现了基于粒子滤波方法实现目标的纯方位测量定位技术，该方法虽然比基于KF的方法定位精度高，但是该方法对实现后验概率的精确估计需要较多的粒子数，导致计算量大，计算时间长，不利于实时应用。

发明内容

为了克服对单站观测***的不可观测性和粒子滤波计算速度慢这两个不足，本发明提供了一种目标定位跟踪***及方法，该***及方法基于上述对传统定位技术的分析基础之上，采用一种新的目标定位技术实现运动目标的分析，可以有效克服单站测量必须进行机动的不足，而且采用一种改进的粒子滤波方法实现定位，其计算时间远远低于采用粒子滤波实现定位的方法。

为了达到上述目的，本发明提供的一种目标定位跟踪***，包括：多个簇定位模块及一指控模块。

所述簇定位模块包括：多个传感器节点及一簇头节点。

所述传感器节点，用于对其所在簇内区域进行监测，当有目标出现，则对目标的方位进行测量，并将该测量值与自身坐标信息发送到本簇内的簇头节点。

所述簇头节点，用于根据本簇内的传感器节点发送的测量值及其坐标信息，完成目标状态估计，实现目标的定位跟踪，并将定位跟踪结果发送到指控模块。

所述指控模块，用于收集并显示各簇头节点的定位跟踪结果。

所述簇头节点进一步包括：

一初始化模块，用于建立目标运动及观测方程、目标的初始状态分布函数和初始状态的估计方差，并随机产生N个粒子。

一粒子滤波模块，用于利用状态方程对每个粒子状态进行基于扩展卡尔曼滤波的预测、更新。

一粒子权值计算模块，用于计算粒子权值。

一重采样判断模块，用于根据有效粒子数N_eff判断是否要重采样，若有效粒子数N_eff小于预先设定值，则进行重采样，根据重要性密度函数重新采样N个粒子并分配权值。及

一估计目标状态模块，用于根据粒子的权值计算估计目标状态。

其中，所述初始化模块进一步包括：一建立目标运动及观测模型模块、一建立目标初始化状态及估计方差模块及一随机粒子产生模块。

所述建立目标运动及观测模型模块，用于建立目标的一阶马尔可夫***方程及目标观测的观测模型：

${\mathrm{\θ}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i;$

其中：X_T＝[x_T，v_x，y_T，v_y]表示目标的运动状态，x_T，y_T分别表示目标在X轴及Y轴的坐标值，v_x，v_y分别表示目标在在X轴及Y轴的绝对速度分量； $S^i=[s_x^i,s_y^i]$ i＝1，2…N，表示传感器节点i分别在X轴及Y轴的坐标，N表示观测到目标的传感器节点个数；θ_i表示目标与传感器节点的相对方位；n_i表示第i个传感器节点的测量噪声。

所述建立目标初始化状态及估计方差模块，用于建立目标的初始状态概率分布函数及估计方差。

所述初始状态概率分布函数为：X_k+1＝F_kX_k+G_kA_k。

其中：A_k＝[a_x，k，a_y，k]^T为***的处理噪声，即由于目标运动环境的不确定性导致分别在X和Y轴引起的加速度噪声，A_k～N(0，R_k)；

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{T}_s^2/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right];$

所述随机粒子产生模块，用于从初始状态分布p(x₀)中随机抽取N个初始粒子{x₀ ⁱ，i＝1，2…N}，与初始粒子对应的协方差矩阵为：{P₀ ⁱ，i＝1，2…N}。

其中，所述粒子滤波模块进一步包括：一预测模块和一更新模块。

所述预测模块，用于利用状态方程预测k时刻的目标的状态

及预测协方差矩阵

其中： ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k.$

所述更新模块：用于利用k时刻的测量值对粒子预测结果及估计协方差矩阵进行更新，得到k时刻更新值及协方差矩阵x_k ⁱ和P_k ⁱ。

其中： $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\∂h}{\∂x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

其中，所述粒子权值计算模块根据贝叶斯及粒子滤波理论计算粒子权值：

${\mathrm{\ω}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p(z_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

其中：q(x_k/x_0:k-1 ⁱ，z_1:k)是重要性密度函数，p(z_k/x_k ^j)是观测似然函数，p(x_k/x_k-1)是***一阶马尔可夫过程的转移概率。

其中，所述有效粒子数N_eff定义如下：

$N_{\mathrm{eff}}=\frac{1}{\underset{i-1}{\overset{N}{\mathrm{\Σ}}}{\left({\stackrel{\‾}{\mathrm{\ω}}}_k^i\right)}^2}.$

其中，所述传感器节点及簇头节点装配有GPS***，目标定位跟踪***采用GPS授时的方式实现网络的时间同步。

本发明提供的一种目标定位跟踪方法，包括如下步骤：

(1)完成***配置，包括传感器节点、簇头节点和指控***。

(2)完成网络初始化。

(3)传感器节点对所在簇内进行监测，依据采集信号判断是否有目标出现。

(4)当出现目标后，传感器节点对目标的方位进行测量。

(5)传感器节点将自己的方位测量值及自身坐标信息发送给本簇内的簇头节点。

(6)簇头节点根据本簇内的传感器节点发送的测量值及其坐标信息，完成目标状态估计，实现目标的定位跟踪，并将定位跟踪结果发送到指控模块；包括以下子步骤：

(61)初始化模块建立目标运动及观测方程、目标的初始状态分布函数和初始状态的估计方差，并随机产生N个粒子。

(62)粒子滤波模块利用状态方程对每个粒子状态进行基于扩展卡尔曼滤波的预测、更新。

(63)粒子权值计算模块计算粒子权值。

(64)重采样判断模块根据有效粒子数N_eff判断是否要重采样，若有效粒子数N_eff小于预先设定值，则进行重采样，根据重要性密度函数重新采样N个粒子并分配权值。

(65)估计目标状态模块根据粒子的权值计算估计目标状态，随观测时刻的增加，返回步骤(62)进行迭代算法。

(7)指控模块收集并显示各簇头节点的定位跟踪结果。

其中，所述步骤(2)包括以下子步骤：

(21)传感器节点完成对自己的编号，唯一标示自己的身份。

(22)传感器节点借助GPS***实现时间同步。

(23)传感器节点借助GPS***实现自身定位。

(24)依据传感器节点的通信距离对网络进行簇划分，选择簇头节点。

其中，所述步骤(61)包括以下子步骤：

(611)建立目标运动及观测模型模块建立目标的一阶马尔可夫***方程及目标观测的观测模型：

${\mathrm{\θ}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i;$

其中：X_T＝[x_T，v_x，y_T，v_y]表示目标的运动状态，x_T，y_T分别表示目标在X轴及Y轴的坐标值，v_x，v_y分别表示目标在在X轴及Y轴的绝对速度分量； $S^i=[s_x^i,s_y^i]$ i＝1，2…N，表示传感器节点i分别在X轴及Y轴的坐标，N表示观测到目标的传感器节点个数；θ_i表示目标与传感器节点的相对方位；n_i表示第i个传感器节点的测量噪声。

(612)建立目标初始化状态及估计方差模块建立目标的初始状态概率分布函数及估计方差。

所述初始状态概率分布函数为：X_k+1＝F_kX_k+G_kA_k。

其中：A_k＝[a_x，k，a_y，k]^T为***的处理噪声，即由于目标运动环境的不确定性导致分别在X和Y轴引起的加速度噪声，A_k～N(0，R_k)；

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{T}_s^2/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right];$

(613)随机粒子产生模块从初始状态分布p(x₀)中随机抽取N个初始粒子{x₀ ⁱ，i＝1，2…N}，与初始粒子对应的协方差矩阵为：{P₀ ⁱ，i＝1，2…N}。

其中，所述步骤(62)包括以下子步骤：

(621)预测模块利用状态方程预测k时刻的目标的状态

及预测协方差矩阵

其中： ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k.$

(622)更新模块利用k时刻的测量值对粒子预测结果及估计协方差矩阵进行更新，得到k时刻更新值及协方差矩阵x_k ⁱ和P_k ⁱ。

其中： $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\∂h}{\∂x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

其中，所述步骤(63)中，粒子权值计算模块根据贝叶斯及粒子滤波理论计算粒子权值：

${\mathrm{\ω}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p(z_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

其中：q(x_k/x_0:k-1 ⁱ，z_1:k)是重要性密度函数，p(z_k/x_k ^j)是观测似然函数，p(x_k/x_k-1)是***一阶马尔可夫过程的转移概率。

本发明的优点在于：

1、本发明提供的目标定位跟踪***及方法采用一种结合卡尔曼和粒子滤波的方法实现目标的被动定位，其在实现高精度定位的基础上，运算速度远远低于粒子滤波方法，具有很高的有效性、精确性及可行性。

2、本发明提供的目标定位跟踪***及方法采用基于传感器网络的***配置实现目标的运动分析，克服了单节点的可观测性原理的局限。

3、本发明提供的目标定位跟踪***及方法在进行目标的定位应用中，对***的配置借鉴无线传感器网络的技术特点，采用多传感器基于对目标方位的观测最终实现对目标的定位跟踪，该网络配置可以克服传统机载或舰载单站***在观测期间必须进行机动的约束，当采用多传感器测量的目标方位信息实现目标协同定位，不需要观测平台的机动，提高了目标定位的灵活性，而且采用该***配置实现可以大大增加目标监测定位的区域面积，避免存在定位盲区的不足。

附图说明

图1是本发明目标定位跟踪***配置示意图；

图2是本发明传感器节点设计功能模块图；

图3是本发明传感器节点功能划分图；

图4是本发明目标定位跟踪流程图；

图5是本发明定位***初始化流程图；

图6是本发明EKF-PF算法流程图；

图7是本发明实施例中传感器节点与目标轨迹跟踪的总体示意图；

图8是本发明实施例中目标跟踪的局部放大结果比较示意图；

图9是本发明实施例中对目标X轴和Y轴坐标的估计结果比较图；

图10是本发明实施例中对目标X轴和Y轴方向速度的估计结果比较图。

具体实施方式

下面结合附图及一个具体实施例对分发明做详细说明。

本实施例假设对水下一航行器目标进行定位与跟踪，本实施例的目标定位跟踪***，如图1所示，由多个簇定位模块及一个指控***组成；每个簇定位模块包括两个传感器节点和一个簇头节点。每个簇头节点进一步包括：一初始化模块、一粒子滤波模块、一粒子权值计算模块、一重采样判断模块及一估计目标状态模块。

传感器节点静态部署，也可以进行小范围的移动。其位置坐标可通过传感器节点利用自带的GPS装置完成自定位，也可以通过几个已知坐标的锚节点依据一定的自定位协议完成自定位；每个传感器节点需具有测向功能，这个功能可通过采用阵列信号处理手段实现目标的方位估计；为了进行数据的交互和组网功能，传感器节点需具有无线通信功能；簇头节点负责本小区域内目标定位跟踪，它可做为本小区的网关节点看待；簇头节点存储有该区域内所有传感器节点的位置坐标；并负责收集该簇内传感器节点所测量的目标方位信息；利用改进的快速EKF-PF算法实现对目标的定位跟踪；指控***负责收集各个区域内簇头节点定位跟踪结果，在终端对定位结果进行显示，以便让定位跟踪结果用于不同的应用需求。其中簇头节点可以具有测向功能，也可以不具有该功能，但是其必须具有无线通信功能。簇头节点可以事先规定，也可以在一个簇内动态选择。

对于整个定位***的同步问题，采用GPS授时的方式实现网络的时间同步。所以本***内节点需装配有GPS***。

对于定位***的网络互连，采用无线通信方式，对于一个簇内传感器节点之间没有通信链路，传感器节点之间无需链接进行数据交换。本***存在三种链路链接方式，第一就是传感器节点与负责该区域内的簇头节点之间的通信链路，负责将自己测量的方位信息发送给簇头节点实现EFK-PF定位跟踪算法；第二是簇头节点与指控***之间的通信链路，簇头节点负责将定位跟踪结果发送给指控***进行显示；第三类是各个区域簇头节点之间的通信链路，随着目标的运动，它进入不同的区域，则前一个区域的簇头节点将最终的定位结果发送给此时的簇头节点做为该簇内目标定位跟踪的初始结果。

图2给出了本发明中涉及到的传感器节点及簇头节点的设计模块，主要有四个模块组成。供电模块完成对整个节点的电能供给；处理模块是节点的计算单元，采用商业的DSP芯片，负责将传感器所采集数据进行分析和处理，并进行数据打包；通信模块，发送打包数据，同时接收其它节点发送的数据包与通信模块一起完成节点的无线通信任务。对于传感器节点来说，其通信模块器件包括无线射频通信***。传感器模块主要完成对目标信号数据进行采集。

基于传感器节点的在定位***中的应用，在网络七层协议的基础上对传感器节点数据处理功能进行了简单的划分，主要分为以下三个部分：通信***；支撑***；应用***。如图3所示，对于通信***主要由物理层，数据链层，网络层和传输层组成，负责完成节点的正常组网功能。支撑***包括节点同步与自定位，传感器节点可通过GPS装置完成传感器节点的时间同步与节点自身的定位技术。应用***主要在基于传感器采集数据的基础上负责完成传感器节点的测向功能，可借鉴阵列信号处理中的相关技术完成。

利用上述目标定位跟踪***对目标进行定位跟踪的方法，如图4所示，包括以下步骤：

(1)完成***配置，包括传感器节点、簇头节点和指控***。

(2)完成网络初始化，如图5所示，包括以下子步骤：

(21)传感器节点完成对自己的编号，唯一标示自己的身份。

(22)传感器节点借助GPS***实现时间同步。

(23)传感器节点借助GPS***实现自身定位。

(24)依据传感器节点的通信距离对网络进行簇划分，选择簇头节点。此步骤可以人为指定簇头，也可以随机在簇内选择簇头。

(3)传感器节点对所在簇内进行监测，依据采集信号判断是否有目标出现。

(4)当出现目标后，传感器节点对目标的方位进行测量。

(5)传感器节点将自己的方位测量值及自身坐标信息发送给本簇内的簇头节点。

(6)簇头节点根据本簇内的传感器节点发送的测量值及其坐标信息，完成目标状态估计，实现目标的定位跟踪，并将定位跟踪结果通过无线通信发送到指控模块；如图6所示，包括以下子步骤：

(61)初始化模块建立目标运动及观测方程、目标的初始状态分布函数和初始状态的估计方差，并随机产生N个粒子。

(611)建立目标运动及观测模型模块建立目标的一阶马尔可夫***方程及目标观测的观测模型：

${\mathrm{\θ}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i$

其中：X_T＝[x_T，v_x，y_T，v_y]表示目标的运动状态，x_T，y_T分别表示目标在X轴及Y轴的坐标值，v_x，v_y分别表示目标在在X轴及Y轴的绝对速度分量；假设目标与水下节点在同一个平面，节点处于静止状态， $S^i=[s_x^i,s_y^i]$ i＝1，2...N表示传感器节点i分别在X轴及Y轴的坐标，N表示观测到目标的传感器节点个数；θ_i表示目标与传感器节点的相对方位；n_i表示第i个传感器节点的测量噪声。***在k时刻的观测模型可以用矩阵形式表示：

${\stackrel{\→}{Z}}_K=\stackrel{\→}{h}({\stackrel{\→}{S}}_K,X_K^T)+{\stackrel{\→}{V}}_K$

其中： ${\stackrel{\→}{Z}}_K={[{\mathrm{\θ}}_1,{\mathrm{\θ}}_2...{\mathrm{\θ}}_N]}^T,$

$\stackrel{\→}{h}({\stackrel{\→}{S}}_k,X_k^T)={[h(S_1,X_k^T),h(S_2,X_k^T),...h(S_N,X_k^T),]}^T,$

$h(S_i,X_k^T)={\tan }^{-1}(y_T-s_y^i/x_T-s_x^i),$

${\stackrel{\→}{V}}_K={[n_1,n_2,...n_N]}^T.$

假定噪声为零均值高斯随机噪声，则Q_k为噪声协方差矩阵，且 $Q_k=\mathrm{diag}({\mathrm{\σ}}_1^2,{\mathrm{\σ}}_2^2...{\mathrm{\σ}}_N^2).$

(612)建立目标初始化状态及估计方差模块建立目标的初始状态概率分布函数及估计方差。

采用白噪声加速度模型(White Noise Acceleration，WNA)来描述目标的弱机动性。则所述初始状态概率分布函数为：X_k+1＝F_kX_k+G_kA_k。

其中：A_k＝[a_x，k，a_y，k]^T为***的处理噪声，即由于目标运动环境的不确定性导致分别在X和Y轴引起的加速度噪声，A_k～N(0，R_k)；

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{T}_s^2/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right].$

(613)随机粒子产生模块从初始状态分布p(x₀)中随机抽取N个初始粒子{x₀ ⁱ，i＝1，2…N}，与初始粒子对应的协方差矩阵为：{P₀ ⁱ，i＝1，2…N}。

通过上述测量方程和状态方程可以看出，基于水下多传感器方位TMA问题的状态方程是线性的，而测量方程是非线性的，而且是时变的，因此实际是一个时变非线性滤波问题。

针对传统的粒子滤波算法对上述问题的求解，影响粒子滤波估计效果的重要因素是重要性密度函数的选择，而对于最优重要性密度函数是后验概率密度，所以自然而然想到将扩展卡尔曼滤波方法与粒子滤波方法相结合，在k时刻先验粒子通过EKF滤波更新后，其值更加接近于后验概率，所以与PF相比，当达到同一性能所需的粒子数要少于PF算法，而且该算法对PF中粒子耗尽问题也有一定的抑制作用。

(62)粒子滤波模块利用状态方程对每个粒子状态进行基于扩展卡尔曼滤波的预测、更新。

(621)预测模块利用状态方程预测k时刻的目标的状态及预测协方差矩阵

其中： ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k.$

(622)更新模块利用k时刻的测量值对粒子预测结果及估计协方差矩阵进行更新，得到k时刻更新值及协方差矩阵x_k ⁱ和P_k ⁱ。

其中： $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\∂h}{\∂x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

(63)粒子权值计算模块计算粒子权值。

粒子权值计算模块根据贝叶斯及粒子滤波理论计算粒子权值：

${\mathrm{\ω}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p(z_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

其中：q(x_k/x_0:k-1 ⁱ，z_1:k)是重要性密度函数，p(z_k/x_k ^j)是观测似然函数，p(x_k/x_k-1)是***一阶马尔可夫过程的转移概率。

(64)重采样判断模块根据有效粒子数N_eff判断是否要重采样，若有效粒子数N_eff小于预先设定值，则进行重采样，根据重要性密度函数重新采样N个粒子并分配权值。

(65)估计目标状态模块根据粒子的权值计算估计目标状态，随观测时刻的增加，返回步骤(62)进行迭代算法。

(7)指控模块收集并显示各簇头节点的定位跟踪结果。

图7至图10是本实施例中采用EKF-PF算法实现目标定位的计算机仿真结果，为了本发明与传统EKF和PF方法比较，仿真过程对这两种定位方法也进行了仿真试验。图7和图8是跟踪的结果，图8是对图7局部的方法，这样更加直观的看出目标跟踪的效果。从图中可以看出，本发明的跟踪效果比EKF方法要好，和PF方法相比其效果相当，但是其计算时间远远要小于PF方法。图9分别给出了目标在X轴和Y轴坐标的估计值与真实值的对比结果，并接给出了三种不同方法下的误差曲线；图10则是对目标在X轴和Y轴速度的估计值，下表列出了本发明方法的运行时间，可以看出在达到同样的定位精度的情况下，本发明所采用的方法远远低于同样精度的PF方法。通过以上实施例的仿真结果，可以看出本发明方法的有效性和精确性，说明该方法在实际应用的可行性。

定位方法	PF	EKF-PF
			运算耗时(秒)	0.0822	0.0346

Claims (12)

1、一种目标定位跟踪***，包括：多个簇定位模块及一指控模块；

所述簇定位模块包括：多个传感器节点及一簇头节点；

所述传感器节点，用于对其所在簇内区域进行监测，当有目标出现，则对目标的方位进行测量，并将该测量值与自身坐标信息发送到本簇内的簇头节点；

所述簇头节点，用于根据本簇内的传感器节点发送的测量值及其坐标信息，完成目标状态估计，实现目标的定位跟踪，并将定位跟踪结果发送到指控模块；

所述指控模块，用于收集并显示各簇头节点的定位跟踪结果；

其特征在于，

所述簇头节点进一步包括：

一初始化模块，用于建立目标运动及观测方程、目标的初始状态分布函数和初始状态的估计方差，并随机产生N个粒子；

一粒子滤波模块，用于利用状态方程对每个粒子状态进行基于扩展卡尔曼滤波的预测、更新；

一粒子权值计算模块，用于计算粒子权值；

一重采样判断模块，用于根据有效粒子数N_eff判断是否要重采样，若有效粒子数N_eff小于预先设定值，则进行重采样，根据重要性密度函数重新采样N个粒子并分配权值；及

一估计目标状态模块，用于根据粒子的权值计算估计目标状态。

2、根据权利要求1所述的目标定位跟踪***，其特征在于，所述初始化模块进一步包括：一建立目标运动及观测模型模块、一建立目标初始化状态及估计方差模块及一随机粒子产生模块；

所述建立目标运动及观测模型模块，用于建立目标的一阶马尔可夫***方程及目标观测的观测模型：

${\mathrm{\θ}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i;$

其中：X_T＝[x_T，v_x，y_T，v_y]表示目标的运动状态，x_T，y_T分别表示目标在X轴及Y轴的坐标值，v_x，v_y分别表示目标在在X轴及Y轴的绝对速度分量； $S^i=[s_x^i,s_y^i]$ i＝1，2…N，表示传感器节点i分别在X轴及Y轴的坐标，N表示观测到目标的传感器节点个数；θ_i表示目标与传感器节点的相对方位；n_i表示第i个传感器节点的测量噪声；

所述建立目标初始化状态及估计方差模块，用于建立目标的初始状态概率分布函数及估计方差；

所述初始状态概率分布函数为：X_k+1＝F_kX_k+G_kA_k；

其中：A_k＝[a_x，k，a_y，k]^T为***的处理噪声，即由于目标运动环境的不确定性导致分别在X和Y轴引起的加速度噪声，A_k～N(0，R_k)；

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{T}_s^2/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right];$

所述随机粒子产生模块，用于从初始状态分布p(x₀)中随机抽取N个初始粒子{x₀ ⁱ，i＝1，2…N}，与初始粒子对应的协方差矩阵为：{P₀ ⁱ，i＝1，2…N}。

3、根据权利要求1所述的目标定位跟踪***，其特征在于，所述粒子滤波模块进一步包括：一预测模块和一更新模块；

所述预测模块，用于利用状态方程预测k时刻的目标的状态

及预测协方差矩阵

其中： ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k;$

所述更新模块：用于利用k时刻的测量值对粒子预测结果及估计协方差矩阵进行更新，得到k时刻更新值及协方差矩阵x_k ⁱ和P_k ⁱ；

其中： $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\∂h}{\∂x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

4、根据权利要求1所述的目标定位跟踪***，其特征在于，所述粒子权值计算模块根据贝叶斯及粒子滤波理论计算粒子权值：

${\mathrm{\ω}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p(z_k./x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

其中：q(x_k/x_0:k-1 ⁱ，z_1:k)是重要性密度函数，p(z_k/x_k ^j)是观测似然函数，p(x_k/x_k-1)是***一阶马尔可夫过程的转移概率。

5、根据权利要求1所述的目标定位跟踪***，其特征在于，所述有效粒子数N_eff定义如下：

$N_{\mathrm{eff}}=\frac{1}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\left({\stackrel{\‾}{\mathrm{\ω}}}_k^i\right)}^2}.$

6、根据权利要求1所述的目标定位跟踪***，其特征在于，所述传感器节点及簇头节点装配有GPS***，目标定位跟踪***采用GPS授时的方式实现网络的时间同步。

7、一种目标定位跟踪方法，包括如下步骤：

(1)完成***配置，包括传感器节点、簇头节点和指控***；

(2)完成网络初始化；

(3)传感器节点对所在簇内进行监测，依据采集信号判断是否有目标出现；

(4)当出现目标后，传感器节点对目标的方位进行测量；

(5)传感器节点将自己的方位测量值及自身坐标信息发送给本簇内的簇头节点；

(6)簇头节点根据本簇内的传感器节点发送的测量值及其坐标信息，完成目标状态估计，实现目标的定位跟踪，并将定位跟踪结果发送到指控模块；包括以下子步骤：

(61)初始化模块建立目标运动及观测方程、目标的初始状态分布函数和初始状态的估计方差，并随机产生N个粒子；

(62)粒子滤波模块利用状态方程对每个粒子状态进行基于扩展卡尔曼滤波的预测、更新；

(63)粒子权值计算模块计算粒子权值；

(64)重采样判断模块根据有效粒子数N_eff判断是否要重采样，若有效粒子数N_eff小于预先设定值，则进行重采样，根据重要性密度函数重新采样N个粒子并分配权值；

(65)估计目标状态模块根据粒子的权值计算估计目标状态，随观测时刻的增加，返回步骤(62)进行迭代算法；

(7)指控模块收集并显示各簇头节点的定位跟踪结果。

8、根据权利要求7所述的目标定位跟踪方法，其特征在于，所述步骤(2)包括以下子步骤：

(21)传感器节点完成对自己的编号，唯一标示自己的身份；

(22)传感器节点借助GPS***实现时间同步；

(23)传感器节点借助GPS***实现自身定位；

(24)依据传感器节点的通信距离对网络进行簇划分，选择簇头节点。

9、根据权利要求7所述的目标定位跟踪方法，其特征在于，所述步骤(61)包括以下子步骤：

(611)建立目标运动及观测模型模块建立目标的一阶马尔可夫***方程及目标观测的观测模型：

${\mathrm{\θ}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i;$

其中：X_T＝[x_T，v_x，y_T，v_y]表示目标的运动状态，x_T，y_T分别表示目标在X轴及Y轴的坐标值，v_x，v_y分别表示目标在在X轴及Y轴的绝对速度分量； $S^i=[s_x^i,s_y^i]$ i＝1，2…N，表示传感器节点i分别在X轴及Y轴的坐标，N表示观测到目标的传感器节点个数；θ_i表示目标与传感器节点的相对方位；n_i表示第i个传感器节点的测量噪声；

(612)建立目标初始化状态及估计方差模块建立目标的初始状态概率分布函数及估计方差；

所述初始状态概率分布函数为：x_k+1＝F_kX_k+G_kA_k；

其中：A_k＝[a_x，k，a_y，k]^T为***的处理噪声，即由于目标运动环境的不确定性导致分别在X和Y轴引起的加速度噪声，A_k～N(0，R_k)；

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{T}_s^2/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right];$

(613)随机粒子产生模块从初始状态分布p(x₀)中随机抽取N个初始粒子{x₀ ⁱ，i＝1，2…N}，与初始粒子对应的协方差矩阵为：{P₀ ⁱ，i＝1，2…N}。

10、根据权利要求7所述的目标定位跟踪方法，其特征在于，所述步骤(62)包括以下子步骤：

(621)预测模块利用状态方程预测k时刻的目标的状态

及预测协方差矩阵

其中： ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k;$

(622)更新模块利用k时刻的测量值对粒子预测结果及估计协方差矩阵进行更新，得到k时刻更新值及协方差矩阵x_k ⁱ和P_k ⁱ；

其中： $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\∂h}{\∂x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

11、根据权利要求7所述的目标定位跟踪方法，其特征在于，所述步骤(63)中，粒子权值计算模块根据贝叶斯及粒子滤波理论计算粒子权值：

${\mathrm{\ω}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p(z_k./x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

其中：q(x_k/x_0:k-1 ⁱ，z_1:k)是重要性密度函数，p(x_k/x_k ^j)是观测似然函数，p(x_k/x_k-1)是***一阶马尔可夫过程的转移概率。

12、根据权利要求7所述的目标定位跟踪方法，其特征在于，所述有效粒子数N_eff定义如下：

$N_{\mathrm{eff}}=\frac{1}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\left({\stackrel{\‾}{\mathrm{\ω}}}_k^i\right)}^2}.$

Images