Hi tho ton quc v in t - Truyn thng An ton thng tin, ATC/REV-2012

1

Robot di ng a cm bin v nh v robot bng
phng php tng hp cm bin vi b lc Kalman m rng
Multi-sensor mobile robot and the sensor fusion-based localization
with Extended Kalman Filter
Trn Thun Hong, Phng Mnh Dng, ng Anh Vit v Trn Quang Vinh
Trng i hc Cng ngh, i hc Quc gia H ni
e-Mail: thuanhoang@donga.edu.vn

Tm tt
Bo co trnh by vic xy dng khi cm nhn
ca mt robot di ng a cm bin v vic dng
phng php tng hp cm bin vi b lc Kalman
m rng nh v chnh xc cho robot. Cc cm bin
hin i nh cm bin lp m trc quay, cm bin ch
hng, cm bin o xa laser, cm bin nh ton
phng, cm bin siu m, b nh v ton cu GPS
cng c s dng trong robot. iu ny i hi phi
thit k mt h thng tin d liu c hiu qu. Trn c
s h thng , cc chng trnh iu khin c
pht trin da vo phng php tng hp cm bin
bng b lc Kalman m rng, cho php xc nh
chnh xc v tr v hng ca robot trong iu kin c
can nhiu ca mi trng.
1. Phn m u
Robot di ng, nh dng cc xe t hnh, cho
nhiu kh nng ng dng trong cng nghip v dch
v. Vic nghin cu pht trin cc robot kiu ny hin
ang l bi ton c nhiu nh nghin cu in t v
iu khin t ng trn th gii cng nh Vit nam
quan tm. Mt trong cc vn cn gii quyt l vic
nh v chnh xc cho robot trong qu trnh dn ng
n ti ch. Nhn chung, mt robot gm 3 thnh phn
c bn: b phn cm nhn, b phn iu khin v b
phn chp hnh [1]. Vi nhim v nh v, b phn
cm nhn gm cc cm bin in t cng cc mch
x l tn hiu phn cng v chng trnh phn mm
ng vai tr then cht cho php robot t c lng
chnh xc v tr v hng ca n ti mt thi im
chuyn ng da vo cc thng s c c t cc
cm bin. Tuy nhin, mi cm bin thng ch o
c mt hoc hai tham s mi trng vi chnh
xc gii hn, v vy mt cch t nhin thy rng nu
s dng cng nhiu cm bin th cng c kh nng
tng c chnh xc c lng v tr ca robot.
l l do phng php tng hp cc cm bin (sensor
fusion) hin nay ang c p dng ph bin trong
cc h thng robot hin i tng chnh xc ca
o lng [2,3]. C s thc hin phng php ny hu
ht c da trn suy lun xc sut. B lc Kalman
m rng (EKF) l gii php xc sut hiu qu c
tnh ng thi cc v tr ca robot da trn mt s
thng tin v cm bin ni v cm bin ngoi ca n.
T cc xut pht im nu trn, bo co ny trnh
by vic xy dng b phn cm nhn cho mt robot di
ng a cm bin (sensor) vi thit k cc mch in
t lin quan v mt mng truyn thng c hiu qu
gia cc sensor v b iu khin. Trn c s , mt
chng trnh phn mm tng hp cc cm bin da
trn l thuyt b lc Kalman m rng c pht
trin v c trnh by cho php ci thin ng k
chnh xc ca php nh v robot.
Ni dung ca bi bo c sp xp nh sau: Mc
2 trnh by v kt cu phn cng in t b phn cm
nhn a cm bin cng h thng truyn thng trong
robot; mc 3 trnh by tm tt cc c s v mch lc
Kalman ni ting, p dng vo m hnh ng hc ca
robot c lng chnh xc v tr; mc 4 l cc kt
qu thc nghim vi robot c xy dng v tho
lun.
2. B phn cm nhn a sensor v h
thng truyn thng trong robot
2.1. Thit k b phn cm bin cho robot
Hnh 1 l nh chp robot vi cc cm bin c
thit k lp t trn .

Hnh 1. Robot di ng v cc cm bin.

