0 概述

目标跟踪技术在军事和民用领域应用都十分广泛^[1]。相比于单传感器, 利用多个传感器进行目标跟踪能有效改善跟踪效果^[2], 尤其近年来随着传感器技术和通信技术的发展, 基于多传感器的目标跟踪方法以探测精度高、探测范围广等优势受到研究人员的重视^[3], 在雷达、声呐、无线基站等具体应用中起到重要作用^[4]。为进一步提高探测能力, 一些运动平台通常搭载多传感器并组建移动网络, 因此, 基于移动传感器网络的目标跟踪技术成为研究热点^[5-7]。

针对移动传感器网络中的目标跟踪问题, 现有的相关研究从不同方面改进技术上的不足, 使整体跟踪效果得到不断改善。文献[2]考虑多传感器之间的通信限制, 提出一种分布式融合估计算法用以实现对合作目标的跟踪。文献[3]针对目标跟踪中的系统误差配准问题, 提出一种增强型卡尔曼滤波算法, 以基于目标的SINGER模型来补偿系统误差, 同时分析误差控制精度与机动频率之间的关系。文献[5]针对基于多运动平台的多传感器系统, 通过组合和顺序融合多传感器的量测结果来增强传感器之间的协作, 进而实现对目标的跟踪。文献[6]通过在无线传感器网络中引入双层监测机制, 提出一种基于象限定位的目标跟踪方法。文献[7]提出一种基于蜂拥控制的移动传感器网络目标跟踪算法, 在提高目标状态估计的同时降低传感器网络能量消耗。然而, 现有多数对于目标跟踪问题的研究都是在目标运动模型^[8]与传感器量测模型^[9]的基础上进行的, 并且通常考虑的传感器量测信息都是位置量测。在一些特定情况下(如水声系统), 传感器只能得到目标的距离信息, 这会导致上述研究结果不可用。因此, 纯测距条件下的移动传感器网络目标跟踪成为新的研究热点^[10-15]。

对于量测信息只有距离的移动传感器网络, 现有研究已经能够很好地解决目标定位问题, 即实现对目标位置的准确估计。文献[11]针对群集飞机设计的基于相互距离的协同定位系统, 可以有效减缓惯导定位误差的发散速度。在多维标度(Multi-Dimensional Scaling, MDS)方法的基础上, 通过处理距离误差^[12]、采用非线性滤波^[13]以及引入权值矩阵^[16]等方法也能有效提高多维标度定位方法的定位精度。文献[14-15]解决了纯测距情况下传感器网络中多传感器的位置选择问题。文献[17]在TOA测量的基础上实现对目标的准确定位并快速引导传感器网络接近目标。文献[18]基于改进的MDS-MAP方法, 利用抑制非视距(Non Line of Sight, NLOS)误差提出一种优化的传感器网络定位算法。文献[19-20]利用传感器的运动信息构建带约束的动态距离模型, 通过处理模型中的状态约束问题并利用改进非线性滤波法方法实现高精度的定位。然而, 以上定位技术只能进行目标位置的估计, 对目标速度的估计仍有待进一步研究。

针对纯测距条件下移动传感器网络中的目标跟踪问题, 本文根据定位、定速的基本思想, 在文献[20]定位算法的基础上提出一种新的速度解算方法。采用文献[20]中的动态距离模型和对应的非线性滤波方法计算目标与传感器间距离及其导数。在此基础上, 利用多维标度方法估计目标位置和速度。该算法考虑目标和传感器之间的相对运动, 并结合传感器的运动信息与量测信息进行目标跟踪, 从而提高跟踪精度。

1 问题描述

考虑n维空间中拥有S个传感器的移动传感器网络。该网络中每个传感器i∈S的状态${\mathit{\boldsymbol{X}}_i}=\left[ {{\mathit{\boldsymbol{p}}_i};{\mathit{\boldsymbol{v}}_i}} \right] \in {\mathbb{R}^{2n}}$先验已知, 其中, ${\mathit{\boldsymbol{p}}_i} \in {\mathbb{R}^n}$和${\mathit{\boldsymbol{v}}_i} \in {\mathbb{R}^n}$分别表示传感器i的位置向量和速度向量, ${\mathbb{R}^n}$和${\mathbb{R}^{m \times n}}$分别表示n维欧几里得空间和m×n维实矩阵集。每一个传感器可以探测到其自身到运动目标t的距离为:

$ {\tilde d_{it}} = {d_{it}} + {e_{it}},i \in S $

(1)

其中, ${{\tilde d}_{it}}$、d_it和e_it~N(0, R)分别表示量测距离、真实距离和量测噪声, N(0, R)表示均值为0、协方差矩阵为R的正态分布。

