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

VCCA-2011
X l nh xc nh hng v khong cch
gia gi v xe nng hng t ng

Image Processing for estimating orientation and distance
from autonomous forklift to pallet

Phm c Long
i hc Cng ngh thng tin v truyn thng Thi Nguyn
e-mail: dlong2na@gmail.com
Phm Thng Ct
Vin Cng ngh thng tin - Vin KH&CN Vit Nam
e-mail: ptcat@ioit.ac.vn

Tm tt
Trong bi bo ny chng ti gii thiu phng php
x l nh thu c t mt camera CNN (Cellular
Neural Network) kiu Bi-i cung cp hng v
khong cch cho mt xe nng hng t ng khi tip
cn gi hng. M hnh camera pinhole c s
dng trong trng hp mt phng nh v mt phng
i tng khng song song vi nhau. Vic tnh ton
c thc hin kt hp gia x l song song v c ni
tip cho php thu c cc cc s liu chnh xc vi
thi gian x l nhanh.

Abstract: In this paper we present a method for
estimating orientation and distance from autonomous
forklift to pallet by image processing. The frames are
captured by a CNN (Cellular Neural Network) camera
type Bi-i. Model pinhole camera in the case of object
plane un-parallel with image plane is used. The
computation is associate of serial also and parallel, so
we can to get exact data in short time.

Gii thiu nhim v

tng t ng ho vic xp r hng ho ti cc nh
kho, cng c cc nh nghin cu trong lnh vc
t ng ho ra t nhng nm cui ca th k 20.
Ngoi cc nhim v nh v cc sn phm, xy dng
cc h nng chuyn trong kho cng, mt nhim v rt
thit thc v khng km phn l th l nghin cu cc
xe nng hng t ng, khng c ngi li c
nhiu nhm nghin cu t ng ho v iu khin
thc hin. Nygards trong [2], Nilsson trong [3],
Jpages, X.Armangue, J.salvi, J.Freixenet v J.Mart
trong [5], [6] tm v tr 3D ca cc gi hng da
trn cc c im mu sc v hnh hc ca chng.
Mt s tc gi khc s dng hai camera quang hc
hoc hai laser scanner thc hin tnh ton khong
cch n i tng [1], [8], [9]. Mt s tc gi khc
s dng 1 camera xc nh hng v khong cch
nh trong [4], [10]. c 1 s sn phm xe nng
hng c gn camera c iu khin t xa c
a vo s dng. y l loi xe nng c gn camera
khng c ngi ngi trn xe iu khin trc tip
Ngi iu khin xe thng qua mt mn hnh iu
khin t xa li xe nng h n cc v tr cn thit, thc
hin cc thao tc cn thit. Tuy nhin, cho n nay
cc xe nng h t ng hon ton khng ngi li th
mi ang dng nghin cu v thc s cha c mu
thng phm no trong thc t.

H. 1 Xe nng hng c ngi li
Mt xe nng hng t ng (thc cht l mt robot) c
nhim v tip cn mt gi hng nng gi (c
hng hoc khng c hng) di chuyn n mt v tr
khc v h n xung. xe tip cn c gi mt
s thng s c bn cn c cung cp cho b x l
trn xe:














H. 2 Gi hng tiu chun v cc kch thc (di x
rng x cao) mm. T trn xung di, t tri qua
phi: 2a): chu u (1200 x 800 x 144) 2b): M 2c):
Hn Quc (1100 x 1100 x 150) 2d): Trung Quc
(1100 x 1100 x 150).

