基于扩展卡尔曼滤波的自适应混合估计算法

余辉荣 ，夏候凯顺 ，叶景志 ，邬依林 ，2

（1.华南理工大学 自动化科学与工程学院，广州 510640；2.广东第二师范学院 计算机科学系，广州 510310）

多个目标的实时、精确跟踪是无线传感器网络的重要应用和研究难点。随着被跟踪目标数量及其运动特征多样性的增加，若要继续保持有效跟踪，则对于分布式传感器节点的协同调度效率和定位算法的要求也同步上升。一个优良的定位算法必须具备自组织性、低能耗、高鲁棒性和分布式计算等特点[1]。无线传感器网络中的目标跟踪算法基本上分为直接通信法[2]、基于二进制探测的方法[3]、基于树状结构的方法[4]、基于簇状结构的方法[5]、基于预测机制的方法[6]、基于粒子滤波的方法[7]、基于自适应机制的方法[8]和对偶空间转换跟踪方法[9]等。

传统的最小二乘定位算法及常规的卡尔曼滤波定位算法是最常用的方法。前者具有收敛速度快、跟踪及时等优点，但测量信号中的噪声在融合后也被凸显，导致定位轨迹波动较大；后者是一种高效率的自回归滤波器，能够从一系列不完全及包含噪声的测量中，估计出动态系统的状态[10]，抗观测噪声干扰能力强，其缺点是信号收敛速度较慢。因此，对于目标运动特性较复杂的情况 （如图1所示），当运动速度发生突变时，运用单一的任一定位算法都无法达到快速且准确的追踪。

图1 目标运动特性示意图Fig.1 Schema of target motion characteristics

本文针对上述两种算法的固有缺陷，提出了一种自适应的混合估计算法。该算法以基于单个观测值的扩展卡尔曼滤波模型为基础，结合收敛速度更快的最小二乘法对卡尔曼滤波估计进行修正，有效结合两者的优点，以达到更优的跟踪效果。

1 传统定位算法分析

常规的TOA定位算法包括最小二乘法、基于泰勒展开的增量式算法、卡尔曼滤波算法等，其中以最小二乘法和卡尔曼滤波算法为主。

图2 最小二乘原理图Fig.2 Schema of least squares

1.1 最小二乘定位算法

在二维平面系统中，通过测得已知的n个传感器节点（x1，y1），（x2，y2），（x3，y3），（xn，yn）…与目标的距离 d1，d2，…，dn，求得目标坐标（x0，y0）。根据图 2 列出以下方程组：

整理得：AX=B

其中：

采用最小方差的估计方法可得到坐标：

1.2 扩展卡尔曼滤波定位算法

对于多个跟踪对象，假设各对象的线性离散系统模型如下：

式中：N为网络中目标个数；k为采样时刻；Xi，k为k时刻被估计的n维系统状态变量；Φi，k/k-1为k-1时刻到k时刻系统n维的状态转移矩阵；Zi，k为k时刻的m维观测量；Hi，k为k时刻的m×n维量测矩阵，过程噪声 Wi，k和观测噪声 Vi，k是均值为 0，符合高斯正态分布的白噪声序列，其协方差矩阵分别为Qi，k-1和 Ri，k。

常规卡尔曼滤波递推方法如下[11]：

在非线性的目标跟踪系统中，要利用卡尔曼算法，将非线性过程围绕当前的状态估计利用泰勒级数展开方法进行近似线性化，再用卡尔曼滤波算法解决非线性系统的滤波问题，即扩展卡尔曼滤波算法（EKF）[12]。

1.3 单个观测值的扩展卡尔曼滤波模型

将轮式移动机器人作为多目标跟踪系统的移动目标，根据机器人的运动学方程建立滤波器模型的状态方程：

其中：第 i个移动机器人的状态为 Xi=［xiνi，xyiνi，y］T，分别代表X轴、Y轴的坐标和速度；mi代表机器人的质量；T为采样周期，此时状态转移矩阵是常数矩阵。

观测方程 Zi，k=Hi，kXi，k的建立如下：

假设簇节点（xi，a，yi，a）在某一时刻 k 与目标 i之间的理论距离值为 di，a：

线性化可得：

其中：Zi，k=di，a，Xi，k=[xi，k，yi，k] 为 k 时刻第 i个机器人的坐标，观测矩阵为

2 基于扩展卡尔曼滤波的混合估计算法