通过传感器之间的相互通信, 每一个传感器i∈S可以接收到其邻居传感器j∈N_i的状态X_j及其对目标的量测${{\tilde d}_{jt}}$, 其中, N_i表示传感器i的邻居传感器集合且N_i⊆S。因此, 基于传感器自身的状态信息X_i、${{\tilde d}_{it}}$和从邻居传感器j∈N_i收集到信息X_j和${{\tilde d}_{jt}}$, 实现对运动目标的跟踪, 即实现对目标t状态${\mathit{\boldsymbol{\widehat X}}_t} = \left[ {{{\mathit{\boldsymbol{\widehat p}}}_t};{{\mathit{\boldsymbol{\widehat v}}}_t}} \right] \in {\mathbb{R}^{2n}}$的估计。

在实际应用中, 为实现对目标的跟踪, 每个传感器至少需要n个邻居传感器, 即|N_i|≥n。为清晰展示本文提出的跟踪算法, 考虑三维空间中(即n=3)4个传感器对1个目标的跟踪场景, 如图 1所示。

2 纯测距情况下的移动传感器网络目标跟踪

本节在文献[20]定位算法的基础上, 提出基于测距的移动传感器网络目标跟踪算法。根据基于传感器自身的距离量测、传感器之间的距离以及收集到的其他传感器的距离量测, 结合动态距离模型进行非线性滤波, 实现对传感器和目标之间距离及距离变化率的估计。在此基础上, 采用文献[16]中的加权多维标度方法估计目标位置, 并进一步结合多维标度的思想, 设计目标速度估计算法。

2.1 距离及其变化率估计

考虑图 1所示传感器和目标之间的相对运动, 构造动态距离模型的状态变量为:

$ \mathit{\boldsymbol{X}} = [\mathit{\boldsymbol{d}};\mathit{\boldsymbol{\dot d}};\mathit{\boldsymbol{\theta }}] \in {\mathbb{R}^{28}} $

(2)

其中, d=[d₁₂; d₁₃; d₁₄; d_1t; d₂₃; d₂₄; d_2t; d₃₄; d_3t; d_4t]、$\mathit{\boldsymbol{\dot d = }}\left[ {{{\dot d}_{12}};{{\dot d}_{13}};{{\dot d}_{14}};{{\dot d}_{1t}};{{\dot d}_{23}};{{\dot d}_{24}};{{\dot d}_{2t}};{{\dot d}_{34}};{{\dot d}_{3t}};{{\dot d}_{4t}}} \right]$和θ=[cos θ₁; cos θ₂; cos θ₃; cos θ₄; cos θ₅; cos θ₆; cos θ₇; cos θ₈]分别表示距离向量、距离变化率向量和角度余弦值向量。

根据文献[20]中的附录I构建动态距离模型如下:

$ \mathit{\boldsymbol{\dot X}} = f(\mathit{\boldsymbol{X}}) + \mathit{\boldsymbol{Gw}} $

(3)

其中, f(·)为文献[20]中定义的非线性状态传递函数(见该文献中的式(3)), G=[0_10×10; I₁₀; 0_8×10]为扰动增矩阵, $\mathit{\boldsymbol{w}} = \left[ {{{\ddot d}_{12}};{{\ddot d}_{13}};{{\ddot d}_{14}};{{\ddot d}_{1t}};{{\ddot d}_{23}};{{\ddot d}_{24}};{{\ddot d}_{22}};{{\ddot d}_{34}};{{\ddot d}_{3t}};{{\ddot d}_{4t}}} \right]$为扰动向量, 其由距离二阶导数的未知变化引起。考虑到实际应用中的测距信息及角度余弦值, 式(3)中模型的状态向量应满足如下物理约束:

$ \left\{ {\begin{array}{*{20}{l}} {{d^{(i)}} = {X^{(i)}} \ge 0,i \in 1,2, \cdots ,10}\\ { - 1 \le |{\theta ^{(i)}}| = |{X^{(20 + i)}}| \le 1,i \in 1,2, \cdots ,8} \end{array}} \right. $

(4)

根据式(1)中的距离量测及传感器之间的距离信息, 每个传感器i∈S可以利用距离信息形成量测方程如下:

$ \mathit{\boldsymbol{Z}} = \mathit{\boldsymbol{HX}} + \mathit{\boldsymbol{\gamma }} $

(5)

其中, $\mathit{\boldsymbol{Z}} = \left[ {{d_{12}};{d_{13}};{d_{14}};{{\tilde d}_{1t}};{d_{23}};{d_{24}};{{\tilde d}_{2t}};{d_{34}};{{\tilde d}_{3t}};{{\tilde d}_{4t}}} \right]$为收集到的量测向量, H=[I₁₀, 0_18×10]为量测矩阵, γ=[0;0;0;e_1t; 0;0;e_2t; 0;e_3t; e_4t]为量测误差向量。

采用离散化方法, 对式(3)和式(5)以时间间隔δ进行采样, 得到离散时间模型下第k个采样时刻的动态距离模型转移方程及对应的量测方程为^[12]:

$ \left\{ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{X}}_k} = F({\mathit{\boldsymbol{X}}_{k - 1}}) + {\mathit{\boldsymbol{G}}_{k - 1}}{\mathit{\boldsymbol{w}}_{k - 1}}}\\ {\mathit{\boldsymbol{Z}} = {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{X}}_k} + {\mathit{\boldsymbol{\gamma }}_k}} \end{array}} \right. $

(6)

由于状态转移函数F(·)是一个非线性函数且存在式(4)所示的状态约束, 因此动态距离模型的状态估计应当通过非线性滤波方法得到。此处可以直接采用文献[20]中基于无相关变换与投影的UKF滤波算法(UC-UKF)得到动态距离模型的状态估计, 如式(7)所示, 同时得到状态估计的协方差矩阵Π_k。

$ {\mathit{\boldsymbol{\hat X}}_k} = \left[ {\begin{array}{*{20}{c}} {{{\mathit{\boldsymbol{\hat d}}}_k}}\\ {{{\mathit{\boldsymbol{\dot d}}}_k}}\\ {{{\mathit{\boldsymbol{\hat \theta }}}_k}} \end{array}} \right] $

(7)

2.2 目标位置估计

根据加权多维标度方法构建相对位置矩阵:

$ \begin{array}{*{20}{l}} {{\mathit{\boldsymbol{P}}_{st,k}} = {\mathit{\boldsymbol{P}}_{s,k}} - {p_{t,k}}{{\bf{1}}_{1 \times 4}} = }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} [\begin{array}{*{20}{l}} {{p_{1,k}} - {p_{t,k}}}&{{p_{2,k}}} \end{array} - {p_{t,k}}\quad {p_{3,k}} - {p_{t,k}}\quad {p_{4,k}} - {p_{t,k}}]} \end{array} $

(8)

其中, ${\mathit{\boldsymbol{P}}_{s, k}} = \left[ {{p_{1, k}}\;{p_{2, k}}\;{p_{3, k}}\;{p_{4, k}}} \right]$为4个传感器的真实位置组成的矩阵, 1_1×4表示维度为1×4的全1矩阵。

在上述位置矩阵的基础上构建标量积矩阵:

$ {\mathit{\boldsymbol{B}}_k} = \mathit{\boldsymbol{P}}_{st,k}^{\rm{T}}{\mathit{\boldsymbol{P}}_{st,k}} = \left[ {\begin{array}{*{20}{l}} {{B_{k,11}}}&{{B_{k,12}}}&{{B_{k,13}}}&{{B_{k,14}}}\\ {{B_{k,21}}}&{{B_{k,22}}}&{{B_{k,23}}}&{{B_{k,24}}}\\ {{B_{k,31}}}&{{B_{k,32}}}&{{B_{k,33}}}&{{B_{k,34}}}\\ {{B_{k,41}}}&{{B_{k,42}}}&{{B_{k,43}}}&{{B_{k,44}}} \end{array}} \right] $

(9)

其中, ${B_{k, ij}} = \frac{1}{2}\left( {d_{it, k}^2 + d_{jt, k}^2 - d_{ij, k}^2} \right)$, 传感器i和传感器j之间的距离为${d_{ij, k}} = \sqrt {{{\left( {{p_{i, k}} - {p_{j, k}}} \right)}^{\rm{T}}}\left( {{p_{i, k}} - {p_{j, k}}} \right)} $。由于传感器并不能获得其对于目标的真实距离, 因此只能通过动态距离模型的状态估计结果来获得距离估计值${{\hat d}_{it, k}}$, 即通过式(10)将式(7)中的${{\hat d}_{it, k}}$代入式(9)来获得矩阵B_k的估计值${\mathit{\boldsymbol{\widehat B}}_k}$:

$ {\hat B_{ij}} = \frac{1}{2}(\hat d_{it,k}^2 + \hat d_{jt,k}^2 - d_{ij,k}^2) $

(10)

此外, 定义如下A_k矩阵:

$ \begin{array}{l} {\mathit{\boldsymbol{A}}_k} = \left[ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{A}}_{k,1}}}&{{\mathit{\boldsymbol{A}}_{k,2}}} \end{array}} \right] = \\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\left[ {\begin{array}{*{20}{c}} {{{\bf{1}}_{1 \times 4}}}\\ {{\mathit{\boldsymbol{P}}_{s,k}}} \end{array}} \right]^\dagger } = {\left[ {\begin{array}{*{20}{c}} {\bf{1}}&{\bf{1}}&{\bf{1}}&{\bf{1}}\\ {{\mathit{\boldsymbol{p}}_{1,k}}}&{{\mathit{\boldsymbol{p}}_{2,k}}}&{{\mathit{\boldsymbol{p}}_{3,k}}}&{{\mathit{\boldsymbol{p}}_{4,k}}} \end{array}} \right]^\dagger } \end{array} $

(11)

其中, ${\mathit{\boldsymbol{A}}_{k, 1}} \in {\mathbb{R}^{4 \times 1}}$是A_k的第1列, ${\mathit{\boldsymbol{A}}_{k, 2}} \in {\mathbb{R}^{4 \times 3}}$由其他列构成, (·)^†表示矩阵的Moore-Penrose逆。

采用文献[13]中的多维标度方法, 可以得到目标的初始位置估计如下:

$ {\mathit{\boldsymbol{\hat p}}_{t0,k}} = - {(\mathit{\boldsymbol{A}}_{k,2}^{\rm{T}}\mathit{\boldsymbol{\hat B}}_k^{\rm{T}}{\mathit{\boldsymbol{\hat B}}_k}{\mathit{\boldsymbol{A}}_{k,2}})^{ - 1}}\mathit{\boldsymbol{A}}_{k,2}^{\rm{T}}\mathit{\boldsymbol{\hat B}}_k^{\rm{T}}{\mathit{\boldsymbol{\hat B}}_k}{\mathit{\boldsymbol{A}}_{k,1}} $

(12)

以上计算并没有用到加权矩阵, 而在实际中, 通过权值矩阵白化残差向量能够进一步提高定位精度^[13]。因此, 计算如下权值矩阵:

$ \mathit{\boldsymbol{W}} = {\mathit{\boldsymbol{M}}^{ - {\rm{T}}}}{\mathit{\boldsymbol{Q}}^{ - 1}}{\mathit{\boldsymbol{M}}^{ - 1}} $

(13)

其中, Q为传感器与目标之间距离估计协方差, 可以从Π中提取。M基于${\mathit{\boldsymbol{\widehat p}}_{t0, k}}$的计算公式如下:

$ \mathit{\boldsymbol{M}} = \left[ {\begin{array}{*{20}{c}} {{a_1}{{\hat d}_{1t,k}}}&{ + {{\hat d}_{1t,k}}}&{\sum\limits_{m = 1}^4 {{a_m}} }&{{a_2}{{\hat d}_{2t,k}}}&{{a_3}{{\hat d}_{3t,k}}}&{{a_4}{{\hat d}_{4t,k}}}\\ {{a_1}{{\hat d}_{1t,k}}}&{{a_2}{{\hat d}_{2t,k}}}&{ + {{\hat d}_{2t,k}}}&{\sum\limits_{m = 1}^4 {{a_m}} }&{{a_3}{{\hat d}_{3t,k}}}&{{a_4}{{\hat d}_{4t,k}}}\\ {{a_1}{{\hat d}_{1t,k}}}&{{a_2}{{\hat d}_{2t,k}}}&{{a_3}{{\hat d}_{3t,k}}}&{ + {{\hat d}_{3t,k}}}&{\sum\limits_{m = 1}^4 {{a_m}} }&{{a_4}{{\hat d}_{4t,k}}}\\ {{a_1}{{\hat d}_{1t,k}}}&{{a_2}{{\hat d}_{2t,k}}}&{{a_3}{{\hat d}_{3t,k}}}&{{a_4}{{\hat d}_{4t,k}}}&{ + {{\hat d}_{4t,k}}}&{\sum\limits_{m = 1}^4 {{a_m}} } \end{array}} \right] $

(14)

其中, $\mathit{\boldsymbol{a}} = {\mathit{\boldsymbol{A}}_k}\left[ {\begin{array}{*{20}{c}} {\bf{1}}\\ {{{\mathit{\boldsymbol{\widehat p}}}_{t0, k}}} \end{array}} \right] = {\left[ {{a_1}, {a_2}, \cdots , {a_m}} \right]^{\rm{T}}}$。因此, 目标最终的位置估计如下:

$ {\mathit{\boldsymbol{\hat p}}_{t,k}} = - {(\mathit{\boldsymbol{A}}_{k,2}^{\rm{T}}\mathit{\boldsymbol{\hat B}}_k^{\rm{T}}\mathit{\boldsymbol{W}}{\mathit{\boldsymbol{\hat B}}_k}{\mathit{\boldsymbol{A}}_{k,2}})^{ - 1}}\mathit{\boldsymbol{A}}_{k,2}^{\rm{T}}\mathit{\boldsymbol{\hat B}}_k^{\rm{T}}\mathit{\boldsymbol{W}}{\mathit{\boldsymbol{\hat B}}_k}{\mathit{\boldsymbol{A}}_{k,1}} $

(15)

2.3 目标速度估计

结合式(8)中的位置矩阵构建相对速度矩阵如下:

$ \begin{array}{*{20}{l}} {{\mathit{\boldsymbol{V}}_{st,k}} = {{\mathit{\boldsymbol{\dot P}}}_{st,k}} = {\mathit{\boldsymbol{V}}_{s,k}} - {\mathit{\boldsymbol{v}}_{t,k}}{{\bf{1}}_{1 \times 4}} = }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{v}}_{1,k}} - {\mathit{\boldsymbol{v}}_{t,k}}}&{{\mathit{\boldsymbol{v}}_{2,k}} - {\mathit{\boldsymbol{v}}_{t,k}}}&{{\mathit{\boldsymbol{v}}_{3,k}} - {\mathit{\boldsymbol{v}}_{t,k}}}&{{\mathit{\boldsymbol{v}}_{4,k}} - {\mathit{\boldsymbol{v}}_{t,k}}} \end{array}} \right]} \end{array} $

(16)

其中, ${\mathit{\boldsymbol{V}}_{s, k}} = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{v}}_{1, k}}}&{{\mathit{\boldsymbol{v}}_{2, k}}}&{{\mathit{\boldsymbol{v}}_{3, k}}}&{{\mathit{\boldsymbol{v}}_{4, k}}} \end{array}} \right]$。计算矩阵B_k对时间的一阶导数如下:

$ \begin{array}{l} \begin{array}{*{20}{l}} {{\mathit{\boldsymbol{E}}_k} = {{\mathit{\boldsymbol{\dot B}}}_k} = \mathit{\boldsymbol{P}}_{st,k}^{\rm{T}}{\mathit{\boldsymbol{V}}_{st,k}} + \mathit{\boldsymbol{V}}_{st,k}^{\rm{T}}{\mathit{\boldsymbol{P}}_{st,k}} = }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \mathit{\boldsymbol{P}}_{st,k}^{\rm{T}}{\mathit{\boldsymbol{V}}_{s,k}} + \mathit{\boldsymbol{V}}_{s,k}^{\rm{T}}{\mathit{\boldsymbol{P}}_{st,k}} - \mathit{\boldsymbol{P}}_{st,k}^{\rm{T}}{\mathit{\boldsymbol{v}}_{t,k}}{{\bf{1}}_{1 \times 4}} - {{\bf{1}}_{4 \times 1}}\mathit{\boldsymbol{v}}_{t,k}^{\rm{T}}{\mathit{\boldsymbol{P}}_{st,k}} = } \end{array}\\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \left[ {\begin{array}{*{20}{c}} {{E_{k,11}}}&{{E_{k,12}}}&{{E_{k,13}}}&{{E_{k,14}}}\\ {{E_{k,21}}}&{{E_{k,22}}}&{{E_{k,23}}}&{{E_{k,24}}}\\ {{E_{k,31}}}&{{E_{k,32}}}&{{E_{k,33}}}&{{E_{k,34}}}\\ {{E_{k,41}}}&{{E_{k,42}}}&{{E_{k,43}}}&{{E_{k,44}}} \end{array}} \right] \end{array} $

(17)

其中, ${E_{k, ij}} = {d_{it}}{{\dot d}_{it, k}} + {d_{jt}}{{\dot d}_{jt, k}} - {d_{ij}}{{\dot d}_{ij, k}}$。

传感器i和传感器j之间的距离变化率计算公式如下:

$ {\dot d_{ij,k}} = \frac{{{{({\mathit{\boldsymbol{p}}_{i,k}} - {\mathit{\boldsymbol{p}}_{j,k}})}^{\rm{T}}}({\mathit{\boldsymbol{v}}_{i,k}} - {\mathit{\boldsymbol{v}}_{j,k}})}}{{\sqrt {{{({\mathit{\boldsymbol{p}}_{i,k}} - {\mathit{\boldsymbol{p}}_{j,k}})}^{\rm{T}}}({\mathit{\boldsymbol{p}}_{i,k}} - {\mathit{\boldsymbol{p}}_{j,k}})} }} $