2
Robot c xy dng l loi c 2 bnh xe, mi
bnh c gn vi mt m-t in c iu khin
c lp. Nhiu loi cm bin c thit k lp t
trong b phn cm nhn ca robot di ng ny, di
y trnh by tm tt nguyn l hot ng v tnh nng
k thut ca chng.
- Cm bin lp m quang (optical encoder) c
gn trc tip vi trc quay m-t, c dng o
vn tc 2 bnh xe robot. B lp m quang nh hnh 2
cho thy v c bn l mt a quay lm ng-ngt
chm tia sng i qua cc khe. Cng b phn in t
chuyn i quang-in (optron), c cu ny pht ra s
xung in li ra tng ng vi mt vng quay ca trc
m-t. Nu ng knh ca bnh xe v t s truyn
lc (t s truyn bnh rng t trc m-t ra bnh xe
robot) bit th v tr gc v tc ca bnh xe c
th xc nh c. Vic xc nh v tr ca robot bng
b lp m quang ny l phng php ph bin trn th
gii c tn l Odometry [1].

Hnh 2. Cu trc b lp m quang.
- Cm bin ch hng (heading sensor) cho php
o gc hng ca robot so vi phng t trng ca
qu t. y l mt m-un cm bin t-a bn
CMPS03 hot ng da trn nguyn tc hiu ng Hall
vi phn gii gc l 0,1. Nh trn hnh 3 thy m-
un c 2 trc hng x v y. Cm bin cung cp s liu
o cng t trng theo cc trc hng ny, t
c th xc nh c gc lch ca m-un so vi
phng t trng qu t khi n c gn c nh trn
robot. Mt vi x l tch hp trong mch in t ca
m-un cung cp cc s o ny theo chun truyn
thng ni tip [4]. Ngi s dng phi lp trnh nhng
thu thp s liu.

Hnh 3. M-un ch hng.
- Cm bin o xa laser LRF (laser ranger finder)
model-LM211 ca hng SICK vi di o t 0,4 m n
80 m c lp t trong robot. y l mt thit b
cho php xc nh nhanh chng v chnh xc khong
cch v gc lch t robot ti vt cn. Cm bin hot
ng trn nguyn l o thi gian t thi im mt
xung sng laser hng ngoi c pht ra n thi
im thu c xung ny khi n c phn x t vt
cn trong mi trng. Mt h thng o thi gian cc
k chnh xc trong thit b cho cc s o c dng
tnh khong cch n vt khi bit vn tc ca nh
sng trong mi trng quanh robot. Bng vic gn
mt gng phn x quay trn ng i ca tia sng,
cc xung laser ny c lm chch hng khi gng
quay. Gng phn x quay vi tc 75 vng/giy
to nn mt di qut tia laser hnh r qut pha trc
cm bin vi mt trng nhn 2D. Trong mi ln qut
nh vy, cc xung laser c pht i chch hng vi
trc x mt gc vi phn gii 0,25; 0,5 hoc 1
nh m t trn hnh 5. Lc ny, b phn in t trong
LRF s pht ra mt tp s liu v khong cch n i
tng v gc qut tng ng li ra [5]. Ngi s
dng phi lp trnh thu thp chnh xc tp s liu
cho mt chu k quay gng (chu k qut tia laser), l
13,3 ms.

Hnh 4. Mt phng tia laser vi cc gc qut .
- Cm bin nh ton phng (omni-directional
camera) loi Hyper-Omni Vision SOIOS 55 c
s dng. N gm mt gng li phn x hnh nh
trong ton khng gian cng mt camera thng thng
c t mt phng nh ca gng nh hnh 5. nh
ton phng cha ng thng tin v v tr ca cc i
tng theo cc gc hng trong ton khng gian 360
[6].

