Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
Mt phng php s dng b lc Kalman kt hp vi thut ton bm nh
Camshift nhm nng cao cht lng bm trong cc h thng robot t ng
tm kim v bm bt mc tiu

A Method using Kalman Filter combining with Image Tracking Camshift
Algorithm to bring higher tracking Quality in automatically searching and
tracking target Robot System

Ng Mnh Tin
Vin Vt L, Vin KH&CN Vit Nam
Email: nmtien@iop.vast.ac.vn
Phan Xun Minh
i Hc Bch Khoa H Ni
Email : minhxp-ac@mail.hut.edu.vn
H Th Kim Duyn
i Hc Cng Nghip H Ni
Email : hkduyen@haui.edu.vn

Tm tt: Nhn bit chnh xc mc tiu l mt vn
rt quan trng trong bi ton iu khin Robot
bm i tng chuyn ng. Bi bo xut mt
phng php mi nhm m bo pht hin chnh
xc mc tiu trong mi trng c nhiu tc ng.
Phng php xut l s kt hp gia b lc
Kalman v thut ton bm nh Camshift. Cc kt
qu thc nghim cho thy cht lng bm ca
phng php xut tt hn khi s dng n thun
thut ton bm nh Camshifft.
Abstract: To find exactly object is very important
in the control problem of automatic image tracking
Robot for moving object. The paper presents a new
method to guarantee searching the object exactly in
random invironment. the proposed method is the
compiling beetween Kalman filter and image
tracking Camshift algorithm. The experimental
results show the tracking quality of the system is
better than using only the image tracking Camshift
algorithm.
Keywords: Camshift, OpenCV, Kalman
Tracking, ATmega128, Computer Vision.
K hiu
K hiu n v ngha
x,y pixel ta im nh
ij
M
Moment bc i,j
k
P

Tm ca khung bm
R
u ,
P
u ,
Y
u rad
gc quay Roll, Pitch, Yaw
Ch vit tt
FOV Field of View
LOS Line of Sight
W Window



1. M u
Mc tiu
Camera ng knh
v quay
My tnh x l
v iu khin

H. 1 Cu to c bn ca mt h bm nh t ng
H thng tng quan [3] bao gm c 3 b phn chnh
H. 2: (1) b cm bin hnh nh (camera), (2) b chp
hnh l cc trc khp quay (gimbal) v bnh xe c
gn ng c in 1 chiu, v (3) my tnh PC v vi
iu khin (b vi x l). Mt vng iu khin phn
hi, c gi l vng bm, lin tc hiu chnh b
chp hnh gi trng tm ca mc tiu vo tm
ca trng nhn vit tt l FOV (Field Of View) ca
b cm bin. B vi x l s khp kn vng ny bng
cch tnh ton lch iu khin c cu chp
hnh. Cc thnh phn hot ng theo th t: (1) b
vi x l nh v tn hiu mc tiu trong cc dng
hnh nh t b cm bin, (2) b vi x l c lng
trng thi mc tiu v to ra cc lnh iu khin c
cu chp hnh trn c s cc thng tin trng thi, (3)
lnh iu khin c p dng vo LOS cm bin, (4)
b cm bin to ra mt vng video mi, v (5) qu
trnh c lp li.










H. 2 S tng quan ca h thng
Cm bin
pan/tilt
Tng bm nh
Xc nh v tr
mc tiu
Tnh ton LOS
Tng iu khin
To lut
iu khin
V tr
mc tiu
D on v tr mc tiu
Trng thi mc tiu
Pixels
Cc o lng khc ng c
xe
513
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011


VCCA-2011
Trong bi bo ny s i su vo phn x l nh bm
bt i tng cn theo di [1,2]. Dng cc thut ton
bm bt nh c lng v tr mc tiu, v tr mc
tiu thng c tnh trn mt nh con nh c gi
l ca s mc tiu. H. 3 m t ca s mc tiu
thng bao quanh mc tiu. Khi xc nh ca s
mc tiu ta s ch x l mt phn ca trng nhn
nm bn trong ca s ny, khi cc nh hng ca
nhiu lon v n trong qu trnh bm l b trit tiu
ln, lm cho h thng bm n nh hn. Tn dng
nh trong ca s c lng v tr mc tiu cng
trnh c hn ch v s lng cn x l. Nh mt
quy tc, ca s (thng l hnh ch nht hoc hnh
vung) nn c kch thc nh nht cn thit chc
chn hon thnh mc tiu.
Ca s mc tiu