(18)

定义如下矩阵:

$ \begin{array}{*{20}{l}} {{\mathit{\boldsymbol{D}}_k} = \mathit{\boldsymbol{P}}_{st,k}^{\rm{T}}{\mathit{\boldsymbol{V}}_{s,k}} + \mathit{\boldsymbol{V}}_{s,k}^{\rm{T}}{\mathit{\boldsymbol{P}}_{st,k}} - {\mathit{\boldsymbol{E}}_k} = }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \left[ {\begin{array}{*{20}{l}} {{D_{k,11}}}&{{D_{k,12}}}&{{D_{k,13}}}&{{D_{k,14}}}\\ {{D_{k,21}}}&{{D_{k,22}}}&{{D_{k,23}}}&{{D_{k,24}}}\\ {{D_{k,31}}}&{{D_{k,32}}}&{{D_{k,33}}}&{{D_{k,34}}}\\ {{D_{k,41}}}&{{D_{k,42}}}&{{D_{k,43}}}&{{D_{k,44}}} \end{array}} \right]} \end{array} $

(19)

其中, ${D_{k, ij}} = {\left( {{\mathit{\boldsymbol{p}}_{i, k}} - {\mathit{\boldsymbol{p}}_{t, k}}} \right)^{\rm{T}}}{\mathit{\boldsymbol{v}}_{j, k}} + \mathit{\boldsymbol{v}}_{i, k}^{\rm{T}}\left( {{\mathit{\boldsymbol{p}}_{j, k}} - {\mathit{\boldsymbol{p}}_{t, k}}} \right) - {d_{it, k}}{{\dot d}_{it, k}} - {d_{jt, k}}{{\dot d}_{jt, k}} + {d_{ij, k}}{{\dot d}_{ij, k}}$。根据矩阵E_k的对称性以及矩阵1_1×4的乘法属性, 由方程式(17)可推导出以下等式:

$ \underbrace {\left[ {\begin{array}{*{20}{l}} {{D_{k,11}}}\\ {{D_{k,12}}}\\ {{D_{k,13}}}\\ {{D_{k,14}}}\\ {{D_{k,22}}}\\ {{D_{k,23}}}\\ {{D_{k,24}}}\\ {{D_{k,33}}}\\ {{D_{k,34}}}\\ {{D_{k,44}}} \end{array}} \right]}_{{\mathit{\boldsymbol{U}}_k}} = \underbrace {\left[ {\begin{array}{*{20}{l}} 2&0&0&0\\ 1&1&0&0\\ 1&0&1&0\\ 1&0&0&1\\ 0&2&0&0\\ 0&1&1&0\\ 0&1&0&1\\ 0&0&2&0\\ 0&0&1&1\\ 0&0&0&2 \end{array}} \right]}_\mathit{\boldsymbol{L}}\mathit{\boldsymbol{P}}_{st,k}^{\rm{T}}{\mathit{\boldsymbol{v}}_{t,k}} $

(20)

由于p_{t, k}、d_{it, k}以及${{\dot d}_{it, k}}$是与目标相关的参数, 其真实值未知, 因此式(20)中的矩阵U_k和P_{st, k}并不能真正得到, 只能利用p_{t, k}、d_{it, k}、${{\dot d}_{it, k}}$对应的估计值${\mathit{\boldsymbol{\widehat p}}_{t, k}}$、${{\hat d}_{it, k}}$、${{\dot d}_{it, k}}$作为替换分别得到U_k和P_{st, k}的估计值${\mathit{\boldsymbol{\widehat U}}_k}$和${\mathit{\boldsymbol{\widehat P}}_{st, k}}$。因此, 式(20)可以近似写为:

$ {\mathit{\boldsymbol{\hat U}}_k} = \mathit{\boldsymbol{L\hat P}}_{st,k}^{\rm{T}}{\mathit{\boldsymbol{v}}_{t,k}} $

(21)

根据最小二乘准则, 目标的速度估计公式为:

$ {\mathit{\boldsymbol{\hat v}}_{t,k}} = {({\mathit{\boldsymbol{\hat P}}_{st,k}}{\mathit{\boldsymbol{L}}^{\rm{T}}}\mathit{\boldsymbol{L\hat P}}_{st,k}^{\rm{T}})^{ - 1}}{\mathit{\boldsymbol{\hat P}}_{st,k}}{\mathit{\boldsymbol{L}}^{\rm{T}}}{\mathit{\boldsymbol{\hat U}}_k} $

(22)

2.4 算法描述

