自动化学报  2017, Vol. 43 Issue (8): 1393-1401   PDF    
适用于事件触发的分布式随机目标跟踪方法
杨旭升1,2, 张文安1,2, 俞立1,2     
1. 浙江工业大学信息工程学院 杭州 310023;
2. 浙江省嵌入式系统联合重点实验室 杭州 310023
摘要: 研究了一类基于RSSI(Received signal strength indication)测距的分布式移动目标跟踪问题,提出了一种适用于事件触发无线传感器网络(Wireless sensor networks,WSNs)的分布式随机目标跟踪方法.首先考虑移动机器人模型的不确定性,引入了带有随机参数的过程噪声协方差,应用改进平方根容积卡尔曼滤波(Square root cubature Kalman filter,SRCKF)得到局部估计;然后采用无模型CI(Covariance intersection)融合估计方法以降低随机过程噪声协方差带来的不利影响.该方法充分利用有模型和无模型方法的优势,实现系统模型和量测不理想情况下的分布式目标跟踪.基于E-puck机器人的目标跟踪实验表明,事件触发的工作模式可有效地减少能量消耗,带随机参数的滤波方法更适合于随机目标的跟踪.
关键词: 事件触发     RSSI     随机目标跟踪     平方根容积卡尔曼滤波    
Distributed Tracking Method for Maneuvering Targets withEvent-triggered Mechanism
YANG Xu-Sheng1,2, ZHANG Wen-An1,2, YU Li1,2     
1. College of Information Engineering, Zhejiang University of Technology, Hangzhou 310023;
2. Zhejiang Provincial United Key Laboratory of Embedded Systems, Hangzhou 310023
Manuscript received : November 18, 2015, accepted: September 5, 2016.
Foundation Item: Supported by National Natural Science Foundation of China (61273117, 61673351, 61573319) and Zhejiang Provincial Natural Science Foundation of China (LR16F030005, LZ15F030003)
Author brief: YANG Xu-Sheng Ph. D. candidate at the College of Information Engineering, Zhejiang University of Technology. His research interest covers intelligent mobile robots, wireless sensor networks, and information fusion estimation;
YU Li Professor at the College of Information Engineering, Zhejiang University of Technology. His research interest covers wireless sensor networks, robust control, and networked control systems
Corresponding author. ZHANG Wen-An Professor at the College of Information Engineering, Zhejiang University of Technology. His research interest covers information fusion estimation, networked control systems, and intelligent mobile robots. Corresponding author of this paper
Recommended by Associate Editor PAN Quan
Abstract: This paper is concerned with distributed target tracking problem using RSSI method and presents a distributed tracking method for maneuvering targets with event-triggered wireless sensor networks (WSNs). Firstly process noise covariance with random parameter is introduced under consideration of modeling uncertainties, and then a modified square root cubature Kalman filter (SRCKF) is employed to generate local estimates. Secondly, the non-model-based CI fusion estimation method is employed to reduce the adverse effects of random process noise covariance. The method combines advantages of both model-based and non-model-based estimation methods in the case of inaccurate model and unreliable measurements. Simulation and experiment of the E-puck robot tracking show that the event-triggered mechanism can greatly reduce energy consumption and that the filtering method with random parameters is more suitable for maneuvering target tracking.
Key words: Event-triggered     received signal strength indication (RSSI)     maneuvering target tracking     square root cubature Kalman filter (SRCKF)    

无线传感器网络(Wireless sensor networks, WSNs)由于成本低、功耗低、自组织、可大范围覆盖等特点, 在家庭服务、环境监测、车辆跟踪、城市交通等领域具有重要的应用价值, 但其缺乏应对突发状况的反应和执行能力.另一方面, 移动机器人具有良好的执行能力, 但对环境的认识能力以及位置感知能力十分有限.无线传感器网络可利用大量分散节点对移动机器人进行协同感知, 提供丰富的环境信息以及有效的目标定位或跟踪服务.无线传感器和机器人网络(Wireless sensor and robot networks, WSRNs)作为一种信息物理融合系统(Cyber-physical systems)在工程应用、家庭服务等方面具有更大的优势并发挥重要的作用[1].

移动机器人的定位问题是其发挥良好的执行能力的前提和基础, 同时目标定位也是无线传感器网络的重要应用之一[2-4].在无线传感器网络中, 使用RSSI (Received signal strength indication)测距方式无需增加额外的设备, 且全向性好、测量范围广, 其在移动机器人定位、导航等应用中具有很大的优势并广泛应用[5-7].然而, RSSI测距方式精度低, 易受环境影响, 往往需要融合多个传感器的信息来提高移动机器人定位的精度[8].在基于WSN的目标跟踪中, 分布式估计方法逐渐取代集中式和分散式估计方法[9-10], 其不存在绝对的信息融合中心, 具有可靠性和容错性高的特点.无线传感器网络资源有限, 文献[2]的实验表明, 相对于传统固定速率采样方式, 采用事件触发方式可以很好地改善目标跟踪的效果.特别地, 事件触发方式可降低通信量, 减少通信冲突, 从而有效延长整个网络的生命周期.但由于各传感器节点具有不同的量测方程, 而事件触发使得收集到量测值的来源和个数都具有随机性, 难以采用传统观测扩维的方式来融合各传感器节点的信息.在纯距离(Range-only)目标跟踪中, 涉及非线性滤波问题, 分布式无迹信息滤波(Distributed unscented information filter, DUIF)算法[9, 11-12]是一种有效的应用广泛的非线性分布式估计方法.文献[13]提出了容积卡尔曼滤波方法(Cubature Kalman filter, CKF), 该方法比无迹卡尔曼滤波(Unscented Kalman filter,UKF)更具优势[14].然而, 传统的CKF算法并不适用于基于事件触发的无线传感器网络目标跟踪系统.

