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

VCCA-2011
Thit k qu o v iu khin xe t hnh vn chuyn trong kho
Trajectory planning and control of mobile robot for transportation
in warehouse
Nguyn Vn Tnh, Phm Thng Ct, Phm Minh Tun, Bi Th Thanh Quyn
Vin Cng ngh thng tin, Vin Khoa hc v Cng ngh Vit Nam
Email: nvtinh@ioit.ac.vn; ptcat@ioit.ac.vn ; pmtuan@ioit.ac.vn; quyenbt@pusan.ac.kr

Tm tt:
Bo co ny xut gii php thit k qu o v
iu khin ti u xe t hnh cho bi ton vn chuyn
k hng (pallets) trong kho. Gii php bao gm thut
ton thit k qu o
3
spline h vi a thc bc 7 kt
hp vi thut iu khin ti u theo chun ton
phng xc nh tc gc cn thit cho r bt
bm qu o. Ngoi ra vic nh v xe t hnh t cc
tn hiu o can nhiu c xc nh bng phng
php lc Kalman m rng. Cc th nghim trn r bt
t hnh Pioneer P3-DX trong phng th nghim cho
kt qu kh quan.
Abstract:
This paper presents methods for trajectory planning
and optimal control of mobile robot for the problem
of pallets pick and place in warehouse. The
3
spline h with 7
th
order polinom is used for
trajectory planning. Based on linearized error
dynamic equations and by using optimal control law a
sequence of angular velocities is generated such that
mobile robot always tracks the desired trajectory.
Furthermore, we propose an approach of mobile robot
localization using Extended Kalman filter. The
implementation of the proposed methods on robot
Pioneer P3-DX gives acceptable results.
1. Gii thiu
R bt di ng l mt h r bt c kh nng thc hin
cc nhim v nhiu v tr khc nhau vi kh nng
dch chuyn bng bnh xe, xch hay bng chn ph
thuc vo a hnh. Kh nng di ng lm r bt c
nhiu ng dng v i hi phi gii quyt nhiu vn
mi. Nhng vn nghin cu ang c nhiu
tc gi trn th gii quan tm cho lnh vc r bt di
ng, xe t hnh l kh nng xc nh phng hng
(navigation) ca r bt, bi ton thit k qy o
chuyn ng trnh vt cn v iu khin xe t hnh
tip cn qu o mong mun.
Do m hnh ng hc v ng lc hc ca r bt
di ng, xe t hnh thng l m hnh phi tuyn dng
h nonholonomic (c cc rng buc v tc chuyn
ng khng kh tch), v c nhiu tham s bt nh
nn bi ton thit k qu o v iu khin i hi
nhiu nghin cu v p dng cc phng php tnh
ton phc tp bo m xe chy n nh trn tru v
bm qu o chnh xc
Hin nay, c nhiu nhm nghin cu trn th gii
t c nhng thnh tu ng k v iu khin r
bt t hnh nng h (forklift) xp xp k hng (pallet)
trong kho. Nhm tc gi ca trng i hc Girona,
Ty Ban Nha nghin cu h thng th gic tm v
tr 3D ca pallet da trn mu sc v c tnh hnh
hc ca n. T , qu o tuyn tnh c to ra
gip xe nng h c th di chuyn v tip cn pallet
[3]. Nhm tc gi ca Vin k thut h thng ca
trng i hc Hannover nghin cu thnh cng
bi ton nhn dng/nh v pallet da trn hai my
qut laser forklift c th tip cn v nng h pallet
[4]. Bellomo v nhm tc gi nghin cu phng
php c lng v tr ca pallet da vo d liu my
qut laser LIDAR v h thng th gic [5]. Ngoi ra
bi bo ny cng trnh by thut ton iu khin
forklift chuyn ng tip cn nng/h pallet da
trn thut ton ti lp qu o dng polinom.
Widyotriatmo v nhm tc gi xut thut ton
tin on hng (predictive navigation) kt hp
vi qu o ngn nht, trnh chng ngi vt
(obstacle avoidance), v s bt nh ca cc cm bin,
c cu chp hnh [6]. Thut ton to qu o
3
spline h c s dng cho r bt di ng loi
bnh xe (wheeled mobile robot) c gii thiu trong
[7], [8] v thut ton m rng ca n trong vic trnh
cc chng ngi vt khi di chuyn trong [9].
Bo co ny xut gii php thit k qu o v
iu khin ti u xe t hnh cho bi ton vn chuyn
k hng (pallets) trong kho. Bo co gm 5 phn.
Phn 2 m t thut ton thit k qu o
3
spline h
vi a thc bc 7 v thut iu khin ti u theo
chun ton phng xc nh tc cn thit cho
r bt bm qu o. Phn 3 trnh by phng php
nh v xe t hnh t cc tn hiu o can nhiu c
xc nh bng phng php lc Kalman m rng. Cc
th nghim trn r bt t hnh Pioneer P3-DX trong
phng th nghim c m t trong phn 4. Phn 5 l
mt s kt lun v hng nhin cu tip tc trin
khai trong thi gian ti.
2. Bi ton iu khin r bt di ng xp
xp hng trong kho
2.1 Xy dng m hnh ng hc cho r bt di dng
R bt di ng s dng trong bo co ny l Pioneer
P3-DX thuc loi xe chuyn ng trn mt phng c
hai bnh y v mt bnh li c th thy Hnh 1
269
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011