根据以上分析, 本文算法描述如下:

1) 离线阶段。对于每一个传感器i∈S, 选择一个动态距离模型状态估计的初始值${{\mathit{\boldsymbol{\hat X}}}_0}$以及相应的协方差矩阵Π₀。

2) 在线阶段。对于每一个采样时刻k(k≥0), 每一个传感器i∈S执行以下步骤:

步骤1   在传感器网络中广播传感器自身的状态X_{i, k}及其量测值${{\tilde d}_{it, k}}$, 接收其邻居传感器j∈N_i传递的信息X_{j, k}、${{\tilde d}_{jt, k}}$, 建立量测向量Z_k。

步骤2  计算当前时刻状态估计$\mathit{\boldsymbol{\hat X = }}\left[ {{{\mathit{\boldsymbol{\hat d}}}_k};{{\mathit{\boldsymbol{\dot d}}}_k};{{\mathit{\boldsymbol{\hat \theta }}}_k}} \right]$。

步骤3   利用距离估计${\mathit{\boldsymbol{\widehat d}}_k}$建立式(10)中的估计矩阵${\mathit{\boldsymbol{\widehat B}}_k}$, 构建矩阵P_{s, k}并根据式(11)计算矩阵A_k, 根据式(12)计算目标的位置估计${{\mathit{\boldsymbol{\hat p}}}_{t, k}}$。

步骤4   根据式(19), 利用距离估计${\mathit{\boldsymbol{\widehat d}}_k}$、距离变化率估计${{\mathit{\boldsymbol{\dot d}}}_k}$以及目标位置估计${{\mathit{\boldsymbol{\hat p}}}_{t, k}}$计算估计矩阵${{\mathit{\boldsymbol{\hat D}}}_k}$, 根据式(20)建立矩阵${\mathit{\boldsymbol{\widehat U}}_k}$, 利用目标位置估计${{\mathit{\boldsymbol{\hat p}}}_{t, k}}$并根据式(8)构建矩阵${\mathit{\boldsymbol{\widehat P}}_{st, k}}$, 根据式(22)计算目标速度估计${\mathit{\boldsymbol{\widehat v}}_{t, k}}$。

步骤5   结合步骤3和步骤5的结果得到目标的状态估计${\mathit{\boldsymbol{\widehat X}}_{t, k}} = \left[ {{{\mathit{\boldsymbol{\widehat p}}}_{t, k}};{{\mathit{\boldsymbol{\widehat v}}}_{t, k}}} \right]$。

3 仿真结果分析

为验证本文算法对运动目标跟踪的有效性, 将其跟踪结果与文献[10]中的ML-KF算法进行对比, 通过100次蒙特卡罗仿真实验并采用均方根误差(Root Mean Squared Error, RMSE)作为估计精度的评价指标。位置估计和速度估计的均方根误差分别计算如下:

$ {P_{{\rm{RMSE}}}} = \sqrt {\frac{1}{N}\sum\limits_{i = 1}^N {{{({\mathit{\boldsymbol{p}}_{t,k}} - {{\mathit{\boldsymbol{\hat p}}}_{t,k}})}^{\rm{T}}}} ({\mathit{\boldsymbol{p}}_{t,k}} - {{\mathit{\boldsymbol{\hat p}}}_{t,k}})} $

(23)

$ {V_{{\rm{RMSE}}}} = \sqrt {\frac{1}{N}\sum\limits_{i = 1}^N {{{({\mathit{\boldsymbol{v}}_{t,k}} - {{\mathit{\boldsymbol{\hat v}}}_{t,k}})}^{\rm{T}}}} ({\mathit{\boldsymbol{v}}_{t,k}} - {{\mathit{\boldsymbol{\hat v}}}_{t,k}})} $

(24)

其中, N为蒙特卡罗仿真次数, p_{t, k}、v_{t, k}为目标的真实位置和真实速度, ${{\mathit{\boldsymbol{\hat p}}}_{t, k}}$、${\mathit{\boldsymbol{\widehat v}}_{t, k}}$为对应的估计值。对于三维空间中的情况, 有:

$ {{\mathit{\boldsymbol{p}}_{t,k}} = [{x_{t,k}};{y_{t,k}};{z_{t,k}}]} $

(25)

$ {{\mathit{\boldsymbol{v}}_{t,k}} = [v_{t,k}^x;v_{t,k}^y;v_{t,k}^z]} $

(26)

其中, x_{t, k}、y_{t, k}、z_{t, k}分别为k时刻目标在x、y、z坐标方向上的位置, v_{t, k}^x、v_{t, k}^y、v_{t, k}^z分别为k时刻目标在x、y、z坐标方向上的速度。