另一方面, 由于移动机器人运动的随机性, 建立精确的运动学模型十分困难.文献[15]和文献[16]分别设计了带噪声估计器的自适应UKF算法, 有效地提高了滤波的精度.文献[17]提出了一种用于GPS导航的自适应模糊强跟踪扩展卡尔曼滤波方法, 该方法的精度优于强跟踪卡尔曼滤波和扩展卡尔曼滤波方法.文献[18]提出了一种基于改进强跟踪和CI融合的分层融合方法, 用于实现移动机器人的跟踪.然而, 由于RSSI测距方式较为不可靠, 使得带噪声估计器或渐消因子的自适应卡尔曼滤波方法不能发挥其应有的作用.特别地, 自适应卡尔曼滤波方法往往涉及历史量测数据的迭代计算, 使之不能很好地适应事件触发的工作模式.

本文的主要工作在于: 1) 采用随机过程噪声协方差来描述建模误差; 2) 针对带随机参数的过程噪声协方差, 提出了一种基于有模型和无模型的分布式分层融合结构; 3) 应用基于SRCKF和CI的分层融合估计器[19-20], 实现了事件触发WSN环境下的移动机器人跟踪.首先, 各估计节点通过应用带有随机参数过程噪声协方差的改进SRCKF算法来融合各量测信息, 得到移动机器人状态的局部估计.然后, 各估计节点收集其邻居节点的局部估计, 通过CI融合估计方法得到更适合当前运动特性的融合估计.仿真和实验结果表明, 事件触发的工作模式可大大减少能量消耗, 带随机参数的分层融合估计方法更适合于随机目标的跟踪.

1 问题提出 1.1 系统描述

考虑一类WSN环境下基于RSSI的分布式移动目标跟踪问题.如图 1所示,基于WSN的目标跟踪系统由移动机器人、感知节点和估计节点组成, 其中感知节点分为工作感知节点和非工作感知节点.文献[2]的实验表明, 采用传统固定周期的采样方式会对跟踪结果造成非常不利的影响.另一方面,通过实验发现(传感器节点采用TI的CC2530芯片), 传感器节点仅对近距离(2 m以内)的RSSI测距有相对较高的测量精度.随着测量距离的不断增大, RSSI信号越容易受环境影响且分辨率也越低.同时, 考虑到无线传感器网络资源有限, 采用事件触发的工作模式可有效地降低能耗以及减少传感器间的通信冲突和干扰.

图 1 无线传感器网络环境下的分布式移动机器人跟踪系统 Figure 1 The distributed mobile robot tracking system in WSNs

在无线传感器网络中, 感知节点采用事件触发的工作模式, 其工作条件可描述为

$ \begin{align} & [{y_k}]_{\rm dBm} > \left[y_{\min}\right]_{\rm dBm}\end{align} $ (1)
$ \begin{align}& {e_{\min }} < \left| {\left[{{\hat y}_k}\right]_{\rm dBm} - \left[{y_k}\right]_{\rm dBm}} \right| < {e_{\max }} \end{align} $ (2)

其中, $[{y_k}]_{\rm dBm}$$k$时刻的RSSI值, $e_{\min }$用于描述RSSI信号是否变化的阈值参数, $e_{\max }$用于限制RSSI信号噪声的阈值参数. $[y_{\min}]_{\rm dBm}$为服务半径. $[{\hat y_k}]_{\rm dBm}$ $=$ $\alpha [{\hat y_{k-1}}]_{\rm dBm} + (1 - \alpha ){[y_k]_{\rm dBm}}$, $[{\hat y_k}]_{\rm dBm}$为预滤波值, 且其初始值为$[{\hat y_0}]_{\rm dBm}$, $0 < \alpha < 1$为预滤波器参数.

移动机器人周期性地向周围感知节点组播定位请求消息, 定位请求信息中包含时间以及其自身信息, 而感知节点通过事件触发的模式发送给各估计节点.感知节点采用的事件触发工作模式分为两步骤: 1) 判断收到的RSSI值是否满足式(1), 若满足式(1), 即其在移动机器人的服务半径内, 则进而判断是否满足式(2);否则, 忽略此处请求. 2) 若RSSI值满足式(2), 则通过组播的方式把量测值发送给估计节点; 否则, 不发送该量测值.

移动机器人位置估计过程分为两部分: 1) 各估计节点分别收集当前时刻所有的量测信息, 随机生成过程噪声协方差参数, 并采用改进SRCKF方法获得局部估计; 2) 各估计节点交换局部估计, 采用CI融合估计方法得到融合估计.

1.2 系统建模

移动机器人运动可通过匀速(CV)、匀加速(CA)以及转弯(CT)等模型来描述, 本文采用CV模型来描述移动机器人的运动, 如式(3) 所示.

$ \begin{equation} {{\pmb{x}}_k} = f\left( {{{\pmb{x}}_{k - 1}}} \right) + {{\pmb{w}}_k} \end{equation} $ (3)

其中,

$ f\left( {{{\pmb{x}}_{k - 1}}} \right) = \left[{\begin{array}{*{20}{c}} 1&{\Delta t}&0&0\\ 0&1&0&0\\ 0&0&1&{\Delta t}\\ 0&0&0&1 \end{array}} \right]{{\pmb{x}}_{k - 1}} $

$\Delta t$为采样周期, ${{\pmb{x}}_k} = [{\begin{array}{*{20}{c}} {{x_{p, k}}}&{{{x}_{v, k}}}&{{y_{p, k}}}&{{{y}_{v, k}}} \end{array}}]$为移动机器人状态, 其中, ${{x_{p, k}}}$${{y_{p, k}}}$分别为$k$时刻移动机器人在$x$$y$轴上的位置, 以及${{{x}_{v, k}}}$${{{y}_{v, k}}}$为其在$x$$y$轴上的速度. ${{\pmb{w}}_k}$为零均值且协方差为${{{Q}}_k}$的高斯噪声.