VCCA-2011
(trn) v m hnh ca n c hnh dng hnh ch nht
(di).


H1. T trn xung di: r bt Pioneer P3-DX v
cu hnh ca n trong h ta OXY.
y
1
l l chiu di,
2
l l chiu rng, v
w
l l
khong cch gia 2 bnh xe sau. Ta OXY v
oxy ln lt l h ta nh kho v h ta cc
b gn trn r bt, v (X, Y) din t ta ca trng
tm r bt trong h ta nh kho. Gc u biu din
gc quay ca trc x-axis so vi trc X. Ta cc b
ca r bt oxy c thit lp nh sau: v tr gc ca
ta im gia ca trc sau v trc x-axis vung
gc vi trc chnh ca r bt. Nh vy cp 3 (X, Y, )
m t y v tr (X,Y) v hng u ca r bt
thng c gi l cu hnh (configuration) ca r
bt di ng. Vi gi thit xe khng b trt (no-
slippage), r bt phi chuyn ng vi rng buc v
tc nh sau:
sin cos . X Y u u = (1)
iu kin rng buc ny thuc dng khng kh tch
(non-holonomic) c ngha l ta khng th tch phn
xc nh c qu tch nhng im (X,Y) rng buc m
chuyn ng ca xe phi tha mn. Trong thc t xe
cu hnh ca xe (v tr v hng) c th l bt k mi
im trong mt phng nhng khi chuyn ng th tc
ca xe phi tha mn iu kin rng buc non-
holonomic (1).
Vi iu kin rng buc ny, chuyn ng ca r bt c
th m t bi hai bin iu khin chuyn ng (v, )
nh sau:
cos
sin
X v
Y v
u
u
u e
=