考虑三维空间中4个运动传感器对1个运动目标的跟踪场景。假设传感器和目标在x方向和y方向上均以平均1 m/s的初始速度做自由运动, 在z方向上均以平均0.1 m/s的初始速度做近似匀速直线(Nearly Constant Velocity, NCV)运动, 传感器、目标的状态按以下方程随时间进行演化:

$ {\mathit{\boldsymbol{X}}_k} = \mathit{\boldsymbol{F}}{\mathit{\boldsymbol{X}}_{k - 1}} + \mathit{\boldsymbol{G}}{\mathit{\boldsymbol{\bar w}}_{k - 1}} $

(27)

其中, $\mathit{\boldsymbol{F}} = \left[ {\begin{array}{*{20}{c}} 1&0&0&{\Delta t}&0&0\\ 0&1&0&0&{\Delta t}&0\\ 0&0&1&0&0&{\Delta t}\\ 0&0&0&1&0&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&1 \end{array}} \right]$, $\mathit{\boldsymbol{G}} = \left[ {\begin{array}{*{20}{c}} {\frac{{\Delta {t^2}}}{2}}&0&0\\ 0&{\frac{{\Delta {t^2}}}{2}}&0\\ 0&0&{\frac{{\Delta {t^2}}}{2}}\\ 1&0&0\\ 0&1&0\\ 0&0&1 \end{array}} \right]$, w_k－1为(k－1)时刻的随机扰动, 服从均值为0、方差为0.1的高斯分布。

假设传感器自身的状态(位置、速度)已知, 传感器对目标的测距误差服从均值为0、方差为1的高斯分布, 仿真时间设定为200 s, 对x、y、z各个运动方向上的估计结果进行比较, 并分别计算位置和速度估计的均方根误差, 均方根误差越小说明估计精度高。

图 2~图 4分别显示了本文算法和文献[10]算法对三维空间中目标在x、y、z坐标方向上的位置估计结果。可以看出, 2种算法的跟踪结果相似, 但本文算法的位置估计曲线更接近目标的真实轨迹。图 5显示了100次蒙特卡罗仿真实验的均方根误差。可以看出, 在测距误差服从均值为0、方差为1高斯分布的情况下, 本文算法的RMSE稳定在1.08 m左右, ML-KF算法的RMSE约为1.26 m。相比于ML-KF算法, 本文算法能够使定位精度提高约16.67%。

图 6~图 8分别显示了本文算法和文献[10]算法对目标在三维空间中x、y、z坐标方向上的速度估计结果。可以看出, 文献[10]算法通过卡尔曼滤波能大致估计出目标速度的变化趋势, 但本文算法的估计结果更接近目标的真实速度。图 9显示了100次蒙特卡罗仿真实验的均方根误差。可以看出, 本文算法的RMSE约为0.50 m/s, ML-KF算法的RMSE约为0.99 m/s。相比于ML-KF算法, 本文算法能够使速度精度提高约49.49%, 速度跟踪均方根误差更小, 精度更高。

文献[10]算法通过最小二乘法统计传感器的噪声特性, 并在此基础上采用极大似然估计对目标进行预定位(即对目标位置提供初始的估计值), 最后将预定位的结果作为新的位置量测结果进行卡尔曼滤波, 从而提高目标位置估计精度, 同时也通过卡尔曼滤波实现对目标速度的估计。本文算法则是在距离信息的基础上建立传感器与目标间的动态距离模型, 通过基于无相关变换与投影的UKF滤波实现对距离、距离变化率的高精度估计, 对距离采用加权MDS方法实现目标位置估计, 在此基础上结合运动学知识及MDS方法实现对目标速度的高精度估计。仿真结果表明, 在纯测距情况下, 本文算法可同时实现对目标位置、速度的高精度估计, 与ML-KF算法相比, 其对未知运动目标具有更好的跟踪效果。

4 结束语

本文采用带约束的非线性滤波算法, 在MDS方法的基础上提出一种纯测距情况下的移动传感器目标跟踪算法。在滤波的基础上利用加权MDS方法避免经典MDS方法计算中的特征值分解, 从而减少计算量, 提高对目标位置的估计精度。在速度估计方面, 通过结合运动学知识及MDS方法提出一种新的速度解算方法, 实现对目标运动速度的准确估计。由于本文算法考虑了传感器自身的位置和速度信息, 因此其适用于运动传感器对运动目标的跟踪。下一步将在本文研究基础上, 通过使用传感器资源优化管理及量测信息管理技术, 实现分布式多运动传感器对目标的跟踪。