系统过程噪声协方差${{{Q}}_k}$为先验知识, 可用于描述建模误差.然而, 由于移动机器人运动的随机性, 建模误差往往难以描述,即建模误差也存在随机性和不确定性.为了更好地描述这种特性, 将过程噪声协方差${{{Q}}_k}$描述为服从均匀分布${\rm U}[Q^1, Q^2]$的随机变量, 其中, ${Q}^1$, ${Q}^2$分别为均匀分布的上下界.

假设无线传感器网络中有$N_s$个感知节点, 其观测模型可描述为

$ \begin{align} z_k^i = &\ {h}\left( {{{\pmb{x}}_k}, {\kern 1pt} {\kern 1pt} {\kern 1pt} {{\pmb{x}}^{i}}} \right) =\nonumber\\ &\ \sqrt {{{({x_{p, k}} - {x_{p}^i})}^2} + {{({y_{p, k}} - {y_{p}^i})}^2}} + v_k^i, \nonumber\\&\qquad\qquad\qquad\qquad\qquad i = 1, 2, \cdots, N_s \end{align} $ (4)

其中, ${{z}_k^i}$$k$时刻感知节点$i$的量测值, 即其与移动机器人距离值, ${\pmb{x}}^{i}$为感知节点$i$的位置坐标, ${x_{p}^i}$${y_{p}^i}$分别为其在$x$$y$坐标轴上的值. ${v_k^i}$为零均值且方差为${{R}_k^i}$的量测噪声, 且与过程噪声${{\pmb{w}}_k}$不相关.其RSSI模型可描述为

$ \begin{equation} {\left[{{y_k^i}} \right]_{\rm dBm}} - {\left[{{y_R}} \right]_{\rm dBm}} = 10n\lg \left( {{z_R}} \right) - 10n\lg \left( {{z_k^i}} \right) \end{equation} $ (5)

其中, $n$为传播缩减系数, $\left[{{y_R}} \right]_{\rm dBm}$${{z_R}}$分别为参考点的RSSI值和对应的距离值. $\left[y_k^i \right]_{\rm dBm}$$k$时刻感知节点$i$的RSSI量测值.随着距离的增大, RSSI测量值越容易受环境影响同时其分辨率也越低, 因此, 量测噪声方差${R_k^i}$描述为测量距离的函数${R^i}\left( {{z_k^i}} \right)$.

注1.由于引入事件触发的采样机制, 使得估计节点收集到的传感器数量和来源都随时间变化.特别地, 各感知节点具有不同的量测方程, 传统的集中式融合方法并不适用量测模型变化的滤波问题.另一方面, 考虑模型误差的补偿问题, 量测信息无法迭代运算且可靠性不高, 增加了模型误差的补偿难度.

注2.采用服从均匀分布的过程噪声协方差来描述移动机器人加减速、转弯等运动特性变化, 这样, 通过随机产生的过程噪声协方差, 可生成多种适用于不同系统动态的局部估计.进而, 选择合适的融合估计器, 得到更适合当前运动特性的融合估计.

注3.为了实现测距, 移动机器人通过单跳的方式跟周围的感知节点进行通信.同时, 为了减少通信的能量消耗, 工作感知节点通过多跳的方式发送量测数据给估计节点.为此, 无线传感器网络应采用网状拓扑结构,所有节点需具备路由能力以完成目标跟踪过程中移动机器人与任意感知节点间的直接通信.

2 分布式跟踪系统的估计器设计

自适应Kalman滤波器通常在迭代的过程中对模型误差进行校正和补偿, 但这往往涉及量测信息的迭代计算.为实现先验知识不足且量测不理想情况下的移动目标跟踪, 设计了一种基于有模型和无模型的分布式分层融合估计器.

2.1 有模型估计器设计

为降低通信冲突和网络拥塞, 同时减少传感器间的相互干扰以提高量测数据的质量, 无线传感器网络采用了事件触发的工作模式.然而, 传统的卡尔曼滤波器往往要求同步的量测数据, 并不适用于事件触发模式下的状态估计问题.本文对SRCKF方法进行了改进, 使其适用于事件触发的工作模式,并采用基于改进SRCKF的有模型估计器以得到移动机器人的局部估计.

由于采用事件触发的工作模式, 量测模型在迭代过程中需根据收集到的量测值进行不停地变化.为此, 各估计节点均建立了量测模型表, 表中包含所有感知节点量测模型.估计节点根据感知节点的ID获取所需节点的量测模型,从而完成SRCKF的量测更新过程.各估计节点收集到的量测数据相同, 然而, 各估计节点随机生成服从均匀分布的过程噪声协方差参数, 从而导致了各局部估计的差异性.基于改进SRCKF的有模型估计器如算法1所示.

算法1.基于改进SRCKF的有模型估计器算法

步骤1.生成$k-1$时刻状态估计值的容积点

$ \begin{align} &{{\pmb{X}}_{i, k - 1|k - 1}} = {{{S}}_{k - 1|k - 1}}{{\pmb{\xi}} _i} + {{\hat{\pmb{ x}}}_{k - 1|k - 1}}, \nonumber \\ &~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ i = 1, 2, \cdots, 2{n_x} \end{align} $ (6)

