Mục lục
I. Mở đầu 3
II. Cơ cấu RBSS 3 chân 6
2.1. Robot song song 3 chân RPS 7
2.1.1 Kết cấu hình học 7
2.1.2. Bậc tự do 8
2.1.3. Hệ trục tọa độ 9
2.1.4. Bài toán vị trí 11
2.1.5. Ví dụ tính toán 18
2.2. Robot song song 3 RSS 22
2.2.1 Kết cấu hình học 22
2.2.2. Bậc tự do 22
2.2.3. Hệ trục tọa độ và ký hiệu 23
2.2.4. Ph-ơng trình liên kết 24
2.2.5. Bài toán động học ng-ợc 26
2.2.6. Bài toán động học thuận 26
2.3. Robot song song phẳng 3 chân 26
2.4. Đồ gá gia công vỏ hộp động cơ xe máy 28
III. Robot song song RBSS 29
3.1. Giới thiệu chung 30
3.2. Thiết kế kết cấu và lập trình điều khiển chuyển động của robot 30
3.3. Sơ đồ hệ thống điều khiển 33
3.4. Ch-ơng trình phần mềm điều khiển RBSS – 322 35
IV. Robot “Nhện n-ớc” 56
4.1. Giới thiệu chung 57 4.2. Mô tả Robot “Nhện n-ớc” 58
4.3. Tính toán xây dựng ch-ơng trình điều khiển Robot “Nhện n-ớc” 60
4.4. Mô phỏng hoạt hình Robot “Nhện n-ớc” 60
V. Mô phỏng RBSS dùng làm đồ gá CNC 61
5.1. Mô phỏng RBSS chân RPS 62
5.2. Robot song song 3 RSS 72
VI. Kết luận 78
Tài liệu tham khảo 80
123 trang |
Chia sẻ: banmai | Lượt xem: 1861 | Lượt tải: 0
Bạn đang xem trước 20 trang tài liệu Nghiên cứu thiết kế, chế tạo các robot thông minh phục vụ cho các ứng dụng quan trọng - Nhóm sản phẩm đồ gá Công nghệ cao, để xem tài liệu hoàn chỉnh bạn click vào nút DOWNLOAD ở trên
åm cã 1 khíp quay R, 1 khíp
tÞnh tiÕn P vµ 1 khíp cÇu S), th−êng ®−îc thiÕt kÕ ®Ó mang ph«i gia c«ng hay
mang c«ng cô ®Ó gia c«ng. Ba ch©n víi chiÒu dµi cã thÓ thay ®æi ®−îc ®iÒu
khiÓn bëi c¸c ®éng c¬ sÏ dÉn ®éng cho bÖ di ®éng mang ph«i hay c«ng cô
chuyÓn ®éng theo quÜ ®¹o x¸c ®Þnh tr−íc. Hai ®Çu cña c¸c ch©n nµy ®−îc
liªn kÕt víi ®Õ cè ®Þnh vµ bÖ di ®éng b»ng c¸c khíp cÇu. ¦u ®iÓm cña lo¹i
R«-bèt nµy lµ khèi l−îng nhá, cÊu tróc gän nhÑ, ®é cøng v÷ng cao, cã 3 bËc
tù do vµ ®é chÝnh x¸c cao.
TÊt c¶ c¸c thµnh phÇn c¬ khÝ ®−îc lùa chän vµ thiÕt kÕ cµng nhá gän
cµng tèt vµ kh«ng cã khe hë theo chiÒu däc trôc cña c¸c ch©n, c¸c ch©n ®−îc
®iÒu khiÓn cña R«-bèt ®−îc dÉn ®éng b»ng c¸c c¬ cÊu chÊp hµnh tuyÕn tÝnh.
H×nh 2.1 m« t¶ s¬ ®å cña r«-bèt nµy.
- Chi tiÕt 1 : Bµn di ®éng cã 3 bËc tù do trong kh«ng gian, trong tr−êng hîp
cô thÓ ë ®©y lµ phÇn bÒ mÆt dïng ®Ó g¸ dông cô c¾t kim lo¹i (®Çu dao
phay,...) hoÆc l¾p ®å g¸ ph«i (th−íc chia ®é, kÑp ph«i gia c«ng ...) cã d¹ng
tam gi¸c (th−êng lµ tam gi¸c ®Òu). Trªn bµn di ®éng sÏ l¾p ®Æt c¸c lo¹i ®å g¸
®Ó kÑp chi tiÕt hoÆc l¾p ®Æt côm ®éng c¬ - ®µi dao gia c«ng. Bµn ®−îc thiÕt
kÕ cã c¸c lç, chèt ®Þnh vÞ ®Ó l¾p ®å g¸. §å g¸ ®−îc l¾p chÆt trªn bµn di ®éng
b»ng c¸c bul«ng.
8
H×nh 4.2.1: C¬ cÊu chÊp hµnh song song 3 RPS
- Chi tiÕt 2: Lµ mét phÇn cña chi tiÕt thanh tr−ît lång, èng tr−ît trong.
TÊt c¶ c¸c èng tr−ît trong cã d¹ng thanh trô ®Æc. Khíp tr−ît ®−îc truyÒn
®éng b»ng c¬ cÊu chÊp hµnh sö dông ®éng c¬ servo, bé truyÒn ®éng vµ côm
c¬ cÊu trôc vÝt - ®ai èc bi. C¸c ch©n cña R«-bèt ®−îc nèi víi bÖ di ®éng vµ ®Õ
cè ®Þnh b»ng c¸c khíp cÇu 6.
- Chi tiÕt 3 : Lµ mét phÇn cña chi tiÕt thanh tr−ît lång, èng tr−ît
ngoµi. TÊt c¶ c¸c èng tr−ît ngoµi cã d¹ng h×nh trô rçng.
- Chi tiÕt 4 : Khíp quay, nèi ch©n víi ®Õ cè ®Þnh.
- Chi tiÕt 5 : MÆt ®Õ cè ®Þnh, cã d¹ng tÊm ph¼ng trßn. BÖ cè ®Þnh ®−îc
l¾p ®Æt trªn bµn g¸ chi tiÕt cña m¸y phay hoÆc cã thÓ ®−îc l¾p ®Æt cè ®Þnh
trªn mét vËt kh¸c. Trªn ®Õ cè ®Þnh cã gia c«ng c¸c lç phôc vô, viÖc cè ®Þnh
®Õ trªn bµn g¸ hoÆc c¸c vËt kh¸c b»ng c¸c bul«ng. Trªn ®Õ cè ®Þnh cßn ®−îc
gia c«ng c¸c r·nh ®Þnh vÞ phôc vô c«ng t¸c c¨n chØnh, l¾p ®Æt r«-bèt.
- Chi tiÕt 6 : Khíp cÇu, nèi ch©n víi bµn m¸y di ®éng.
2.1.2. BËc tù do
Sö dông c«ng thøc:
9
1
( 1)
j
i b
i
F n j f fλ
=
= − − + −∑
Víi 6, 8, 9, 0n j fbλ = = = = , ba khíp quay, ba khíp l¨ng trô, vµ ba
khíp cÇu, ta cã:
6(8 9 1) (3.1 3.1 3.3) 0 3F = − − + + + − =
Nh− vËy bµn di ®éng sÏ chuyÓn ®éng víi ba bËc tù do trong kh«ng
gian.
2.1.3. HÖ trôc to¹ ®é
Trªn h×nh 2.2 m« t¶ s¬ ®å ®éng häc cña r«-bèt song song 3 RPS
B3
1B
A1
A2
3A
B2
O
P
z
y
x 1
1
2z
3z
0x
x 3
z
y
x 2
α1 α2
α3
0
0
z
x
H×nh 4.2.2: S¬ ®å ®éng häc cña robot song song 3 RPS
Do yªu cÇu cña kÕt cÊu Robot nªn AiBi ⊥Zi (c¸c trôc quay)
O vµ P lµ träng t©m cña hai tam gi¸c A1A2A3 vµ B1B2B3.
Ta ®Æt c¸c hÖ täa ®é:
{Ox0y0z0} : HÖ cè ®Þnh.
{Pxyz} : HÖ täa ®é ®éng g¾n liÒn víi bµn m¸y ®éng.
10
{Aixiyizi}(i=1,2,3) : HÖ ®éng g¾n víi ch©n thø i.
Trong ®ã i i ix A B≡
uuuur
vµ zi ≡ trôc quay, cßn yi x¸c ®Þnh theo tam diÖn
thuËn (hay qui t¾c bµn tay ph¶i).
Ta ®−a thªm vµo 3 täa ®é suy réng iα (i=1,2,3) nh− h×nh vÏ.
0i iz xα =
Sö dông c¸c ký hiÖu:
ARB : Ma trËn cosin chØ h−íng cña hÖ {Pxyz} so víi hÖ cè ®Þnh
{Ox0y0z0}.
ARi : Ma trËn cosin chØ h−íng cña hÖ {Aixiyizi} so víi hÖ cè ®Þnh
{Ox0y0z0}.
ia : Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm Ai trªn hÖ cè ®Þnh.
ib : Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm Bi trªn hÖ cè ®Þnh.
B
ib : Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm Bi trªn hÖ ®éng.
P: Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm P trªn hÖ cè ®Þnh.
di : §é dµi ch©n thø i.
Trong ®ã :
C¸c ma trËn A iR cã thÓ biÓu diÔn d−íi d¹ng:
A
iR =
01 1 01 2 01 3
02 1 02 2 02 3
03 1 03 2 03 3
i i i
i i i
i i i
e e e e e e
e e e e e e
e e e e e e
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
r r r r r r
r r r r r r
r r r r r r
(i=1,2,3) (2.1)
01 02 03, ,e e e
r r r
: Lµ 3 vector ®¬n vÞ trªn c¸c trôc Ox0, Oy0,Oz0.
1 2 3, ,i i ie e e
r r r
: Lµ 3 vector ®¬n vÞ trªn c¸c trôc Aixi, Aiyi, Aizi (i=1,2,3).
C¸c phÇn tö cña ma trËn nµy tïy theo kÕt cÊu cña bµn ®Õ cè ®Þnh, lµ
hµm cña gãc iα .
11
Ma trËn A BR cã thÓ biÓu diÔn d−íi d¹ng 3 phÐp quay Roll, Pitch, Yaw
t−¬ng øng víi 3 gãc ,ϕ θ vµ ψ .
ia vµ B ib : X¸c ®Þnh ®−îc tõ h×nh d¸ng, kÕt cÊu cña R«-bèt.
2.1.4. Bµi to¸n vÞ trÝ
Víi c¸ch ®Æt vµ biÓu diÔn c¸c ®¹i l−îng nh− trªn, vÞ trÝ cña ®iÓm Bi
trªn hÖ cè ®Þnh cã thÓ biÓu diÔn d−íi d¹ng:
i i i iOB OA A B= +
uuur uuur uuuur
(i=1,2,3) (2.2)
vµ : i iOB OP PB= +
uuur uuur uuur
(i=1,2,3) (2.3)
Hay d−íi d¹ng ®¹i sè:
= +i ib a A .iR 0
0
id⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(i=1,2,3) (2.4)
vµ : = +ib P A BR .B ib (i=1,2,3) (2.5)
KÕt hîp hai ph−¬ng tr×nh trªn ta cã:
+P A BR .B ib = +ia A iR . 0
0
id⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(i=1,2,3) (2.6)
Trong ®ã:
P 1, 2 3,
T
p p p⎡ ⎤= ⎣ ⎦ ; B ib = , ,
T
ix iy izb b b⎡ ⎤⎣ ⎦ ; =ia 1, 2 3,
T
i i ia a a⎡ ⎤⎣ ⎦
A BR =
x x x
y y y
z z z
u v w
u v w
u v w
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
; A iR =
ix ix ix
iy iy iy
iz iz iz
u v w
u v w
u v w
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(i=1,2,3) (2.7)
12
X0
1A
3A
2A
1Z
0Y
2Z
3Z
O
3β
1β
2βη2
3
η
H×nh 4.2.3
C¸c ma trËn cosin chØ h−íng A iR ®−îc x¸c ®Þnh bëi c¸c phÐp quay liªn tiÕp:
Ma trËn A 1R ®−îc x¸c ®Þnh bëi c¸c phÐp quay sau :
• Quay mét gãc ( 1/ 2π β− ) quanh trôc z.
• Quay quanh trôc x mét gãc ( / 2π ).
• Quay quanh trôc z mét gãc ( 1/ 2π α+ ).
Ma trËn A 2R ®−îc x¸c ®Þnh bëi c¸c phÐp quay sau :
• Quay mét gãc 2η quanh trôc z.
• Quay mét gãc ( 2/ 2π β− ) quanh trôc z.
• Quay quanh trôc x mét gãc ( / 2π ).
• Quay quanh trôc z mét gãc ( 2/ 2π α+ ).
Ma trËn A 3R ®−îc x¸c ®Þnh bëi c¸c phÐp quay sau :
• Quay mét gãc 3η quanh trôc z.
• Quay mét gãc ( 3/ 2π β− ) quanh trôc z.
13
• Quay quanh trôc x mét gãc ( / 2π ).
• Quay quanh trôc z mét gãc ( 3/ 2π α+ ).
NÕu ta coi iAα lµ ma trËn cosin chØ h−íng cña 2 phÐp quay liªn tiÕp
quanh trôc x mét gãc ( / 2π ) vµ quanh trôc z mét gãc ( / 2 iπ α+ ) th× :
α =iA
1 0 0 sin cos 0
0 0 1 . cos sin 0
0 1 0 0 0 1
i i
i i
α α
α α
− −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥− −⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦
α =iA
sin cos 0
0 0 1
cos sin 0
i i
i i
α α
α α
− −⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥−⎣ ⎦
(2.8)
§Æt : ( / 2 )iπ β− = iγ ;
VËy ta cã :
A
1 1 1( ). αγ= zR A A
A
1 =R
1 1 1 1
1 1
1 1
cos sin 0 sin cos 0
sin cos 0 0 0 1
0 0 1 cos sin 0
γ γ α α
γ γ
α α
− − −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦
1 =A R
1 1 1 1 1
1 1 1 1 1
1 1
cos sin cos cos sin
sin sin sin cos cos
cos sin 0
γ α γ α γ
γ α γ α γ
α α
− −⎡ ⎤⎢ ⎥−⎢ ⎥⎢ ⎥⎣ ⎦
2 2 2 2( ). ( ).
A
z zR A A Aαη γ=
A
2 =R
2 2 2 2 2 2
2 2 2 2
2 2
cos sin 0 cos sin 0 sin cos 0
sin cos 0 sin cos 0 0 0 1
0 0 1 0 0 1 cos sin 0
η η γ γ α α
η η γ γ
α α
− − − −⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦ ⎣ ⎦
14
2 =A R
2 2 2 2 2 2 2 2 2 2 2 2 2 2
2 2 2 2 2 2 2 2 2 2 2 2 2 2
2 2
( ) ( )
( ) ( )
0
η γ η γ α η γ η γ α η γ η γ
η γ η γ α η γ η γ α η γ η γ
α α
− − + − −⎡ ⎤⎢ ⎥− + − + − +⎢ ⎥⎢ ⎥−⎣ ⎦
C C S S S C S S C C C S S C
S C C S S S C C S C S S C C
C S
Trong ®ã : C 3η = cos 3η ; S 3η = sin 3η ; C 3γ = cos 3γ ; S 3γ = sin 3γ
C 2α =cos 2α ; S 2α = sin 2α
3 3 3 3( ). ( ).
A
z zR A A Aαη γ= −
3 =A R
3 3 3 3 3 3
3 3 3 3
3 3
cos sin 0 cos sin 0 sin cos 0
sin cos 0 sin cos 0 0 0 1
0 0 1 0 0 1 cos sin 0
η η γ γ α α
η η γ γ
α α
− − −⎡ ⎤⎡ ⎤ ⎡ ⎤ ⎢ ⎥⎢ ⎥ ⎢ ⎥− ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ −⎣ ⎦ ⎣ ⎦ ⎣ ⎦
3 =A R
3 3 3 3 3 3 3 3 3 3 3 3 3 3
3 3 3 3 3 3 3 3 3 3 3 3 3 3
3 3
( ) ( )
( ) ( )
0
η γ η γ α η γ η γ α η γ η γ
η γ η γ α η γ η γ α η γ η γ
α α
− + − + − +⎡ ⎤⎢ ⎥− − + − − + +⎢ ⎥⎢ ⎥⎣ ⎦
C C S S S C C S S C C S S C
S C C S S S C C S C S S C C
C S
Trong ®ã : C 3η = cos 3η ; S 3η = sin 3η ; C 3γ = cos 3γ ; S 3γ = sin 3γ ;
C 3α = cos 3α ; S 3α = sin 3α
Ta thÊy c¸c thµnh phÇn cña c¸c ma trËn A iR chØ chøa c¸c Èn lµ c¸c gãc iα
cßn 2η , 3η vµ iβ ®· biÕt do kÕt cÊu cña r«-bèt.
Ta viÕt l¹i ph−¬ng tr×nh (2.6) d−íi d¹ng ®¹i sè.
Chó ý: Do Ai thuéc mÆt ph¼ng X0Y0 nªn 3ia = 0 (i=1,2,3)
A1 trªn trôc X0 nªn 12 0a =
Vµ Bi thuéc mÆt ph¼ng X0Y0 nªn izb = 0 (i=1,2,3)
15
+Víi i =1:
1 1 11 1 1
2 1 1 1
3 1 1 1
.x x x
y x y
z x z
p u b a u d
p u b u d
p u b u d
⎧ + = +⎪ + =⎨⎪ + =⎩
Suy ra :
11 1 1 1
1
1 1 2
1
1 1 3
1
+ −⎧ =⎪⎪⎪ −=⎨⎪⎪ −=⎪⎩
x
x
x
y
y
x
z
z
x
a u d pu
b
u d p
u
b
u d pu
b
(2.9)
+Víi i=2
1 2 2 21 2 2
2 2 2 22 2 2
3 2 2 2 2
(a)
(b)
(c)
x x x y x
y x y y y
z x z y z
p u b v b a u d
p u b v b a u d
p u b v b u d
⎧ + + = +⎪ + + = +⎨⎪ + + =⎩
(2.10)
+Víi i=3
1 3 3 31 3 3
2 3 3 32 3 3
3 3 3 3 3
(a)
(b)
(c)
x x x y x
y x y y y
z x z y z
p u b v b a u d
p u b v b a u d
p u b v b u d
⎧ + + = +⎪ + + = +⎨⎪ + + =⎩
(2.11)
Thùc hiÖn c¸c phÐp biÕn ®æi sau:
((3.10 ) (3.11 ))
((3.10 ) (3.11 ))
((3.10 ) (3.11 ))
λ
λ
λ
−⎧⎪ −⎨⎪ −⎩
a a
b b
c c
Ta ®−îc:
1 1 21 2 2 31 3 3
2 1 22 2 2 32 3 3
3 1 2 2 3 3
( 1) ( ) ( )
( 1) ( ) ( )
( 1) )
λ λ λ
λ λ λ
λ λ λ
⎧ − + = + − +⎪ − + = + − +⎨⎪ − + = −⎩
x x x
y y y
z z z
p u a u d a u d
p u a u d a u d
p u u d u d
(2.12)
16
Víi 3
2
y
y
b
b
λ = , 1 2 3x xb bλ λ= −
Thay c¸c kÕt qu¶ cña hÖ (2.9) vµo hÖ (2.12) ta ®−îc:
11 1 1 1
1 1 21 2 2 31 3 3
1
1 1 2
2 1 22 2 2 32 3 3
1
1 1 3
3 1 2 2 3 3
1
( 1) ( ) ( )
( 1) ( ) ( )
( 1) )
x
x x
x
y
y y
x
z
z z
x
a u d pp a u d a u d
b
u d p
p a u d a u d
b
u d pp u d u d
b
λ λ λ
λ λ λ
λ λ λ
+ −⎧ − + = + − +⎪⎪⎪ −− + = + − +⎨⎪⎪ −− + = −⎪⎩
(2.13)
MÆt kh¸c, dùa vµo kÕt cÊu cña bµn di ®éng B ta cã :
B1
B2
3B
b1
3b
2b
H×nh 2.2.4
2
1 2 =
uuuur
B B 1 2 1 2( ) ( )
Tb b b b− − =b32
2
1 3 =
uuuur
B B 1 3 1 3( ) ( )
Tb b b b− − =b22
2
2 3 =
uuuur
B B 2 3 2 3( ) ( )
Tb b b b− − =b12
víi : = +i ib a A .iR 0
0
id⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(i=1,2,3)
17
1⇒ =b
11 1 1
1 1
1 1
x
y
z
a u d
u d
u d
+⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
; 2 =b
21 2 2
22 2 2
2 2
x
y
z
a u d
a u d
u d
+⎡ ⎤⎢ ⎥+⎢ ⎥⎢ ⎥⎣ ⎦
; 3 =b
31 3 3
32 3 3
3 3
x
y
z
a u d
a u d
u d
+⎡ ⎤⎢ ⎥+⎢ ⎥⎢ ⎥⎣ ⎦
11 21 1 1 2 2 11 21 1 1 2 2
2
1 1 22 2 2 1 1 22 2 2 3
1 2 2 1 2 2
.
T
x x x x
y y y y
z z z z
a a u d u d a a u d u d
u d a u d u d a u d b
u u d u u d
− + − − + −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⇒ − − − − =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦
11 31 1 1 3 3 11 31 1 1 3 3
2
1 1 32 3 3 1 1 32 3 3 2
1 1 3 3 1 1 3 3
.
T
x x x x
y y y y
z z z z
a a u d u d a a u d u d
u d a u d u d a u d b
u d u d u d u d
− + − − + −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥− − − − =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦
21 31 2 2 3 3 21 31 2 2 3 3
2
22 32 2 2 3 3 22 32 2 2 3 3 1
2 2 3 3 2 2 3 3
.
T
x x x x
y y y y
z z z z
a a u d u d a a u d u d
a a u d u d a a u d u d b
u d u d u d u d
− + − − + −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥− + − − + − =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦
Hay :
2 2 2 2
11 21 1 1 2 2 1 1 22 2 2 1 1 2 2 3
2 2 2 2
11 31 1 1 3 3 1 1 32 3 3 1 1 3 3 2
2 2 2 2
21 31 2 2 3 3 22 32 2 2 3 3 2 2 3 3 1
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( )
x x y y z z
x x y y z z
x x y y z z
a a u d u d u d a u d u d u d b
a a u d u d u d a u d u d u d b
a a u d u d a a u d u d u d u d b
⎧ − + − + − − + − =⎪⎪ − + − + − − + − =⎨⎪ − + − + − + − + − =⎪⎩
KÕt hîp víi hÖ (2.13) ta cã hÖ 6 ph−¬ng tr×nh, 6 Èn:
(2.14)
18
HÖ ph−¬ng tr×nh (2.14) chøa 9 Èn sè 1 2 3 1 2 3 1 2 3, , , , , , , ,d d d p p pα α α . C¸c
thµnh phÇn , ,ix iy izu u u ®· x¸c ®Þnh ®−îc, c¸c thµnh phÇn , ,x y zu u u x¸c ®Þnh
theo (2.9)
Khi gi¶i quyÕt bµi to¸n ®éng häc thuËn hay ng−îc, ta biÕt tr−íc ®−îc 3
Èn. C«ng viÖc cßn l¹i chØ ph¶i gi¶i hÖ 6 ph−¬ng tr×nh 6 Èn sè.
a. Bµi to¸n ®éng häc thuËn
Bµi to¸n ®éng häc thuËn lµ bµi to¸n biÕt ®é dµi c¸c ch©n di (i=1,2,3), ta
ph¶i t×m vÞ trÝ cña bµn m¸y ®éng P vµ ma trËn ARB.
Theo phÇn trªn ta thay c¸c gi¸ trÞ di (i=1,2,3) vµo hÖ (4.14), ta sÏ ®−îc
hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ : 1 2 3 1 2 3, , , , ,p p pα α α
Chó ý lµ 3 ph−¬ng tr×nh sau cña hÖ (2.14) chØ chøa di vµ iα nªn viÖc
gi¶i 6 ph−¬ng tr×nh ®−îc ®¬n gi¶n l¹i cßn gi¶i hÖ 3 ph−¬ng tr×nh víi 3 Èn lµ
iα . Sau ®ã thay c¸c gi¸ trÞ cña di vµ iα vµo 3 ph−¬ng tr×nh ®Çu ta sÏ tÝnh
®−îc c¸c gi¸ trÞ cña P.
C¸c gi¸ trÞ cßn l¹i tÝnh ®−îc b»ng c¸ch thay trùc tiÕp vµo c¸c ph−¬ng
tr×nh (2.9), (2.10), (2.11).
b. Bµi to¸n ®éng häc ng−îc
Bµi to¸n ®éng häc ng−îc lµ bµi to¸n biÕt vÞ trÝ bµn m¸y ®éng P, ta ph¶i
t×m ®é dµi c¸c ch©n di (i=1,2,3) vµ c¸c gãc iα (i=1,2,3) .
T−¬ng tù nh− c¸ch lµm ®èi víi bµi to¸n ®éng häc thuËn ta thay c¸c
gi¸ trÞ cña P vµo hÖ (2.14), ta sÏ ®−îc hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ :
1 2 3 1 2 3, , , , ,d d dα α α .
C¸c gi¸ trÞ cßn l¹i tÝnh ®−îc b»ng c¸ch thay trùc tiÕp vµo c¸c ph−¬ng
tr×nh (2.9), (2.10), (2.11).
2.1.5. VÝ dô tÝnh to¸n
Ta tÝnh to¸n cho mét r«-bèt song song 3 RPS cô thÓ :
- Tam gi¸c A1A2A3 vµ tam gi¸c B1B2B3 lµ c¸c tam gi¸c ®Òu.
19
- PB1 = h; OA1 = g; 2 3 2 /3η η π= =
- Do kÕt cÊu cña c¬ cÊu ta cã i i iz A B⊥
- Trôc i iz OA⊥ ⇒ / 2iβ π=
Khi ®ã c¸c ®¹i l−îng trong c«ng thøc (4.6) trë thµnh :
1
Bb = 0
0
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
h
; 2
Bb =
2
3
2
0
⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
h
h ; 3
B b =
2
3
2
0
⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
h
h (2.15)
1a = 0
0
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
g
; 2a =
2
3
2
0
⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
g
g ; 3a =
2
3
2
0
⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
g
g (2.16)
Do / 2iβ π= nªn iγ = ( / 2 )iπ β− = 0
Khi ®ã c¸c ma trËn cosin chØ h−íng ARi trë thµnh:
1
A R =
1 1 1 1 1
1 1 1 1 1
1 1
cos sin cos cos sin
sin sin sin cos cos
cos sin 0
γ α γ α γ
γ α γ α γ
α α
− −⎡ ⎤⎢ ⎥−⎢ ⎥⎢ ⎥⎣ ⎦
1
A R =
1 1
1 1
sin cos 0
0 0 1
cos sin 0
α α
α α
−⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(2.17)
20
2
A R =
2 2 2 2 2 2 2 2
2 2 2 2 2 2 2 2
2 2
1 3 1 3 1 3( cos sin )sin ( cos sin )cos sin cos
2 2 2 2 2 2
3 1 3 1 3 1( cos sin )sin ( cos sin )cos sin cos
2 2 2 2 2 2
cos sin 0
γ γ α γ γ α γ γ
γ γ α γ γ α γ γ
α α
⎡ ⎤+ − + −⎢ ⎥⎢ ⎥⎢ ⎥− + − − −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
2
A R =
2 2
2 2
2 2
1 1 3sin cos
2 2 2
3 3 1sin cos
2 2 2
cos sin 0
α α
α α
α α
⎡ ⎤− −⎢ ⎥⎢ ⎥⎢ ⎥− −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(2.18)
3
A R =
3 3 3 3 3 3 3 3
3 3 3 3 3 3 3 3
3 3
1 3 1 3 1 3( cos sin )sin ( cos sin ) sin cos
2 2 2 2 2 2
3 1 3 1 3 1( cos sin )sin ( cos sin )cos sin cos
2 2 2 2 2 2
cos sin 0
γ γ α γ γ α γ γ
γ γ α γ γ α γ γ
α α
⎡ ⎤− − + +⎢ ⎥⎢ ⎥⎢ ⎥+ − − −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
cos
3
A R =
3 3
3 3
3 3
1 1 3sin cos
2 2 2
3 3 1sin cos
2 2 2
cos sin 0
α α
α α
α α
⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥− −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(2.19)
Khi ®ã : 3
2
y
y
b
b
λ = = -1 ; 1 2 3x xb bλ λ= − = h;
Thay vµo hÖ (2.14) ta ®−îc :
21
1 2 2 3 3 1 1
2 2 2 3 3
3 1 1 2 2 3 3
2 2 2
1 1 2 2 1 2 1 2 1 2
13 ( sin sin ) sin
2
33 ( sin sin )
2
3 cos cos cos (4.20)
3 3 sin 3 sin sin sin
p d d d
p d d
p d d d
g gd gd d d d d
α α α
α α
α α α
α α α α
= + −
−= −
= + +
− − + + + 21 2 1 2
2 2 2 2
1 1 3 3 1 3 1 3 1 3 1 3 1 3
2 2 2 2
2 2 3 3 2 3 2 3 2 3 2 3 2 3
2 cos cos 3
3 3 sin 3 sin sin sin 2 cos cos 3
3 3 sin 3 sin sin sin 2 cos cos 3
d d h
g gd gd d d d d d d h
g gd gd d d d d d d h
α α
α α α α α α
α α α α α α
⎧⎪⎪⎪⎪⎪⎨⎪ − =⎪⎪ − − + + + − =⎪⎪ − − + + + − =⎩
a) Bµi to¸n ®éng häc thuËn
Bµi to¸n ®éng häc thuËn lµ bµi to¸n biÕt ®é dµi c¸c ch©n di (i=1,2,3),
ta ph¶i t×m vÞ trÝ cña bµn m¸y ®éng P vµ ma trËn ARB.
Theo phÇn trªn ta thay c¸c gi¸ trÞ di (i=1,2,3) vµo hÖ (2.20), ta sÏ ®−îc
hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ : 1 2 3 1 2 3, , , , ,p p pα α α
Chó ý lµ 3 ph−¬ng tr×nh sau cña hÖ (2.20) chØ chøa di vµ iα nªn viÖc
gi¶i 6 ph−¬ng tr×nh ®−îc ®¬n gi¶n l¹i cßn gi¶i hÖ 3 ph−¬ng tr×nh víi 3 Èn lµ
iα . Sau ®ã thay c¸c gi¸ trÞ cña di vµ iα vµo 3 ph−¬ng tr×nh ®Çu ta sÏ tÝnh
®−îc c¸c gi¸ trÞ cña P
C¸c gi¸ trÞ cßn l¹i tÝnh ®−îc b»ng c¸ch thay trùc tiÕp vµo c¸c ph−¬ng
tr×nh (2.9), (2.10), (2.11).
b) Bµi to¸n ®éng häc ng−îc
Bµi to¸n ®éng häc ng−îc lµ bµi to¸n biÕt vÞ trÝ bµn m¸y ®éng P, ta ph¶i
t×m ®é dµi c¸c ch©n di (i=1,2,3) vµ c¸c gãc iα (i=1,2,3) .
T−¬ng tù nh− c¸ch lµm ®èi víi bµi to¸n ®éng häc thuËn ta thay c¸c
gi¸ trÞ P vµo hÖ (2.20), ta sÏ ®−îc hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ :
1 2 3 1 2 3, , , , ,d d dα α α .
C¸c gi¸ trÞ cßn l¹i tÝnh ®−îc b»ng c¸ch thay trùc tiÕp vµo c¸c ph−¬ng tr×nh
(2.9), (2.10), (2.11).
22
2.2. Robot song song 3 RSS
2.2.1. KÕt cÊu h×nh häc
H×nh 4.2.5: KÕt cÊu h×nh häc cña Robot song song 3 RSS
Deltarca cÊu t¹o gåm ba ch©n, bµn di ®éng 12 vµ bµn cè ®Þnh 1. Mçi
ch©n gåm cã kh©u trªn, kh©u d−íi vµ n¨m khíp, trong ®ã cã bèn khíp cÇu 4,
6, 8, 10 (S) vµ mét khíp quay 2 (R). Kh©u AC ®−îc gäi lµ kh©u c¸nh tay,
kh©u BC ®−îc gäi lµ kh©u c¼ng tay. Kh©u AC nèi víi bµn cè ®Þnh bëi khíp
quay b¶n lÒ 2, kh©u BC nèi víi bµn di ®éng bëi hai khíp cÇu 8, 10 vµ nèi víi
kh©u AC bëi hai khíp cÇu 4, 6. Kh©u BC thùc chÊt lµ c¬ cÊu h×nh b×nh hµnh
gåm c¸c kh©u 5, 7, 9, 11 víi c¸c khíp lµ khíp cÇu chø kh«ng ph¶i lµ khíp
quay. Nh− vËy Deltarca cã tÊt c¶ 11 kh©u vµ 15 khíp, ®−îc gäi lµ r«-bèt
song song 3 RSS (H×nh 4.2.5).
2.2.2. BËc tù do
Sö dông c«ng thøc:
1
( 1)
j
i b
i
F n j f fλ
=
= − − + −∑
Víi 6, 11, 15, 6n j fbλ = = = = , 12 khíp cÇu vµ 3 khíp quay, ta cã:
6(11 15 1) (12.3 3.1) 6 3F = − − + + − =
23
Nh− vËy bµn di ®éng sÏ chuyÓn ®éng víi ba bËc tù do trong kh«ng
gian.
2.2.3. HÖ trôc täa ®é vµ kÝ hiÖu
HÖ trôc Oxyz g¾n víi t©m bµn cè ®Þnh, hÖ trôc Puvw g¾n víi t©m bµn
di ®éng. C¸c trôc x, y n»m trong bµn cè ®Þnh, c¸c trôc u, v n»m trong bµn di
®éng. T¹i mäi thêi ®iÓm x vµ y lu«n song song cïng chiÒu víi u vµ v.
Dïng hÖ trôc i i i iA x y z g¾n víi t©m cña khíp quay iA , sao cho ix n»m theo
h−íng kÐo dµi cña iOA vµ trôc iy däc theo trôc c¸c khíp quay, cßn trôc iz
song song víi z. Gãc iα ®o tõ x ®Õn ix lµ mét h»ng sè cña qu¸ tr×nh thiÕt kÕ
r«-bèt (H×nh 2.6). Gãc iϕ lµ gãc t¹o bëi trôc ix vµ i iAC .
H×nh 4.2.6: §Æt hÖ trôc täa ®é lªn r«-bèt
24
2.2.4. Ph−¬ng tr×nh liªn kÕt
§Ó thuËn tiÖn cho qu¸ tr×nh thµnh lËp hÖ ph−¬ng tr×nh liªn kÕt, ta quy
®Þnh mét sè ®iÓm sau:
Chän hÖ trôc cè ®Þnh lµ hÖ trôc Oxyz
Ta dïng ch÷ in nghiªng ®Ó kÝ hiÖu vÐc-t¬ ®¹i sè, vÝ dô ai lµ vÐc-t¬
®¹i sè biÓu diÔn täa ®é cña ®iÓm iA so víi hÖ täa ®é cè ®Þnh Oxyz. Cßn biP
lµ to¹ ®é cña ®iÓm iB trong hÖ täa ®é ®éng Puvw.
Khi nãi tíi täa ®é cña mét ®iÓm nµo ®ã mµ ta kh«ng nãi râ lµ xÐt
trong hÖ to¹ ®é cô thÓ nµo th× hiÓu r»ng ta ®ang xet täa ®é cña ®iÓm ®ã trong
hÖ täa ®é cè ®Þnh Oxyz.
Sau ®©y ta x¸c ®Þnh täa ®é cña tõng ®iÓm
Täa ®é ®iÓm iA (H×nh 4.2.7): tuú thuéc vµo kÝch th−íc bµn cè ®Þnh
[ ] [ ], , *cos( ), *sin( ),0 TT i iai xai yai zai ra raα α= = (2.21)
Täa ®é ®iÓm iC
[ ], , .Tci xci yci zci ai Ri ciA= = + (2.22)
Trong ®ã:
Ri lµ ma trËn cosin chØ h−íng cña hÖ trôc i i i iA x y z so víi hÖ Oxyz. §Ó
tÝnh Ri ta chØ viÖc xoay hÖ Oxyz quanh trôc z mét gãc iα , khi ®ã:
cos( ) sin( ) 0
( , ) sin( ) cos( ) 0
0 0 1
i i
i i i iR R z
α α
α α α
−⎡ ⎤⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦
(2.23)
ciA lµ täa ®é cña ®iÓm iC trong hÖ trôc i i i iA x y z (H×nh 4.2.4)
[ ]*cos( ),0, *sin( ) Ti iciA la laϕ ϕ= − (2.24)
25
H×nh 4.2.7 H×nh 4.2.8
Thay (2.23) vµ (2.24) vµo (2.22) ta ®−îc:
[ ]cos( )*( *cos( )),sin( )*( *cos( )), *sin( ) Ti i i i ici ra la ra la laα ϕ α ϕ ϕ= + + − (2.25)
X¸c ®Þnh täa ®é ®iÓm B
[ ], , *Tbi xbi ybi zbi p Rp biP= = + (2.26)
Trong ®ã:
[ ], , Tp px py pz= (2.27)
lµ vÐc-t¬ täa ®é cña ®iÓm P trong hÖ trôc täa ®é Oxyz
Rp lµ ma trËn cosin chØ h−íng cña hÖ Puvw so víi Oxyz. V× bµn di
®éng chuyÓn ®éng song ph¼ng so víi bµn cè ®Þnh nªn:
1 0 0
0 1 0
0 0 1
Rp
⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦
(2.28)
biP lµ täa ®é cña ®iÓm iB trong hÖ trôc täa ®é Puvw:
[ ] [ ], , *cos( ), *sin( ),0 TT i ibiP xbiP ybiP zbiP rb rbβ β= = (2.29)
Thay (2.27), (2.28), (2.29) vµo (4.26) ta ®−îc:
26
*cos( )
*sin( )
i
i
px rb
bi py rb
pz
α
α
+⎡ ⎤⎢ ⎥= +⎢ ⎥⎢ ⎥⎣ ⎦
(2.20)
C¸c ®iÓm iB vµ iC rµng buéc víi nhau bëi ®iÒu kiÖn:
2 2 2 2* ( ) ( ) ( )Tlb bici bici xci xbi yci ybi zci zbi= = − + − + − (2.21)
Víi i=1,2,3 th× hÖ (2.21) lµ hÖ gåm ba ph−¬ng tr×nh vµ s¸u Èn:
1 2 3, , ; , ,px py pz ϕ ϕ ϕ . Th«ng th−êng ta ®· biÕt tr−íc ba Èn vµ chØ cßn ba Èn
ch−a biÕt. Nh− vËy hÖ (2.21) víi sè ph−¬ng tr×nh lµ 3 vµ b»ng sè Èn nªn lu«n
cho ta lêi gi¶i.
2.2.5. Bµi to¸n ®éng häc thuËn
Khi cho biÕt quy luËt chuyÓn ®éng cña ba gãc: 1 2 3, ,ϕ ϕ ϕ ta sÏ x¸c ®Þnh
®−îc px, py vµ pz tõ hÖ (2.21) vµ do vËy x¸c ®Þnh ®−îc quy luËt chuyÓn ®éng
cña bµn di ®éng.
2.2.6. Bµi to¸n ®éng häc ng−îc
Khi cho biÕt quy luËt chuyÓn ®éng cña bµn di ®éng, tøc lµ cho biÕt px,
py, pz ta sÏ x¸c ®Þnh ®−îc 1 2 3, ,ϕ ϕ ϕ tõ hÖ (2.21).
2.3. Robot song song ph¼ng 3 ch©n
RBSS ph¼ng 3 ch©n cã ®Æc ®iÓm lµ mçi ch©n gåm cã 2 kh©u vµ 3
khíp, mçi khíp cÇn 1 bËc tù do, tøc lµ khíp p5: khíp quay (R), khíp tr−ît
(ký hiÖu lµ T - Translatrion hoÆc P – Prismatic). V× vËy cã 7 ph−¬ng ¸n:
RRR, RRP, RPR, PRR, RPP, PPR, PPP
H×nh 2.8 minh häa c¬ cÊu RBSS ph¼ng 3 ch©n dïng cÊu tróc RRR
H×nh 2.9 dïng cÊu tróc PRR
H×nh 2.10. dïng cÊu tróc PRP, trong ®ã 3 khíp quay R vu«ng gãc víi
mÆt ph¼ng chøa 1 ph−¬ng tÞnh tiÕn cña 1 khíp P trªn tÊm cè ®Þnh, cßn khíp
P kia liªn hÖ víi tÊm ®éng.
27
RBSS ph¼ng 3 ch©n th−êng ®−îc dïng ®Ó t¹o ra c¸c ®å g¸ CNC ®Ó
mang ph«i gia c«ng hay mang dông cô gia c«ng.
A1 B1
B2
3B
A3
2A
D
E
C
H×nh 4.2.8. RBSS ph¼ng 3 ch©n RRR
H×nh 4.2.9. RBSS ph¼ng 3 ch©n PRR
B1
B2
3B
D
E
C
A3
A2
A1
28
H×nh 4.2.10. RBSS ph¼ng 3 ch©n PRP
2.2. §å g¸ gia c«ng vá hép ®éng c¬ xe m¸y
Trong phÇn phô lôc tr×nh bµy b¶n thuyÕt minh “C«ng tr×nh thiÕt kÕ chÕ
t¹o c¬ cÊu kÑp ph«i nhanh cho m¸y gia c«ng CNC” vµ giíi thiÖu mét sè b¶n
vÏ thiÕt kÕ ®å g¸ vÒ hÖ thèng c¸c xi lanh khÝ nÐn ®−îc dïng cho ®å g¸ nµy.
A B
C
F
D
E
G
H
I
P
R
P
TÊm ®éng
TÊm cè ®Þnh
29
III. Robot song song RBSS – 322
30
3.1. Giíi thiÖu chung
Vấn đề Robot song song (RBSS) trở nên hấp dẫn nhiều nhà nghiên
cứu từ giữa thập kỷ 90 khi nó được ứng dụng dưới dạng thiết bị có tên là
Hexapod để tạo ra máy công cụ CNC 5 trục. Hexapod là một môđun RBSS
được kết cấu trên nguyên lý cơ cấu Stewart. Cơ cấu này gồm có 6 chân, với
độ dài thay đổi được, nối với giá và tấm động đều bằng các khớp cầu. Bằng
cách thay đổi độ dài của các chân có thể điều khiển sự định vị và định hướng
của tấm động theo ý muốn.
Ngoài các ứng dụng trong ngành chế tạo máy công cụ RBSS còn được
áp dụng hiệu quả trong dụng cụ y học, trong hệ thống mô phỏng, trong thiết
bị thiên văn, trắc địa và trong kỹ thuật phòng không v.v. Cơ cấu RBSS cũng
có nhiều biến thể khác nhau và xuất hiện nhiều tên gọi khác nhau, ví dụ như
Hexapod, Hexaglide [4], Hexagons [5], Hexa [2], Delta [3] v.v.
Dưới đây trình bày kết quả nghiên cứu, thiết kế, chế tạo và điều khiển
một kiểu robot song song. Trong thiết kế đã tham khảo phần tạo dáng từ
mẫu chào hàng robot Flex Picker của hãng ABB.
3.2. Thiết kế kết cấu và lập trình điều khiển chuyển động của Robot.
Trên hình 3.1 là bản vẽ 3D của Robot đã thiết kế và trên hình 3.2 là
ảnh chụp sau khi đã lắp đặt vận hành.
Các chân của Robot được xếp thành 3 nhánh đôi và được truyền động
từ 3 động cơ riêng biệt. Mỗi chân gồm có 2 phần: phần trên (đùi) và phần
dưới (ống chân). Chân nhánh nối với giá đỡ bằng khớp quay (khớp háng) và
nối với tấm động bằng “khớp cổ chân”. Chỗ “cổ chân” và đầu gối” đều bố trí
một khớp quay với trục quay có 2 đầu khớp cầu
31
Hình 4.3.1. Bản vẽ 3D thiết kế RBSS-322
Hình 4.3.2. Ảnh chụp RBSS - 322
32
Gọi điểm đầu mút của dụng cụ gắn liền với tấm động là “điểm tác
động cuối” (end - effector) của Robot. Khi lập trình điều khiển chuyển động
của Robot, ta quan tâm đến sự dịch chuyển của ‘điểm tác động cuối” này.
Ngôn ngữ lập trình là Visual C++ tạo điều kiện dễ dàng kết nối với các thư
viện đồ hoạ mạnh và phổ biến như OpenGL.
Sơ đồ giải thuật chương trình điều khiển giới thiệu trên hình 4.3.3.
Hình 4.3.3. Sơ đồ giải thuật chương trình điều khiển
Sau khi đã nhập dữ liệu, chúng kiểm tra có nằm trong vùng làm việc
không, rồi được gửi tới các chương trình con để giải bài toán động học
ngược. Các kết quả đó được xử lý và truyền tới cổng nối tiếp R232C để điều
khiển hoạt động của Robot qua bộ thiết bị điều khiển. Sau khi di chuyển tới
điểm đã định, tín hiệu phản hồi về sẽ là thông điệp gửi tiếp thông tin về điểm
di chuyển tiếp theo. Quá trình này được thực hiện lặp lại một cách nhịp
nhàng, không phải chờ đợi, cho tới khi đã tới đích cuối cùng của hành trình.
Begin
Nhập dữ liệu
Trong vùng làm việc
Giải bài toán Động học
nghịch
Truyền tín hiệu điều khiển
tới R232C
Nhận Tín hiệu từ
R232C End
Đúng Sai
Đúng
Sai
33
Trên hình 4.3.4 là minh hoạ kết quả tính toán về hành trình di chuyển
của “điểm tác động cuối”.
Hình 4.3. 4. Trich kết quả tính toán về hành trình
Ngoài ra kết quả tính toán cho chương trình điều khiển cũng đồng thời
được gửi tới chương trình mô phỏng hoạt hình của Robot.
Hình 4.3.5: Sơ đồ chức năng bảng mạch chính
3.3. Sơ đồ hệ thống điều khiển
Trên hình 4.3.5 là sơ đồ chức năng bảng mạch chính (Main board).
Chương trình điều khiển được nạp trong bộ nhớ trong bộ vi xử lý của 89C51
DB9
2
3
5
MAX2
32
TxD
PO
RxD
89C51
P1 P2
Joy-
stick
AD
0809
ANALOG
INPUT
CONTROL
BUS
34
thực hiện các chức năng kiểm soát cổng P1 để nhận lệnh điều khiển từ
Joystick trong chế độ điều khiển bằng tay (manual), kiểm tra cổng nối tiếp
để nhận lệnh điều khiển từ máy tính PC. Chuỗi lệnh từ máy tính điều khiển
truyền tới gồm 11byte. Khi nhận xong một lệnh từ cổng nối tiếp thì Main
board sẽ truyền trở về PC một byte trả lời. Khi thực hiện xong lệnh đã nhận
thì nó lại tiếp tục truyền trở về PC một byte báo cáo thứ 2.
Trên hình 4.3.6 là sơ đồ chức năng bảng mạch bộ điều khiển động cơ
(Driver board).
Hình 4.3.6. Sơ đồ chức năng bảng mạch bộ điều khiển động cơ
H×nh 4.3.7.Chu trình nhận và thực hiện lệnh điều khiển động cơ Driver board
89c2051
CONTROL BUS
Mạch
công
suất
Kiểm soát
dòng
Mạch
phối
hợp
DC EN SENSOR
Mạch
phối
hợp
BUS
điều
khiển
Nhận lệnh Thực hiện lệnh
Kiểm soát
dòng ĐC
Đọc
Encoder
SENSER
vị trí
ĐK Động
cơ
35
Chương trình điều khiển được nạp vào bộ nhớ trong của bộ vi xử lý
89C 2051 thực hiện chức năng kiểm tra bus điều khiển (control bus). Giá trị
xung đếm vòng quay từ encoder đưa về qua mạch phối hợp được so dánh
với giá trị vòng quay từ lệnh điều khiển là tín hiệu để động cơ dừng hay hoạt
động tiếp tục. Dòng điện tiêu thụ của động cơ cũng được kiểm soát trong
quá trình hoạt động. Nếu giá trị này vượt quá ngưỡng đã đặt thì chương trình
sẽ cho dừng động cơ và báo lỗi về bảng mạch chính. Lỗi sẽ được hiển thị
bằng đèn LED và có thể thông báo về máy tính điều khiển. Các sensor chỉ
báo vị trí ban đầu của Robot cũng được kiểm soát để khi có lệnh chúng sẽ
trở về vị trí xuất phát. Chu trình nhận và thực hiện lệnh của Driver board
được mô tả trên hình 3.7.
Hệ thống thiết bị đã được vận hành thử nghiệm tốt và đang chuẩn bị
triển khai ứng dụng trong gia công điêu khắc.
3.4. Ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn RBSS - 322
Chương trình gồm các file vitridau.m, nhap.m, tinhdulieu.m, chayhopmay.m
%vitridau.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
s=serial('COM4');
set(s, 'Terminator', 'CR');
s.Timeout=7;
%s
so_xung1=2000; %21100 1 vong
highbyte1=floor(so_xung1/256);
lowbyte1=mod(so_xung1,256);
so_xung2=1000; %21100 1 vong
highbyte2=floor(so_xung2/256);
36
lowbyte2=mod(so_xung2,256);
so_xung3=1000; %21100 1 vong
highbyte3=floor(so_xung3/256);
lowbyte3=mod(so_xung3,256);
fopen(s)
fwrite(s,254) %ma Header1
fwrite(s,253) %ma Header2
fwrite(s,104) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115 dung)
%(h or 104 vi tri dau)
fwrite(s,lowbyte1) %so xung lowbyte dc1
fwrite(s,highbyte1) %so xung highbyte dc1
fwrite(s,104) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115
%dung) (h or 104 vi tri dau)
fwrite(s,lowbyte2) %so xung lowbyte dc2
fwrite(s,highbyte2) %so xung highbyte dc2
fwrite(s,104) %ma1 dc3 (+ or 43 len) (- or 45 xuong) (s or 115
%dung) (h or 104 vi tri dau)
fwrite(s,lowbyte3) %so xung lowbyte dc3
fwrite(s,highbyte3) %so xung highbyte dc3
fprintf(s,'%s','s') %ma4 dc4 (+ len) (- xuong) (s dung)
fwrite(s,1)
fwrite(s,0)
data=fscanf(s)
if data=='b'
fwrite(s,254)
fwrite(s,253)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
fprintf(s,'%s','s')
37
fwrite(s,0)
fwrite(s,0)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
end
fclose(s);
%nhap.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
% ch=input('\n ban nhap so lieu hay doc tu dia (n/d) : ','s');
% if (ch == 'd')
% filename=input('\n Nhap ten file: ','s');
% [command,mss]=sprintf('load %s Aa Bb Cc OA OB OC TAa TBb
% TCc.OtTA zTA OtTB zTB OtTC zTC',filename);
% eval(command);
% fprintf('\n Aa=%10.3f',Aa);
% fprintf('\n Bb=%10.3f',Bb);
% fprintf('\n Cc=%10.3f',Cc);
% fprintf('\n OA=%10.3f',OA);
% fprintf('\n OB=%10.3f',OB);
% fprintf('\n OC=%10.3f',OC);
% fprintf('\n TAa=%10.3f',TAa);
% fprintf('\n TBb=%10.3f',TBb);
% fprintf('\n TCc=%10.3f',TCc);
38
% fprintf('\n khoang cach TA den tam=%10.3f',OtTA);
% fprintf('\n zTA=%10.3f',zTA);
% fprintf('\n khoang cach TB den tam=%10.3f',OtTB);
% fprintf('\n zTB=%10.3f',zTB);
% fprintf('\n khoang cach TC den tam=%10.3f',OtTC);
% fprintf('\n zTC=%10.3f',zTC);
% elseif (ch == 'n')
% Aa=input('Aa=');
% Bb=input('Bb=');
% Cc=input('Cc=');
% OA=input('OA=');
% OB=input('OB=');
% OC=input('OC=');
% TAa=input('TAa=');
% TBb=input('TBb=');
% TCc=input('TCc=');
% OtTA=input('khoang cach TA den tam=');
% zTA=input('zTA=');
% OtTB=input('khoang cach TB den tam=');
% zTB=input('zTB=');
% OtTC=input('khoang cach TC den tam=');
% zTC=input('zTC=');
% filesave=input('\n Nhap ten file save: ','s');
% [command,mss]=sprintf('save %s Aa Bb Cc OA OB OC TAa TBb
% TCc OtTA zTA OtTB zTB OtTC zTC',filesave);
% eval(command);
% end
%--------------------------------------------------------------------------
39
Aa=800
Bb=800
Cc=800
OA=40
OB=40
OC=40
TAa=365
TBb=365
TCc=365
OtTA=200
zTA=615
OtTB=200
zTB=615
OtTC=200
zTC=615
xO=input('xO=');
yO=input('yO=');
zO=input('zO=');
xA=xO-OA
yA=yO
zA=zO
xB=xO+OB*sin(30*pi/180)
yB=yO-OB*cos(30*pi/180)
zB=zO
xC=xO+OC*sin(30*pi/180)
yC=yO+OC*cos(30*pi/180)
zC=zO
40
xTA=-OtTA
yTA=0
zTA
xTB=OtTB*sin(30*pi/180)
yTB=-OtTB*cos(30*pi/180)
zTB
xTC=OtTC*sin(30*pi/180)
yTC=OtTC*cos(30*pi/180)
zTC
%-----------------
ya=0;
y=ya;
Aa_binh=Aa^2;
TAa_binh=TAa^2;
eqt1=convert(xA,yA,zA,Aa_binh);
eqt2=convert(xTA,yTA,zTA,TAa_binh);
syms x z x1 z1
[x,z]=solve(eqt1,eqt2,'x,z');
x1=eval(x)
z1=eval(z)
for i=1:length(z1)
xa=x1(i);
za=z1(i);
if z1(i)>zA
41
if x1(i)<xTA
xa=x1(i);
ya;
za=z1(i);
break;
end
end
end
xa
ya
za
%------------------------------------------------------
Bb_binh=Bb^2;
TBb_binh=TBb^2;
syms x y z x2 y2 z2
eqt3=y+x*cot(30*pi/180);
eqt1=convert(xB,yB,zB,Bb_binh);
eqt2=convert(xTB,yTB,zTB,TBb_binh);
[x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z');
x2=eval(x)
y2=eval(y)
z2=eval(z)
for i=1:length(z2)
xb=x2(i);
yb=y2(i);
zb=z2(i);
if z2(i)>zB
if x2(i)>xTB
xb=x2(i);
42
yb=y2(i);
zb=z2(i);
break;
end
end
end
xb
yb
zb
%------------------------------------------------------
Cc_binh=Cc^2;
TCc_binh=TCc^2;
syms x y z x3 y3 z3
eqt3=y-x*cot(30*pi/180);
eqt1=convert(xC,yC,zC,Cc_binh);
eqt2=convert(xTC,yTC,zTC,TCc_binh);
[x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z');
x3=eval(x)
y3=eval(y)
z3=eval(z)
for i=1:length(z3)
xc=x3(i);
yc=y3(i);
zc=z3(i);
if z3(i)>zC
if x3(i)>xTC
xc=x3(i);
43
yc=y3(i);
zc=z3(i);
break;
end
end
end
xc
yc
zc
%--------------------------------------------------------
%tinh cac goc di xuong
goc1=asin((zTA-za)/TAa)*180/pi
goc2=asin((zTB-zb)/TBb)*180/pi
goc3=asin((zTC-zc)/TCc)*180/pi
s=serial('COM2');
set(s, 'Terminator', 'CR');
s.Timeout=5;
%s
so_xung1=21100*goc1/360; %21100 1 vong
highbyte1=floor(so_xung1/256);
lowbyte1=mod(so_xung1,256);
so_xung2=21100*goc2/360; %21100 1 vong
highbyte2=floor(so_xung2/256);
lowbyte2=mod(so_xung2,256);
so_xung3=21100*goc3/360; %21100 1 vong
highbyte3=floor(so_xung3/256);
lowbyte3=mod(so_xung3,256);
44
if goc1>0
chieu1=45;
else
chieu1=43;
end
if goc2>0
chieu2=45;
else
chieu2=43;
end
if goc3>0
chieu3=45;
else
chieu3=43;
end
fopen(s)
fwrite(s,254) %ma Header1
fwrite(s,253) %ma Header2
fwrite(s,chieu1) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115
%dung) (h or 104 vi tri dau)
fwrite(s,lowbyte1) %so xung lowbyte dc1
fwrite(s,highbyte1) %so xung highbyte dc1
fwrite(s,chieu2) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115 dung) (h or
104 vi tri dau)
fwrite(s,lowbyte2) %so xung lowbyte dc2
fwrite(s,highbyte2) %so xung highbyte dc2
fwrite(s,chieu3) %ma1 dc3 (+ or 43 len) (- or 45 xuong) (s or 115 dung) (h or
104 vi tri dau)
fwrite(s,lowbyte3) %so xung lowbyte dc3
45
fwrite(s,highbyte3) %so xung highbyte dc3
fprintf(s,'%s','s') %ma4 dc4 (+ len) (- xuong) (s dung)
fwrite(s,1)
fwrite(s,0)
data=fscanf(s,'%s')
if data=='b'
fwrite(s,254)
fwrite(s,253)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
end
fclose(s);
%tinhdulieu.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
Aa=800
Bb=800
Cc=800
46
OA=40
OB=40
OC=40
TAa=365
TBb=365
TCc=365
OtTA=200
zTA=615
OtTB=200
zTB=615
OtTC=200
zTC=615
ch=input('\n ban nhap so lieu hay doc tu dia (n/d) toa do mui khoan: ','s');
if (ch == 'd')
filename1=input('\n Nhap ten file luu toa do: ','s');
[command,mss]=sprintf('load %s n xO yO zO',filename1);
eval(command);
for i=1:n
fprintf('\n du lieu tai diem :%d \n',i);
fprintf('\n xO=%10.3f',xO(i));
fprintf('\n yO=%10.3f',yO(i));
fprintf('\n zO=%10.3f',zO(i));
end
elseif (ch == 'n')
n=input('So diem tren can nhap =');
for i=1:n
fprintf('\n Nhap du lieu tai diem :%d \n',i);
47
xO(i)=input('xO=');
yO(i)=input('yO=');
zO(i)=input('zO=');
end
filesave1=input('\n Nhap ten file save luu toa do: ','s');
[command,mss]=sprintf('save %s n xO yO zO',filesave1);
eval(command);
end
for j=1:n
xA(j)=xO(j)-OA
yA(j)=yO(j)
zA(j)=zO(j)
xB(j)=xO(j)+OB*sin(30*pi/180)
yB(j)=yO(j)-OB*cos(30*pi/180)
zB(j)=zO(j)
xC(j)=xO(j)+OC*sin(30*pi/180)
yC(j)=yO(j)+OC*cos(30*pi/180)
zC(j)=zO(j)
xTA=-OtTA
yTA=0
zTA
xTB=OtTB*sin(30*pi/180)
yTB=-OtTB*cos(30*pi/180)
zTB
48
xTC=OtTC*sin(30*pi/180)
yTC=OtTC*cos(30*pi/180)
zTC
end
%-----------------
for j=1:n
ya(j)=0;
y=ya(j);
Aa_binh=Aa^2;
TAa_binh=TAa^2;
eqt1=convert(xA(j),yA(j),zA(j),Aa_binh);
eqt2=convert(xTA,yTA,zTA,TAa_binh);
syms x z x1 z1
[x,z]=solve(eqt1,eqt2,'x,z');
x1=eval(x)
z1=eval(z)
for i=1:length(z1)
xa(j)=x1(i);
za(j)=z1(i);
if z1(i)>zA(j)
if x1(i)<xTA
xa(j)=x1(i);
ya(j);
za(j)=z1(i);
break;
end
end
49
end
xa(j)
ya(j)
za(j)
%------------------------------------------------------
Bb_binh=Bb^2;
TBb_binh=TBb^2;
syms x y z x2 y2 z2
eqt3=y+x*cot(30*pi/180);
eqt1=convert(xB(j),yB(j),zB(j),Bb_binh);
eqt2=convert(xTB,yTB,zTB,TBb_binh);
[x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z');
x2=eval(x)
y2=eval(y)
z2=eval(z)
for i=1:length(z2)
xb(j)=x2(i);
yb(j)=y2(i);
zb(j)=z2(i);
if z2(i)>zB(j)
if x2(i)>xTB
xb(j)=x2(i);
yb(j)=y2(i);
zb(j)=z2(i);
break;
end
end
50
end
xb(j)
yb(j)
zb(j)
%------------------------------------------------------
Cc_binh=Cc^2;
TCc_binh=TCc^2;
syms x y z x3 y3 z3
eqt3=y-x*cot(30*pi/180);
eqt1=convert(xC(j),yC(j),zC(j),Cc_binh);
eqt2=convert(xTC,yTC,zTC,TCc_binh);
[x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z');
x3=eval(x)
y3=eval(y)
z3=eval(z)
for i=1:length(z3)
xc(j)=x3(i);
yc(j)=y3(i);
zc(j)=z3(i);
if z3(i)>zC(j)
if x3(i)>xTC
xc(j)=x3(i);
yc(j)=y3(i);
zc(j)=z3(i);
break;
end
end
51
end
xc(j)
yc(j)
zc(j)
end
%--------------------------------------------------------
filesave2=input('\n Nhap ten file save luu toa do z de tinh: ','s');
[command,mss]=sprintf('save %s n za zb zc zTA zTB zTC TAa TBb TCc',filesave2);
eval(command);
% filename2=input('\n Ghi ten file de save toa do z de tinh: ','s');
% fid = fopen(filename2,'w');
% fprintf(fid,'"%d"',n);
%
fprintf(fid,'"%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f"',zTA,zTB,zTC,TAa,TBb,TCc);
% for j=1:n
% fprintf(fid,'"%8.4f,%8.4f,%8.4f"',za(j),zb(j),zc(j));
% end
% fclose(fid)
%chayhopmay.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
filename2=input('\n Nhap ten file: ','s');
[command,mss]=sprintf('load %s n za zb zc zTA zTB zTC TAa TBb
TCc',filename2);
eval(command);
za;
52
zb;
zc;
so_xung1_xuong=21100*6.2138/360; %21100 1 vong
highbyte1_xuong=floor(so_xung1_xuong/256);
lowbyte1_xuong=mod(so_xung1_xuong,256);
so_xung2_xuong=21100*6.2138/360; %21100 1 vong
highbyte2_xuong=floor(so_xung2_xuong/256);
lowbyte2_xuong=mod(so_xung2_xuong,256);
so_xung3_xuong=21100*6.2138/360; %21100 1 vong
highbyte3_xuong=floor(so_xung3_xuong/256);
lowbyte3_xuong=mod(so_xung3_xuong,256);
for j=1:n
%tinh cac goc di xuong
goc1(j)=asin((zTA-za(j))/TAa)*180/pi;
goc2(j)=asin((zTB-zb(j))/TBb)*180/pi;
goc3(j)=asin((zTC-zc(j))/TCc)*180/pi;
s=serial('COM4');
set(s, 'Terminator', 'CR');
s.Timeout=10;
%s
so_xung1(j)=21100*goc1(j)/360; %21100 1 vong
highbyte1(j)=floor(so_xung1(j)/256);
lowbyte1(j)=mod(so_xung1(j),256);
so_xung2(j)=21100*goc2(j)/360; %21100 1 vong
highbyte2(j)=floor(so_xung2(j)/256);
lowbyte2(j)=mod(so_xung2(j),256);
53
so_xung3(j)=21100*goc3(j)/360; %21100 1 vong
highbyte3(j)=floor(so_xung3(j)/256);
lowbyte3(j)=mod(so_xung3(j),256);
%di len
if goc1(j)>0
chieu1(j)=45;
else
chieu1(j)=43;
end
if goc2(j)>0
chieu2(j)=45;
else
chieu2(j)=43;
end
if goc3(j)>0
chieu3(j)=45;
else
chieu3(j)=43;
end
fopen(s)
fwrite(s,254) %ma Header1
fwrite(s,253) %ma Header2
fwrite(s,chieu1(j)) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115
%dung) (h or 104 vi tri dau)
fwrite(s,lowbyte1(j)) %so xung lowbyte dc1
fwrite(s,highbyte1(j)) %so xung highbyte dc1
fwrite(s,chieu2(j)) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115
%dung) (h or 104 vi tri dau)
54
%---------------------------------------------------------------------
s=serial('COM4');
set(s, 'Terminator', 'CR');
s.Timeout=5;
fopen(s)
fwrite(s,254) %ma Header1
fwrite(s,253) %ma Header2
fwrite(s,104) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115
%dung) (h or 104 vi tri dau)
fwrite(s,0) %so xung lowbyte dc1
fwrite(s,0) %so xung highbyte dc1
fwrite(s,104) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115 dung)
%(h or 104 vi tri dau)
fwrite(s,0) %so xung lowbyte dc2
fwrite(s,0) %so xung highbyte dc2
fwrite(s,104) %ma1 dc3 (+ or 43 len) (- or 45 xuong) (s or 115 dung)
%(h or 104 vi tri dau)
fwrite(s,0) %so xung lowbyte dc3
fwrite(s,0) %so xung highbyte dc3
fprintf(s,'%s','s') %ma4 dc4 (+ len) (- xuong) (s dung)
fwrite(s,1)
fwrite(s,0)
data=fscanf(s,'%s')
if data=='b'
fwrite(s,254)
fwrite(s,253)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
55
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
fprintf(s,'%s','s')
fwrite(s,0)
fwrite(s,0)
end
%-------------------------------------------
fclose(s);
end
56
IV. Robot “NhÖn n−íc
57
4.1. Giíi thiÖu chung
Robot “NhÖn n−íc” lµ mét ®Ò xuÊt míi trong nghiªn cøu øng dông
robot song song
Robot song song (RBSS) còng lµ mét lo¹i robot, nh−ng s¬ ®å ®éng cña
c¬ cÊu cã c¸c kh©u kh«ng nèi liªn tiÕp víi nhau nh− c¬ cÊu chuçi cña robot
th«ng th−êng (serie robot) mµ ë mét kh©u nÇo ®ã nèi víi nhiÒu kh©u kh¸c,
tøc lµ nèi song song víi nhau. §èi víi tr−êng hîp RBSS th× bµi to¸n thuËn l¹i
khã h¬n nhiÒu so víi bµi to¸n ng−îc.
Mét trong nh÷ng lÜnh vùc øng dông thµnh c«ng cña RBSS lµ ®å g¸ cho
m¸y gia c«ng CNC, trong ®ã c¬ cÊu Hexapod lµ vÝ dô ®iÓn h×nh. Trong
nh÷ng n¨m gÇn ®©y RBSS cßn më réng ra nhiÒu lÜnh vùc n©ng vËn chuyÓn vµ
xuÊt hiÖn tªn gäi míi Robocrane (robot cÈu).
Robot-cÈu lµ tªn gäi chung lo¹i robot lµm viÖc víi chøc n¨ng nh− mét
thiÕt bÞ n©ng vËn chuyÓn dïng d©y c¸p n©ng t¶i. VÒ ®¹i thÓ, robot - cÈu còng
ho¹t ®éng theo nguyªn lý c¬ cÊu Stewart.
Trªn h×nh 4.4.1 lµ s¬ ®å c¬ cÊu Stewart, gåm cã tÊm ®éng (1) nèi víi
tÊm g¸ ®ì (3) b»ng 6 ch©n (2) cã c¸c khíp cÇu ë c¶ 2 ®Çu. Khi thay ®æi ®é
dµi cña 6 ch©n nµy th× “®Þnh vÞ vµ ®Þnh h−íng” cña tÊm ®éng sÏ thay ®æi
theo.
58
H×nh 4.4.1. S¬ ®å c¬ cÊu Stewart
Trong thiÕt bÞ robot - cÈu, tÊm ®éng (1) ®−îc treo trªn tÊm gi¸ cè ®Þnh
(3) b»ng 6 d©y c¸p cã ®é dµi thay ®æi ®−îc nhê c¬ cÊu kiÓu têi quay vµ ®¶m
b¶o linh ho¹t nèi ghÐp gi÷a d©y c¸p víi c¸c tÊm nhê cã khíp cÇu. Trong
tr−êng hîp robot – cÈu th× tÊm ®éng g¾n liÒn víi ®èi t−îng vËt thÓ cÇn ®−îc
n©ng chuyÓn hoÆc xÕp ®Æt v.v.
Khi ®ã, viÖc ®iÒukhiÓn “®Þnh vÞ vµ ®Þnh h−íng” cña tÊm ®éng trë
thµnh viÖc ®iÒu khiÓn ®ång thêi 6 ®éng c¬ cña 6 têi quay nãi trªn
Robot “NhÖn n−íc” gåm cã 2 phÇn: PhÇn trªn cã tÊm ®éng treo trªn
gi¸ ®ì nhê 6 d©y c¸p, ®iÒu khiÓn b»ng 6 têi quay. §ã lµ mét robot – cÈu.
PhÇn d−íi lµ mét hÖ thèng c¸c c¬ cÊu mang c¸c vßi phun dïng ®Ó tÈy röa c¸c
khoang hÇm tÇu. HÖ thèng c¸c c¬ cÊu nµy cã 4 nh¸nh, mçi nh¸nh ®Òu cã thÓ
“co côm” l¹i hoÆc “xße réng” ra, nªn tr«ng h×nh d¸ng bªn ngoµi gièng nh−
“con nhÖn n−íc”.
4.2. M« t¶ Robot “NhÖn n−íc”
“Robo - NhÖn n−íc” lµ tªn gäi t¾t cña mét thiÕt bÞ míi ®−îc ®Ò xuÊt.
§©y lµ mét vÝ dô øng dông cña RBSS. Nã gåm 2 phÇn (h×nh 4.4.2): PhÇn trªn
1
2
3
59
cã tÊm ®éng treo trªn gi¸ ®ì nhê 6 d©y c¸p, ®iÒu khiÓn b»ng 6 têi quay. §ã
lµ mét robo - cÈu. PhÇn d−íi lµ mét hÖ thèng víi 4 c¬ cÊu nh¸nh h×nh b×nh
hµnh. HÖ thèng c¬ cÊu nµy cã thÓ “xoÌ ra” hoÆc “co l¹i” nhê chuyÓn ®éng
tÞnh tiÕn cña thanh trôc gi÷a trong khíp ®éng tÞnh tiÕn, g¾n liÒn víi tÊm ®éng
cña robo - cÈu.
ThiÕt bÞ nµy lµ mét c¬ cÊu mang c¸c èng n−íc mÒm cã vßi phun dïng
®Ó tÈy röa c¸c khoang hÇm ngÇm, mµ ë ®ã con ng−êi rÊt khã kh¨n hoÆc
kh«ng thÓ thao t¸c, vÝ dô v× m«i tr−êng qu¸ ®éc h¹i. ThiÕt bÞ nµy cã thÓ ‘co
côm” l¹i vµ h¹ thÊp dÇn qua miÖng hÇm cã kÝch th−íc giíi h¹n vµ sÏ “xoÌ
réng” ra, ®ång thêi nhê cã thÓ thay ®æi ®−îc “sù ®Þnh vÞ vµ ®Þnh h−íng” cña
tÊm ®éng, mµ quü ®¹o phun n−íc ®−îc ®iÒu khiÓn theo ý muèn.
H×nh 4.4.2 lµ cöa sæ mµn h×nh víi mét pha ho¹t ®éng cña Robo - NhÖn
n−íc ®· ®−îc m« pháng ho¹t h×nh. Trªn c¬ së ®ã cã thÓ kh¶o s¸t c¸c chuyÓn
®éng cña Robo - NhÖn n−íc vµ thö nghiÖm c¸c ch−¬ng tr×nh ®iÒu khiÓn.
H×nh 4. 4.2. Robo - NhÖn n−íc
60
4.3. TÝnh to¸n, x©y dùng ch−¬ng tr×nh ®iÒu khiÓn Robot - “NhÖn n−íc”
§øc
H×nh 4. 4.3. VÝ dô tÝnh to¸n theo ch−¬ng tr×nh ®iÒu khiÓn robo-cÈu
4.4. M« pháng ho¹t h×nh Robot - “NhÖn n−íc”
H×nh 4.4.4. M« pháng RBSS b»ng Open GL dïng cho “NhÖn n−íc”
61
V. M« pháng RBSS dïng lµm ®å g¸ CNC
62
5.1. M« pháng RBSS ch©n RPS
Bµi to¸n ®éng häc thuËn lµ bµi to¸n biÕt ®é dµi c¸c ch©n di (i=1,2,3), ta
ph¶i t×m vÞ trÝ cña bµn m¸y ®éng P vµ ma trËn ARB.
Theo phÇn trªn ta thay c¸c gi¸ trÞ di (i=1,2,3) vµo hÖ (4.20), ta sÏ ®−îc
hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ : 1 2 3 1 2 3, , , , ,p p pα α α
Chó ý lµ 3 ph−¬ng tr×nh sau cña hÖ (4.20) chØ chøa di vµ iα nªn viÖc
gi¶i 6 ph−¬ng tr×nh ®−îc ®¬n gi¶n l¹i cßn gi¶i hÖ 3 ph−¬ng tr×nh víi 3 Èn lµ
iα . Sau ®ã thay c¸c gi¸ trÞ cña di vµ iα vµo 3 ph−¬ng tr×nh ®Çu ta sÏ tÝnh
®−îc c¸c gi¸ trÞ cña P
VÝ dô:
- C¸c ®iÒu kiÖn ®Çu
10 20 30 / 2α α α π= = =
d10 = d20 = d30 = 4.44
Hai tam gi¸c lµ ®Òu vµ h = g = 8.89
- Qui luËt chuyÓn ®éng cña c¸c ch©n r«-bèt
d1 = d10(1+0.08cos10t)
d2 = d20(1+0.1sin10t)
d3 = d30(1+0.05cos10t)
63
- Giao diÖn chÝnh cña ch−¬ng tr×nh
- KÕt qu¶ m« pháng sè b»ng Matlab:
H×nh 4.5.1: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng x
64
H×nh 4.5.2: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng y
H×nh 4.5.3: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng z
65
H×nh 4.5.4: §å thÞ vÞ trÝ ®iÓm P
H×nh 4.5.5: §å thÞ vËn tèc ®iÓm P theo ph−¬ng x
66
H×nh 4.5.6: §å thÞ vËn tèc ®iÓm P theo ph−¬ng y
H×nh 4.5.7: §å thÞ vËn tèc ®iÓm P theo ph−¬ng z
67
- KÕt qu¶ m« pháng ®éng b»ng Matlab:
H×nh 4.5.8: VÝ dô m« pháng ®éng RBSS 3 ch©n RPS
(bµi to¸n ®éng häc thuËn)
5.1.2. Bµi to¸n ®éng häc ng−îc
Bµi to¸n ®éng häc ng−îc lµ bµi to¸n biÕt vÞ trÝ bµn m¸y ®éng P, ta ph¶i
t×m ®é dµi c¸c ch©n di (i=1,2,3) vµ c¸c gãc iα (i=1,2,3) .
T−¬ng tù nh− c¸ch lµm ®èi víi bµi to¸n ®éng häc thuËn ta thay c¸c gi¸
trÞ P vµo hÖ (4.20), ta sÏ ®−îc hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ :
1 2 3 1 2 3, , , , ,d d dα α α . Gi¶i hÖ ph−¬ng tr×nh nµy ta sÏ thu ®−îc c¸c nghiÖm cÇn
thiÕt.
VÝ dô:
- C¸c ®iÒu kiÖn ®Çu
10 20 30 / 2α α α π= = =
68
p10 =0; p20 =0; p30 = 4.44
Hai tam gi¸c lµ ®Òu vµ h = g = 8.89
- Qui luËt chuyÓn ®éng cña bµn di ®éng
p1=0;
p2=0;
p3=p30*(1+0.08*sin(10*t));
- KÕt qu¶ m« pháng sè b»ng Matlab:
H×nh 4.5.9: §å thÞ vÞ trÝ ch©n sè 1
69
H×nh 4.5.10: §å thÞ vÞ trÝ ch©n sè 2
H×nh 4.5.11: §å thÞ vÞ trÝ ch©n sè 3
70
H×nh 4.5.12: §å thÞ vËn tèc ch©n sè 1
H×nh 4.5.13: §å thÞ vËn tèc ch©n sè 2
71
H×nh 5.1.14: §å thÞ vËn tèc ch©n sè 3
- KÕt qu¶ m« pháng ®éng b»ng Matlab:
H×nh 4.5.15: VÝ dô m« pháng ®éng RBSS 3 ch©n RPS
(bµi to¸n ®éng häc ng−îc)
72
5.2. Robot song song 3 RSS
KÝch th−íc cña Deltarca nh− sau:
B¸n kÝnh bµn cè ®Þnh: ra = 200 (mm)
B¸n kÝnh bµn di ®éng: rb = 50
§é dµi c¸nh tay: AC = 365
§é dµi c¼ng tay: BC = 800
VÞ trÝ c¸c ch©n r«-bèt x¸c ®Þnh bëi c¸c gãc: 1 2 30, 120, 240α α α= = =
- Dao diÖn chÝnh cña ch−¬ng tr×nh
5.2.1. Bµi to¸n ®éng häc thuËn
- Cho biÕt qui luËt chuyÓn ®éng cña c¸c gãc ë kh©u c¸nh tay r«-bèt.
1ϕ = t
2ϕ = t
3ϕ = t
73
Trong ®ã ®iÒu kiÖn ®Çu: 10 20 300, 0, 0ϕ ϕ ϕ= = =
- KÕt qu¶ m« pháng sè b»ng Matlab:
H×nh 4.5.16: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng x
H×nh 4.5.17: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng y
74
H×nh 4.5.18: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng z
H×nh 4.5.19: §å thÞ vÞ trÝ ®iÓm P
75
- KÕt qu¶ m« pháng ®éng b»ng Matlab:
H×nh 4.5.20: VÝ dô m« pháng ®éng RBSS 3 ch©n RSS
5.2.2. Bµi to¸n ®éng häc ng−îc
- C¸c ®iÒu kiÖn ®Çu
p10 =50; p20 =0; p30 = -615 (mm)
- Cho biÕt tr−íc quy luËt chuyÓn ®éng cña bµn di ®éng
p1=50cos(t);
p2=50sin(t);
p3=p30;
76
- KÕt qu¶ m« pháng sè:
H×nh 4.5.21: §å thÞ vÞ trÝ gãc ch©n sè 1
H×nh 4.5.22: §å thÞ vÞ trÝ gãc ch©n sè 2
77
H×nh 4.5.23: §å thÞ vÞ trÝ gãc ch©n sè 3
- KÕt qu¶ m« pháng ®éng b»ng Matlab:
H×nh 4.5.24: VÝ dô m« pháng ®éng RBSS 3 ch©n RSS
78
VI. kÕt luËn
79
Mét lÜnh vùc øng dông næi bËt cña robot song song (RBSS) lµ lµm
thiÕt bÞ ®å g¸ (CNC) ®iÒu khiÓn theo ch−¬ng tr×nh m¸y tÝnh. TiÕp cËn víi
lÜnh vùc nµy còng nh− víi c¸c lÜnh vùc øng dông kh¸c cña RBSS, §Ò tµi ®·
nhËn ®−îc kÕt qu¶ b−íc ®Çu, chñ yÕu vÒ ch−¬ng tr×nh tÝnh to¸n, ®iÒu khiÓn,
nh−ng rÊt c¬ b¶n v× ®ã lµ nh÷ng c¬ së ®Ó triÓn khai c¸c øng dông trong thùc
tÕ khi cã ®−îc sù ®Çu t− hoÆc ®Æt hµng cô thÓ sau nµy.
Trong nh÷ng kÕt qu¶ ®ã nªn kÓ ®Õn:
- Ph−¬ng ph¸p t¹o ra c¸c thiÕt bÞ ®å g¸ nhanh phèi gia c«ng b»ng c¸ch
theo ch−¬ng tr×nh ®iÒu khiÓn sè thay ®æi chiÒu dµi 3 ch©n cña RBSS ph¼ng.
- T¹o ra robot song song RBSS - 322 ho¹t ®éng nh− mét thiÕt bÞ gia
c«ng CNC trong c«ng viÖc ®iªu kh¾c trªn nh÷ng vËt thÓ lín, cång kÒnh
kh«ng ®−a lªn bÖ c¸c m¸y th«ng th−êng ®−îc.
- §Ò xuÊt mét kiÓu Robot - “NhÖn n−íc” nh»m phôc vô c«ng viÖc
phun röa c¸c khoang hÇm tÇu chë dÇu th«, rÊt khã thao t¸c b»ng tay vµ rÊt
®éc h¹i.
80
Tµi liÖu tham kh¶o
1. ABB Robotics - Catalog
2. Bruyninckx H. The 321 - HEXA - a fully - parallel manipulator with
closed-form position and velocity kinematics. IEEE Int Conf. Robotics and
Automation, 1997
3 .Clavel R. Delta, a fast robot with parallel geometry - Int. Symp. Industrial
Robots, Lausanne, Switzerland, 1988
4. Honegger M., Codourey a., Burdet E. Adaptive control of the Hexaglide,
a 6 dof. Parallel manipulator IEEE Int Conf. Robotics and Automation,
Albuquerque, 1997
5. Lee H.Y And Roth B. A closed form solution of the forward displacement
analysis of a class of in parallel mechanisnas IEEE Int. Conf. Robotics and
Automation, Atlanta, 1993.
6. Proceedings of the 10th World Congress on the Theory of Machines and
Mechanisms. Oulu, Finland, 1999.
7. Waldron K. J., Kincel G.L., The relation between actuator geometry and
mechanical effcienncy in robots. Preprints 4th CISM – IFToMM Symposium
on Theory and Pract. Of Manipul. Warsawa, 1981
8. Shimon Y. Nof, Handbook of Industrial Robotics, 2nded. John Wiley &
Sons. Inc. 1999.
9. NguyÔn ThiÖn Phóc, Ng−êi m¸y pháng sinh vµ m« h×nh ®éng häc d¹ng
tæng qu¸t. T¹p chÝ C¬ häc, sè 4, 1995.
10. NguyÔn ThiÖn Phóc “Robot c«ng nghiÖp” NXB khoa häc vµ kü thuËt,
Hµ Néi, 2004.
81
11. NguyÔn ThiÖn Phóc, TrÇn Minh NghÜa, NguyÔn §×nh Nin. Nghiªn cøu
t¹o dùng robot song song lo¹i hexa. TuyÓn tËp b¸o c¸o khoa häc Héi nghÞ Tù
®éng hãa toµn quèc lÇn thø VI.
Các file đính kèm theo tài liệu này:
- 62464.pdf