- Trang Chủ
- Tự động hoá
- Xây dựng hệ thí nghiệm điều khiển robot với quá trình nhận dạng vật thể động trên cơ sở camera.
Xem mẫu
- X©y dùng hÖ thÝ nghiÖm §iÒu khiÓn R« bèt
víi qu¸ tr×nh nhËn d¹ng vËt thÓ ®éng
trªn c¬ së Camera
TS Lª B¸ Dòng
Ths Hµ M¹nh §µo
KS NguyÔn Thanh Tïng
ViÖn CNTT
lbdung@ioit.ncst.ac.vn
Tãm t¾t:
Bµi b¸o tr×nh bÇy qu¸ tr×nh x©y dùng, thiÕt kÕ hÖ thÝ nghiÖm ®iÒu khiÓn R« bèt víi qu¸ tr×nh nhËn d¹ng
vËt thÓ ®éng trªn c¬ së Camera. Víi qu¸ tr×nh nhËn d¹ng ®éng c¸c vÞ trÝ vµ h−íng cña vËt thÓ th«ng qua ¶nh
vËt thÓ tõ thêi ®iÓm c¾t mÉu, mét thuËt to¸n −íc l−îc vÞ trÝ ®−îc x©y dùng. Víi c¸c vÞ trÝ ®· −íc l−îng, thuËt
to¸n ®iÒu khiÓn ®−îc thiÕt kÕ ®Ó ®−a ®Çu tay n¾m ®Õn vÞ trÝ ®· ®−îc −íc l−îng vµ di chuyÓn vËt thÓ ®Õn
mét vÞ trÝ x¸c ®Þnh trong kh«ng gian lµm viÖc vµ kh«ng gian quan s¸t ®−îc cña 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
§iÒu khiÓn Robot trªn c¬ së Camera ®−îc thiÕt kÕ theo 1 sè yªu cÇu c¬ b¶n nh− sau vÒ
mÆt kÜ thuËt:
- Camera ®−îc ®Æt cè ®Þnh trong kh«ng gian lµm viÖc cña Robot
- Camera ®−îc treo t¹i ®Çu tay n¾m end- effector cña Robot.
Trong bµi b¸o nµy c¸c t¸c gi¶ ®Ò cËp ®Õn m« h×nh thÝ nghiÖm nhËn d¹ng vËt thÓ ®éng vµ
®iÒu khiÓn R«bèt víi Camera ®Æt cè ®Þnh trong kh«ng gian lµm viÖc cña R« bèt
2. M« h×nh ®iÒu khiÓn Robot víi víi Camera ®Æt cè ®Þnh trong
kh«ng gian lµm viÖc cña R« bèt
M« h×nh ®iÒu khiÓn Robot víi Camera ®Æt cè ®Þnh trong kh«ng gian lµm viÖc cña R«
bèt cho phÐp ta thùc hiÖn hÖ ®iÒu khiÓn kÝn víi qu¸ tr×nh quan s¸t (®o vÞ trÝ vµ h−íng cña vËt
thÓ th«ng qua thiÕt bÞ ®o quang). ë ®©y c¸c tÝn hiÖu ®o ®−îc thùc hiÖn qua ®Çu ®o (visual
sensor ) vµ ®−îc ®Æt trong hÖ kÝn víi viÖc nhËn d¹ng vËt thÓ trong thêi gian thùc. M« h×nh ®iÒu
khiÓn víi c¸c phÐp ®o nh×n trong m¹ch ph¶n håi kÝn ®−îc gäi lµ hÖ visual sensor system. Víi
hÖ visual sensor system ®−îc ph©n ra hai nhãm:
. §iÒu khiÓn Robot trªn c¬ së camera th«ng qua viÖc x¸c lËp vÞ trÝ cña vËt thÓ [1], [2],
[3]
. §iÒu khiÓn Robot víi camera nh»m x¸c lËp c¸c tÝnh chÊt cña vËt thÓ [1], [2],[3]
1
- 3. Bµi to¸n ®iÒu khiÓn Robot víi qu¸ tr×nh nhËn d¹ng ®éng
3.1 Dù b¸o vÒ vÞ trÝ vµ h−íng cña vËt thÓ
1
2 1
6 1 1
4 9
5 PhÇn mÒm
11
Adapte 1 Adapte
Nguån ®iÖn 1 Keyboard
H×nh 1 S¬ ®å hÖ thÝ nghiÖm
C¸c th«ng tin tõ c¸c ®Çu ®o ®−îc chÝnh x¸c ho¸ th«ng qua c¸c phÐp nhËn d¹ng. C¸c vËt
thÓ chuyÓn ®éng ®−îc x¸c ®Þnh b»ng c¸c ph−¬ng ph¸p ¶nh tr«i (optic flow method), ë ®©y
kh«ng xÐt cô thÓ c¸c ®iÓm ¶nh mµ vÞ trÝ vµ tèc ®é ¶nh coi nh− lµ c¸c gi¸ trÞ gi¸n tiÕp. NÕu vÞ trÝ
cña vËt thÓ chuyÓn ®éng ®−îc x¸c ®Þnh tõ ¶nh, th× gi¸ trÞ vÞ trÝ ®ã khi tÝnh to¸n ®· lµ gi¸ trÞ cò
cña mét vµi thêi ®iÓm c¾t mÉu tr−íc ®ã. Do ®ã ®Ó cã thÓ cã ®−îc gi¸ trÞ tÝnh to¸n vÞ trÝ vµ tèc
®é mét c¸ch chÝnh x¸c, hay ta cßn gäi lµ vÞ trÝ vµ tèc ®é cña vËt thÓ t−¬ng ®èi víi camera, trong
tr−êng hîp nµy chóng ta cÇn ph¶i x©y dùng mét m« h×nh dù b¸o. Th«ng sè dù b¸o ®−îc x¸c
®Þnh th«ng qua chuyÓn ®éng cña vËt thÓ. Tõ ®ã cã thÓ thiÕt kÕ ®−îc quü ®¹o theo thêi gian thùc
tÕ cña chuyÓn ®éng Robot vµ thiÕt kÕ thuËt ®iÒu khiÓn
Gi¶ sö vËt thÓ víi vÞ trÝ vµ tèc ®é ®−îc x¸c ®Þnh liªn tôc. NÕu ë thêi ®iÓm kTV cã vÞ trÝ lµ
P (kTV) vµ h−íng cña vËt thÓ lµ Φ0(kTV), nh− vËy chuyÓn ®éng cña vËt thÓ trong qu¸ tr×nh nh×n
0
thÊy qua c¸c thêi ®iÓm kTV (k = 1 ...) ®−îc x¸c ®Þnh
W0(kTV)= [P0x(kTv), P0y(kTv), P0z(kTv), Φ0x(kTv),
Φ0y(kTv), Φ0z(kTv)]T (1)
Víi TV thêi gian quan s¸t
x,y,z lµ to¹ ®é §Ò C¸c cña hÖ
T.... miªu t¶ chuyÓn vÞ
ë ®©y cã thÓ hiÓu lµ:
Vector vÞ trÝ bao gåm vÞ trÝ vµ h−íng cña Robot
2
- Vector tèc ®é bao gåm phÐp quay vµ phÐp tÞnh tiÕn
Tõ viÖc x¸c ®Þnh vÞ trÝ vËt thÓ qua ¶nh. C¸c gi¸ trÞ ®o ®−îc sö dông cho viÖc ®iÒu khiÓn
lµ vÞ trÝ ®−îc −íc l−îng ë thêi ®iÓm Tv + mTc th«ng qua c¸c th«ng tin ë thêi ®iÓm (k -1)Tv víi
Tc lµ chu kú c¾t mÉu cña vßng ®iÒu khiÓn (Tv >Tc) vµ m = 1....T/Tv lµ sè nguyªn
kTV+TC kTV+2TC
(k-1)TV kTV (k+1)TV
H×nh 2 Qu¸ tr×nh c¾t mÉu
Tèc ®é chuyÓn ®éng vËt thÓ ®−îc tÝnh to¸n qua vÞ trÝ t©m ®iÓm cña vËt thÓ ®−îc x¸c
®Þnh ë thêi ®iÓm hiÖn t¹i vµ thêi ®iÓm tr−íc ®ã. Gäi v0(kTv) lµ vËn tèc cña vËt thÓ ë thêi ®iÓm k
Tc ®−îc x¸c ®Þnh b»ng
v0(kTv) = {W0(kTv) - W0[(k-1)Tv]}/Tv (2)
Víi v0(kTv) tèc ®é ë thêi ®iÓm kTv cã thÓ viÕt d−íi d¹ng sau
v0(kTv) = [v0x(kTv) v0y(kTv) v0z(kTv), ω0x(kTv) ω 0y(kTv) ω 0z(kTv) ]T
3 thµnh phÇn ®Çu lµ tèc ®é chuyÓn ®éng th¼ng.
3 thµnh phÇn sau lµ tèc ®é chuyÓn ®éng quay.
Víi qu¸ tr×nh chuyÓn ®éng cña vËt thÓ, tèc ®é trong (2) ®−îc x¸c ®Þnh th«ng qua m«
h×nh AR vµ m« h×nh AR ®−îc chän nh− sau theo [9]:
−1
v o ( kT v ) = A 0
(z ) v 0 ( kT v ) + e 0 ( kT v ) (3)
o o -1
vector e (kTv) lµ sai sè m« h×nh vµ ma trËn A (z ) ®−îc ®Þnh nghÜa
Ao ( z −1 ) = A10 z −1 + A2 z −2 + ... + An z − n
0 0
(4)
0 -1
Ma trËn vu«ng A i , i=1,2,...,n chøa c¸c gi¸ trÞ ch−a biÕt, z lµ to¸n tö trÔ liªn quan ®Õn
chu kú cÊt mÉu quan s¸t vµ z-1v0(kTi)=v0[(k-1)T] ; n- sè nguyªn lµ ®é lín cña Ma trËn
[
M 0 ( k − 1) = ( v 0 ( k − 1)) T ( v 0 ( k − 2 )) T .....( v 0 ( k − n )) T ]
T
(5)
α 0 = [A10 A 20 ... A n ] = [α
0 T 0
1 α 2 ...α 6 ]
0 0 (6
α i0 = [a i10 ... a i10 ... a in10 ... a in60 ]
T
1 6 (7)
víi i=1,2,...,6
T- chu kú c¾t mÉu
§Ó −íc l−îng c¸c th«ng sè ch−a biÕt cña (3) ta sö dông ph−¬ng ph¸p trong [5]
3
- v 0 (k ) = (α 0 )T M 0 (k − 1) + e0 (k ) (8)
víi ph−¬ng ph¸p b×nh ph−¬ng tèi thiÓu, −íc l−îng gi¸ trÞ vector α0 trong [5]
∧ T
∧
∧
α 0
j
( k ) = α 0 ( k − 1) + P 0 ( k ) * M 0 ( k − 1)( v i0 ( k ) − α 0 ( k − 1) M 0 ( k − 1))
i i
P 0 (k ) =
1 0
P ( k − 1) −
[T
]
P 0 ( k − 1) M 0 ( k − 1) M 0 ( k − 1) P 0 ( k − 1)
(9 )
µ0
[ T
]
µ 0 + M 0 ( k − 1) P 0 ( k − 1) M 0 ( k − 1)
0< µ0 ≤1
VÞ trÝ vËt thÓ cã thÓ viÕt tõ (2)
∧ ∧ ∧
W 0
[kT + mT C /( k − 1)TV ] = W 0
[kT V /( k − 1)TV ] + v 0 [kT V /( k − 1)TV ]mT C (10 )
3.2 ThiÕt kÕ bé ®iÒu khiÓn:
3.2.1 Mét sè vÊn ®Ò c¬ b¶n cña ®éng häc vÞ trÝ
Mét vËt thÓ trong kh«ng gian th−êng ®−îc m« t¶ b»ng s¸u tham sè sau nh− [8]:
- 3 tham sè vÒ vÞ trÝ (position): x, y, z hoÆc px, py, pz, nh− biÓu diÔn ë (1)
- 3 tham sè vÒ h−íng (orientation): φx , φy , φz.
XÐt robot trong hÖ to¹ ®é §Òc¸c víi lo¹i robot nèi khíp (chØ cã khíp quay mµ kh«ng cã
khíp tÞnh tiÕn) víi gi¶ thiÕt robot co 6 khíp quay
z θ3
θ2
θ4
θ5
θ1 y θ6 o
a
n
x
H×nh 3 R«bèt chØ cã kíp quay
C¸c biÕn trong θ1 , θ2 , θ3 , θ4 , θ5 , θ6 lµ c¸c gãc chuyÓn ®éng t−¬ng ®èi cña c¸c khíp (so
víi vÞ trÝ cò).
C¸c biÕn ngoµi x, y, z, φx , φy , φz lµ vÞ trÝ h×nh häc cña tay n¾m robot.
- x, y, z lµ to¹ ®é cña tay n¾m robot.
- φx , φy , φz lµ c¸c gãc quay cña tay n¾m robot quanh c¸c trôc x, y, z.
4
- Sù biÕn ®æi qua l¹i gi÷a c¸c biÕn ngoµi vµ biÕn trong thÓ hiÖn nh− sau:
θ1 x
θ y
2 §éng häc thuËn
θ3 z
θ= φ
θ
4 §éng häc ng−îc x
θ5 φ y
θ6
φ z
VÊn ®Ò c¬ b¶n cña bµi to¸n g¾p vËt trªn sµn lµ tõ vÞ trÝ (x, y, z, φx , φy , φz) x¸c ®Þnh cña vËt, ta
ph¶i tÝnh to¸n ®éng häc ng−îc ®Ó x¸c ®Þnh c¸c gãc quay (θ1 , θ2 , θ3 , θ4 , θ5 , θ6) cña c¸c khíp
vµ ®iÒu khiÓn c¸c khíp quay cña robot theo c¸c gãc ®· tÝnh to¸n ®Ó di chuyÓn tay n¾m robot
®Õn ®óng vÞ trÝ vµ h−íng cña vËt. ViÖc tÝnh to¸n bé th«ng sè c¸c gãc quay (θ1 , θ2 , θ3 , θ4 , θ5 ,
θ6) cña c¸c khíp tuú thuéc vµo hÖ ph−¬ng tr×nh ®éng häc ng−îc ®· x©y dùng cã tèi −u hay
kh«ng. NhiÒu khi ph¶i gi¶i hÖ ph−¬ng tr×nh siªu viÖt trong bµi to¸n ng−îc víi thêi gian rÊt l©u,
®«i khi lêi gi¶i kh«ng ®¹t sù héi tô. §iÒu nµy kh«ng ®¶m b¶o thêi gian thùc trong ®iÒu khiÓn
tay m¸y. Do ®ã viÖc gi¶i bµi to¸n ®éng häc ng−îc ®−a ra ®−îc hÖ ph−¬ng tr×nh tèi −u cã ý
nghÜa võa khoa häc võa thùc tiÔn. Khi ¸p dông vµo thùc tiÔn th× tuú theo tõng lo¹i robot mµ ta
cã thÓ x©y dùng ®−îc hÖ ph−¬ng tr×nh ®éng häc ng−îc theo cÊu tróc hÖ thèng cña nã.
3.2.2 Mét ph−¬ng ph¸p tÝnh to¸n ®éng häc ng−îc cho robot 5 bËc tù do cña hÖ
1. CÊu tróc hÖ thèng cña robot 5 bËc tù do
z θ2 y1 y4 o
z0 θ1 y2
10 mm
y3 x4 n
93 mm x1
x3
170 mm
y0 O1 x2 H a
θ3 O4
θ z4
O hand
O2 O3
R O x0 z2 θ
z3
75 mm
28 mm
55 mm
10 mm
38 mm 70 mm
H×nh 4 CÊu tróc R«Bèt 5 bËc tù do
2. §éng häc thuËn robot 5 bËc tù do theo [8]
Ma trËn biÕn ®æi ®ång nhÊt cña tay n¾m robot nh×n trong hÖ to¹ ®é gèc R
5
- nx ox ax px
n oy ay py
TH = R2 . R3 . R4 . R5 . RH =
R 1 2 3 4 5 y
(11)
nz oz az pz
0
0 0 1
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 trËn ®ång nhÊt phÐp quay cña tay n¾m robot RPY (φz , φy , φx) vµ phÐp tÞnh tiÕn mét kho¶ng
(px , py , pz) ®−îc x¸c ®Þ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 C φ z Sφ y Sφ x − Sφ z C φ x C φ z Sφ y C φ x + Sφ z Sφ x px
Sφ C φ Sφ z Sφ y Sφ x + C φ z C φ x Sφ z Sφ y C φ x − C φ z Sφ x py
= z y (13)
− Sφ y Cφ y Sφ x Cφ y C φ x pz
0 0 0 1
Tõ (11), (12), (13) ta rót ra ®−îc hÖ ph−¬ng tr×nh ®éng häc thuËn:
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 , nxcosφz + nysinφz ) (18)
φx = atan2 (axsinφz - aycosφz , oycosφz - oxsinφz ) (19)
3. §éng häc ng−îc robot 5 bËc tù do [8]
θ5 = atan2( -Sφy , CφySφx )
θ1 = atan2( S1 , C1 )
θ2 = θ12 - θ1 (20)
θ3 = atan2( S3 , C3 )
θ4 = θ34 - θ3
6
- 3.2.3. øng dông ph−¬ng ph¸p nhËn d¹ng ®éng vÞ trÝ, h−íng vµo ch−¬ng tr×nh
®iÒu khiÓn robot 5 bËc tù do trªn c¬ së camera
Trªn c¬ së nhËn d¹ng vÞ tri vËt thÓ tÜnh trªn sµn tõ camera quan s¸t, ta cã ®−îc bé
th«ng sè cña vËt thÓ (x, y, z, φx , φy , φz ). Tõ ®ã dïng c¸c ph−¬ng tr×nh ®éng häc ng−îc ®·
tÝnh ë môc 3.3 , ta x¸c ®Þnh ®−îc bé gãc quay (θ1 , θ2 , θ3 , θ4 , θ5) t−¬ng øng víi tõng khíp.
Sau ®ã ch−¬ng tr×nh sÏ ®iÒu khiÓn c¸c khíp quay cña robot ®Ó ®−a tay n¾m tíi vÞ trÝ mong
muèn. Sau ®©y lµ l−u ®å ®iÒu khiÓn robot 5 bËc tù do g¾p vËt thÓ trªn c¬ së camera quan s¸t:
B¾t ®Çu
Camera quan s¸t
x, y, z, φx, φy , φz
VÞ trÝ vËt thÓ n»m Kh«ng
trong vïng lµm
viÖc cña robot ?
Cã
¦íc l−îng vµ TÝnh
®éng häc ng−îc
§iÒu khiÓn c¸c khíp
quay theo c¸c gãc
θ1 , θ2 , θ3 , θ4 , θ5
G¾p vËt
KÕt thóc
7
- 3.2.4. C¸c module ch−¬ng tr×nh vµ c¸c kÕt qu¶ ®¹t ®−îc
8
- 4. kÕt luËn
Nh×n chung thuËt to¸n ®· ®¹t ®−îc c¸c yªu cÇu ®Ò ra. Qu¸ tr×nh ®iÒu khiÓn ®· ®−îc thùc hiÖn
trªn m« h×nh vµ ®· cho kÕt qu¶ tèt. C¸c kÕt qu¶ ®· thÓ hiÖn tÝnh ®óng ®¾n cña m« h×nh
Tµi liÖu tham kh¶o
[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¸ Dòng vµ céng sù, Mét thuËt to¸n ®iÒu khiÓn R«bèt trªn c¬ së Camera, T¹p chÝ Khoa
häc vµ c«ng nghÖ c¸c tr−êng §¹i häc s« 34+35, n¨m 2002
[7] Hµ M¹nh §µo, Mét thuËt to¸n ®iÒu khiÓn R«bèt trªn c¬ së Camera, T¹p chÝ Khoa häc vµ
c«ng nghÖ c¸c tr−êng §¹i häc s« 34+35, n¨m 2002
[8] B¸o c¸o ®Ò tµi Trung t©m KHTN & CNQG VÒ ®iÒu khiÓn R« bèt trªn c¬ së Camera,
n¨m 2001
[9] Vladimir Strejtr, Automatic theory, Praha 1980
9
nguon tai.lieu . vn