其中, ${\pmb{\xi} _i} = \sqrt {{n_x}} {[1]_i}$, ${{{S}}_{k - 1|k - 1}}$$k-1$时刻状态估计协方差的平方根, ${{{P}}_{k - 1|k - 1}} = {{{S}}_{k - 1|k - 1}}{{S}}_{k - 1|k - 1}^{\rm{T}}$, ${\hat{\pmb{ x}}_{k - 1|k - 1}}$, ${{{P}}_{k - 1|k - 1}}$, 分别为$k-1$时刻状态估计值及其估计协方差, $n_x$为系统状态的维数.

步骤2.计算过程传播容积点和状态预测值及其协方差的平方根

$ \begin{align} &{\pmb{X}}_{i, k|k - 1}^* = f\left( {{{\pmb{X}}_{i, k - 1|k - 1}}} \right) \end{align} $ (7)
$ \begin{align} &{\hat{\pmb{ x}}_{k|k - 1}} = \frac{1}{{2{n_x}}}\sum\limits_{i = 1}^{2{n_x}} {{\pmb{X}}_{i, k|k - 1}^*} \end{align} $ (8)
$ \begin{align} &{{{S}}_{k|k - 1}} = {\rm{tria}}\left( {\left[ {\begin{array}{*{20}{c}} {{{\chi }}_{k|k-1}^*}&{{{{S}}_{Q, k}}} \end{array}} \right]} \right) \end{align} $ (9)

其中, ${\hat {\pmb{x}}_{k|k - 1}}$$k$时刻状态预测值及其协方差的平方根为${{{S}}_{k|k - 1}}$, ${{{S}}_{Q, k}} = \sqrt {{{Q}}_{k}}$, ${{Q}}_{k}$为估计节点随机产生的过程噪声协方差, ${\rm{tria}}(\cdot)$为三角分解, 即${{{S}}_{k|k - 1}}$是三角阵. ${{\chi }}_{k|k - 1}^* = {1}/{{\sqrt {2{n_x}} }}[{\pmb{X}}_{1, k|k-1}^*-{{\hat{\pmb{ x}}}_{k|k-1}}$ $\cdots$ ${\pmb{X}}_{2{n_x}, k|k - 1}^* - {{\hat{\pmb{ x}}}_{k|k - 1}}]$.

步骤3.生成状态预测值的容积点

$ \begin{align} &{{\pmb{X}}_{i, k|k - 1}} = {{{S}}_{k|k - 1}}{{\pmb{\xi}} _i} + {{\hat{\pmb x}}_{k|k - 1}}, \nonumber\\ &~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ i = 1, 2, \cdots, 2{n_x} \end{align} $ (10)

步骤4.估计节点收集量测值, 记录感知节点ID并根据时间戳信息把同一时刻的量测值扩张成一个量测向量, 即

$ \begin{equation} {{\pmb{z}}_{k}} = {\left[{\begin{array}{*{20}{c}} {z_{k}^1}&{z_{k}^2}& \cdots \end{array}} \right]^{\rm{T}}} \end{equation} $ (11)

步骤5.根据感知节点ID, 从量测模型表调用其量测函数${h}( {{{\pmb{x}}_k}, {\kern 1pt} {\kern 1pt} {\kern 1pt} {{\pmb{x}}^{j}}})$, 计算量测传播容积点

$ \begin{equation} Z_{i, k|k - 1}^j = {h}\left( {{{\pmb{X}}_{i, k|k - 1}}, {{\pmb{x}}^{j}}} \right) \end{equation} $ (12)

其中, $j=1, \cdots, n_s$, $n_s \le N_s$, $n_s$为工作感知节点个数, ${\pmb{Z}}_{k|k - 1}^j = [{\begin{array}{*{20}{c}} {Z_{1, k|k-1}^j}& \cdots &{Z_{2{n_x}, k|k-1}^j} \end{array}}]$.

步骤6.根据量测值扩维顺序, 对量测传播容积点进行扩维, 即

$ \begin{equation} {{{Z}}_{k|k - 1}} = {\left[{\begin{array}{*{20}{c}} {{{\left( {{\pmb{Z}}_{k|k-1}^1} \right)}^{\rm{T}}}}&{{{\left( {{\pmb{Z}}_{k|k-1}^2} \right)}^{\rm{T}}}}& \cdots \end{array}} \right]^{\rm{T}}} \end{equation} $ (13)

步骤7.计算量测预测值及其协方差的平方根

$ \begin{align} &{\hat{\pmb{ z}}_{k|k - 1}} = \frac{1}{{2{n_x}}}\sum\limits_{i = 1}^{2{n_x}} {{{\pmb{Z}}_{i, k|k - 1}}}\end{align} $ (14)
$ \begin{align} &{{{S}}_{zz, k|k - 1}} = {\rm{tria}}\left( {\left[ {\begin{array}{*{20}{c}} {{{{Z}}_{k|k-1}^*}}&{{{{S}}_{R, k}}} \end{array}} \right]} \right) \end{align} $ (15)

其中,

$ \begin{align*} &{{{S}}_{R, k}} = \sqrt {{{R}}_{k}}\\ &{{{R}}_{k}} = {\rm diag}\left\{R_k^1, R_k^2, \cdots, R_k^{n_s}\right\}\\ &{{Z}}_{k|k - 1}^* =\frac{1}{{\sqrt {2{n_x}} }} \left[{{\pmb{Z}}_{1, k|k-1}}-{\hat{\pmb{ z}}_{k|k-1}} \cdots \right.\\ &\qquad\qquad\left. {{\pmb{Z}}_{2{n_x}, k|k - 1}} - {\hat{\pmb{ z}}_{k|k - 1}}\right] \end{align*} $

步骤8.计算状态与量测的互协方差

$ \begin{equation} {{{P}}_{xz, k|k - 1}} = {{{\chi }}_{k|k - 1}}\left({{Z}}_{k|k - 1}^{*}\right)^{\rm{T}} \end{equation} $ (16)

其中,

