BỘ KHOA HỌC VÀ CÔNG NGHỆ
CHƯƠNG TRÌNH KC.03
YZ YZ YZ YZ YZ YZY YZ YZ YZY YZ YZ YZY

“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”
MÃ SỐ: KC.03.08 BÁO CÁO CÁC KẾT QUẢ NGHIÊN CỨU
THEO NHIỆM VỤ 4 - ĐỀ TÀI KC.03.08
Nhãm s¶n phÈm ®å g¸ cnc
6246-4
25/12/2006

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

2
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


CNC mọi thao tác đều đợc điều khiển bằng số. Các sản phẩm đợc gia công
trên máy CNC rất đa dạng. Ngoài những sản phẩm có thể gia công bằng
những đồ gá có sẵn của máy, còn có nhiều trờng hợp phải tự tạo ra đồ gá
cho phù hợp. Nếu sản phẩm không phải chỉ chế tạo đơn chiếc mà có số lợng
lớn thì việc tạo ra các đồ gá nhánh, chính xác và cũng đợc điều khiển số là
loại việc có nhu cầu bức xúc.
Một trong những giải pháp kỹ thuật làm đồ gá CNC có thể dùng robot
song song (RBSS). Vấn đề 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. Thực chất, cũng có thể hiểu Hexapod là
một loại đồ gá CNC.
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 đợc định vị và định hớng của tấm động theo ý muốn.
Với ý thức tiếp cận một lĩnh vực mới mẻ của Robotics, vấn đề RBSS,
Đề tài khi đăng ký muốn nghiên cứu tạo dựng các sản phẩm đồ gá CNC,
trong đó nội dung chủ yếu là Hexapod. Tuy nhiên nội dung Hexapod không
đợc duyệt để cấp kinh phí vì nội dung này trùng lắp với một đề tài khác. Vì
thế nội dung về Hexapod không có trong Đề tài này.

4
Cơ cấu RBSS còn có nhiều loại hình khác nhau. Đề tài đã đi sâu
nghiên cứu vào 3 loại sau và đạt đợc những kết quả bớc đầu:
1. Cơ cấu RBSS phẳng, 3 chân
là loại cơ cấu RBSS dạng đơn giản này rất
thích hợp cho đồ gá CNC. Phụ thuộc vào đối tợng đợc gia công, có thể
dùng loại cơ cấu này với những biến thể rất khác nhau. Các chân đợc thay
đổi độ dài có thể bằng xilanh thủy lực, xilanh khí nén hoặc bằng vítme bi.
Liên quan đến vấn đề này Đề tài đã hoàn thành một công trình nghiên cứu

Hệ thống 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 hạn hẹp và khi đã lọt qua miệng hầm sẽ xòe rộng ra, đồng thời
nhờ khả năng thay đổi định vị và định hớng của tấm động, gắn liền với hệ
thống 4 cơ cấu này, mà quỹ đạo phun nớc đang đợc điều khiển theo ý
muốn.
Mọi thao tác nh làm thay đổi độ dài của 6 dây cáp treo tấm động và
làm co cụm hoặc xòe rộng 4 cơ cấu bình hành mang các vòi phun nớc,
đều đợc điều khiển theo chơng trình và các thiết bị điều khiển đều đợc
lắp đặt phía trên miệng hầm, nên rất thuận tiện.
Nội dung nghiên cứu của công trình này đợc xuất phát từ những lần
trao đổi, bàn bạc với Vietxopetro về nhiệm vụ rất bức xúc thay thế các lao
động dới các khoang hầm tầu chở dầu thô, vừa rất độc hại, vừa rất khó rửa
sạch nên rất chóng rỉ làm thủng vỏ tàu.
Các kết quả nghiên cứu tính toán và mô phỏng hoạt hình các thao tác
của Robot - Nhện nớc phục vụ cho nhiệm vụ này đã đợc trình bày với Xí
nghiệp Cơ điện dầu khi thuộc liên doanh Dầu khí Vietxopetro và đã đợc
đa vào kế hoạch triển khai. Nhng vì có sự thay đổi của cơ quan ứng dụng
nên đến nay vẫn cha thực hiện đợc. 6

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

B
2
O
P
z
y
x
1
1
2
z
3
z
0
x
x
3
z
y
x
2

1

2

3
0
0
z

} : Hệ cố định.
{Pxyz} : Hệ tọa độ động gắn liền với bàn máy động.