Hnh 5. Cu to ca camera ton phng, nh ton
phng v nh ton cnh.
- Cm bin siu m: c dng rt ph bin cho
robot di ng v r v d s dng. Loi cm bin ny
cho cc thng tin v khong cch n vt cn vi
thi gian thu thp s liu nhanh. Tuy nhin vic s
dng chng mc phi cc nhc im do tnh phn
x xuyn m v cc s o khng n nh do chm tia
siu m b tn x hnh nn ch khng phi l mt tia
hp [7]. V vy, chng ti ch s dng cc cm bin
ny pht hin nhng vt cn gn, gi an ton cho
robot. 8 module cm bin siu m loi SRF05
c lp t cho php pht hin c vt t 3cm n
4m vi sai s l 2%; gc m chm siu m pht ra
khong 22,5
0
.
3
- Cm bin nh v ton cu GPS: ch yu p
dng cho nh v mi trng ngoi tri. Mt m-
un HOLUX GPS UB-93 c lp t v pht
trin phn mm thu thp d liu trong robot [8]. V
robot c thit k lp t c mng ghp ni Internet,
nn mt h thng m rng A-GPS cng c pht
trin, cho php tr gip cm bin GPS thu nhn d
liu nh v tt ngay c trong cc thi khong gp
iu kin thu thng tin v tinh khng c tt.
Ngoi h thng cc cm bin nu trn dng cho
hot ng t tr (autonomous), chng ti cn thit k
h thng iu khin khin Robot t xa qua mng
Internet 3G, cho php ngi iu khiu c th iu
khin robot bng Joystick.
2.2. Phn cng mng thng tin cm nhn trong
robot
S khi kt ni mng thng tin trong robot
c ch ra trn hnh 6.

















Hnh 6. Mng thng tin cm nhn trong robot.
Cc cm bin k trn c kt ni vi b phn
iu khin ca robot. Do c im v tc thng tin
khc nhau gia cc cm bin v thng tin y l loi
im-a im, nn mt thit k c bit cho mng
truyn tin trong robot c thc hin. Ngoi tr
cm bin nh ton phng c kt ni vi h thng
vi tnh trong robot qua knh thng tin tc cao
IEEE-1394; cc cm bin cn li c thit k thng
tin vi h iu khin qua mt chun cng nghip
im-a im theo phng php hi vng (polling)
vi ng truyn RS-485 c iu khin qua vi iu
khin MCUdsPIC 30F4011.
2.3. Pht trin phn mm thu thp thng tin t cc
cm bin
T hnh 6 cho thy c th chia phn mm thu thp
thng tin t cc cm bin thnh cc m-un c cc
c im ring sau.
- Thng tin hnh nh ton phng c thu thp
trc tip vo my vi tnh v x l qua chng trnh
c pht trin trn mi trng Visual C ++ vi cng
c l gi phn mm m OpenCV ca Intel.
- Cc thng tin cn li c trao i t 14 nt
mng vi nt iu khin t my tnh (3 nt cho mch
o b lp m quang v quay LRF, 1 nt cho cm bin
t-a bn ch hng, 1 nt cho truyn lung d liu t
LRF, 1 nt cho cm bin GPS, 8 nt cho 8 cm bin
siu m) c truyn qua ng truyn RS-485 s
dng giao thc Modbus/RTU [9].
- Chng trnh thu thp v x l d liu t my o
xa laser LRF l loi c bit c pht trin ring
trong ngn ng Visual C++. Sau khi nhn lnh yu cu
truyn s liu t my tnh, LRF s gi v lin tc cc
khung d liu o tng ng vi mi mt qut ngang
trong thi gian 13,3 ms. nh dng d liu khung gm
7 byte tiu , 2N byte s liu vi 2 byte cho mt im
o v cui cng l 2 byte m pht hin li CRC [5].

Chng trnh cn xc nh c im khi pht ca
lung k t d liu ra LRF bng vic xc nh chnh
xc 7 byte tiu . Cc byte ny l khc nhau vi cc
ch o khc nhau. Vi nh dng khung truyn 720
im d liu LRF th b m c chn t 1.700 n
2.000 byte. Kch thc ny cng khng nn ln qu v
lm chm li qu trnh truyn d liu.
3. Chng trnh tng hp cc cm bin
dng cho nh v robot di ng
Bt u vi m hnh ng hc ca robot di ng
c thit k ch to ca chng ti. Hnh 7 biu din
h to v k hiu robot, trong (X
G
Y
G
) l h ta
ton cc, (X
R
, Y
R
) l ta cc b gn vi tm
robot. R l bn knh ca bnh xe, v L l khong cch
gia cc bnh xe.

Hnh 7. Th v cc tham s ca robot.
M un
GPS
M un
t-a bn
Cc m-un
siu m
Trigger cho
LRF
My o xa laser
(LMS)
iu khin
PID
Encoder
M0
MCU
dsPIC
30F4011
Trigger switch
USB to
RS-485
PC
USB
1394
Card
bt hnh
iu khin
PID
iu khin
PID
M t bnh xe 2
M1
M2
Encoder
Encoder
Camera ton
phng
M-t quay LRF
M t bnh xe 1
ni mng
Internet