$ \begin{align*} &{{{\chi }}_{k|k - 1}} =\frac{1}{{\sqrt {2{n_x}} }} \left[{{\pmb{X}}_{1, k|k-1}}-{{\hat{\pmb{ x}}}_{k|k-1}} \cdots \right.\\ &\qquad\qquad \left.{{\pmb{X}}_{2{n_x}, k|k - 1}} - {{\hat{\pmb{ x}}}_{k|k - 1}}\right] \end{align*} $

步骤9.计算滤波增益

$ \begin{equation} {{{W}}_k} = \left( {{{{P}}_{xz, k|k - 1}}\left({{S}}_{zz, k|k - 1}^{\rm{T}}\right)^{-1}} \right){\left({{S}}_{zz, k|k - 1}\right)^{-1}} \end{equation} $ (17)

步骤10.计算系统状态估计值及其协方差

$ \begin{align} &{\hat{\pmb{ x}}_{k|k}} = {\hat{\pmb{ x}}_{k|k - 1}} + {{{W}}_k}\left( {{{\pmb{z}}_k} - {{\hat{\pmb{ z}}}_{k|k - 1}}} \right)\end{align} $ (18)
$ \begin{align} &{{{S}}_{k|k}} = {\rm{tria}}\left( {\left[{\begin{array}{*{20}{c}} {{{{\chi }}_{k|k-1}}-{{{W}}_k}{{{Z}}_{k|k-1}^*}}&{{{{W}}_k}{{{S}}_{R, k}}} \end{array}} \right]} \right) \end{align} $ (19)

为了适应事件触发的工作模式, 首先, 各估计节点对收集到的量测值进行扩维; 然后, 分别调用相应的量测模型生成其量测传播容积点; 最后, 按相应顺序对量测传播容积点进行扩维.

算法2.基于改进SRCKF的量测融合过程

1.初始化

2. while

3.  算法1的步骤1~3

4.   计算量测值数目$n_s$

5.   for $j=1:n_s$

6.     算法1的步骤4

7.     for $i=1:2n_x$

8.      算法1的步骤5

9.     end for

10.      算法1的步骤6

11.   end for

12.   算法1的步骤7 $\sim$ 10

13.   $k=k+1$

14.end while

注4. $[1] \in {{\bf R}^{{n_x}}}$, 若${n_x} = 2$, ${[1]_i}$为集合$[1]$中的第$i$个元素[17],

$ [1] = \left\{ {\left( {\begin{array}{*{20}{c}} 1\\ 0 \end{array}} \right), \left( {\begin{array}{*{20}{c}} 0\\ 1 \end{array}} \right), \left( {\begin{array}{*{20}{c}} { - 1}\\ 0 \end{array}} \right), \left( {\begin{array}{*{20}{c}} 0\\ { - 1} \end{array}} \right)} \right\} $
2.2 无模型估计器设计

各估计节点在过程噪声协方差中引入了随机参数, 使得各估计节点在相同量测信息的情况下取得了差异性的局部估计.进而, 将设计一种无模型的融合估计器, 得到更符合移动机器人当前运动特性的融合估计.由于各局部估计由相同的量测信息以及系统模型(CV模型)生成, 因此各局部估计之间都是相关的.在互协方差未知的情况下, CI融合估计方法可用于得到优于各局部估计的融合估计.

假设无线传感器网络中有$n_e$个估计节点, 估计节点$i$ $(i=1, 2, \cdots, n_e)$在完成局部估计后, 收集所有邻居节点的局部估计, 采用如式(20) 和式(21) 的CI融合估计方法生成融合估计.

$ \begin{align} &{({{P}}_{k|k}^{({\rm CI})})^{ - 1}} = \sum\limits_{j = 1}^{n_e} {{\omega _j}} {({{P}}_{k|k}^{(j)})^{ - 1}} \label{eq3}\end{align} $ (20)
$ \begin{align} &{\hat{\pmb x}}_{k|k}^{({\rm CI})} = {{P}}_{k|k}^{({\rm CI})}\; \sum\limits_{j = 1}^{{n_e}} {{\omega _j}} {({{P}}_{k|k}^{(j)})^{ - 1}}\hat{\pmb x}_{k|k}^{(j)} \label{eq4} \end{align} $ (21)

其中, ${{\hat{\pmb x}}_{k|k}^{({\rm CI})}}$, ${{{P}}_{k|k}^{({\rm CI})}}$分别为CI融合估计的状态及其协方差, ${{\hat{\pmb x}}_{k|k}^{(j)}}$, ${{{P}}_{k|k}^{(j)}}$分别为局部估计$j$的状态估计值及其协方差, ${\omega _j}$为CI融合估计的权重参数且需满足

$ \begin{align} &{\omega _j}:\ \mathop {\min }\limits_{\omega_j} {\kern 1pt} \left\{ {\rm tr}({{{P}}_{k|k}^{({\rm CI})}})\right\}, \quad j = 1, 2, 3, \cdots, {n_e} \notag\\ &{\rm s.t.}\ \ \sum\limits_{j = 1}^{{n_e}} {{\omega _j}} = 1 \label{eq1} \end{align} $ (22)

实际经验表明, 协方差矩阵对角线上的元素要大于非对角线上的元素[21].因此, 为简化式(22) 中${\omega _j}$的求解, 通过式(23) 给出了${\omega _j}$的近似解.

$ \begin{align} {\omega _j} = \dfrac{\dfrac{1}{{\rm tr}\left({P}_{k|k}^{(j)}\right)}}{\sum\limits_{h = 1}^{{n_e}} {\dfrac{1}{ {\rm tr}\left({P}_{k|k}^{(h)}\right)}}} \label{eq2} \end{align} $ (23)