10
{A
i
x
i
y
i
z
i
}(i=1,2,3) : Hệ động gắn với chân thứ i.
Trong đó
iii
x AB
uuuur
và z
i


trục quay, còn y
i
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ẽ.


{Ox
0
y
0
z
0
}.
i
a
: Vector đại số chứa các tọa độ của điểm A
i
trên hệ cố định.
i
b
: Vector đại số chứa các tọa độ của điểm B
i
trên hệ cố định.
B
i
b
: Vector đại số chứa các tọa độ của điểm B
i
trên hệ động.
P: Vector đại số chứa các tọa độ của điểm P trên hệ cố định.
d
i
: Độ dài chân thứ i.
Trong đó :
Các ma trận
A

, Oy
0
,Oz
0
.
123
,,
ii i
eee
rr r
: Là 3 vector đơn vị trên các trục A
i
x
i
, A
i
y
i
, A
i
z
i
(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

OB OP PB=+
uuur uuuruuur
(i=1,2,3) (2.3)
Hay dới dạng đại số:

=+
ii
ba
A
.
i
R
0
0
i
d (i=1,2,3) (2.4)
và :
=+
i
bP
A
B
R
.

(i=1,2,3) (2.6)
Trong đó:

P
1, 2 3
,
T
p pp

=

;
B
i
b
=
,,
T
ix iy iz
bbb


;
=
i
a
1, 2 3
,
T
ii i

uvw (i=1,2,3) (2.7) 12
X
0
1
A
3
A
2
A
1
Z
0
Y
2
Z
3
Z
O
3

1


Quay quanh trục z một góc (
1
/2

+
).
Ma trận
A
2
R
đợ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 (
3
/2
π α
+
).
NÕu ta coi
i
A
α
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× :
α
=
i
A
10 0 sin cos 0
00 1.cos sin 0
01 0 0 0 1
ii
ii

(/2 )
i
π β
−
=
i
γ
;
VËy ta cã :
A
111
().
α
γ
=
z
RA A

A
1
=
R
11 1 1
11
11
cos sin 0 sin cos 0
sincos00 01
001cossin0
γ γαα
γγ
2222
(). ().
A
zz
RA A A
α
ηγ
=

A
2
=
R
22 22 2 2
22 22
22
cos sin 0 cos sin 0 sin cos 0
sin cos 0 sin cos 0 0 0 1
001001cossin0
η ηγγ αα
ηη γ γ
αα
−−−−
⎡⎤⎡⎤⎡ ⎤
⎢⎥⎢⎥⎢ ⎥
⎢⎥⎢⎥⎢ ⎥
⎢⎥⎢⎥⎢ ⎥
−

3

= cos
3

; S
3

= sin
3

; C
3

= cos
3

; S
3

= sin
3
C
2

=cos
2









3
=
A
R
3 3 33 3 3 3 33 3 33 3 3
33 33 3 33 33 3 33 33
33
()()
()()
0



+ + +



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
i
R
chỉ chứa các ẩn là các góc
i


còn
2

,
3


Y
0
nên
iz
b
= 0 (i=1,2,3)

15
+Víi i =1:
111111
2111
3111
.
xx x
yx y
zx z
p ub a u d
pub ud
pub ud
⎧
+=+
⎪
+=
⎨
⎪
+=
⎩

Suy ra :
11 1 1 1

u
b
ud p
u
b
ud p
u
b
(2.9)
+Víi i=2

12 22122
22 22222
32 222
(a)
(b)
(c)
xx xy x
yx yy y
zx zy z
pub vb a ud
pub vb a ud
pub vb ud
⎧
++=+
⎪
++=+
⎨
⎪
++=

λ
λ
λ
−
⎧
⎪
−
⎨
⎪
−
⎩
aa
bb
cc

Ta ®−îc:
1 1 21 2 2 31 3 3
2 1 22 2 2 32 3 3
312233
(1) ( )( )
(1) ( )( )
(1) )
λλλ
λλλ
λλλ
⎧
−+ = + − +
⎪
−+ = + − +
⎨

11 1 1 1
1 1 21 2 2 31 3 3
1
11 2
2 1 22 2 2 32 3 3
1
11 3
312233
1
(1) ( )( )
(1) ( )( )
(1) )
x
xx
x
y
yy
x
z
zz
x
audp
p aud aud
b
ud p
paudaud
b
ud p
pudud
b

2
b

H×nh 2.2.4
2
12
=
uuuur
BB
12 12
()()
T
bb bb
−−
=b
3
2
2
13
=
uuuur
BB
13 13
()()
T
bb bb
−−=
b
2
2