Xe t ng dng 2 cng gn u xe mc vo 2 l
gi hng. Khong cch gia hai cng c
iu chnh cho ng vi khong cch gia hai l trn
gi . Vi cc pallet c chiu di v chiu rng khc
nhau (kiu EU v kiu M) trn mi mt u c 2 l
525
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
mc vo nhng kch thc v khong cch gia
cc l khng nh nhau nn chng trnh x l cn
phn bit c cc nhm l ny. Vi cc pallet m
kch thc di = kch thc rng (pallet hnh vung)
nh ca Hn Quc v Trung Quc (lc a) th khng
cn phn bit sn di hay sn ngn ca pallet.
- Hng ca gi hng so vi xe nng t
ng
H to ca chng ta c k hiu vi cc trc Ox,
Oy, Oz. Trong O l gc to
Oxyz
W
l h to ca khng gian thc vi cc trc
O
w
x
w
, O
y
x
y,
O
z
x
z
.
Oxyz
camera
l h to ca camera vi cc trc O
c
x
c
,
O
c
x
c,
O
c
x
c
.
Oxyz
fork
l h to ca cng xe nng vi cc trc
O
f
x
f
, O
f
x
f,
O
f
x
f
.
Oxyz
body
l h to ca xe nng vi cc trc O
b
x
b
,
O
b
x
b,
O
b
x
b
.


















H. 3 Cc h to

Nu chng ta chiu song song xe nng v gi ln
mt phng O
w
x
w
y
w
th gc gia hnh chiu ca trc
i xng s-s ca gi vi trc O
b
y
b
ca xe nng
c gi l hng ca xe nng vi gi (hoc
ngc li).
- Khong cch t xe t ng n gi hng
y l khong cch t mt im trn xe nng n mt
im trn gi . Chng ta c th nh ngha l
khong cch gia tm hnh hc theo ba chiu ca xe
nng n tm hnh hc theo ba chiu ca gi .
Trong bi vit ny chng ti ly l khong cch t
tm im tiu c ca camera gn trn xe nng n
im tm hnh hc ca mt trc ca gi .

Thut ton xc nh hng v khong
cch pallet

- Xc nh mt trc ca gi :

Mt trc ca gi c xc nh bng cc c
im:
C hai cnh song song v cc cnh ny c nhn
dng khi gi nm trn mt t tc l chng song
song vi nhau v song song vi trc to Oy (hnh
H3), c hai l mc vo. nhn bit cc c im
ny trong trng hp tng qut chng ta c th s
dng bin i Hough kt hp xem xt cc c im
ph tm ra mt trc. Kch thc chiu cao ca 2
l trn mt trc ln hn chiu cao ca hai l trn
cnh. V d: Xem hnh 1a, 1b. Bng chng trnh x
l nh chng ta c th d dng xc nh cc c im
ny. Vi cc trng hp gi hnh vung (kiu Hn
Quc 1c v Trung Quc 1d), mt trc v mt sn
nh nhau, vn xc nh mt trc l khng cn
thit.
- Xc nh hng



H. 4 Cc kh nng v tr ca gi so vi xe t ng
(lch tri so vi xe, i din vi xe, lch phi so vi xe) v
cc hnh tng ng khi xc nh hng theo hnh 5

Khi xe nng tip cn n gi s c 3 kh nng tip
cn nh hnh 4.
T cc hnh mt trc v sn ca gi thu c s
dng phng php xc nh gc ca trc elip di ca
mt i tng trong CNN [11] vi trc ngang (chnh
l gc hng ca xe nng v gi ) [7].





H. 5 Thng s Orientation xc nh hng

- Xc nh khong cch:
M hnh xc nh khong cch dng camera c s
dng l m hnh camera pinhole:
s
s
O
x
z
y
526
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011




H. 6 M hnh camera pinhole vi hai mt phng nh v
mt phng i tng song song vi nhau.

Gi s mt phng nh v mt i tng song song vi
nhau. Cc tia chiu v truc Ox cng trong mt mt
phng. Quan h gia khong cch thc t tiu im
ng knh n mt phng i tng D v nh ca i
tng h, kch thc thc ca i tng H v tiu c f
ca ng knh. Cc kch thc tnh bng milimet.

f
h
D
H
=
(1)
T suy ra:
k n
H
f
h
H
f D
.
= =
(2)

n: s pixel ca nh biu din kch thc theo hng h
k: h s th hin s milimt/pixel.