为了减少带宽和能量消耗, 在CI融合估计算法的基础上提出了LCI (Local covariance intersection)融合估计算法, 即各估计节点不进行当前时刻的协方差矩阵的交换, 而是通过邻居节点的状态估计值本地计算出其估计协方差矩阵.然后, 将在线估计的协方差代入式(20) 和式(21) 得到CI融合估计.协方差的在线估计算法如式(24) 所示.

$ \begin{align} {{P}}_{k}^{(j)} =&\ a\left( {{{\hat{\pmb{x}}}_{k|k}}^{(j)} - {{{\bar{\pmb x}}}_{k|k}}} \right){\left( {{{\hat{\pmb{x}}}_{k|k}}^{(j)} - {{{\bar{\pmb x}}}_{k|k}}} \right)^{\rm{T}}}+\notag\\[1mm] & \left( {1 - a} \right){{P}}_{k - 1|k - 1}^{(j)} \end{align} $ (24)

其中, ${{{\bar{\pmb x}}}_{k|k}} =\sum_{j = 1}^{{n_e}} {{\pmb{x}}_k^{(j)}} /{{{n_e}}}$, ${{P}}_{k}^{(j)}$$k$时刻本地估计出来的估计节点$j$的协方差, $0 < a < 1$为历史协方差和当前协方差所占的权重值.

注5.当所有局部估计满足一致性估计时, 则可保证CI融合估计的一致性[22]. LCI本地在线计算邻居局部估计的协方差, 而CI融合估计"继承"邻居局部估计的协方差.当局部估计满足一致性时, CI融合估计精度要优于LCI融合估计, 而其估计精度的鲁棒性要差于LCI融合估计.因此, 在实际应用中可结合对系统的了解程度选择合适的融合估计方法.

3 实验分析

本文以E-puck机器人为跟踪目标, 实现2D环境下基于WSN和RSSI测距的移动机器人跟踪, 并通过仿真和实验的方式来验证算法的正确性和有效性.

3.1 仿真实验

无线传感器和机器人网络由1个移动机器人、3个估计节点以及12个感知节点组成, 移动机器人周期性地向周边的感知节点组播定位请求消息, 感知节点通过事件触发的方式向估计节点发送量测信息, 即感知节点满足式(1) 和式(2) 发送量测等信息给估计节点.其中, 组播周期为$1.0$ s, 服务半径$[y_{\min}]_{\rm dBm}$$-55$ dBm, $e_{\min}=0.2$, $e_{\max}=0.6$, 传播衰减系数$n$$3.0$, 监测区域大小为$360 {\rm cm}$ $\times$ $240 {\rm cm}$.为了利于诊定过程噪声的统计特性以及仿真分析, 令移动机器人在监测区域内以$6$ cm/s匀速运动, 并已知量测噪声方差为$\sqrt{{{R}_k^i}}=0.12z_k^i$, $i=$ $1$, $2$, $\cdots$, $12$.为了简化诊定过程噪声统计特性, 假设过程噪声协方差满足${{{S}}_{Q, k}}$ $=$ $q$ $\times$ ${{I}}$, ${I}$为相应维数的单位阵, $q\sim {\rm{U}}[{q^1}, {q^2}]$, 其中, $q^1$, $q^2$分别为均匀分布的上下界.

为便于对仿真结果进行分析和比较, 定义误差指标(Error indicator, EI):

$ \begin{align} { EI} =&\ \frac{1}{{MN}}\sum\limits_{k = 1}^M \sum\limits_{h = 1}^N \left( \left| {{{\hat x}_{p, k|k}} - {x_{p, k}}} \right|\right. +\notag\\ &\ \left. \left| {{{\hat y}_{p, k|k}} - {y_{p, k}}} \right| \right)_h \end{align} $ (25)

其中, ${( \cdot )_h}$为第$h$次仿真, $N$为仿真次数, $M$为仿真步数, ${\hat x}_{p, k|k}$${\hat y}_{p, k|k}$分别为$x$$y$轴上的位置估计值, ${x_{p, k}}$${y_{p, k}}$分别是对应的真实值.

图 2所示, 当$q=7.0$时, 可以取得在当前运动速率下满意的跟踪结果.同时, 不难发现, 当$q$值过大时, 将导致估计结果过于保守, 而当$q$值过小时, 将导致不能满足估计的一致性, 从而使得估计结果迅速变差, 且其估计结果比$q$值过大的情况更为糟糕.

图 2 不同过程噪声协方差下的估计误差 Figure 2 Results of the estimation errors with different process covariances

通过对目标跟踪仿真场景下的一系列仿真实验, 发现当$q=7.0$时, 可得到较为满意的估计结果.在此先验知识的基础上, 将进一步验证SRCKF-CI分层融合估计算法的有效性、正确性和局限性.为了便于分析CI融合估计算法的性能, 引入了平均和LCI的估计方法作为对比.平均估计方法如下所示:

$ \begin{equation} {\hat{\pmb x}}_{k|k}^{(m)} = \frac{1}{{{n_e}}}\sum\limits_{j = 1}^{{n_e}} {\hat{\pmb x}_{k|k}^{(j)}} \end{equation} $ (26)

其中, ${\hat{\pmb x}}_{k|k}^{(m)}$为局部估计的平均值, 不涉及先验信息以及协方差的计算.

表 1所示, 当$q$值的分布区间小于真实值, CI融合估计误差反而大于局部估计, 而LCI和平均的估计结果略优于各局部估计. CI融合估计方法的协方差矩阵来自于各局部估计, 当各局部估计不能满足一致估计时, 将导致CI融合估计放大局部估计的误差. LCI和平均的估计方法对先验知识的依赖性较低, 因此其估计精度的鲁棒性要优于CI融合估计方法.然而, 当$q$值的分布区间大于真实值, 3种估计方法都要优于各局部估计结果, 其中, CI估计方法优势更为明显.

