基于视觉的机动目标定位跟踪的滤波算法概要.docx
《基于视觉的机动目标定位跟踪的滤波算法概要.docx》由会员分享，可在线阅读，更多相关《基于视觉的机动目标定位跟踪的滤波算法概要.docx（17页珍藏版）》请在冰豆网上搜索。
基于视觉的机动目标定位跟踪的滤波算法概要
第27卷 第2期2006年2月
仪 器 仪 表 学 报
ChineseJournalofScientificInstrument
Vol.27No.2Feb.2006
基于视觉的机动目标定位跟踪的滤波算法
朱 枫1,2 侯菲莉1,21（中国科学院沈阳自动化研究所 沈阳 1100162（中国科学院研究生院 北京 100080
摘要 在视觉定位系统中,由于各种噪声的影响,运动目标的三维位置和姿态的计算精度受到一定限制。
为了提高运动物体的定位跟踪精度,提出了一种有效的滤波算法。
和已有的方法相比,这种算法具有以下两个特点:
第一,不再局限于平缓运动的物体,它对于未知运动规律的机动目标同样有效;第二,由于避免了扩展卡尔曼滤波器的使用,滤波复杂度有所下降。
通过分析噪声对位姿计算误差的影响,建立了一组描述位姿测量值和真实值关系的线性测量方程。
然后,分别给出了两种滤波算法:
基于有限记忆的检测自适应滤波和基于数值微分模型的卡尔曼滤波。
在检测自适应滤波算法中,给出了分别适用于快机动和慢机动的最优机动检测函数。
一旦检测出机动发生,系统采用有限记忆滤波进行矫正。
在第二种滤波算法中,系统采用数值微分技术构造出了描述机动目标运动行为的鲁棒估计模型。
并且,引入了衰减因子,以防止滤波器的发散现象。
该衰减因子可以根据位姿计算值自适应估计。
最后,通过伪贝叶斯估计算法,将两种滤波器进行数据融合,有效的降低了机动时刻位姿估值的误差抖动,进一步提高了定位跟踪精度。
仿真结果验证了本算法的有效性。
关键词 滤波算法 位姿计算 机动检测 数值微分 伪贝叶斯估计
中图分类号 TP391.9 文献标识码 A 国家标准学科分类代码 520.6040
FilterAlgorithmforVisualPositioningandTrackingofManeuveringTarget
ZhuFeng1,2 HouFeili1,21（RoboticsLaboratory,ShenyangInstituteofAutomation,ChineseAcademyofSciences,Shenyang110016,China2（GraduateSchool,ChineseAcademyofSciences,Beijing100080,China
Abstract Itpresentsafilteralgorithmforaccuratelyvisualtrackingofamaneuveringtarget.Emphasisisgiventofindasolutionforthe3Dpose（positionandorientationestimationdegradationcausedbynoiseintheimages.Superiortopreviousapproachesthatwerelimitedtotheassumptionthatthetargetmotionisslowandsmooth,thisalgorithmisapplicableforamaneuveringtargetthatactsinanunknownmanner.Moreover,thecomputationloadisreducedbyavoidingtheneedofextendedKalmanfiltering.First,byanalyzingtheeffectofnoisein2Dimageson3Dposeerrors,linearmeasureequationsrelatingthenominalposeparameters（obtainedby3Dreconstructionandthetrueonesarederived.Then,twofilterschemesareintroducedrespectively.Thefirstfilterusesmaneuverdetectiontechnique,inwhichthecriterionofanoptimaldetectorisdeduced,andtwosub-optimaldetectorssuitableforfastandslowmaneuversaregivenrespectively.Whenamaneuverisdeclared,limitedmemoryfilteringisadoptedforupdate.Inthesecondfilter,arobustdynamicmodelisconstructedbythenumericaldifferentiationtechnique.Torestrainthedivergencecausedbytruncationerrorsofestimatemodelofnumericaldifferentiation,afadingfactorisimported,whichcanbecontrolledadaptivelybasedonthemeasurements.Finally,generalizedpseudoBayesalgorithmisemployedtocombinethetwofiltersformoreaccuratetracking.Experimentresultsillustratethecapacityofthisalgorithm.
Keywords Filteralgorithm Positionandorientationestimation Maneuverdetection Numericaldifferentiation PseudoBayesalgorithm
1 引 言
在基于视觉的运动目标定位跟踪系统中,一个重
要的问题是对物体的三维位置和姿态的计算。
由于各
种噪声的影响（如传感器噪声、背景噪声和量化噪声
等,物体的位姿估计值存在很大的误差。
由于长图像
序列能够提供更多的信息来抑制噪声,因此扩展卡尔
曼滤波器一直以来被广泛的采用,并取得了很好的效
果[1-4]。
然而,在这些算法中,物体的运动模型都近似为
常（加速度的平移和旋转模型,从而限制了这些算法,使其仅仅适用于目标运动比较平缓的情况。
当物体的运动规律比较复杂,运动状态突然改变时,这些滤波算法则不能达到预期的效果。
文中在利用单目视觉进行物体位姿计算的基础上,提出了一种有效的滤波算法,用于准确估计机动目标的位姿。
同时,根据滤波器提供的位姿预测值和位姿变化率,可以预测下一时刻物体各特征点的像点坐标的范围,从而大大减少了图像处理的时间。
中文所提到的算法有以下特点:
（1和现有算法不同的是,将位姿参数的计算值代替特征点的像点坐标作为测量值,建立了线性测量模型。
一则避免了使用扩展卡尔曼滤波,降低了计算复杂度;二则可以对位姿并行滤波,加快了计算速度,更好满足实时性要求。
（2在机动检测自适应滤波算法中,针对不同的情况,推导出了分别适用于快机动和慢机动的检测方法,并采用有限记忆滤波进行机动矫正。
和基于加速度输入估值的矫正方法[5,7-8]相比,有限记忆滤波矫正不需要估计机动起始时刻,滤波精度有所提高。
（3在基于数值微分模型的滤波算法中,利用数值微分技术构造出的运动模型具有较强的鲁棒性。
同时,衰减因子的引入防止了数值微分模型的截断误差可能引起的滤波发散的现象。
该衰减因子可以根据位姿的测量值进行自适应估计。
（4两种滤波器交互输出,有效的降低了机动时刻位姿估值的误差抖动,进一步提高了定位跟踪精度。
2 位姿估计
2.
1 位姿估计模型计的方法,并给出了位姿的测量模型。
如图1所示,分别在摄像机上和物体上设定摄像机坐标系和物体坐标系,并在物体上固定一组特征点。
摄像机实时摄取物体的图像,通过图像处理,得到各特征点的像点。
由于特征点在物体上的位置已知,根据基于模型的单目视觉计算方法,即可求出它们在摄像机坐标系下的坐标。
最后,通过几何变换,即可得到两坐标系间的旋转矩阵R（t和平移矢量T（t,进而求得相对位置参数（相对方向x（t,水平方向y（t,垂直方向z（t和旋转参数（横滚角（t,俯仰角（t,偏航角（t。
图1 位姿估计模型
2.2 测量模型
文献[1-4]建立的测量模型都是以特征点的像点坐标为观测值。
考虑到位姿参数和像点坐标的非线性关系,直接采用位姿参数为观测值,建立观测模型为:
′（t
′（t
′（t
x′（t
y′（t
z′（t
=
（t
（t
′（t
x（t
y（t
z′（t
+
n（t
n（t
n（t
nx（t
ny（t
nz（t
（1
其中:
[n（t,n（t…nz（t]T为观测噪声。
下面将分析观测噪声的方差特性。
假定观测噪声服从零均值高斯分布,根据特征点像点和目标位姿的对应关系,由各个像点的误差范围,可以得到位姿观测值的最大误差。
根据3!
原理,若某个位姿参数的误差不超过L（t,则可近似认为其方差为:
（L（t/32。
根据以上线性测量方程,可以采用相同的滤波算法对各位姿参数同时滤波,以更好的满足实时性要求。
下面,以x为代表,叙述整个滤波过程。
166仪 器 仪 表 学 报 第27卷
3 机动检测自适应滤波
3.1 运动模型
建立离散化常加速模型:
Xk+1=∀Xk+#k,∃BUk+Wk
（2
其中,Xk=
x（kx・（kx・（kT
∀=
1TT2/
01T0
1
B=
0,#k,∃
=1 发生机动0 没有机动
3.2 机动检测
3.2.1 最优检测函数
假定系统在m时刻发生机动,其加速度输入为U。
取有偏新息#i的加权和作为检测统计量,令机动检测时间为d,则检测判别函数为:
Lk=
∑d
i=1
bi
#
k-d+i
∑d
i=1
b2i!
2
k-d+i
（3
其中:
!
2
k
为新息的方差。
如果Lk>TH（TH为阈值,认为机动发生。
下面,需要求取加权系数bi,使得Lk检测概率最大。
令ek为无偏新息,则#k和ek的关系为:
#k=H
k-m
U+ek其中:
Hk-m=
k≤mC∀Bk=m+1
C∀[
∏k-1
l=m+1
（I-KlC∀B
k>m+1
（4
由于ek为零均值白噪声,得到#k服从正态分布:
#k∞N（Hk-mU,!
2
k
（5
根据式（3
、（5,可以得到Lk服从正态分布:
LkN（0,1
k≤m
N
∑d
bi
H
k-m-d+i
U∑d
i=1
b2i!
2
k-d+i
1
k>m
（6
所以,要使k>m后式（3的检测概率最大,即等价于∑d
i=1biHk-m-d+iU
∑d
i=
1
b2
i
!
2
k-d+i
最大。
令 Jk=
∑d
i=1
bi
H
k-m-d+i
U
∑d
i=1
b2i!
2
k-d+i
（7
对Jk求偏导,得到当bi,并满足:
bopti
=
H
k-m-d+i
!
-2k-d+i
dj=1
k--2
-d（8
此时,式（3为具有最大检测概率的最优检测函数。
3.2.2 快慢机动的检测函数
在实际情况中,机动起始时刻m是未知的,所以需要针对不同的情况,求取合适的次优检测函数。
分析文献[5,7-8]中提出的基于加速度输入估值的检测函数:
Lk=U^
k
Var（Uk
（9
其中:
U^
k是对k时刻的加速度输入估计值,且:
U^
k=
∑d
i=1
bi
#
k-d+i=
∑
d
i=1
Hi!
-2
k-d+i
∑d
j=1
H
j
!
-2k-d+j
#k-d+i
（10
机动检测延时的时间越短,式（9越接近文中给出的最优检测函数,所以,式（9是适合快机动的检测函数。
慢机动时,机动检测的滞后时间较长。
此时,可以近似认为Hk-m-d+1
…Hk-m相等,又由于d不大时,亦可
近似认为!
-2
k-d+i（i=1…d也相等,因此式（8中的加权
系数可近似认为bopt
i≈1/d,即有函数:
Lk=
∑d
i=1
#
k-d+i
∑d
i=1
!
2
k-d+i
（11
式（11为适合于慢机动的检测函数。
3.3 有限记忆滤波矫正
考虑到发生机动后,离当前时刻较远的历史数据意义不大,所以在此采用有限记忆滤波法进行机动矫正。
和文献[5,7-8]中基于加速度输入估值的矫正方法
比较,该算法不需要估计机动起始时刻,避免了由于起始时刻估计不准确造成的滤波精度下降。
滤波矫正如图2所示,如果在k时刻检测到机动
发生,则认为k-N时刻以前的量测值不再作为估计现在状态的依据（N为有限记忆长度,根据k-N+1…k时刻的测量值,将状态变量X^k和误差协方差矩阵
P^k
更新为:
Xk和Pk:
图2 有限记忆滤波矫正
Xk=（P^-1k-P^-1k‖k-N-1（P^-1
kX^k-
P^-1kk-NX^kk-N
Pk=（P^-1k-P^-1kk-N
-1（12
167
第2期基于视觉的机动目标定位跟踪的滤波算法
4 基于数值微分模型的滤波
4.1 运动模型
利用数值微分技术
[13-14]
构造速度的三点微分模型:
vt+T=2T[3vt+T-4vt+vt-T]+2
3
v%1vt=
2T[vt+T-vt-T]-2
6v
%2
vt-T=
2T[-vt+T+4vt-3vt-T]+2
3v%
3（13
其中:
T为采样周期,v%1、v%2、v%3为截断误差,%1、%2、%3∈（t-T,t+T。
由此,构造出运动模型:
X
t=∀Xt+Wt
（14
其中:
Xt=
xt
xtxt-Txt-2,
∀=
01
0003/2T-2/T1/2T0
1/2T
0-1/2T0-1/2T2/T
-3/2T
离散化,得到:
Xk+1=∀Xk+Wk
（15
其中:
Xk=
xk
xk
xk-1
xk-,
∀=
123T/12-4T/35T/03-31
1
0010
该模型不需要目标运动行为的先验信息,是一种鲁棒性估计模型。
4.2 滤波发散抑制
文献[13-14]将微分模型中的截断误差并入系统噪声Wk中处理,这使得Wk的方差难以估计。
若!
2
w取得过大,滤波精度降低,反之,则容易引起滤波发散。
因此,文中引入衰减因子Sk。
当滤波器发散的时候,该衰,波器发散的效果。
发散抑制的方法如下:
若#Tk#k>tr（E（#k#T
k,认为滤波器发散。
将预测
误差协方差阵更新如下:
Pkk-1=Sk∀Pk-1∀′
+Qk-1（16
其中:
Sk根据当前的新息,由闭环回路自动选择:
Sk=tr{[#k#T
k-CQk-1CT
-Rk][C∀Pk-1∀T
CT
]
-1
}（17
5 数据融合
为了进一步提高滤波精度,将以上两个模型的滤波器同时工作,并根据贝叶斯推理计算出各模型的权值,系统的输出为两滤波器输出值的加权和。
假定模型之间的转移服从状态有限、时间离散的时齐马尔可夫链。
令Mi（k表示在[k-1,k],滤波周期
内i模型与系统运动模式匹配,Xi（kk表示k时刻第i个滤波器的输出,Zk
为到k时刻为止累积的测量值,{&i（k}为k时刻模型i的概率权值,∋i（k为第i个模型的似然函数。
融合步骤为:
（1并行滤波:
将k-1时刻的Xi（k-1k-1和Pi（k-1k-1输入第i个滤波器中,得到预测值Xi（kk-1和Pi（kk-1。
再根据测量值,得到各个滤波器的输出Xi（kk和Pi（kk。
（2计算各模型的似然函数:
∋i（k=p（zkMi（kZ
k-1
=
2（‖Pi（kk-1‖-1/2exp -2
#i（kP-1
i（kk-1#i（k（18
（3,更新概率权值:
&i（k=p（zkMi（kZk-1
p（Mi（kZk-1
∑j
p（zkMj（kZk-1p（Mj（kZk-1
=
∋i（k∑j
∋j（k（∑
k
Hjk&i（k-1∑kHik&k-1
i（19（4由全概率公式,得到交互输出值:
X（kk=&1（kX1（kk+&2（kX2（kk
（20
6 实验结果
整个滤波算法流程如图3所示,通过计算机仿真,来验证本算法的有效性。
先设定一组理想的运动目标相对于摄像机的位姿变化曲线,得到各个时刻特征点的像点坐标。
将各像点加上高斯白噪声,然后通过基于模型的单目视觉计算方法,得到带噪声的位姿参数的,168
仪 器 仪 表 学 报 第27卷
数的误差均方根曲线。
选取x方向位移和横滚角的滤波结果为代表,
如以下各图所示。
图3 滤波算法流程图
图4和图5表示设定的x和的理想运动曲线。
x在t=0~60s加速度为a=0.0461m/s2
;t=60s时,加速度发生变化,变为a=-0.0395m/s2
;当t>90s时,x
的加速度为零,横滚角则以加速度a=0.873mrad/s2
作匀加速运动;当t>50s时,加速度变为a=0.140mrad/s2
。
图4 x运动规律
图6是x误差均方根曲线。
LM为基于有限记忆的机动检测自适应滤波的结果,ND为基于数值微分模型的卡尔曼滤波结果,GPB为融合以上两种滤波算法的最终滤波结果。
虚线为未滤波以前的误差均方根。
从图中可以看出,分别采用文中给出的两种滤波器,都能实现很好的位置估计。
而通过数据融合将两滤波器
性能进一步提高。
图5 运动规律
图6 x误差均方根 图7 x速度跟踪曲线
图7为x的速度跟踪曲线（实线为真实值,虚线为滤波值,图8表示速度的误差均方根值。
可以看出,该滤波器对速度也实现了很好的估计。
图8 x速度误差均方根 图9 误差均方根
图9、图10和图11给出了对横滚角进行滤波的结果。
图9为的误差均方根曲线,图10为的速度跟踪曲线,图11为对应的速度误差均方根。
可见,算法对姿态角的定位跟踪也取得了良好的效果。
图10 速度跟踪曲线 图11 速度误差均方根
169
第2期基于视觉的机动目标定位跟踪的滤波算法
170仪 器 仪 表 学 报 第27卷 [4] BROIDATJ,fromaCHANDRASHEKHARimagesequence.S,IEEE 通过机器人跟踪实验,也验证了算法的有效性。
图12为1号机器人跟踪2号机器人的实现过程。
1号机器人上的摄像机实时拍摄2号机器人的图像,再由图像处理器检测出安装在2号机器人上的各特征点的像点。
通过视觉计算,得到两机器人的相对位姿测量值,并将其输入滤波器中。
滤波器的输出反馈给1号机器人的控制器,实现对2号机器人的实时跟踪控制。
同时,通过位姿预测值得到的特征点坐标的预测值反馈给图像处理器,简化了特征点的检测过程,加快了图像处理的速度。
CHELLAPPAR.Recursive3-motionestimationDmonocularTransactionsonAerospaceandElectronicSystems,1990,26（4:
639-656.[5]
升级会员 