Xy dng h th nghim iu khin R bt

vi qu trnh nhn dng vt th ng

trn c s Camera
TS L B Dng
Ths H Mnh o
KS Nguyn Thanh Tng
Vin CNTT
lbdung@ioit.ncst.ac.vn
Tm tt:
Bi bo trnh by qu trnh xy dng, thit k h th nghim iu khin R bt vi qu trnh nhn dng
vt th ng trn c s Camera. Vi qu trnh nhn dng ng cc v tr v hng ca vt th thng qua nh
vt th t thi im ct mu, mt thut ton c lc v tr c xy dng. Vi cc v tr c lng, thut
ton iu khin c thit k a u tay nm n v tr c c lng v di chuyn vt th n
mt v tr xc nh trong khng gian lm vic v khng gian quan st c ca Camera.
Abstract:
The paper presents a process to create a robot control system with dynamic recognition of image
object based on Camera. With the position and orientation of object is determined on the sampling time via
image recognition, an estimation algorithm is created. By the position and orientation are determined, an
robot control algorithm is constructed to move the end effectors to estimated position and orientation of the
object and take the object to other position and orientation on the working space and camera space.

1. M u
iu khin Robot trn c s Camera c thit k theo 1 s yu cu c bn nh sau v
mt k thut:
- Camera c t c nh trong khng gian lm vic ca Robot
- Camera c treo ti u tay nm end- effector ca Robot.
Trong bi bo ny cc tc gi cp n m hnh th nghim nhn dng vt th ng v
iu khin Rbt vi Camera t c nh trong khng gian lm vic ca R bt

2. M hnh iu khin Robot vi vi Camera t c nh trong

khng gian lm vic ca R bt
M hnh iu khin Robot vi Camera t c nh trong khng gian lm vic ca R
bt cho php ta thc hin h iu khin kn vi qu trnh quan st (o v tr v hng ca vt
th thng qua thit b o quang). y cc tn hiu o c thc hin qua u o (visual
sensor ) v c t trong h kn vi vic nhn dng vt th trong thi gian thc. M hnh iu
khin vi cc php o nhn trong mch phn hi kn c gi l h visual sensor system. Vi
h visual sensor system c phn ra hai nhm:
. iu khin Robot trn c s camera thng qua vic xc lp v tr ca vt th [1], [2],
[3]
. iu khin Robot vi camera nhm xc lp cc tnh cht ca vt th [1], [2],[3]

3. Bi ton iu khin Robot vi qu trnh nhn dng ng

3.1 D bo v v tr v hng ca vt th

1
1

2
1

4
5

Adapte

Adapte 1
Ngun in

11

Phn mm

1 Keyboard

Hnh 1 S h th nghim
Cc thng tin t cc u o c chnh xc ho thng qua cc php nhn dng. Cc vt
th chuyn ng c xc nh bng cc phng php nh tri (optic flow method), y
khng xt c th cc im nh m v tr v tc nh coi nh l cc gi tr gin tip. Nu v tr
ca vt th chuyn ng c xc nh t nh, th gi tr v tr khi tnh ton l gi tr c
ca mt vi thi im ct mu trc . Do c th c c gi tr tnh ton v tr v tc
mt cch chnh xc, hay ta cn gi l v tr v tc ca vt th tng i vi camera, trong
trng hp ny chng ta cn phi xy dng mt m hnh d bo. Thng s d bo c xc
nh thng qua chuyn ng ca vt th. T c th thit k c qu o theo thi gian thc
t ca chuyn ng Robot v thit k thut iu khin
Gi s vt th vi v tr v tc c xc nh lin tc. Nu thi im kTV c v tr l
0
P (kTV) v hng ca vt th l 0(kTV), nh vy chuyn ng ca vt th trong qu trnh nhn
thy qua cc thi im kTV (k = 1 ...) c xc nh
W0(kTV)= [P0x(kTv), P0y(kTv), P0z(kTv), 0x(kTv),
0y(kTv), 0z(kTv)]T
Vi TV
x,y,z
T....

(1)

thi gian quan st

l to Cc ca h
miu t chuyn v

y c th hiu l:
Vector v tr bao gm v tr v hng ca Robot

Vector tc bao gm php quay v php tnh tin

T vic xc nh v tr vt th qua nh. Cc gi tr o c s dng cho vic iu khin
l v tr c c lng thi im Tv + mTc thng qua cc thng tin thi im (k -1)Tv vi
Tc l chu k ct mu ca vng iu khin (Tv >Tc) v m = 1....T/Tv l s nguyn
kTV+TC

(k-1)TV

kTV+2TC

(k+1)TV

kTV

Hnh 2 Qu trnh ct mu
Tc chuyn ng vt th c tnh ton qua v tr tm im ca vt th c xc
nh thi im hin ti v thi im trc . Gi v0(kTv) l vn tc ca vt th thi im k
Tc c xc nh bng
v0(kTv) = {W0(kTv) - W0[(k-1)Tv]}/Tv