在多目标跟踪应用系统中，由于目标运动特性及其跨越网格区域的多样性，同时存在无线传感器网络因数据掉包而引起的观测值丢失问题，观测环境变得异常复杂。最小二乘定位算法受测量值噪声影响大，而传统的扩展卡尔曼滤波定位算法也存在一定程度的延时问题，使得网络对于速度发生突变的目标的精确跟踪变得困难，甚至出现丢失目标的严重后果。两者的性能对比如表1所示。

表1 两种传统定位算法的性能比较Tab.1 Performance comparison of two traditional location algorithms

采用卡尔曼滤波定位算法时，当目标发生突变运动时，目标状态一般也发生较大的变化。此时卡尔曼的单步预测状态估计与真实状态误差很大，但是由观测值体现出来的系统状态误差却较小。因此，以观测值为基准，通过增强新加入信息的修正作用，以此加大滤波增益K，达到加快滤波跟踪速度、减小估计误差的目的。

以单个目标为例：卡尔曼增益Kk与状态估计误差协方差矩阵Pk+1/k和观测噪声协方差矩阵Rk有关。若系统采用相同结构和参数的传感器节点，则Rk可视为常数。Kk随Pk+1/k的增大而增大，且Pk+1/k体现了滤波的精度，Pk+1/k越小说明滤波越准确[13]。因此，Kk的增大是由滤波不准确，协方差矩阵较大所要求的。

其中：Kk为常规卡尔曼滤波在k时刻的增益；为附加增益。系统状态估计方程重写为

其余递推公式保持不变。

式中：k0为系统状态发生突变的时刻；Kz，k0为 k0时刻足以实现目标快速跟踪的增益矩阵；λ为常数，一般取0.8～0.9。可通过设定一个非常大的协方差阵Pz，对式（8）、式（9）进行修正以确定 Kz，k0[13]。 协方差阵Pz代表滤波估值与状态真值之间的误差，但其并没有一种标准的理论取法。

本文结合收敛速度快的最小二乘法来确定Kz，k0。在目标发生运动突变时，最小二乘法以其快速收敛的特性保证获取较为准确的状态估计，但其仍然存在较大的噪声，因此先采用一阶低通滤波器消除高频噪声：

假设利用最小二乘法经低通滤波器得到k0时刻的系统状态估计为Xˆz，k0，则通过设计一个 Pz，k0：

可得：

最终卡尔曼增益设计为

模型参数及初值的确定：

上述混合估计算法的模型参数及初值可按常规的卡尔曼滤波器来确定。若多目标跟踪系统采用周期驱动异步测量策略且采样周期为T，则过程噪声协方差：

其中，λ为过程噪声因子。

过程噪声和量测噪声必须满足一定的数量级关系才能得到好的跟踪效果。量测噪声协方差的辨识原则为

P0可选择范围比较大，可设为单位阵I。系统初值X0则可通过最小二乘定位算法确定。

3 算法效果验证

3.1 实验平台简介

系统在逻辑结构上分为感知层、网络通信层、应用层，实体垂直架构依次为分布式测量节点、基站、USB-ZigBee通信网关、数据服务中心。系统架构如图3所示。

图3 实验平台架构图Fig.3 Configuration of experimental platform

12个分布式网络超声波测距节点均匀分布，在平面上组成一个大小为4 m×6 m，坐标从 （0，0）到（400，600）的有效监视区域。将监视区域划分为6个网格，每个网格大小为2 m×2 m，包含4个节点；基站是在无线网络覆盖区中，负责整个网络的资源分配和信息解析的单元；USB-ZigBee通信网关是负责基站和数据服务中心之间的信息传递，实现上下行双向通信的单元；数据服务中心是采集无线传感器网络原始测量数据和利用数据融合定位算法计算目标位置信息的单元，并负责绘制目标轨迹曲线和存储历史数据；轮式移动机器人可在监测网络内任意移动，作为系统的跟踪目标。

3.2 速度突变情况的效果

针对移动目标速度突变运动的情况，将本文的混合估计算法嵌入到多目标跟踪实验平台中，可得到如图4的实验结果。图4（a）是目标以0.5 rad/s的速度运动到两网格交界处并突然停止（速度变为0）的理想轨迹；图 4（b）、（c）、（d）分别为最小二乘算法、常规卡尔曼滤波定位算法、混合估计算法的实际跟踪轨迹图。与收敛快速的最小二乘法相比，混合估计算法虽然收敛速度稍慢，但其对目标速度不发生大突变时的跟踪更趋平滑准确；而与收敛速度慢的标准卡尔曼滤波相比，其对于目标速度不会突变时的跟踪效果相当，但是对于移动目标速度突变时的收敛速度则快得多，跟踪效果更佳。