H. 3 Ca s mc tiu
Dy detector v
s liu in t
Hiu chnh pixel
v nh dng nh
cm bin c
khp cc-ng
B iu khin
khp cc-ng
servo
Xc nh ca
mc tiu
c lng v tr
mc tiu
Tm breaklock
c lng trng
thi mc tiu
D on v
tr mc tiu
To lut iu
khin vng bm
Cm bin nh c khp cc-ng
Cc
pixel
Lnh
khp
cc-
ng

H. 4 Kin trc tng th ca mt vng bm
Khi dng cc thut ton bm nh v xc nh
c v tr mc tiu, gi tr v tr mc tiu s c
gi sang phn iu khin xoay nhm mc ch
duy tr LOS hay l duy tr v tr ca s mc tiu trn
tm mn hnh chnh.
Cc thut ton bm i tng trong nh (Target
Tracking) rt a dng, phi k n mt s thut ton
nh: Thut ton KLT, Correlation-base template
matching, thut ton Meanshift, thut ton Camshift.
Trong bi bo ny chng ti dng thut ton
Camshift bm i tng da trn mu sc nh.
2. Thut ton bm nh Camshift
Thut ton Camshift [4,7,8] c pht trin trn c
s thut ton Meanshift. Camshift vit tt ca
Continuously Adaptive Meanshift. N bao gm
thut ton c s Meanshift vi thch ng cc bc
thay i kch c ca vng. Cc bc thc thi thut
ton:
Cc bc tin hnh thut ton Camshift:
1. Kch chn kch thc khung bm
2. Chn v tr khi to ca khung bm (ca s bm)
3. Tnh gi tr trung bnh bn trong khung bm
4. t tm khung bm v tr trung bnh tnh
trong bc 3
5. Lp li bc 3 v bc 4 cho n khi hi t (hoc
cho n khi dch chuyn v tr tm nh hn mt
ngng t trc)
Chng minh hi t:
Gi s trong khng gian euclid c mt phn b f ,
ta s chng minh tng bc:
1 Chn khung bm W c kch thc l s
2. Tm ca khung bm im P
k

3. Tnh gi tr trung bnh bn trong khung bm

1

( ) .
k
j W
p W p j
W
e
=

(1)
s thay i l gradien ca ( )
k
f p

( )

( )
( )
k
k k
k
f p
p W p
f p
'
~ (2)
4. Tm ca khung bm:

( )
k
p W (3)
5. Lp li bc 3 v bc 4 cho n khi hi t
hoc ( ) 0 f p do thut ton Camshift l hi t
i vi nh 2D ri rc th v tr tm ca khung bm
c tnh nh sau:
tm Moment bc khng

00
( , )
x y
M I x y =

(4)
sau tnh moment bc 1 cho x v y:

00 01
( , ); ( , )
x y x y
M xI x y M yI x y = =

(5)
v v tr tm ca khung bm l:

10 01
00 00
;
c c
M M
x y
M M
= = (6)
y ( , ) I x y l gi tr im nh (xc sut phn b)
ti v tr (x,y) v x, y nm trong khung bm.
Lu thut ton Camshift:























H. 5 Lu thut ton Camshift
nh HSV
Tnh Histogram
ca vng tnh ton
Xc xut phn b mu
Tnh trng tm
i tng
t tm khung
bm vo tm i
tng v tnh din
tch li
Hi t
kch chn khi to
v tr v kch thc
khung bm
thit lp vng tnh
ti tm khung bm
nhng kch thc
ln hn khung
bm
S dng (X,Y) l tm
ca khung bm, din
tch khung l kch thc
ca khung bm
Kt qu X, Y, Z v
Roll
514
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011


