基于扩展卡尔曼滤波的自适应混合估计算法
小学生环保手抄报-有关祖国的作文
文章编号:1001—9944(2014)01.0006—05
基于扩展卡尔曼滤波
的自适应混合估计算法
余辉荣1,夏候凯顺1,叶景志1,邬依林
1,2
(1.华南理
S-大学自动化科学与工程学院,广州510640;2.广东第二师范学院计算机科学系,广州510310)
摘要:在无线传感器网络多目标跟踪应用中,针对被跟踪对象运动特征的多样性和速度突
变等复
杂运动情况而导致的定位不准确、对延,甚至目标丢失等问题,分析了传统定位算
法各自的优劣,在扩展
卡尔曼滤波算法平滑跟踪的基础上有效融合最小二乘算法快速跟
踪的特点,提出一种基于扩展卡尔曼滤波
的自适应混合估计定位算法。实验平台结果验证
了该算法能够实现对多个复杂运动目标的快速准确跟踪。
关键词:多目标跟踪;定位算法:卡尔曼滤波:混合估计
中图分类号:TP212.6文献标志
码:A
Self-adaptive
Hybrid
Estimation
Al
gorithm
Based
on
Extended
Kalman
Fi
lter
YU
Hui—ron91,XIAHOU
(1.School
Kai
—shunl,YE
Jing・zhil,WU
Yi—linl,2
and
E
ngineering,South
China
University
of
T
echnology,Guangzhou
5
10640,China;
of
Automation
Science
2.Department
of
Com
puterScience,GuangdongUniversity
of
Educatio
n,Guangzhou
510310,China)
multi—targettracki
ng
application
ofwireless
sensor
Abstr
act:In
and
even
networks,for
the
ti
me
delay,inaccuratepositioning
object
los
t
problems
resultedfromthe
diversity
a
nd
algorithms,a
complexity
strategy
of
movement
characteristicsofthe
trackingeffectively
merging
objects
and
byjudging
thetraditionallocationofthe
smooth
lytracking
of
es—
can
Kalmanfilter
algorithm
timation
achieve
algorithm
a
ccurate
withthe
on
rapidity
ofleast
square
algorithm
was
presented,result
Kalman
filter.Experimental
in
in
a
self-adaptivehybrid
algorithm
platform
b
asedextendedresults
time.
showthatthe
tra
cking
for
multiplecomplex
mobile
targe
ts
real
Keywords:multi-targettracking;locati
onalgorithm;Kalmanfilter;hybrid
estimation
多
个目标的实时、精确跟踪是无线传感器网
络的重要应用和研究难点。随着被跟踪目标数量及
其运
动特征多样性的增加,若要继续保持有效跟
踪。则对于分布式传感器节点的协同调度效率和定
位
算法的要求也同步上升。一个优良的定位算法必
须具备自组织性、低能耗、高鲁棒性和分布式计算
等特点【”。无线传感器网络中的目标跟踪算法基本
收稿日期:2013—06—18:修订日期:2
013-07—17
上分为直接通信法【2】、基于二进制探测的方法【3】、
基于树状结构的
方法…、基于簇状结构的方法[51、
基于预测机制的方法f61、基于粒子滤波的方法【71、
基于自适应机制的方法【81和对偶空间转换跟踪方
法191等。
传统的最小二乘定位算法及
常规的卡尔曼滤
波定位算法是最常用的方法。前者具有收敛速度
基金项目:广东省自然科学基金
资助项目(¥2012010008462);国家自然科学基金资助项目(61273109)
作者简
介:余辉荣(1988~),男,硕士研究生,研究方向为无线传感器网络、多智能体协同控制等。
■<
br>万方数据
Automation&Instrumentation
2014(1)
p>
快、跟踪及时等优点,但测量信号中的噪声在融合
后也被凸显,导致定位轨迹波动较
大;后者是一种
高效率的自回归滤波器,能够从一系列不完全及包
含噪声的测量中。估计出动态
系统的状态【10],抗观测
噪声干扰能力强,其缺点是信号收敛速度较慢。因
此,对于目标运
动特性较复杂的情况(如图1所
示).当运动速度发生突变时,运用单一的任一定位
算法都无法
达到快速且准确的追踪。
图1
目标运动特性示意图
Fig.1
Schemao
f
target
motioncharacteristics
本文针对上述两种算法
的固有缺陷,提出了一
种自适应的混合估计算法。该算法以基于单个观测
值的扩展卡尔曼滤波模
型为基础,结合收敛速度更
快的最小二乘法对卡尔曼滤波估计进行修正,有效
结合两者的优点,
以达到更优的跟踪效果。
1
传统定位算法分析
常规的TOA定位算法包括最小二乘法、
基于泰
勒展开的增量式算法、卡尔曼滤波算法等,其中以
最小二乘法和卡尔曼滤波算法为主。<
br>图2最小二柔原理图
Fig.2
Schema
ofleast
squa
res
1.1
最小二乘定位算法
在二维平面系统中,通过测得已知的n个传感
器节点(菇-,Y,),(X'2,i,,z),(X3y3),(Xn%)…与目标的
距离d.,d2
,…,以,求得目标坐标(z。,y0)。根据图2列
a动化s仪表2014(1)
万方数据<
br>出以下方程组:
(Xl叫o)2+(),1一yo)2=d12
(茏2叫o)2+(),
2一yo)2=d22
(Xn叫o)2+(%一yo)2=砰
整理得:AX=B
其中:
2(x1一戈。)
2(yl—n)
A=
2(x2--X。)2(y2-Y。)
(2)
X12-xj七7}一Yj—d12+dnl
B=
X22一x:+y壬
一y:一d22+d.2
(3)
Xn_12一xj+'‘一'j—a0七a:
x=I<
br>XOyo]
(4)
J
采用最小方差的估计方法可得到坐标:
群5=(A
TA)一1A啊
(5)
1.2扩展卡尔曼滤波定位算法
对于多个跟踪对象,假设各对象
的线性离散系
统模型如下:
p21‘些tk.-,l+Wi,k-I^…,眦-1,2,…(6
)
’’’
’。
【Z拙=皿,&置。£+K,^
式中:Ⅳ为网络中目标个数;k
为采样时刻;置.^为k
时刻被估计的n维系统状态变量;痧㈨一。为五一1时
刻到五时刻系统
n维的状态转移矩阵;Z“为是时
刻的m维观测量;日.^为后时刻的m×凡维量测矩
阵,过程
噪声W,。和观测噪声K.。是均值为0,符合
高斯正态分布的白噪声序列,其协方差矩阵分别为
Q啪一l和冠加
常规卡尔曼滤波递推方法如下【ll】:
Stepl
状态的单步预测
方程为
置,k/k-1=痧i,k/k-IXi如l
(7)
Step2单步预测状态协
方差方程为
Pi,肌_l-西i,k/k-1Pi忙l艄咖l肌一1+Qnl(8)
Step3
最优卡尔曼增益:
K,^-_it,m臆-1日1.々(日。。∥i.孤一】日l。k+R涤)J(9)
Step4修正状态预测估计置.船_1.
置.klk:Xi,k/k-l+K.I[Zm啦,
k置.腓一1]
(10)
Step5当前状态误差协方差矩阵为
Pi,
klk=[五一K.%日,tleⅢ一1
(11)
在非线性的目标跟踪系统中,要利用卡尔曼算
法,将非线性过程围绕当前的状态估计利用泰勒级
数展开方法进行近似线性化,再用卡尔曼滤波
算法
解决非线性系统的滤波问题.即扩展卡尔曼滤波算
法(EKF)[121。
1.3
单个观测值的扩展卡尔曼滤波模型
将轮式移动机器人作为多目标跟踪系统的移
动目标,根据机器
人的运动学方程建立滤波器模型
的状态方程:
OO
1丁0
.。...T...
——0
010
mi
置“l=币。置.女+J1。Ui.产
00100
U.≈
O00
0旦
m.
(12)
其中:第i个移动机器人的状态为X
i=Ex。Ⅳh
yi
1-'ti,y]1,
分别代表X轴、y轴的坐标和速度;m。代
表机器人
的质量:丁为采样周期,此时状态转移矩阵是常数
矩阵。
观测方程Z啪堀.。
置,。的建立如下:
假设簇节点(Xi,a儿.。)在某一时刻后与目标i之
间的理论距离值为
d知:
di,o、/(互“吨抽)2+(竹.女了。)2
(13)
线性化可得:
din2一=了7云云i三;妻专盂赢并i^+
丽差需嘉亍扎t
。4)
、/(戈m—
戈i.。)2+(y“一y。,。)2。’
其中:Zm=盔。X,≈=Ix泓,yi,t】为而时刻第i
个机器人
的坐标.观测矩阵为
Hi,k=‘丽X丢i筹杀yi矛yl
o
、/(
,^—戈。)‘+(.k一.。)‘
、/(戈汕一戈。,。)2+(ym—M,。)2
瓦差嚣高
歹叫Q5)
2基于扩展卡尔曼滤波的混合估计算法
在多目标跟踪应用系统中,由于目标运动特性
及其跨越网格区域的多样性,同时存在无线传感器
网络因数据掉包而引起的观测值丢失问题,观
测环
■
万方数据
境变得异常复杂。最小二乘定位算法受测量值噪声
影响大.而
传统的扩展卡尔曼滤波定位算法也存在
一定程度的延时问题.使得网络对于速度发生突变
的目标
的精确跟踪变得困难,甚至出现丢失目标的
严重后果。两者的性能对比如表1所示。
表1两种传
统定位算法的性能比较
Tab.1
Performance
comparisonoftwotraditionallocation
algorithms
采用卡尔曼滤
波定位算法时,当目标发生突变
运动时,目标状态一般也发生较大的变化。此时卡
尔曼的单步预
测状态估计与真实状态误差很大.但
是由观测值体现出来的系统状态误差却较小。因
此,以观测
值为基准,通过增强新加入信息的修正
作用.以此加大滤波增益K,达到加快滤波跟踪速
度、减
小估计误差的目的。
以单个目标为例:卡尔曼增益K与状态估计误
差协方差矩阵Rm和观测噪声
协方差矩阵露。有
关。若系统采用相同结构和参数的传感器节点,则
现了滤波的精度,R川。越
小说明滤波越准确【13】。因
此,K的增大是由滤波不准确,协方差矩阵较大所
要求的。根据以上分析,改进后的卡尔曼增益凰具有如
下形式:
甄=甄+甄(16)
其中:
噩为常规卡尔曼滤波在k时刻的增益;甄为
附加增益。系统状态估计方程重写为
墨肮=墨心+甄
[Z^一巩凰腑一1]
(17)
其余递推公式保持不变。
缸的设计目标:当检测到系统
状态发生很大变
化时,尬同步增大,加大信息的校正作用,提高跟踪
速度:当系统状态趋于稳定
后甄迅速减小,使Kk兰
甄,转化成标准的卡尔曼。令
一
^一女.
凰=K,k
A
,
O<A<I
0)
(
18
式中:k。为系统状态
发生突变的时刻;K^为ko时刻
足以实现目标快速跟踪的增益矩阵;A为常数,一般
取0.8
。0.9。可通过设定一个非常大的协方差阵只,
Automation&Instrumentati
on
201钺J)
蛾可视为常数。甄随Rm的增大而增大,且只枞体
对
式(8)、式(9)进行修正以确定K^【131。协方差阵罡
代表滤波估值与状态真值之间的误差,但
其并没有
一种标准的理论取法。
本文结合收敛速度快的最小二乘法来确定K^。
在目标
发生运动突变时,最小二乘法以其快速收敛
的特性保证获取较为准确的状态估计,但其仍然存
在
较大的噪声.因此先采用一阶低通滤波器消除高
频噪声:
五.产仅・置.I+(1一a)・五扣
l(19)
应用层
3算法效果验证
3.1
实验平台简介
系统在逻辑结
构上分为感知层、网络通信层、应
用层,实体垂直架构依次为分布式测量节点、基站、
USB—
ZigBee通信网关、数据服务中心。系统架构如
图3所示。
.。。
粼专心
箭叫I心—‘,反\
’
,,
\、\、
其中:置.。为本次滤波输出值;置。。
为本次采样值;
置肛.为上次滤波输出值;0c为滤波系数,其值通常
远小于l。
假设
利用最小二乘法经低通滤波器得到k。时
刻的系统状态估计为置^,则通过设计一个只,k:
只
.t。=研(五,t。域。)(五.k啦。)1’)
可得:
(20)
K.kZ.^。H
l。=(巩。只,t。日^。t皿k)一
最终卡尔曼增益设计为
(21)
鲨繁≥一≯<
br>嘶‘二窖’h。矿
图3实验平台架构图
Fig.3
网络通信层
_-‘/
…ZigBcc■二纱¨
_
认
、。
。
矗‰
o_矿
飞:
。薹国y上
Configuration
of
exper
imentalplatform
噩—‰一。HT(风f‰一1日14最)。1+
I—I
12个分布式网络超声波测距节点均匀分布,在
平面上组成一个大小为4
mx6
A。
只,^。日l。(日‘。只,^。日k1搬‘。)~,O<A<I(22)
模型参数及初值的确定:上述混合估计算法的模型参数及初值可按常
规的卡尔曼滤波器来确定。若多目标跟踪系统采用
周期驱动异步测量策略且采样周期为r,则过程噪
声协方差:
m,坐标从(O,0)到
m,包含4个节点;
(400。600)的有效监视区域。将监视区域划分为6
个网格,每个
网格大小为2
mx2
基站是在无线网络覆盖区中.负责整个网络的资源
分配和信息解析
的单元;USB—ZigBee通信网关是负
责基站和数据服务中心之间的信息传递,实现上下
行双向通信的单元:数据服务中心是采集无线传感
器网络原始测量数据和利用数据融合定位算法计算}丁3}r2
级=A
00
oo
争r2丁0
r
00
0
目标位置信息的单元.并负责绘制目标轨迹曲线和
(23)
存储历史数据:轮式移
动机器人可在监测网络内任
意移动。作为系统的跟踪目标。
r
扣r
3.2速度
突变情况的效果
针对移动目标速度突变运动的情况.将本文的
混合估计算法嵌入到多目标跟踪实
验平台中,可得
到如图4的实验结果。图4(a)是目标以0.5rad/s的
速度运动到两网
格交界处并突然停止(速度变为0)
的理想轨迹;图4(b)、(c)、(d)分别为最小二乘算
其中,A为过程噪声因子。
过程噪声和量测噪声必须满足一定的数量级
关系才能得到好的跟踪
效果。量测噪声协方差的辨
识原则为
^
戤=}艺[(zi讯钆)(Zi也乱)7]托
扛1
(24)
法、常规卡尔曼滤波定位算法、混合估计算法的实际
跟踪
轨迹图。与收敛快速的最小二乘法相比。混合
估计算法虽然收敛速度稍慢,但其对目标速度不发
生大突变时的跟踪更趋平滑准确;而与收敛速度慢
9{
Po可选择范围比较大,可设为单位阵J
。系统初
值凰则可通过最小二乘定位算法确定。
自动诧s仪表2014(t)
万方数据
的标准卡尔曼滤波相比,其对于目标速度不会突变
时的跟踪效果相当,但是对于
移动目标速度突变时
的收敛速度则快得多,跟踪效果更佳。
网络2网络2‘网络2网络2
沙。}纠∥伊
.
网络I....用聱1..
彤爵1..
圈聱1.
(
。-)删思轨迹
(h)最小‘乘(t}杯准f、尔曼(tlJ混合什ff
图4速度突变情况下性
能对比
Fig.4
Performance
comparison
ofmut
ationrate
case
3.3实际跟踪结果分析
图5是两台轮式移动机器人同相
位协同控制
的实际效果图。其中一台机器人在左上方的网格内
围绕网格中心做半径为0.5nl
的圆周运动。线速度
为O.3
m/s,角速度为0.6rad]s,角速度变化在10%以内;另一台机器人则在最下面四个网格围绕中心节
点做半径为1m.通过无线网络反馈位置信息给机
器人调节控制器输出,使得其角速度和轴向角跟前
一台机器人一致的圆周运动,实现双机器人的
同相
位协同控制。
图5双机器人协同控制效果图
Fig.5Diagram
o
f
dual-robot
cooperative
control
实验结果表
明。混合估计算法合理地融合了两
种定位算法的优点,不仅能准确跟踪正常运动的目
标而不丢失
,且对于移动机器人在网格切换时速度
和观测模型发生突变时的运动也能达到平滑、快速
跟踪的
效果。
4结语
本文讨论了无线传感器网络多目标跟踪应用
中目标运动特性多样化的复杂
情况及由其导致的
跟踪困难问题.并针对该问题提出了混合估计算
10
万方数据
法。该算法是一个自适应的校正过程。当目标移动
速度不突变时,其能够以常规的卡尔曼滤波定位算<
br>法出现,达到平滑且准确的目标跟踪性能;而当目标
运动速度发生突变时。其又能够结合收敛速度
快的
最/Ix--乘法来校正卡尔曼滤波增益。达到快速跟踪
的目的。通过搭建一个基于卡尔曼
滤波的无线传感
器网络多目标实时跟踪实验平台。进行相关算法的
效能验证。实验结果表明,本
文的自适应混合估计
算法具有良好的跟踪性能。
参考文献:
【l】杜晓通.无线传感器
网络技术与工程应用【M】.北京:机械出版社,
2010.
【2】2
YongD,C
hen
W,Li
X.Using
mobilehebron8
to
l
ocate
sensors
inobstructed
environments[j
]journal
of
ParallelandDistributed
Comput
ing,2010,70(6):“4—656.
【3】3
Zhang
R
B,
Zhang
L
L,Feng
Y
B.Very
low
ener
gy
consump-
tionwireless
sensor
locali
zation
for
danger
environments
with
single
mobile
anchor
node[J].Wireless
Personal
Communications,
2008,47(4):497—521
.
【4】4
Kuang
XH,Shao
HH,Feng
R.A
new
distributed
localization
∞hemefor
wireless
sensor
networksL刀.Acta
Automati
ca
Sinica,
2008,34(3):344-348.
【5】Mirela<
br>M,Mihaela
C.Impmved
sensor
networklife
timewith
multiple
mobile
sinks[J].Pervasi
ve
andMobile
Computing,2009,
5(5):542-555
.
【6】郑巍,刘三阳,寇晓丽.动态传感器网络移动代理路由算法叨.控
制与决策,2010
,25(7):1035—1039.
【7】Wang
G
J,Wang
T,J
ia
W
J.Adaptive
location
updates
fo
rmo-
bilesinksin
wirelesssen∞r
networks[J
].Journal
of
Supereomput・
ing,2009,47(2):
127-145.
【8】Camp
T,Bolenq
J,Davies
V.A
survey
of
mobility
modelsfor
ad
hoc
network
research[J].Wireless
Communi
cations
andMobile
Computing,2002,2(5):483—50
2.
【9】Kuo-Feng
Ssu,Chia-Ho
Ou,Jiau
HC.
Localizationwith
mobile
anchor
points
in
wireless
sensor
networks[J].IEEE
Tr
ansactions
on
Vehicular
Technology,2005,5
4(3):1187-1197.
【10】Yang
Liu.Sun
Zhen-don
g.EKF-based
adaptive
sorlsor
schedulingfor
target
tracking[C]//IEEE
Internatio
nal
Symposium
on
Infor-
mationSciencea
nd
Engieefing,2008:171—174.
【11】Rajendran
V,Obraezka
K,Garcia-Luna—Aeeves
JJ.Energy—e
ffi・
eient,collision-free
medium
accessco
ntrol
for
wireless
sensor
networks[C]/
/Proceeding
of
SenSys03,2003:181—192.
【12
】Ye
Jingzhi,Zhao
Ling,Luo
Wenfeng.Perform
ances
of
localization
algorithms
in
a
prototype
WSN
system[J].Advanced
Ma
terials
Research,2012(457--458):723-727.
【13
】孙枫,刘希斌.一个快速跟踪的卡尔曼滤波算法及其在舰船组合
导航中的应用册.中国惯性技术学报,
2000,8(4):19—23.
●
基于扩展卡尔曼滤波的自适应混合估计算
法
作者:
作者单位:
余辉荣, 夏候凯顺, 叶景志, 邬依林, YU Hui-
rong, XIAHOU Kai-shun, YE Jing-zhi, WU
Yi-
lin
余辉荣,夏候凯顺,叶景志,YU Hui-rong,XIAHOU Kai-
shun,YE Jing-zhi(华南理工大学自动化科学与工程
学院,广州,510640),
邬依林,WU Yi-lin(华南理工大学自动化科学与工程学院,广州510640;广东第二师
范
学院计算机科学系,广州510310)
自动化与仪表
Automation & Instr
umentation
2014,29(1)
刊名:
英文刊名:
年,卷(期)
:
参考文献(13条)
1.杜晓通
无线传感器网络技术与工程应用 2010
D;Chen W;Li X
Using mobile beacons to locate sensors in
obstructed environments[外文期刊] 2010(06)
R
B;Zhang L L;Feng Y B
Very low energy
consumption wireless sensor localization for
danger environments
with single mobile anchor
node[外文期刊] 2008(04)
XH;Shao HH;Feng R
A new distributed localization scheme for
wireless sensor
networks[期刊论文]
-
Acta
Automatica Sinica
2008(03)
M;Mihaela C
Improved sensor
network lifetime with multiple mobile sinks[外文期刊]
2009(05)
6.郑巍;刘三阳;寇晓丽
动态传感器网络移动代理路由算法[期刊论文]
-
{H}控制与决策
2010(07)
G J;Wang T;Jia W J
Adaptive
location updates for mobile sinks in wireless
sensor networks[外文期刊]
2009(02)
T;Bolenq
J;Davies V
A survey of mobility models for
ad hoc network research[外文期刊] 2002(05)
-Feng
Ssu;Chia-Ho Ou;Jiau HC
Localization with
mobile anchor points in wireless sensor
networks[外文期刊]
2005(03)
Liu;Sun Zhen-
dong
EKF-based adaptive sensor scheduling
for target tracking 2008
ran V;Obraczka
K;Garcia-Luna-Aceves JJ
Energy-
efficient,collision-free medium access control for
wireless
sensor networks 2003
Jingzhi;Zhao
Ling;Luo Wenfeng
Performances of
localization algorithms in a prototype WSN system
2012(457-
458)
13.孙枫;刘希斌
一个快速跟踪的卡尔曼滤波算法及其在舰船组合导航中的应用 2000(04)
引用本文格式:余辉荣.夏候凯顺.叶景志.邬依林.YU Yi-lin
基于扩展卡尔曼滤波的
自适应混合估计算法[期刊论文]
-
自动化与仪表
2014(1)