(i=1,2,3)

17
1
⇒=b
11 1 1
11
11
x
y
z
aud
ud
ud
+
⎡⎤
⎢⎥
⎢⎥
⎢⎥
⎣⎦
;
2
=b
21 2 2
22 2 2
22
x
y
z
aud

2
11 22 2 2 11 22 2 2 3
122 122
.
T
xx xx
yy yy
zz zz
a a ud u d a a ud u d
ud a u d ud a u d b
uud uud
−+ − −+ −
⎡⎤⎡⎤
⎢⎥⎢⎥
⇒−− −− =
⎢⎥⎢⎥
⎢⎥⎢⎥
−−
⎣⎦⎣⎦11 31 1 1 3 3 11 31 1 1 3 3
2
11 32 33 11 32 33 2
11 33 11 33
.
T
xx xx
yy yy
zz zz

⎢⎥⎢⎥
−−
⎣⎦⎣⎦

Hay :
2222
11 21 1 1 2 2 1 1 22 2 2 1 1 2 2 3
2222
11 31 1 1 3 3 1 1 32 3 3 1 1 3 3 2
2222
21 31 2 2 3 3 22 32 2 2 3 3 2 2 3 3 1
()()()
()()()
()( )()
xx y y zz
xx y y zz
xx yy zz
aaudud udaud udud b
aaudud udaud udud b
a a ud ud a a ud ud ud ud b
⎧
−+ − + −− + − =
⎪
⎪
−+ − + −− + − =
⎨
⎪
−+ − + −+ − + − =
⎪
⎩

Bài toán động học thuận là bài toán biết độ dài các chân d
i
(i=1,2,3), ta
phải tìm vị trí của bàn máy động P và ma trận
A
R
B
.
Theo phần trên ta thay các giá trị d
i
(i=1,2,3) vào hệ (4.14), ta sẽ đợc
hệ 6 phơng trình với 6 ẩn là :
123123
,,,,,
p pp Chú ý là 3 phơng trình sau của hệ (2.14) chỉ chứa d
i
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 d
i
và
i

và tam giác B
1
B
2
B
3
là các tam giác đều.

19
- PB
1
= h; OA
1
= g;
23
2/3

==

- Do kết cấu của cơ cấu ta có
iii
zAB

- Trục
ii
zOA



/2


h
h
;
3
B
b
=

2
3
2
0



g
g
;
3
a =

2
3
2
0 g
g
(2.16)
Do
/2
i







1
A
R
=

11
11
sin cos 0
001
cos sin 0








(2.17)

20

A
R
=

22
22
22
11 3
sin cos
222
33 1
sin cos
22 2
cos sin 0
αα
αα
αα
⎡⎤
−−
⎢⎥
⎢⎥
⎢⎥
−−
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎣⎦
(2.18)
3

A
R
=

33
33
33
113
sin cos
222
331
sin cos
222
cos sin 0
αα
αα
αα
⎡⎤
−
⎢⎥
⎢⎥
⎢⎥
−−
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎣⎦
(2.19)
Khi ®ã :

3 3 sin 3 sin sin sin
pd d d
pdd
pd d d
ggd gd dd dd




=+

=
=+ +
+ ++
2
12 1 2
2 22 2
1 1 3 3 13 1 3 1 3 13 1 3
2 22 2
22 33232323 23 2 3
2coscos 3
3 3 sin 3 sin sin sin 2 cos cos 3
3 3 sin 3 sin sin sin 2 cos cos 3
dd h
ggd gd dd dddd h
ggd gd dd dddd h

Chú ý là 3 phơng trình sau của hệ (2.20) chỉ chứa d
i
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 d
i
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 d
i
(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à :


=
=+


Với
6, 11, 15, 6njfb

== = =
, 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
iiii
Ax yz
gắn với tâm của khớp quay
i
A
, sao cho
i
x
nằm theo


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
i
A
so với hệ tọa độ cố định Oxyz. Còn biP
là toạ độ của điểm
i
B
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
i
A
(Hình 4.2.7): tuỳ thuộc vào kích thớc bàn cố định
[]
[]
,, *cos(),*sin(),0
TT
ii
ai xai yai zai ra ra





==



(2.23)
ciA là tọa độ của điểm
i
C
trong hệ trục
iiii
Ax yz
(Hình 4.2.4)
[]
* cos( ),0, *sin( )
T
ii
ciA la la

=
(2.24)