4
Trong khong thi gian ly mu s liu o t, cc
tc gc ca cc bnh xe bn tri
L
v bn phi
R

to ra cc lng di chuyn tng ng ca bnh tri
s
L
v bnh phi s
R
:
s
L
= t.R.
L
; s
R
= t.R.
R

T dn n s dch chuyn ca tm robot s v
gc hng ca robot :

2
L R R L
s s s s
s
L

A + A A A
A = A =
Trng thi ca robot ti thi im k+1 trong khung
ta ton cc c th c cp nht nh sau:
( )
( )
1
1
1
cos / 2
sin / 2

k k k
k k
k k k k k
k k k
s
x x
y y s



+
+
+
( A + A
( (
(
( (
= + A + A
(
( (
(
( (
A
(

(1)
Trong thc t, h thng (1) gp phi cc sai s h
thng (nh sai s kch thc cc chi tit c kh,
lch trc, gii hn phn gii ca encoder, ...) v
cc sai s khng h thng (nh trt ca cc bnh
xe vi mt sn, ...) Do c tnh tch ly ca cc sai s
ny nn chnh xc ca vic c lng v tr s cng
b km i khi robot di chuyn [10]. C nhiu cng
trnh c gng tng tin cy ca vic nh v ny
trong s dng b lc Kalman. Chng ti cng
p dng phng php tng hp cm bin bng thut
ton lc Kalman nh v cho robot c ch to v
nhn c kt qu ci thin c cht lng ca
cc php o ny ng k.
C th tm tt phng php ny nh sau. Nu
[ ]
T
X x y = l vect trng thi ca robot ang ta
x, y v gc hng , trn mt phng m n di
chuyn, trng thi ny c th c quan st bi mt
s php o tuyt i, z. Cc php o ny c miu t
bi mt hm phi tuyn, h, ca h to robot v mt
qu trnh nhiu Gaussian c lp, v. Tng qut, (2) l
mt hm phi tuyn, f, vi mt vector u vo u, c
phng trnh trng thi v phng trnh li ra nh
sau:
1
( , )
k k k k
x f x u w
+
= + (2)
( )
k k k
z h x v = + (3)
Trong cc bin ngu nhin w
k
v v
k
biu din cho
qu trnh v nhiu o tng ng. Chng c gi nh
l c lp vi nhau, n trng, v vi phn b xc sut
chun:
~ (0, ) ~ (0, ) ( ) 0
T
k k k k i j
w N Q v N R E wv =

Da trn cc s liu o z, c th tm c mt h
s Kalman K trong mi chu k ly mu tn hiu o
thuc mt vng lp quy gi l b lc Kalman, sao
cho gi tr c lng trng thi ca h gn vi gi tr
thc nht. B lc Kalman m rng, p dng cho cc
h phi tuyn, c thc hin qua cc bc nh sau
[11]:
1. Bc d bo vi cc phng trnh cp nht:

-
-1 -1
( , )
k k k
x f x u = (4)
-
-1 -1
T T
k k k k k k k
P A P A W Q W = +
(5)
Trong
-

n
k
x e9 l c lng trng thi tin nghim
ti thi im k cho bit gi tr trc qu trnh ti thi
im k.

-

k
P l ma trn hip phng sai ca sai s d bo
trng thi.
A
k
l Jacobi ca o hm ring ca f theo x.
W
k
l Jacobi ca o hm ring ca f theo w.
Q
k-1
l ma trn hip phng sai nhiu u vo ph
thuc vo lch chun ca nhiu ca tc gc ca
cc bnh xe. Chng c m hnh ha nh l t l
thun vi tc gc
R-k
, v
L-k
ca cc bnh xe ti
thi im k. iu ny dn n phng sai
bng
2
R
v
2
L
, trong l mt hng s xc nh
bi cc thc nghim.
Ma trn hip phng sai Q c xc nh:
2
,
2
,
. 0
0 .
R k
k
L k
Q


(
= (
(

(6)
2. Bc hiu chnh vi cc phng trnh cp nht
php o:
- - -1
( )
T T T
k k k k k k k k k
K P H H P H V R V = +
(7)

- -
( - ( , 0))
k k k k k
x x K z h x = + (8)

-
( - )
k k k k
P I K H P = (9)
Trong
n
k
x e9 l c lng trng thi hu nghim
ti thi im k, gi tr thu c sau khi o lng z
k
.
K
k
l h s li Kalman.
V
k
l Jacobi o hm ring ca h theo v.
H
k
l Jacobi o hm ring ca h theo x.
R
k
l ma trn hip phng sai ca nhiu c c
lng t nhiu trong php o ca b m ha bnh xe
v cc cm bin khc. Cc php o ca cc b cm
bin ny c thu thp vo trong vc t z
k
nh sau:
Bng phng trnh h thng (2), cc tham s trng
thi (x
m
.
odometry
, y
m..odometry
v
m.odometry
) nhn gin tip
bng cc ch s c t b m ha bnh xe.
Gc hng robot c th cng c o trc tip
bi php o tuyt i vi cm bin t-a bn

m.compass
.
My o xa laser LRF cho php o cc im c
trng ca mi trng quanh robot. Trong trng hp
ny chng ti chn l mt ng thng c pht
hin bi LRF (v d nh ng ct ngang mt bc
5
tng trong phng). Bng php bin i Hough, hai
thng s ca ng thng ny c th nhn c,
l: khong cch r t ng thng n gc ta v
gc ga ng thng v mt trc ta nh hnh 8
ch ra.

Hnh 8. My o LRF t trn robot o 2 thng s ca
mt ng thng trong mi trng.
Camera ton phng cng d dng cho c cc
thng s o c im mi trng vi cc ct mc. Ct
mc l cc vt t ti cc ta bit trc (x
L
, y
L
)
trong mi trng di chuyn ca robot. Ct c mu sc
hay hnh dng nh trc chng trnh th gic
my tnh c th nhn dng c. T gi tr o cc
gc
i
gia hng robot R vi cc ct mc M
i
c th
nhn c (hnh 9).

Hnh 9. Camera ton phng o gc ti cc ct mc.
Kt hp cc php o gin tip t Odometry v cc
php o trc tip t cc cm bin nh ni trn ta c
ma trn z
k
trong phng trnh sa gi tr c lng
(8) c dng nh sau:

m.odometry
m.odometry
m.odometry
m.compass
m.LRF
m.LRF
m. camera
k
x
y
z
r

| |
|
|
|
|
|
=
|
|
|
|
|
\ .
(10)
Ma trn hip phng sai R
k
ca nhiu o c cu
trc ng cho. Nhiu ca php o tc bnh xe
c th c xc nh bi thc nghim. S chnh xc
ca cc cm bin t-a bn, ca cc php o LRF v
cameara nhn c t c im k thut ca nh sn
xut. Cc s liu ny c in vo R
k
cho bc iu
chnh EKF.
var( ) 0 0 0 0 0
0 var( ) 0 0 0 0
0 0 var( ) 0 0



L
R
compass
k
R

=
0
0 0 0 var( ) 0 0
0 0 0 0 var( ) 0
0 0 0 0




LRF
LRF
r

)
0 var(
camera

(
(
(
(
(
(
(
(
(


4. Kt qu thc nghim v tho lun
Robot c thit k, ch to v cho chy th
nghim trong mt phng c mt sn g phng. Cm
bin t-a bn c chnh xc 0,1. My o xa LRF
c chnh xc o khong cch l 30 mm v gc lch
0,25. Thi gian ly mu tn hiu At ca LRF l
0,1 s. H s t l ca ma trn hip phng sai li
vo Q
k
c xc nh bng thc nghim l 0,01.
Robot c lp trnh di chuyn trong phng vi
cc trng hp khc nhau:
- Ch s dng nh v bng phng php
Odometry.
- S dng nh v bng phng php tng hp
cm bin vi b lc EFK. Vi phng php ny
cng th vi cc cu hnh khc nhau: trn ln
ch sensor t-a bn, trn ln ch RLF, trn ln
c hai cm bin vi phng php Odometry.
Kt qu cc qu o di chuyn nhn c t cc s
liu so vi qu o thc c ch ra trn hnh 10 cho
thy s ci thin ng k khi p dng phng php
tng hp cm bin v kt qu cng cho thy cng c
nhiu cm bin tham gia vo qu trnh ny th gi tr
c lng v tr robot cng gn vi gi tr thc
[12].

Hnh 10. Qu o robot c lng c vi
cc cu hnh b lc EFK khc nhau.
Hnh 11 l kt qu chi tit v lch theo phng
chuyn ng Y ca robot nhn c t cc php o so
vi gi tr thc vi cc trng hp khc nhau. Hnh
12 l kt qu khc chng minh hiu qu ca b lc
Kalman khi cho robot di chuyn theo mt qu o
hnh vung trong phng vi cc trng hp c v
khng c b lc Kalman. ng v tr robot c c
lng vi EKF cho thy n rt gn vi ng i thc
ca robot.

6

Hnh 11. lch theo phng Y gia cc v tr c lng
v v tr thc vi cc cu hnh EKF khc nhau.

Hnh 13. Hiu qu ca b lc Kalman.
5. Kt lun
Cc kt qu ci thin gi tr nh v ca robot di
ng bng phng php tng hp cm bin vi b lc
Kalman cho thy vic xy dng robot a sensor l c
ngha thit thc gip cho qu trnh dn ng robot
trong hot ng t tr ca n.
Nhng nghin cu nh lng tip theo s c p
dng trn h thng robot c thit k lp t ha hn
s c thm cc kt qu hu ch ng gp cho lnh vc
nghin cu v robot di ng, mt loi sn phm tim
nng trong cc lnh vc cng nghip, dch v, an ninh
quc phng.
ti nhn c s h tr t phng th nghim
cc h tch hp thng minh SIS thuc i hc Cng
ngh, HQG HN.

Ti liu tham kho
[1] J. Borenstain, H.R. Everette, and L. Feng, "Where
I am ? Sensor and Methods for Mobile Robot
Positioning", University of Michigan, 1996.
[2] H. F. Dunant-Whyte , Sensor models and multi-
sensor integration,Z nt.J.Robot. Res., vol. 7, no.
6, pp. 97-113, 1988.
[3] Y. C. Tang and C. S. G. Lee, A geometric
feature relation graph formulation for consistent
sensor fusion , in Proc. IEEE 1990 Int. ConSyst.,
Man, Cybern., Los Angeles, CA, 1990, pp. 188-
193.
[4] http://www.robotelectronics.co.uk/htm/cmps3tec
h.htm
[5] Sick AG., 2006-08-01 Telegrams for Operating/
Configuring the LMS 2xx (Firmware Version
V2.30/X1.27), www.sick.com , Germany.
[6] N. Winters et al, Omni-directional vision for
robot navigation, Omnidirectional Vision, 2000.
Proceedings. IEEE Workshop on, Hilton Head
Island , USA, Jun 2000.
[7] J. L. Crowly, World modeling and position
estimation for a mobile robot using ultrasonic
ranging, in Proc. IEEE Int. Conf. Robot.,
Automat., 1989, pp. 674-680.
[8] http://www.holux.com
[9] http://www.modbus.org/. MODBUS over serial
line specification and implementation guide
V1.02. Dec 20, 2006.
[10] J. Borenstein and L. Feng, A Method for
Measuring, Comparing, and Correcting Dead-
reckoning Errors in Mobile Robots, Technical
Report UM-MEAM-94-22, University of
Michigan, 1994.
[11] Greg Welch and Gary Bishop, "An Introduction
to the Kalman Filter", Department of Computer
Science, University of North Carolina at Chapel
Hill, Chapel Hill, NC 27599-3175, 2006.
[12] Manh Duong PHUNG, Thuan Hoang TRAN,
Thanh Van NGUYEN, Anh Viet DANG, and
Quang Vinh TRAN, "A Novel Platform for
Internet-based Mobile Robot System", 7
th
IEEE
Conference on Industrial Electronics &
Applications ICIEA 2012, Singapore.

Abstract
The report presents the design of a multi-sensor
mobile robot and the robots localization based on the
sensor fusion method with Extended Kalman Filter
(EKF). Modern sensors such as optical encoders,
heading sensor, laser range finder, omni-directional
camera, ultrasonic sensor, GPS sensor used
concurrently so that an efficient data communication
system has been designed and installed in robot.
Based on this system, the sensor fusion based
controlling program with Extended Kalman Filter was
developed which permits to estimate accurately the
robots position and orientation in condition of
environments noise.