(2)

Vi v0(kTv) tc thi im kTv c th vit di dng sau

v0(kTv) = [v0x(kTv) v0y(kTv) v0z(kTv), 0x(kTv) 0y(kTv) 0z(kTv) ]T
3 thnh phn u l tc chuyn ng thng.
3 thnh phn sau l tc chuyn ng quay.
Vi qu trnh chuyn ng ca vt th, tc trong (2) c xc nh thng qua m
hnh AR v m hnh AR c chn nh sau theo [9]:
v o ( kT

) = A

(z

) v 0 ( kT

) + e 0 ( kT

(3)

-1

vector e (kTv) l sai s m hnh v ma trn A (z ) c nh ngha

(4)
Ao ( z 1 ) = A10 z 1 + A20 z 2 + ... + An0 z n
0
-1
Ma trn vung A i , i=1,2,...,n cha cc gi tr cha bit, z l ton t tr lin quan n
chu k ct mu quan st v z-1v0(kTi)=v0[(k-1)T] ; n- s nguyn l ln ca Ma trn

M 0 ( k 1) = ( v 0 ( k 1)) T ( v 0 ( k 2 )) T .....( v 0 ( k n )) T

0 = [A10 A 20 ... A

] = [

0 T
n

0
1

20 ... 60 ]

i0 = [a i101 ... a i106 ... a in10 ... a in60 ]

(5)
(6
(7)

vi i=1,2,...,6
T- chu k ct mu
c lng cc thng s cha bit ca (3) ta s dng phng php trong [5]

v 0 (k ) = ( 0 )T M 0 (k 1) + e0 (k )
vi phng php bnh phng ti thiu, c lng gi tr vector 0 trong [5]

(8)

0
( k ) = 0 ( k 1) + P 0 ( k ) * M 0 ( k 1)( v i0 ( k ) 0 ( k 1) M 0 ( k 1))
j
i

0< 0

T
P 0 ( k 1) M 0 ( k 1) M 0 ( k 1) P 0 ( k 1)
1 0

P
k
(
1
)

T
0
0 + M 0 ( k 1) P 0 ( k 1) M 0 ( k 1)
1

P 0 (k ) =

(9 )

V tr vt th c th vit t (2)

[kT

+ mT C /( k 1)TV ] = W

[kT V

/( k 1)TV ] + v 0 [kT V /( k 1)TV ]mT C

(10 )

3.2 Thit k b iu khin:

3.2.1 Mt s vn c bn ca ng hc v tr
Mt vt th trong khng gian thng c m t bng su tham s sau nh [8]:
-

3 tham s v v tr (position): x, y, z hoc px, py, pz, nh biu din (1)

3 tham s v hng (orientation): x , y , z.

Xt robot trong h to cc vi loi robot ni khp (ch c khp quay m khng c

khp tnh tin) vi gi thit robot co 6 khp quay

o
a

n
x
Hnh 3 Rbt ch c kp quay
Cc bin trong 1 , 2 , 3 , 4 , 5 , 6 l cc gc chuyn ng tng i ca cc khp (so
vi v tr c).
Cc bin ngoi x, y, z, x , y , z l v tr hnh hc ca tay nm robot.
- x, y, z l to ca tay nm robot.
- x , y , z l cc gc quay ca tay nm robot quanh cc trc x, y, z.

S bin i qua li gia cc bin ngoi v bin trong th hin nh sau:

1

2
3
=

4
5
6

x
y

z

x
y
z

ng hc thun

ng hc ngc

Vn c bn ca bi ton gp vt trn sn l t v tr (x, y, z, x , y , z) xc nh ca vt, ta

phi tnh ton ng hc ngc xc nh cc gc quay (1 , 2 , 3 , 4 , 5 , 6) ca cc khp
v iu khin cc khp quay ca robot theo cc gc tnh ton di chuyn tay nm robot
n ng v tr v hng ca vt. Vic tnh ton b thng s cc gc quay (1 , 2 , 3 , 4 , 5 ,
6) ca cc khp tu thuc vo h phng trnh ng hc ngc xy dng c ti u hay
khng. Nhiu khi phi gii h phng trnh siu vit trong bi ton ngc vi thi gian rt lu,
i khi li gii khng t s hi t. iu ny khng m bo thi gian thc trong iu khin
tay my. Do vic gii bi ton ng hc ngc a ra c h phng trnh ti u c
ngha va khoa hc va thc tin. Khi p dng vo thc tin th tu theo tng loi robot m ta
c th xy dng c h phng trnh ng hc ngc theo cu trc h thng ca n.
3.2.2 Mt phng php tnh ton ng hc ngc cho robot 5 bc t do ca h
1. Cu trc h thng ca robot 5 bc t do

y1

y0 O1

R O

x0

y2

x1

93 mm

170 mm

z0

3
z2

10 mm

y4

y3
x3

x2
O2
z3

x4

O3

O4

z4

O hand

75 mm
28 mm
38 mm

70 mm

10 mm

55 mm

Hnh 4 Cu trc RBt 5 bc t do

2. ng hc thun robot 5 bc t do theo [8]
Ma trn bin i ng nht ca tay nm robot nhn trong h to gc R

nx
n
y
R
1
2
3
4
5
TH = R2 . R3 . R4 . R5 . RH =
nz
0

ox

ax

oy

ay

oz

az

px
py

pz
1

(11)

nx = -S12C5 - C12S34S5

ny = - C12C5 - S12S34S5

nz = C34S5

ox = S12S5 - C12S34C5

oy = -C12S5 - S12S34C5

oz = C34C5

ax = C12C34

ay = S12C34

az = S34

(12)

px = ( 125C34 + 38C3 +75 ) C12 + 18S12 + 93C1

py = ( 125C34 + 38C3 +75 ) S12 - 18C12 + 93S1
pz = 125S34 + 38S3 +160
Ma trn ng nht php quay ca tay nm robot RPY (z , y , x) v php tnh tin mt khong
(px , py , pz) c xc nh nh sau:
R

TH = Trans (z, pz ). Trans (y, py ). Trans (x, px ). Rot( z , z ). Rot( y , y ). Rot( x , x )

C z C y
S C
= z y
S y

C z S y S x S z C x
S z S y S x + C z C x
C y S x
0

C z S y C x + S z S x
S z S y C x C z S x
C y C x
0

px
p y
pz

(13)

T (11), (12), (13) ta rt ra c h phng trnh ng hc thun:

px = ( 125C34 + 38C3 +75 ) C12 + 18S12 + 93C1

(14)

py = ( 125C34 + 38C3 +75 ) S12 - 18C12 + 93S1

(15)

pz = 125S34 + 38S3 +160

(16)

z = atan2 ( ny , nx )

(17)

y = atan2 ( -nz , nxcosz + nysinz )

(18)

x = atan2 (axsinz - aycosz , oycosz - oxsinz )

(19)

3. ng hc ngc robot 5 bc t do [8]

5 = atan2( -Sy , CySx )

1 = atan2( S1 , C1 )
2 = 12 - 1

(20)

3 = atan2( S3 , C3 )
4 = 34 - 3

3.2.3. ng dng phng php nhn dng ng v tr, hng vo chng trnh
iu khin robot 5 bc t do trn c s camera
Trn c s nhn dng v tri vt th tnh trn sn t camera quan st, ta c c b
thng s ca vt th (x, y, z, x , y , z ). T dng cc phng trnh ng hc ngc
tnh mc 3.3 , ta xc nh c b gc quay (1 , 2 , 3 , 4 , 5) tng ng vi tng khp.
Sau chng trnh s iu khin cc khp quay ca robot a tay nm ti v tr mong
mun. Sau y l lu iu khin robot 5 bc t do gp vt th trn c s camera quan st:

Bt u
Camera quan st

x, y, z, x, y , z

V tr vt th nm
trong vng lm
vic ca robot ?

Khng

C
c lng v Tnh
ng hc ngc
iu khin cc khp
quay theo cc gc
1 , 2 , 3 , 4 , 5
Gp vt
Kt thc

3.2.4. Cc module chng trnh v cc kt qu t c

4. kt lun
Nhn chung thut ton t c cc yu cu ra. Qu trnh iu khin c thc hin
trn m hnh v cho kt qu tt. Cc kt qu th hin tnh ng n ca m hnh
Ti liu tham kho
[1] Hager. A tutrial on visual control , IEEE transactions on Robotics and Automation vol 12 ,
N0 5 -10-1996
[2] P. Papanikolopnous, Six degree freedom Hand/eye visual tracking with Uncertain
Parameters, IEEE transaction on Robotics and Automation. vol 11 No5 October 1995
[3] Koichi Hashimoto, Visual servoing with Hand/eye manipulator optimal control approach,
IEEE transcraction on Robotics and Automation vol 12 No 5 October 1996
[4] Le Ba Dung Ha Manh Dao, An Algorithm to Robot Control using a Camera, Military
workshop in Hanoi 12/2000
[5] L B Dng v cng s, Mt thut ton iu khin Rbt trn c s Camera, Tp ch Khoa
hc v cng ngh cc trng i hc s 34+35, nm 2002
[7] H Mnh o, Mt thut ton iu khin Rbt trn c s Camera, Tp ch Khoa hc v
cng ngh cc trng i hc s 34+35, nm 2002
[8] Bo co ti Trung tm KHTN & CNQG V iu khin R bt trn c s Camera,
nm 2001
[9] Vladimir Strejtr, Automatic theory, Praha 1980