图4 速度突变情况下性能对比Fig.4 Performance comparison of mutation rate case

3.3 实际跟踪结果分析

图5是两台轮式移动机器人同相位协同控制的实际效果图。其中一台机器人在左上方的网格内围绕网格中心做半径为0.5 m的圆周运动，线速度为0.3 m/s，角速度为0.6 rad/s，角速度变化在10%以内；另一台机器人则在最下面四个网格围绕中心节点做半径为1 m，通过无线网络反馈位置信息给机器人调节控制器输出，使得其角速度和轴向角跟前一台机器人一致的圆周运动，实现双机器人的同相位协同控制。

图5 双机器人协同控制效果图Fig.5 Diagram of dual-robot cooperative control

实验结果表明，混合估计算法合理地融合了两种定位算法的优点，不仅能准确跟踪正常运动的目标而不丢失，且对于移动机器人在网格切换时速度和观测模型发生突变时的运动也能达到平滑、快速跟踪的效果。

4 结语

本文讨论了无线传感器网络多目标跟踪应用中目标运动特性多样化的复杂情况及由其导致的跟踪困难问题，并针对该问题提出了混合估计算法。该算法是一个自适应的校正过程，当目标移动速度不突变时，其能够以常规的卡尔曼滤波定位算法出现，达到平滑且准确的目标跟踪性能；而当目标运动速度发生突变时，其又能够结合收敛速度快的最小二乘法来校正卡尔曼滤波增益，达到快速跟踪的目的。通过搭建一个基于卡尔曼滤波的无线传感器网络多目标实时跟踪实验平台，进行相关算法的效能验证。实验结果表明，本文的自适应混合估计算法具有良好的跟踪性能。

[1] 杜晓通.无线传感器网络技术与工程应用[M].北京：机械出版社，2010.

[2]Yong D，Chen W，Li X.Using mobile beacons to locate sensors in obstructed environments[J].Journal of Parallel and Distributed Computing，2010，70（6）：644-656.

[3]Zhang R B，Zhang L L，Feng Y B.Very low energy consumption wireless sensor localization for danger environments with single mobile anchor node[J].Wireless Personal Communications，2008，47（4）：497-521.

[4] Kuang XH，Shao HH，Feng R.A new distributed localization scheme for wireless sensor networks[J].Acta Automatica Sinica，2008，34（3）：344-348.

[5]Mirela M，Mihaela C.Improved sensor network lifetime with multiple mobile sinks[J].Pervasive and Mobile Computing，2009，5（5）：542-555.

[6] 郑巍，刘三阳，寇晓丽.动态传感器网络移动代理路由算法[J].控制与决策，2010，25（7）：1035-1039.

[7] Wang G J，Wang T，Jia W J.Adaptive location updates for mobile sinks in wireless sensor networks[J].Journal of Supercomputing，2009，47（2）：127-145.

[8]Camp T，Bolenq J，Davies V.A survey of mobility models for ad hoc network research[J].Wireless Communications and Mobile Computing，2002，2（5）：483-502.

[9] Kuo-Feng Ssu，Chia-Ho Ou，Jiau HC.Localization with mobile anchor points in wireless sensor networks[J].IEEE Transactions on Vehicular Technology，2005，54（3）：1187-1197.

[10]Yang Liu.Sun Zhen-dong.EKF-based adaptive sensor scheduling for target tracking[C]//IEEE International Symposium on Information Science and Engieering，2008：171-174.

[11]Rajendran V，Obraczka K，Garcia-Luna-Aceves JJ.Energy-efficient，collision-free medium access control for wireless sensor networks[C]//Proceeding of SenSys03，2003：181-192.

[12]Ye Jingzhi，Zhao Ling，Luo Wenfeng.Performances of localization algorithms in a prototype WSN system[J].Advanced Materials Research，2012（457-458）：723-727.

[13]孙枫，刘希斌.一个快速跟踪的卡尔曼滤波算法及其在舰船组合导航中的应用[J].中国惯性技术学报，2000，8（4）：19-23.