VCCA-2011
u tin l mt Histogram c to ra, Histogram
ny cha cc thuc tinh lin quan n mu sc v
tip theo tm v kch c ca mc tiu c tinh ton
theo di mc tiu khi hnh dng v kch c ca n
thay i. Tnh xc sut phn b mc tiu cn c v
Histogram nhn c. Dch chuyn n v tr mi
vi mi khung hnh va nhn c t video.
Camshift s dch chuyn n v tr m n c lng
trn i tng n m n tp trung nhiu im sng
nht trong bc nh xc sut. n tm v tr mi bt
u t v tr trc v tnh ton gi tr trng tm
va tm c.
Trong bi bo ny chng ti lp trnh x l nh bm
bt mc tiu t ng trn nn Visual C 6.0 v c s
dng th vin OpenCV ca Intel c h tr
cvCamshift() thc hin thut ton bm Camshift.
3. Kt hp b lc Kalman vi thut
ton bm nh Camshift
Trong thut ton bm i tng trong nh Camshift
c gii thiu phn trn c th thy i tng
c bm tt khi nn nh khng nh hng n i
tng, nh ln mu hay che khut i tng. Khi
i tng bm b che khut hay ln mu vi nn
nh, s gy ra hin tng mt bm. khc phc
nhc im ny v nng cao cht lng bm, b lc
Kalman c s dng [3,6,7].
Sau khi tin hnh thut ton Camshift ta s c ng
trng thi h thng x
k,
, y
k
, v tr x v y ca i tng
ti thi im k. S nh gi c cp pha trn ta
c th s dng b lc Kalman to ra phng php
gii hn v tr ca i tng hiu qu hn, iu l
ni thay v tm kim i tng trong ton b mt
phng nh ta s nh ngha mt ca s tm kim (hay
ca s mc tiu c d cp trong phn 1) tp trung
vo gi tr d on xk

ca b lc.











H. 6 S kt hp b lc Kalman v Camshift
Cc bc s dng b lc Kalman cho vic bm i
tng l:
c Khi to k Trong bc ny n s tm
kim i tng trong ton b bc nh do chng ta
khng bit v tr trc ca bc nh. Theo cch ny ta
nhn c x
0
. Tng t ta c th nh gi mt lng
dung sai ln ban u (P
0
).
c on k0). Trong giai on ny s
dng b lc Kalman ta d on v tr tng i ca
i tng, nh vy v tr xk

c coi nh trung tm
tm kim tm kim i tng.
c iu chnh k Trong phn ny ta t i
tng (l mt chui im c d on trong trng
thi tin nghim xk

) v ta s dng v tr thc ca n
(o c) thc hin vic hiu chnh trng thi bng
cch s dng b lc kalman tm kim xk

Bc 2 v 3 c thc hin trong khi vic bm i
tng vn ang tin hnh.
minh ha nhng kt qu ca vic dng b lc
kalman trong vic bm i tng, chng ti la
chn cho bm theo mt qu bng v xem xt cc
trng hp sau:
a) Trong bi th nghim ny ta thc hin bm theo
mt qu bng, qu bng s di chuyn tng ng
vi mt ng thng tuyn tnh, c miu t bi h
thng cc phng trnh sau:

1 1,
x x w
k k k k k
F
+ +
= + (7)