表 1 不同过程协方差情况下移动机器人跟踪的仿真结果 Table 1 The simulation results of the mobile robot tracking with the different process noise covariances

另一方面, 结合表 1图 2, 若$q$为常数时, 当$q=7.0$时, $EI=9.2233$, 其估计效果却不及$q$$[6.0$, $8.0]$上随机给定的情况.原因在于, 虽然移动机器人匀速运动, 但机器人相对于感知节点的运动往往不是线性的.因此, 在使用匀速模型的情况下, 模型误差往往也是时变的, 采用固定的过程噪声协方差不能取得最佳的估计效果.

3.2 E-puck目标跟踪实验

本文以E-puck机器人为跟踪对象, 目标跟踪系统由12个感知节点、1个汇聚节点、1个移动节点和一台PC机组成.如图 3所示, 12个感知节点均匀地分布在$360 {\rm cm}\times 240 {\rm cm}$的区域内, 移动节点放置在E-puck机器人上以实现移动机器人与感知节点的通信, 并事先设置E-puck移动机器人运动的起始位置、路径和速率.汇聚节点负责建立和维护网络以及收集量测数据传输给PC机, 通过PC机模拟了3个估计节点, 并在PC机上完成移动机器人位置的估计过程.无线传感器网络节点采用TI的CC2530芯片并在Z-Stack的基础上进行开发.所有节点都具有路由功能并采用网状的拓扑结构, 实现移动节点与任意感知节点在网络拓扑上的直接连通, 从而实现测距的目的.移动节点周期性地读取自身的时间、ID等信息, 通过单跳组播的方式发送给周围的感知节点.感知节点接收到数据包后, 解析数据包以及读取RSSI信息.若满足发送条件, 则感知节点以多跳方式发送量测值、时间戳、机器人ID等消息给汇集节点, 汇聚接收到数据后转发给PC机, 从而在PC机上完成移动机器人的位置估计, 其中, $[y_{\min}]_{\rm dBm}$ $=$ $-55$ dBm, $e_{\min}=0.2$, $e_{\max}=0.6$; 否则, 不转发该量测数据.

图 3 E-puck机器人目标跟踪实验平台 Figure 3 The E-puck robot-based target tracking experiment platform

通过E-puck机器人目标跟踪实验, 将验证CI融合估计的有效性, 即能否在具有差异性的局部估计基础上, 生成更符合当前运动特性的融合估计.通过仿真不难发现, CI融合估计方法在大多数情况下要优于LCI和平均融合方法, 特别是在局部估计满足一致性估计的情况下.因此, 采用SRCKF-CI分层融合估计算法时, 需根据先验知识, 给出保守的过程噪声协方差参数的下界.

为了比较局部估计和CI融合估计的目标跟踪结果, 定义误差衡量指标(Error sumindicator, ESI)

$ \begin{equation} { ESI} = \sum\limits_{h = 1}^{{L_s}} {\sum\limits_{s = 1}^k {{{\left( {\left| {{{\hat x}_{i, s|s}} - {x_{i, s}}} \right|} \right)}_h}} } \end{equation} $ (27)

其中, $(\cdot)_h$为第$h$次实验, $L_s$为实验次数, ${\hat x_{i, s|s}}$$s$时刻目标状态第$i$个分量的估计值, ${x_{i, s}}$为对应的真实值(预设轨迹).

图 4所示, 移动机器人的位置估计结果受过程噪声协方差影响大.如图 5所示, CI融合估计要优于SRCKF局部估计, 更加接近预设轨迹(黑线), $X$$Y$轴的上位置累积估计误差如图 6图 7所示, 从而验证了SRCKF-CI分层融合估计方法的有效性和合理性.同时, 实验表明, 采用事件触发的工作模式可减少大约$56.54 \%$的数据量, 从而减少了量测间干扰以及能量消耗.

图 4 不同过程噪声协方差下的移动机器人跟踪结果 Figure 4 Results of the mobile robot tracking with different process covariances
图 5 基于SRCKF的局部跟踪结果和CI融合估计结果的对比 Figure 5 Comparison of the SRCKF-based local estimates and the CI fusion estimates of the mobile robot tracking
图 6 基于SRCKF的局部估计和CI融合估计$X$轴误差对比 Figure 6 Comparison of the SRCKF-based local estimation and the CI fusion estimation errors in the $X$-coordinate
图 7 基于SRCKF的局部估计和CI融合估计$Y$轴误差对比 Figure 7 Comparison of the SRCKF-based local estimation and the CI fusion estimation errors in the $Y$-coordinate
4 结论

本文提出了一种针对RSSI测距的分布式移动机器人跟踪方法.该方法采用有模型和无模型的分布式分层融合结构, 首先, 通过随机过程噪声协方差来描述模型不确定性, 并应用改进SRCKF方法得到多个适用于不同系统动态的局部估计; 然后, 采用CI融合估计方法得到更符合移动机器人当前运动特性的融合估计.该方法充分利用了有模型和无模型方法的优势, 采用随机产生-定向选择的滤波模式, 以简化过程噪声协方差的确定过程以及提高对随机目标的自适应能力.