N
W
k =
(3)
.
W: Kch thc tm sensor cm bin nh
N: phn gii ca cm bin nh.
Trong thc t chng ta thng s dng chng trnh
x l nh bng my tnh (PC hoc mt h thng tnh
ton khc) khi chng ta nh x tm sensor cm
bin ln mt mn hnh my tnh. Lc ny:
W l kch thc ca mn hnh theo phng tnh ton
tnh bng milimt.
N: Tng s pixel ca mn hnh theo phng tnh ton.
Trong trng hp mt phng nh ca camera v mt
phng i tng khng song song vi nhau chng ta
c m hnh (hnh H7). Trong m hnh ny u l gc
nghing ca camera vi mt phng ngang theo
phng ng (gc til), u l gc lch ca camera vi
trc i xng ca i tng theo phng ngang (gc
pan).





H. 7 M hnh camera pinhole khi mt phng nh khng
song song mt phng i tng.

































Ox
Oy
u
O
A
B M
D
C
N
Y
X
Tiu im ng knh
Mt phng nh
Mt phng i tng
u
+u
u
D
Mt phng song song vi
mt phng nh

P
Q
T
Z
K
L

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

VCCA-2011
- Kch thc nh thc ca i tng:
Do tnh ng dng ca nh trn mt phng nh v i
tng trn mt phng i tng ta c th xem xt
quan h gia kch thc thc X, Y v nh ca n trn
hai hng ti mt phng song song vi mt phng
nh. Nh vy nu k hiu:
Ox l kch thc nh theo chiu ngang (tnh bng
mm),
x l kch thc nh ngang - horizontal image
dimension (tnh bng pixels),
Sx l kch thc sensor theo chiu ngang - the
horizontal sensor size (tnh bng mm),
Px kch thc sensor ngang - the horizontal sensor
size tnh bng pixels.
Oy l kch thc nh ng - vertical image dimension
(tnh bng mm),
y l kch thc nh ng - vertical image dimension
(tnh bng pixels),
Sy kch thc sensor ng - the vertical sensor size
(tnh bng mm) v
Py l kch thc sensor ng - vertical sensor size
tnh bng pixels.
Chng ta c quan h:


X
X
X
fP
D xS
O =
v
Y
Y
Y
fP
D yS
O =
(4)
Khi bit f, Px, Sx,x, Ox chng ta c th tnh ra D

X
X X
xS
O fP
D =
(5)
Khong cch KM t tiu im n ng tm ca i
tng theo phng X c th c tnh ton nh sau:
Cha bit gc u
Theo thng s k thut cho ca ng knh v chiu
cao hnh hc ca v tr t camera chng ta c kch
thc KT v khong cch KM t camera n i
tng chiu trn mt phng xOy:

2 2
KT D KM =
(6)
y l khong cch m b x l iu khin xe nng
h t ng cn bit iu khin khi tip cn n gi
.
Cc gc u, v u c th c tnh t cc gi tr
bit :

|
.
|

\
|
= |
.
|

\
|
=
|
.
|

\
|
= |
.
|

\
|
=


TO
O
TO
O
D
O
KT
D
X X
Y

|
u
cos
tan tan
tan ; sin
1 1
1 1
(7)
Trong trng hp bit gc u

KM=Dsinu (8)
MN=(D+f) sinu (9)

Cc khong cch i tng thc c th c tnh t
cc cc gc v cc gi tr bit nh sau:

u u u
u u
cos ) tan( sin
cos ) tan(
Y Y
Y
O O
O OC MC BC Y
+ + =
= + + = + =

(10)
( ) ( )
|
u
u
|
u
|
tan
cos
sin
tan
cos
tan
|
|
.
|

\
|
+
+ =
+
+ =
= + = + =
Y
X X
X X
O
O
OC
O
OB O AL O X

(11)

- Thut ton tnh ton hng v khong cch

H. 8 Thut ton xc nh hng v khong cch
M t thut ton:
Thut ton gm cc bc tnh ton song song phi
hp ni tip, c thc hin trn thit b Bi-i. Cc


S
3. Chuyn nh thc thnh
cc i tng c (H4).

4. Tnh ton hng
n
5. Tnh khong cch KM
6. Gi thng tin cho h
iu khin xe. iu khin
gc u. i xe di chuyn
7b.
n
-
n+1
>0 ?

7c.
n+1
= 0?
9. Chun b. Mc cng
Pallet vo gi .
7e. i
xe iu
chnh
cho ng
hng
Pallet
8b. i xe
tin n
Bt u
1. Thu nh
7a. Tnh li hng

7d.
iu
khin
xe
quay
lui


S
2. Loi nhiu, xo
i tng nh
8a. Tnh khong cch KM
Mc cng
c cha ?
10. Nhn nhim v di chuyn gi
n v tr yu cu.
Kt thc
S

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

VCCA-2011
bc thc hin song song s dng cng ngh CNN
vi cc b mu tng ng. Thut ton lin tc cung
cp cc s liu v hng v khong cch cho xe t
ng trong qu trnh tip cn gi .
Bc 1: Thu nh t camera.
Bc 2: X l song song: (Loi nhiu, xo i tng
nh) s dng mu SMKILLER

Bc 3: T nh pallet mang hng tch ly cc phn
nh mt trc v sn ca pallet nh hnh 4 d,e,f.
Dng cc mu HOLLOW v PATCHMAK
mu HOLLOW

mu PATCHMAK


Bc 4: Tnh ton hng s dng phng php xc
nh gc ca trc elip di ca mt i tng trong
CNN vi trc ngang (H.5).
Bc 5: Tnh ton khong cch KM theo (6) hoc (8).
Bc 6: Gi thng tin cho h vi x l iu khin xe
nng h t ng. iu chnh gc til u. i xe di
chuyn.
Bc 7: Tnh li hng xem ng hng cha. Nu
ng tin thng n 7c. Nu sai li li.
Bc 8: Tnh khong cch KM.
Bc 9: Chun b. Mc cng vo gi cha.
Bc 10: Nhn nhim v di chuyn n v tr yu
cu.
Kt qu thc nghim
Thc nghim nhn bit i tng gi hng kiu
Hn Quc. M hnh gi c thu nh vi kch
thc 1:4 (Kch thc thc tnh theo milimet ca gi
l 1100x1100. Kch thc ca m hnh l
275x275). Vic nhn dng i tng c kch thc
thu nh d nhin l kh khn hn nhn dng i tng
vi kch thc thc s ca n. Camera s dng loi
Bi-i vi ng knh 1:1.4 50mm. Tc bt nh
1000fps. nh sng chiu bng n 150w khong
cch 0.5m. cao t camera: 570 mm.



H. 9 M hnh gi hng t l 1:4



H. 10 M hnh thc nghim

Bng 1 So snh kt qu tnh ton v kch thc thc.

Hng (radial) ti
khong cch
2500 mm
Khong cch KM
(mm)
T
T
Thc t
Tnh
ton
Sai
s
(%)
Thc
t
Tnh
ton
Sai
s (%)
1
30
0
(0.523)
0.528 +1.00 1950 1955 +0.26
2
35
0
(0.610)
0.615 +0.82 2000 1990 -0.5
3
40
0
(0.697)
0.705 +1.14 2050 2058 +0.39
4
45
0
(0.785)
0.780 -0.64 2100 2110 +0.47
5
50
0
(0.872)
0.871 -0.11 2150 2150 0.00
6
55
0
(0.959)
0.625 +0.98 2200 2180 -0.90
7
60
0
(1.046)
1.053 +0.67 2250 2243 +0.31
8
65
0
(1.133)
1.142 +0.97 2300 2318 +0.78
9
70
0
(1.220)
1.207 -1.06 2350 2340 -0.42
10
75
0

(1.308)
1.317 +0.84 2400 2420 +0.83
11
80
0
(1.394)
1.405 +0.79 2450 2475 +1.02
12
85
0
(1.482)
1.413 +0.74 2500 2525 +1.00


Camera Bi-i
529
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011

H. 11 Chng trnh thc hin

Kt lun

S dng mt camera vi m hnh pinhole chng ta c
th xc nh c hng v khong cch t camera
n mt i tng xc nh. Cc kch thc thc ca
i tng cng c th tnh ton ra t nh trong c
trng hp tng qut khi mt phng nh v mt phng
i tng khng song song vi nhau. Cc s liu v
hng v khong cch ny s c cung cp cho b
x l trong bi ton c th trn xe nng t ng
tip cn chnh xc gi hng cng nh trong nhiu
trng hp cc bi ton tng t khc.

Ti liu tham kho

[1] Giovanni Garibotto, Stefan0 Masciangelo,
Marco Ilic, Paolo Bassino, "ROBOLIFT: a
Vision Guided Autonomous Fork-Lift for Pallet
Handling," IEEE, 1996, pp. 656-663.
[2] Nygrds, J., "On Robot Feedback from Range
Sensors, Reliable Control by Active Reduction
of Uncertainty and Ambiguities," Dissertation,
Robotics/Autonomous Mechanical Systems,
Department of Mechanical Engineering,
Linkping University, 1998.
[3] Nilsson, B., "On robot Control using Range
Sensors with Special Reference to Active
Uncertainty and Ambiguities", Dissertation,
Robotics/Autonomous Mechanical Systems,
Department of Mechanical Engineering,
Linkping University, 1997.






[4] Jacob Roll, "A System for Visual-Based
Automated Storage Robots, " Master's Thesis,
3-1999.
[5] J. Pags, X. Armangu, J. Salvi, J. Freixenet
and J. Mart, "A Computer Vision System for
Autonomous Forklift Vehicles in Industrial
Environments," Mediteranean Conference on
Control and Automation (MED) 2001.
[6] Norman Coleman, Ying K. Lam, James L.
Willis, "Image-based Vision Processing for the
Smart Crane Ammunition Transfer System,"
The fourth International Conference on Control
and Automation ICCA 03 10-12 June 2003
Canada, IEEE pp. 320-324.
[7] AnaLogic Computers Ltd., Bi-i Programming
Guide, H-1111, Budapest Hungary 2004.
[8] Michael Seelinger, John-David Yoder,
"Automatic Pallet Engagment by a Vision
Guided Forklift," Internatinal Conference on
Robotics and Automation Baccelona, Spain,
April 2005, pp 4068-4073.
[9] Daniel Lecking, Oliver Wulf, Bernardo
Wagner,"Variable Pallet Pick-Up for
Automatic Guided Vehicles in Industrial
Environments," IEEE, 2006, pp. 1169-1174.
[10] Sungmin Byun and Minhwan Kim, "Real-Time
Positioning and Orienting of Pallets Based on
Monocular Vision," 2008 20th IEEE
International Conference on Tools with
Artificial Intelligence, IEEE, pp 505-508.
[11] Phm c Long, Phm Thng Ct, "Cng
ngh mng nron t bo CNN v ng dng,"
Tp ch Tin hc v iu khin hc, tp 22, s 1
2006, tr. 37-44.

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

VCCA-2011
Pham Duc Long was born
1955. Mechanical Engineer,
Electronics Engineer,
Informatic MSc. (is PhD
student from 2006). Vice
leader at Faculty of
Automation - Thai Nguyen
University of Information and
Communication Technology
He is Member of VietNam
Mechatronics Association. He
is alsso Member of Asian Control Association. Leader
of some projects in practice using technology of
automatic and IT in industry steel, cement. One of
main researchers in international project co-research
about image processing by Cellular Neural Network
Viet Nam - Hungarian (2006-2007). One of main
researchers in international project co-research about
visional computer Viet Nam - Korea (2010-2011).
Co-chair session ASCC 2009 in Hong Kong, ChiNa.
Fields of research: Automatical control, use image
processing in industry.
































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.
































531