1
1
1
1
1010
0101
0010
0001
k k
k k
k
k k
k k
x x
y y
w
x x
y y
+
+
+
+
( ( (
( ( (
( ( (
= +
( ( ( A A
( ( (
A A
( (


(8)

y x
k k k k
H v = + (9)

1000
0100
k
k k
k
k k
k
x
xm y
v
ym x
y
(
(
( (
(
= +
( (
(
A

(
A
(

(10)



























H. 7 S tm tt ca b lc Kalman
Trong H. 8 l kt qu m phng cho thy v tr c
lng bm rt st vi v tr thc ca i tng bm.
Camsifht Kalman
D liu nh
m hnh
chuyn ng
Vector o lng v tn hiu
khng chc chn
c lng trng
thi v phng sai
c lng
vi hiu
chnh o
c lng
trng thi
Khi to
0
0
0 0 0 0 0
[x ]
[(x [x ])(x [x ] )]
T
x E
P E E E
=
=

, 1 1

k k k k
x F x

=
, 1 1 , 1
T
k k k k k k k
P F P F Q

= +
T
k k
k
T
k k k k
P H
G
H P H R

=
+

x x (y x )
k k k k k k
G H

= +
( )
k k k k
P I G H P

=
515
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011


VCCA-2011

H. 8 Kt qu d on v tr vi b lc Kalman

b) Mt li th ca b lc Kalman cho vic bm bt
i tng l n c th c lng c v tr i
tng ngay c khi b che khut nh. Cch thc
hin vic ny l xem xt hai giai on ca b lc: d
on v hiu chnh. C ngha l nu vic gii hn v
tr ca i tng l khng trong mt chui ca trng
thi d on ca b lc (trong thi im k), ta c th
coi rng i tng b che khut bi mt vi i
tng khc, do ta s khng th dng hiu chnh
o v s ch ly gi tr lc tin nghim v tr ca i
tng. H. 9 ch ra hot ng ca b lc khi i
tng b che khut. H thng ny c m hnh vi
nhng biu thc tng t c dng trong trng
hp a).

H. 9 lc Kalman khi b che khut
c) Hu ht cc qu o chuyn ng ca i tng
l thay i phc tp (thay i c vn tc v gia tc)
khng th m hnh ha bi cc h thng tuyn tnh,
khi chng ta phi dng cc phng trnh phi
tuyn, do trong nhng trng hp ny ta s dng
b lc kalman m rng EKF (Extended Kalman
Filter). H. 11 cho thy r cht lng bm s dng b
lc Kalman m rng cho vic bm i tng vi
qu o phc tp tt hn hn ca b lc Kalman
thng thng. Vi b lc Kalman m rng c m
hnh ha s dng cc phng trnh khng gii hn
chuyn ng ca Brownian:

1
x ( , x ) w
k k k
f k
+
= + (11)

1
1
1
1
1
exp( ( 1.5 ))
4
1
exp( ( 1.5 ))
4
w
1
exp( )
4
1
exp( )
4
k k
k
k k
k
k
k
k
k
k
x x
x
y y
y
x
x
y
x
+
+
+
+
(
+ A
(
(
(
(
+ A (
(
(
= +
(
(
A
(

(
(
A
(

(
(

(12)
( , )
k k k
y h k x v (13)

1000
0100
k
k k
k
k k
k
x
xm y
v
ym x
y
(14)





























H. 10 S tm tt ca b lc Kalman m rng

H. 11 ic theo di cc chun ng phc tp s dng
b lc Kalman m rng
0
0
0 0 0 0 0
x [x ]
[(x [x ])(x [x ] )]
T
E
P E E E
=
=

1
1,
x f(k,x ]
f(k,x) (k,x)
x
x x
k k
k k
x t x t
h
F H
x

+
= =
=
c c
= =
c c

, 1 1 , 1
T
k k k k k k k
P F P F Q

= +
T
k k
k
T
k k k k
P H
G
H P H R

=
+

x x y ( , x )
k k k k k
G h k

= +
( )
k k k k
P I G H P

=
c
lng
trng thi
c lng vi
hiu chnh o
Khi
to
Tuyn
tnh ha
516
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011


VCCA-2011
Trong khi b lc Kalman thng thng, m hnh h
thng c s dng cc phng trnh ca trng
hp a).
4. Chy th nghim h thng
H thng chy th nghim bao gm mt xoay
c gn camera, mt hp iu khin c thit k da
trn vi iu khin AVR v mt my tnh trung tm
cha phn mm c lp trnh cc thut ton bm
nh kt hp vi b lc Kalman c vit trn nn
Visual C kt hp s dng th vin OpenCV ca
Intel.
p ng yu cu v kh nng iu khin c
cu chp hnh v va c kh nng x l, chng
nhiu tt ti chn v s dng vi iu khin AVR,
ATmega128 v n c y tnh nng v c kh
nng chng nhiu tt. s dng iu khin cc c
cu chp hnh v kim sot tc v gc quay
chng ti s dng mt s linh kin chuyn dng
iu khin cu ng c l dng MC ca Freescale
v DVR ca Texas Intrument.







H. 12 nh chp h thng th nghim

H. 13 Cu trc phn cng ca h thng

H. 14 Lu thut ton
Cu trc ca h bm gm hai phn chnh:
- Phn 1: x l nh da vo chui hnh nh thu c
t camera nhn dng v tnh ton v tr ca i
tng mc tiu.
- Phn 2: l phn iu khin, da vo thng s v tr
i tng nhn c trong mt phng nh, tnh ton
tn hiu iu khin robot sao cho i tng di
chuyn c duy tr mt v tr nht nh trn mt
phng nh v mt khong cch nht nh.
Cc kt qu chy th nghim khi dng n thun
Camshift (H. 15) v khi kt hp thm b lc Kalman
(H. 16). C th thy r khi kt hp thm b lc
Kalman th khc phc c hin tng mt bm
mc tiu khi mc tiu b ln vo nn nh.
5. Kt lun
Bi bo trnh by v mt phng php kt hp b
lc Kalman vi thut ton bm nh Camshift: l
thuyt, phn cc trng hp m phng v chy th
nghim so snh kt qu trc khi v sau khi thm b
lc Kalman. Cc kt qu cho thy khi kt hp thm
b lc Kalman cht lng bm ca h thng tt hn,
khi i tng b che kht h vn duy tr bm tt.
Mt hn ch ca vic kt hp thm b lc kalman
vo thut ton bm l khi lng tnh ton ln
hn do thi gian x l lu hn, dn n tc
bm chm: khi khng c b lc Kalman tc bm
tm 1520 frame/s, khi c b lc Kalman tc l
710 frame/s. Tuy nhin hn ch ny c th khc
phc bng cch tng tc x ca CPU my tnh
hoc nhng thut ton trn cc DSP, hay FPGA
chuyn dng.
Mt hn ch khc l khi i tng b che khut vi
thi gian lu (trn 3s), hoc khi c i tng
khc chuyn ng cng chiu che khut dn cng
dn n hin tng mt bm. iu ny cng m ra
hng pht trin tip theo l kt hp thm thut ton
Correlation-base template matching s c th khc
phc c hn ch ny.
Ti liu tham kho
[1]. Gio trnh Nhp mn x l nh s, H Bch
Khoa H Ni.
[2]. Nguyn Vn Ng, X l nh dch t qun
Two Dimensional Signal and Image Processing
ca tc gi Jae SLim, 2002
[3]. Nathan Funk,: A Study of the Kalman Filter
applied to Visual Tracking, Project for CMPUT
652, 7- 2003
[4]. Gary Bradski and Adrian Kaehler: Learning
OpenCV Computer Vision with the OpenCV
Library; OReilly; 2008
[5]. David A. Forsyth & Jean Ponce, Computer
vision a moderm approach, Prentic Hall PTR,
2003
[6]. Erik Cuevas, Daniel Zaldivar, Raul Rojas:
Kalman filter for vision tracking, Technical
Report B 05-12, 2005
[7]. JYoung Min Kim: Object Tracking in a Video
Sequence, CS 229 Final Project Report, 2006
[8]. Ahmad Ali, Sikander Mirza: Object Tracking
using Correlation, Kalman Filter and Fast
Means Shift Algorithms, IEEE-ICET, 2008
Start
Chp nh

Ly mu
Histogram
Lc nhiu
Camshift+Kalman
Tnh ta
tm, hng
Giao tip
RS232,RF
iu kin
dng
Khi to
Dng
Camera X l nh
frame RF
RS232
ATmega128
RF
RS232
Driver K
C
Cng tc
hnh trnh
Cm bin
ng c
Mch VK My tnh v lu tr
517
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011


VCCA-2011




H. 15 Chp cc frame ch thc nghim vi thut ton bm Camshift khng c b lc Kalman




H. 16 Chp cc frame ch thc nghim vi thut ton bm Camshift c b lc Kalman

Ng Mnh Tin: Hc i hc
Bch Khoa H Ni, chuyn ngnh
iu khin t ng t nm 1996-
2001. Bo v Thc s nm 2004
v hin nay ang theo hc Tin s
ti i Hc Bch Khoa H Ni.
Hin ti cng tc ti phng Quang
in t - Vin Vt L-Vin Khoa
Hc v Cng Ngh Vit Nam.
Hng nghin cu chnh: iu khin qu trnh, iu
khin thng minh v thch nghi, h M v mng
Neuron, iu khin Robot, Robot t hnh,h thng
quang in t nhn m, x l nh.

Phan Xun Minh: Nhn bng
Thc s k thut (1976) v Tin s
k thut (1989) - chuyn nghnh:
iu khin hc ti trng Ilmenau
University of Technology
Ilmenau, c.
Hin l Ph Gio s, cng tc v
ging dy ti B mn iu khin
t ng Vin in - Trng i hc Bch Khoa
H Ni.
Hng nghin cu chnh: iu khin ti u v bn
vng, iu khin thch nghi, h m v mng Nron,
iu khin qu trnh.

H Th Kim Duyn: Hc i
Hc Bch khoa H Ni, chuyn
ngnh iu khin t ng t nm
1996-2001, Bo v Thc s ti
Hc Vin k Thut qun s 2007.
Hin cng tc v ging dy ti
Khoa in t - trng i Hc Cng Nghip H
Ni.
Chuyn mn nghin cu chnh: iu khin qu
trnh, Cc b iu khin kh trnh PLC v mng
truyn thng cng nghip, B iu khin thng minh
v thch nghi, M v mng Neuron, X l nh.
518