参考文献
1
Li X, Lille I, Falcon R, Nayak A, Stojmenovic I. Servicing wireless sensor networks by mobile robots. IEEE Communications Magazine, 2012, 50(7): 147-154. DOI:10.1109/MCOM.2012.6231291
2
Yoo J H, Kim W, Kim H J. Event-driven Gaussian process for object localization in wireless sensor networks. In:Proceedings of the 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems. San Francisco, USA:IEEE, 2011. 2790-2795
3
Zhang W A, Yang X S, Yu L. Sequential fusion estimation for RSS-based mobile robots localization with event-driven WSNs. IEEE Transactions on Industrial Informatics, 2016, 12(4): 1519-1528. DOI:10.1109/TII.2016.2585350
4
Chang D C, Fang M W. Bearing-only maneuvering mobile tracking with nonlinear filtering algorithms in wireless sensor networks. IEEE Systems Journal, 2014, 8(1): 160-170. DOI:10.1109/JSYST.2013.2260641
5
Zickler S, Veloso M. RSS-based relative localization and tethering for moving robots in unknown environments. In:Proceedings of the 2010 IEEE International Conference on Robotics and Automation. Anchorage, USA:IEEE, 2010. 5466 -5471
6
Menegatti E, Zanella A, Zilli S, Zorzi F, Pagello E. Range-only slam with a mobile robot and a wireless sensor networks. In:Proceedings of the 2009 IEEE International Conference on Robotics and Automation. Kobe, Japan:IEEE, 2009. 8-14
7
Luo Q H, Peng Y, Li J B, Peng X Y. RSSI-based localization through uncertain data mapping for wireless sensor networks. IEEE Sensors Journal, 2016, 16(9): 3155-3162. DOI:10.1109/JSEN.2016.2524532
8
Yue Yuan-Long, Zuo Xin, Luo Xiong-Lin. Improving measurement reliability with biased estimation for multi-sensor data fusion. Acta Automatica Sinica, 2014, 40(9): 1843-1852.
( 岳元龙, 左信, 罗雄麟. 提高测量可靠性的多传感器数据融合有偏估计方法. 自动化学报, 2014, 40(9): 1843-1852.)
9
Tang Wen-Jun, Zhang Guo-Liang, Zeng Jing, Sun Yi-Jie, Wu Jin. An improved distributed unscented information filter algorithm for sparse wireless sensor networks. Acta Automatica Sinica, 2014, 40(11): 2490-2498.
( 汤文俊, 张国良, 曾静, 孙一杰, 吴晋. 一种适用于稀疏无线传感器网络的改进分布式UIF算法. 自动化学报, 2014, 40(11): 2490-2498.)
10
Yang Wen, Shi Hong-Bo, Wang Xiao-Fan. A survey of consensus based Kalman filtering algorithm. Control and Decision, 2011, 26(4): 481-488.
( 杨文, 侍洪波, 汪小帆. 卡尔曼一致滤波算法综述. 控制与决策, 2011, 26(4): 481-488.)
11
Vercauteren T, Wang X D. Decentralized sigma-point information filters for target tracking in collaborative sensor networks. IEEE Transactions on Signal Processing, 2005, 53(8): 2997-3009. DOI:10.1109/TSP.2005.851106
12
Li W L, Jia Y M. Consensus-based distributed multiple model UKF for jump Markov nonlinear systems. IEEE Transactions on Automatic Control, 2012, 57(1): 227-233. DOI:10.1109/TAC.2011.2161838
13
Arasaratnam I, Haykin S. Cubature Kalman filters. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269. DOI:10.1109/TAC.2009.2019800
14
Wei Xi-Qing, Song Shen-Min. Model-free cubature Kalman filter and its application. Control and Decision, 2013, 28(5): 769-773.
( 魏喜庆, 宋申民. 无模型容积卡尔曼滤波及其应用. 控制与决策, 2013, 28(5): 769-773.)
15
Zhao Lin, Wang Xiao-Xu, Sun Ming, Ding Ji-Cheng, Yan Chao. Adaptive UKF filtering algorithm based on maximum a posterior estimation and exponential weighting. Acta Automatica Sinica, 2010, 36(7): 1007-1019.
( 赵琳, 王小旭, 孙明, 丁继成, 闫超. 基于极大后验估计和指数加权的自适应UKF滤波算法. 自动化学报, 2010, 36(7): 1007-1019.)
16
Wang Lu, Li Guang-Chun, Qiao Xiang-Wei, Wang Zhao-Long, Ma Tao. An adaptive UKF algorithm based on maximum likelihood principle and expectation maximization algorithm. Acta Automatica Sinica, 2012, 38(7): 1200-1210.
( 王璐, 李光春, 乔相伟, 王兆龙, 马涛. 基于极大似然准则和最大期望算法的自适应UKF算法. 自动化学报, 2012, 38(7): 1200-1210.)
17
Jwo D J, Wang S H. Adaptive fuzzy strong tracking extended Kalman filtering for GPS navigation. IEEE Sensors Journal, 2007, 7(5): 778-789. DOI:10.1109/JSEN.2007.894148
18
Yang X S, Zhang W A, Yu L, Xing K X. Multi-rate distributed fusion estimation for sensor network-based target tracking. IEEE Sensors Journal, 2016, 16(5): 1233-1242. DOI:10.1109/JSEN.2015.2497464
19
Zhang W A, Chen B, Chen M. Hierarchical fusion estimation for clustered asynchronous sensor networks. IEEE Transactions on Automatic Control, 2016, 61(10): 3064-3069. DOI:10.1109/TAC.2015.2498701
20
Deng Z L, Zhang P, Qi W J, Yuan G, Liu J F. The accuracy comparison of multisensor covariance intersection fuser and three weighting fusers. Information Fusion, 2013, 14(2): 177-185. DOI:10.1016/j.inffus.2012.05.005
21
Bolognani S, Tubiana L, Zigliotto M. Extended Kalman filter tuning in sensorless PMSM drives. IEEE Transactions on Industry Applications, 2003, 39(6): 1741-1747. DOI:10.1109/TIA.2003.818991
22
Uhlmann J K. Covariance consistency methods for fault-tolerant distributed data fusion. Information Fusion, 2003, 4(3): 201-215. DOI:10.1016/S1566-2535(03)00036-8