(2)
trong v l vn tc tuyn tnh (translational velocity)
ca r bt dc theo trc x-axis v l vn tc gc ca
r bt. Gi nh
max
v v s v
max
e e s , trong
max
v v
max
e ln lt l cc gi tr ln nht ca
chng. H (2) l h ng hc kh ph bin cho nhiu
loi xe t hnh dng trong cng nhip cng nh trong
cc phng th nghim.
2.2. Bi ton xp xp k hng trong kho v thut
ton iu khin ti u
Trong ta ton cc OXY , r bt c cu
hnh l ) , , ( u Y X , k hng c cu hnh l
) , , (
P P P
Y X u , v cu hnh cn chuyn k hng n l
) , , (
G G G
Y X u (Hnh 2).

H 2. Bi ton xp xp pallet trong kho.
Cu hnh ca k hng trong h ta ton cc c th
c xc nh nh cc cm bin nh my qut laser,
camera gn trn r bt. Cu hnh ca r bt c b
nh v (localization) c lng da trn tn hiu thu
v t cc cm bin trong ca r bt. Bi ton t ra l
cn tm thut ton c th iu khin r bt di
chuyn t v tr hin ti tip cn k hng v sau
mang k hng ti v tr ch cho trc. Cc tc v
r bt cn thc hin trong nhim v xp xp ny l:
I- Di chuyn t cu ban u ti cu hnh ca k hng
II- Nhc k hng ln
III- Di chuyn t cu hnh nhc k hng ti cu
hnh ch
IV- t k hng xung v ng nguyn v tr ch
S khi ca b iu khin cho r bt nh trong
Hnh 3.

H 3. S khi ca h iu khin.
B iu khin r bt bao gm nhiu khi chc nng,
nh sau:
Khi nh v c chc nng thu nhn tn hiu t cc
cm bit trong nh gyro, encoder v tn hiu t cc
X
Y
x

y


v
1
l

2
l

w
l

O
o

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



VCCA-2011
cm bin ngoi nh camera, GPS, .. c lng v
tr v hng hin ti ca r bt. y l b phn rt
quan trng trong qu trnh r bt di chuyn. R bt
cn cp nht v tr ca n thng xuyn c hng
iu chnh thch hp cho qu trnh tip cn pallet hoc
di chuyn v ch.
Khi nhn dng mi trng c ngha v phn tch v
nhn dng mi trng hot ng xung quanh r bt
da trn tn hiu t cc cm bin th gic, v siu m.
Khi nhn dng pallet c nhim v nhn dng v c
lng v tr v hng ca pallet da trn tn hiu thu
c t h thng th gic.
Khi iu khin chnh l b phn c nhim v to v
iu khin r bt bm theo qu o r bt c th
tip cn v nhc pallet ln, sau iu khin r bt i
v ch nh trc. y, chuyn ng ca r bt
s b hn ch bi iu kin rng buc khng kh tch
v tc (1).
Hai tc v (II) v (IV) s khng c cp chi tit
v thut ton cng nh cch thc thc hin bo co
ny. Bo co s i su chi tit v thut ton v kt qu
thc nghim cho vic thc hin tc v (I) v (III).
thc hin hai tc v (I) v (III), chng ta cn gii
quyt bi ton tm tn hiu iu khin u vo (.) v v
(.) e r bt c th di chuyn t v tr v hng
ban u ( (0), (0), (0))
A
P X Y u ti thi im 0 t = vi
(0)
A
v v = , (0)
A
v v = , (0)
A
e e = , (0)
A
e e = ti v
tr v hng ch ( ( ), ( ), ( ))
B f f f
P X t Y t t u ti thi
im
f
t t = vi ( )
B f
v v t = , ( )
B f
v v t = , ( )
B f
t e e = ,
( )
B f
t e e = .
3. Thit k qu o v iu khin ti u r
bt di ng.
r bt thc hin c cc dch chuyn cn thit
nu trn ta cn thit k c qu o chuyn ng ri
tm thut ton iu khin xe bm theo qu o.
3.1 Thit k qu o vi ng cong spline
3
q
Bi ton thit k qu o y nhm tm ra mt qu
o trn m bo iu kin rng buc v tc (1)
cho r bt (2) khi ng t cu hnh ban u
( (0), (0), (0))
A
P X Y u di chuyn n cu hnh ch
( ( ), ( ), ( ))
B f f f
P X t Y t t u trong thi gian 0
f
t > .
Ta bit rng vi bt k cu hnh ch
( ( ), ( ), ( )); 0
f f f f
X t Y t t t q , cng c th to c mt
qu o spline
3
q (
3
G ) cho h (2) tip cn c
bng cc tn hiu iu khin ( ), ( ) v t t e ;
( ) 0 [0, ]
f
v t t t = e . Ngc li, vi qu o
3
G
cho trc th lun tn ti cp tn hiu iu khin
( ), ( ) v t t e vi ( ) 0 [0, ]
f
v t t t = e v cc iu kin
ban u h (2) bm st qu o
3
G cho [7].
Gi thit 0
A
v > v 0
B
v > , gc lch
A
u v
B
u gia
trc X v cc vc t n v tip tuyn (tangent) phi
trng vi gc lch ca mobile r bt ti cc im ban
u v im ch (nh trong Hnh 4). Qu o
3
G -
paths, ng cong ni suy ca hm a thc bc 7 i
qua 2 im A v B c tnh nh sau [7]:
( ; ) ( ), ( )
T
x y
s p s p s ( =

P , (3)
2 3
0 1 2 3
4 5 6 7
4 5 6 7
( )
x
p s s s s
s s s s
o o o o
o o o o
= + + +
+ + + +
, (4)
2 3
0 1 2 3
4 5 6 7
4 5 6 7
( )
y
p s s s s
s s s s
| | | |
| | | |
= + + +
+ + + +
, (5)
Trong [0, 1] s e l tham s qung ng c
chun ha. Ti im khi u s=0, ( )
x
p s =X
A
,
( )
y
p s =Y
A
, v ti im kt thc th s=1, ( )
x
p s =X
B
,
( )
y
p s =Y
B
.Cc h s ca a thc ( )
x
p s c tnh
nh sau:
|| , ||
A B A B
X X Y Y q =
0 A
X a ,
1
cos
A
a h q ,
1
cos
B
g h q ,
2
0 a ,
3
0 a ,
4 1 2 3 1 2
35( ) 20 10 4 15 5
B A
X X a a a a g g
5 1 2 3 1
84( ) 45 20 6 39
B A
X X a a a a g ,
6 1 2 3 1
70( ) 36 15 4 34
B A
X X a a a a g ,
7 1 2 3 1
20( ) 10 4 10
B A
X X a a a a g . (6)
Cc h s ca a thc ( )
y
p s :
0 A
Y b ,
1
sin
A
b h q ,
1
sin
B B
g h q ,
2
0 b , ,
3
0 b , ,
4 1 2 3 1
35( ) 20 10 4 15
B A B
Y Y b b b b g ,
5 1 2 3 1
84( ) 45 20 6 39
B A B
Y Y b b b b g ,
6 1 2 3 1
70( ) 36 15 4 34
B A B
Y Y b b b b g ,
7 1 2 3 1
20( ) 10 4 10
B A B
Y Y b b b b g .

H. 4. Qu o
3
G i qua im A v B.
Cc im trn qu o
3
G cn c gi l way point
(WP) bi r bt c th di chuyn qua cc im ny
ti im ch nh trc. Gi thit c N im WP
trn qu o
3
G ( s im WP N c th ty chn v
nm trong di gi tr ti hn), ta c 1 + N on thng
c ni gia cc im bao gm c im u v im
271
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011



VCCA-2011
ch. Nh vy thc hin tc v (I) v (III) trong
bi ton nng h k hng t ra, b to qu o cho
qu trnh chuyn ng s dng
3
spline h l hon
ton ph hp vi m hnh r bt (2).
H 5. S khi ca vng iu khin r bt di ng bm qu o
3
G .
Chng ta s s dng qu o
3
G vi cc thng s
c tnh theo phng trnh (3), (4) v (5) trong vic
tnh ton vn tc tuyn tnh v vn tc gc cho r bt
phn tip theo.
3.2 Xc nh vn tc tuyn tnh v vn tc gc
iu khin r bt di ng
Sau khi c c tp cc im WP, ta cn iu khin
r bt di chuyn tun t qua cc im WP ti ch.
S khi ca vng iu khin kn r bt c m t
trong Hnh 5. Da vo sai s gia qu o thc t v
qu o
3
G thit k ta c th s dng phng
php iu khin ti u tnh vn tc gc e trong
khi vn tc tuyn tnh ca r bt c tnh ton ph
thuc vo v tr ca cc chng ngi vt trong khu
vc mi trng r bt hot ng. Vn tc tuyn tnh
v vn tc gc ca r bt c m bo lun nm
trong vng gii hn nh trc khi c gi xung
c cu chp hnh ca r bt.

H.6: H ta qu o ( )
d d d
O X Y
Gi thit ( , , )
d d d
X Y u l v tr v hng tc thi ca
im WP trn
3
G . nh ngha h ta qu
o ( )
d d d
O X Y nh m t trong hnh 6. H ta qu
o thc cht l h ta gc ( ) OXY b tnh tin mt
on ;
d d
X Y theo hng X, Y v quay xung quanh
trc Z mt gc .
d
q Sai s v tr v gc gia qu o
thc ca r bt v qu o
3
G tnh trong h ta
( ) OXY l ; ;
e d e d e d
X X X X Y Y q q q .
Cc sai s ny nu nhn tnh trong h ta qu o
( )
d d d
O X Y s c tnh nh sau:
cos sin 0
sin cos 0
0 0 1
d
e
d d d
d
e d d d
d
d
e
X
X X
Y Y Y
u u
u u
u u
u
(
( (
(
( (
= (
( (
(
( (

(

(6)
Gi ) , (
d d
v e l vn tc tuyn tnh v vn tc gc
mong mun. Mc tiu iu khin l xc nh tc
di v tc gc w ca r bt sao cho cc sai lnh
v v tr v gc trong (6) tin v 0. Chin lc iu
khin y l t
d
trc v tmw sao cho r
bt bm st qu o
3
G . t [ , ]
T
e e
Y q e l vc t
sai lch, bao gm sai lch ngang
e
Y v sai lch gc
e
q . T h phng trnh ng hc (2) v vi gi thit
sai lch gc
d
e
q nh ta c th tnh c o hm ca
vc t sai lch nhn trng h ta ( )
d d d
O X Y nh
sau:
d d
e d e
d
e d
Y v u
u e e
=
=
, (7)
H phng trnh trng thi ca (7) c dng :
e
u
c c
e = A e +b , (8)
trong
0
0 0
d
v
c
A
(
=
(

;
0
1
c
b
(
=
(

;
e d
u e e = l tn
hiu v hng.
Do r bt Pioneer P3-DX c cung cp cc s liu o
di dng s nn ta chuyn h phng trnh sai s (8)
sang dng ri rc c th tnh c tn hiu iu
khin tc quay ca r bt trong tng thi im ct
mu:
( 1) ( ) ( )
e
k k u k e Ae b + = + , (9)
trong
s
T
c
A = I +A ,
s
T
c
B = B , v
s
T l thi gian
ly mu. Tn hiu iu khin ti u ( )
e
u k c xc
nh sao cho ti thiu ha hm mc tiu sau:
( ) ( ) ( ) ( ( ) ( ) ( ))
0
N
T T
e e
k
J k k k u k R k u k e Q e
=
= +

(10)
trong Q v R l cc ma trn trng s, 0 Q > ,
0 R > . Ma trn Q c chn ph thuc vo sai lch
272
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011



VCCA-2011
ngang
| |
50
) 1 , 1 (
2
1
1
e
Y c
c
c
Q
+
= trong
2 1
, c c l cc
hng s dng. Vi cch la chn ny nu r bt cch
xa qu o
3
G , trng s ca lch ngang nh hn,
v nu r bt gn vi qu o th (1, 1) Q tr nn ln
hn. Tn hiu iu khin u
e
c tnh ton theo lut
iu khin ti u ton phng:
( ) ( ) ( )
* T
e o
u k k k k e = (11)
Trong vc t phn hi trng thi ti u c tnh
ntheo biu thc

( )
R
-1
T T
o o o
k = b M b+ b M A (12)
y
o
M l ma trn i xng, xc nh dng l
nghim ca ca phng trnh Ricatti:
( )
R
-1
T T T T
o o o o o
M - A M A- Q+A M b +b M b b M A 0 =
(13)
T suy ra
( ) ( ) ( )
* *
e d
k u k k e e = + (14)

3. S dng b lc Kalman m rng cho
c lng v tr v hng ca r bt
Do cc s liu o t r bt b can nhiu nn cn phi
c phng php c lng ph hp c th xc
nh c v tr v hng tc thi ca r bt chnh
xc hn Trong bo co ny, b lc Kalman m rng
c s dng do m hnh ng hc ca r bt di ng
l m hnh phi tuyn. u ra ca b lc Kalman s
c s dng nh cc gi tr phn hi (X,Y,u) trong
vng iu khin H.5.
Gi ,
Lk Rk
s s A A ln lt l khong cch i c ca
bnh xe bn tri v bn phi ca r bt trong khong
thi gian trch mu T
s
. Khong cch m r bt i
c v s thay i hng ca r bt trong khong
thi gian T
s
c tnh nh sau:
2
Rk Lk
k
s s
s
A + A
A = (15)
w
Rk Lk
k
s s
l
u
A A
A = (16)
T cc s liu ny ta c th tip tc tnh ra c s
dch chuyn ca r bt trong h ta c nh l:
( ) cos / 2
k k k k
x s u u A = A +A (17)
( ) sin / 2
k k k k
y s u u A = A +A (18)
w
Rk Lk
k
s s
l
u
A A
A = (19)
Do cc s liu o t cc encoder ca hai bnh xe b
cn nhiu nn cc d liu hiu ny l cc d liu b
can nhiu nn cn c phng php lc ph hp c
th xc nh c v tr v hng ca r bt tt hn.
y chng ti chn phng php lc Kalman m
rng dng ri rc vi phng trnh trng thi l h
ng hc ri rc b can nhiu ca r bt. T h
phng trnh ng hc (2) ca r bt ta c th xy
dng m hnh ri rc b can nhiu nh sau :
( )
1 1 1
,
k
k k k k k
k
x
y x f x u
u

(
(
= = +
(
(

(20)
trong
( )
1 1
1 1 1 1
cos / 2
2
Rk Lk
k x k k k xk
s s
x f x u u


A + A
= = + + A +
(21)
( )
1 1
1 1 1 1
sin / 2
2
Rk Lk
k y k k k yk
s s
y f y u u


A + A
= = + + A +
(22)
1 1
1 1
w
Rk Lk
k k k
s s
f
l
u u
u u


A A
= = + + (21)
Trong
x y
[ ( ), ( ), ( )]
k
k k k
u
= l nhiu h
thng, c k vng l 0 v ma trn hip bin l
k
M .
,
T
k Lk Rk
s s u = A A (

l tn hiu o c t cc encorder
bnh xe tri v phi ca r bt. Tn hiu u ra ca b
lc Kalman l:
( )
k k k
z h x = + (22)
1 1 k x k k
z h x z (23)
2 2 k y k k
z h y z (24)
3 3 k k k
z h
q
q z (25)
Trong
1 2 3
, ,
T
k k k k
z z z z l tn hiu u ra xc
nh ta v hng ca r bt;
1 2 3
, ,
T
k k k k
z z z l
nhiu u ra c k vng bng 0 v ma trn hip bin
l
k
N . y, Cc ma trn hip bin ca cc nhiu
c xc nh bng thc nghim:
4
3 3
10 .
k
M I ;
2
3 3
10 .
k
N I .
B lc Kalman m rng c lng trng thi ca h
thng
k
x

v ma trn hip bin sai lch trng thi

k
P

da trn m hnh h thng f(.) trong pha d on:
( )
1 1
,
k k k
x f x u

= (26)
( ) 1 1 1 1

cos / 2
k x k k k k
x f x s u u

= = +A +A (27)
( ) 1 1 1 1

sin / 2
k y k k k k
y f y s u u

= = +A +A (28)
1 1

k k k
f
u
u u u

= = +A (29)
k 1 1 1 1
T
k k k k
P A P A M

= + (30)
Vc t trng thi c lng
k
x v ma trn hip bin
sai lch trng thi
k
P trong pha cp nht nh sau:
( )
1
T T
k k k k k k k
K P H H P H N


= + (31)
( ) ( ) k

k k k k
-
x x K z h x

= + (32)
( )
k k k k
P I K H P

= (33)
Trong I l ma trn n v. Cc ma trn ,
k k
A H l
cc ma trn Jacobi c tnh nh sau :
273
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011



VCCA-2011
1 1
1
,
k k
k T
x u
f
A
x
(34)

k
k T
x
h
H
x
(35)
Tnh chi tit cc ma trn Jacobi
k
A v
k
H ta c :
x x x
k k k
y y y
k
k k k
k k k
f f f
x y
f f f
x y
f f f
x y
A
u u u
u
u
u
c c c (
(
c c c
(
c c c (
=
(
c c c
(
(
c c c
(
c c c


1 0 .sin
2
0 1 .cos
2
0 0 1
k
k k
k
k k
s
s
u
u
u
u
( A | |
A +
( |
\ .
(
(
A | |
= A + (
|
\ . (
(
(
(



3 3 k
H I

= .
Nh vy bng phng php s dng b lc Kalman
m rng nh trn, ta s xc nh c v tr v
hng ca r bt ti thi im k chnh l vc t

, ,
T
k k k k
x y x u
(
=

. Tn hiu

, ,
T
k k k k
x y x u
(
=

s
c s dng trong vng iu khin Hnh 5.
4. Kt qu thc nghim
Phn ny trnh by kt qu th nghim khi p dng
thut ton thit k qu o v iu khin r bt thc
hin cc tc v (I) v (III) phn trn. Th nghim
c thc hin trn r bt Pioneer P3-DX c kch
thc WH=0,4438m. Tc ln nht ca vn tc
tuyn tnh v vn tc gc ln lt l 5 . 0
max
= v m/s v
523 . 0
max
= e rad/s. Tt c cc qu trnh thit k qu
o/iu khin r bt,... c thc hin bi ngn ng
lp trnh C++, thi gian trch mu l 100 ms.
Gi thit v tr v hng ban u ca r bt ti A, v
tr v hng ca k hng ti B v v tr v hng ca
ch cui cng l C. R bt i t cu hnh A(0m; 0m;
0
o
) B(1,4m; 1,4m; 90
o
) C(0,2m; 2,8m; 90
o
). Kt
qu tnh ton thu c nh sau: B(1,3960m; 1,3947m;
90,0460
o
); C(0,2030m; 2,7951m; 89,8418
o
).
Trong thc nghim, trc mt chng ti ch dung
thit b o duy nht l encoder (o tng i) tnh
vn tc tc thi v d on ta , hng r bt m
cha s dng cm bin ngoi nh camera o v tr
tuyt i ca r bt trong khng gian hot ng nn
b lc Kalman m rng cha th phn nh chnh xc
v tr thc ca r bt. Do , trn thc t khi gp th
k hng, ta ca r bt thc b sai lch t 1 n 3
cm so vi mong mun; hng thc b sai lch vi
so vi mong mun.

H.7:Qu o mong mun v qu o thc ca r bt.


H.8: R bt Pioneer P3-DX tip cn mc tiu.
Trong tng lai, chng ti s lp thm camera o
v tr tuyt i ca r bt v s p dng phng php
phi hp cm bin (sensor fusion) gia encoder v
camera trong b lc Kalman m rng. Khi , u ra
ca b lc Kalman m rng s phn nh chnh xc v
tr v hng ca r bt hn.

5. Kt lun
Bo co trnh by phng php iu khin t ng
xe nng h xp xp k hng trong kho. Phn thit k
qu o s dng phng php thit k qu o
spline
3
q . Hai tc v (I) v (III) c quy v bi
ton thit k thut iu khin r bt c th di
chuyn t v tr ban u ti im ch cho trc.
Phng trnh ng hc ca r bt P3-DX c m t
ngn gn. Thut ton iu khin c thit k theo
phng php iu khin ti u theo chun ton
phng cho m hnh ng hc r bt ri rc. Cc tn
hiu o c a vo b lc Kalman m rng dng
ri rc xc nh v tr v hng tc thi ca r bt.
Kt qa thc nghim thu c cho thy thut ton
iu khin hon ton thc hin c cc nhim v
t ra nh tip cn k hng hay tr v mt v tr bit
trc.

Ti liu tham kho
[1] J. Pags, X. Armangu, J. Salvi, J. Freixenet
and J. Martis: A Computer Vision System for
Autonomous Forklift Vehicles in Industrial
274
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011



VCCA-2011
Environments, Proc. of the 9th Mediterranean
Conf. on Control and Automation, 2001, pp.
379-384.
[2] D. Lecking, O. Wulf, and B. Wagner: Variable
Pallet Pick-Up for Automatic Guided Vehicles
in Industrial Environments, Proc. of the IEEE
conference on Emerging Technologies and
Factory Automation ETFA06, Prague 2006,
pp. 1169 - 1174.
[3] N. Bellomo, E. Marcuzzi, L. Baglivo, M.
Pertile, E. Bertolazzi, and M. De Cecco: Pallet
Pose Estimation with LIDAR and Vision for
Autonomous Forklifts, Proceedings of the 13th
IFAC Symposium on Information Control
Problems in Manufacturing, Moscow, Russia,
June 3-5, 2009, pp. 612-617.
[4] A. Widyotriatmo, B. Hong, and K.-S. Hong:
Predictive navigation of an autonomous
vehicle with nonholonomic and minimum
turning radius constraints, Journal of
Mechanical Science and Technology, 23
(2009) 381-388.
[5] A. Piazzi, C. Guarino Lo Bianco, and M.
Romano:
3
q -Splines for the Smooth Path
Generation of Wheeled Mobile Robots, IEEE
Trans. Robotics, 23 (5) (2007) 1089-1095.
[6] J. Connors, G. Elkaim: Experimental Results
for Spline Based Obstacle Avoidance of an
Off-Road Ground Vehicle, ION Global
Navigation Satellite Systems Conference ION
GNSS 2007, Fort Worth, TX, Sept. 25-28,
2007, pp. 1484-1490.
[7] J. H. Wei and J. S. Liu: Collision-free
Composite
3
q Splines Generation for
Nonholonomic Mobile Robots by Parallel
Variable-Length Genetic Algorithm, Proc. of
the Int. Conf. on Computational Intelligence
for Modelling Control and Automation,
Vienna 2008, pp. 545 - 550.
[8] M. Hentschel, O. Wulf and B. Wagner: A
Hybrid Feedback Controller for Car-Like-
Robots, Proc. of the Int. Conf. on Informatics
in Control, Automation and Robotics
(ICINCO), 2006.
[9] K. Kanjanawanishkul, M. Hofmeister: and A.
Zell, Path following with an optimal forward
velocity for a mobile robot, Proc. of the 7th
IFAC Symposium on Intelligent Autonomous
Vehicles (IAV), pp. 1-6, Lecce, Italy, Sept.
2010.
[10] W. H. Kwon and S. Han: Receding horizon
control: Model predictive control for state
models, Springer-Verlag, London, 2005.






Pham Thuong Cat is a
Honorary Research Professor in
Computational Sciences of
Computer and Automation
Research Institute of the
Hungarian Academy of
Sciences. He is the Editor-in-
Chief of the Journal of
Computer Science and
Cybernetics of the Vietnamese Academy of Science
and Technology (VAST) and a Senior Researcher of
the Institute of Information Technology of VAST. He
is the representative of the Vietnam NMO at the
International Federation of Automatic Control
(IFAC) and the Vice President of the Vietnamese
Association of Mechatronics. His research interests
include robotics, control theory, cellular neural
networks and embedded control systems. He co-
authored 3 books and published over 140 papers on
national and international journals and conference
proceedings.

Nguyn Vn Tnh nhn bng k
s ti i Hc Bch Khoa H
Ni nm 2008. Anh ang lm
nghin cu vin ti Vin Cng
ngh Thng tin thuc Vin Khoa
hc Cng ngh Vit Nam, s 18
Hong Quc Vit, Cu Giy, H
Ni.
T. T. Quyen Bui received her B.S.
and M.S. degrees in Hanoi
University of Technology,
Vietnam, in 2001 and 2006,
respectively. She is currently a
Ph.D. program student in the
School of Mechanical
Engineering, Pusan National
University, Korea. Her research interests include
robotics, vision systems, and navigation of autonomous
vehicles.
TS. Phm Minh Tun tt nghip
i hc Bch khoa H Ni,
chuyn ngnh Cng ngh Thng
tin nm 1997, sau nhn bng
Thc s v Tin s v K thut
iu khin trng i hc
Cng ngh Nanyang, Singapore,
vo cc nm 2002 v 2006. T
2006 n 2011, ng lm vic ti
Phng Cng ngh T ng ha,
Vin Cng ngh Thng tin, Vin KHCNVN. Hin ti,
ng ang l Ph Gim c Trung tm iu khin v
Khai thc V tinh nh thuc Vin Cng ngh V tr,
Vin KHCNVN.
Lnh vc nghin cu chnh ca ng bao gm iu
khin xe t hnh, iu khin t th v tinh, cc h
nhng, v h thng nng lng ti to. ng cho
ng trn 30 bi bo ti cc hi ngh, tp ch trong
nc v quc t. ng cn l thnh vin Hi C in
t Vit Nam.
275