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 5.

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

HÀ NỘI 2006

1
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

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

3
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


I. mở đầu
Đồ gá gia công phụ thuộc vào loại hình phôi gia công và quy trình
công nghệ đối với loại sản phẩm đang đợc gia công. Trên máy công cụ

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

tạo ra thiết bị gá lắp nhanh để gia công vỏ hộp động cơ xe máy trên Trung
tâm gia công CNC nhập từ Anh Quốc của Công ty Đồng Tháp. Phơng án
thiết kế đã chọn lựa là dùng hệ thống xilanh khí nén, bố trí tác động lần lợt
vào 3 chân của cơ cấu RBSS. Hợp đồng và bản thuyết minh công trình này
giới thiệu trong phần phụ lục.
2. Robot song song RBSS - 322
có thể hoạt động nh một thiết bị gia công
CNC. Các chân của RBSS đợc xếp thành 3 nhánh đôi, mỗi chân có 2 phần:
phần chân trên và phần chân dới. Dáng dấp bên ngoài của RBSS - 322
giống nh kiểu Robot Flex - Picker của Hãng ABB. Khi thiết kế đã tham
khảo các thông tin chào hàng của Hãng này, nhng kết cấu các bộ truyền
bên trong đã đơn giản hóa đi nhiều cho phù hợp khả năng tự tạo trong nớc
với khoản kinh phí dành cho công việc này rất hạn chế. Định hớng ứng
dụng RBSS trong công việc điêu khắc trên các vật liệu dễ gia công nhng có
kích thớc lớn, cồng kềnh, không đa lên các máy gia công thông thờng
đợc. Phần đạt đợc kết quả hơn và thu hoạch đợc nhiều hơn là xây dựng
chơng trình phần mềm điều khiển RBSS - 322.
3. Robot Nhện nớc
là một đề xuất mới, đợc phát triển trên nguyên lý
RBSS và gồm 2 phần. Phần thứ nhất là một RBSS hoạt động theo nguyên lý
cơ cấu Stewart. Tấm động đợc treo trên tấm giá cố định 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. Phần thứ 2 gắn với tấm động
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ó

5
thể xòe rộng ra hoặc co cụm lại. Hệ thống này làm nhiệm vụ 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 đợc, ví dụ vì môi trờng
quá độc hại.

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

II. C¬ cÊu RBSS 3 ch©n









7
2.1. Robot song song 3 chân RPS

2.1.1. Kết cấu hình học

Rô-bốt song song 3 RPS (Tên gọi Rô-bốt song song 3 RPS là do loại
rô-bốt này có cấu tạo gồm 3 chân, mỗi chân gồ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

ib
i
Fnj ff

=
=+


Với
6, 8, 9, 0njfb

=== =
, 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
B
3
1
B
A
1
A
2
3
A

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

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 A
i
B
i
Z
i
(các trục quay)
O và P là trọng tâm của hai tam giác A
1
A
2
A
3
và B
1
B
2
B
3
.
Ta đặt các hệ tọa độ:
{Ox
0
y
0
z
0

} : 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ẽ.

0ii
zx

=

Sử dụng các ký hiệu:
A
R
B
: Ma trận cosin chỉ hớng của hệ {Pxyz} so với hệ cố định
{Ox
0
y
0
z
0
}.
A
R
i
: Ma trận cosin chỉ hớng của hệ {A
i
x
i
y
i
z
i
} so với hệ cố định

{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

i
R
có thể biểu diễn dới dạng:
A
i
R
=
01 1 01 2 01 3
02 1 02 2 02 3
03 1 03 2 03 3
ii i
ii i
ii i
ee ee ee
ee ee ee
ee ee ee





rr rr rr
rr rr rr
rr rr rr
(i=1,2,3) (2.1)
01 02 03
,,eee
rrr
: Là 3 vector đơn vị trên các trục Ox
0

, 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

Ma trận
A
B
R
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à

.
i
a
và
B
i
b
: 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 B
i

trên hệ cố định có thể biểu diễn dới dạng:

iiii
OB OA A B=+
uuur uuur uuuur
(i=1,2,3) (2.2)
và :
ii

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
.

B
i
b
(i=1,2,3) (2.5)
Kết hợp hai phơng trình trên ta có:

+P
A
B
R
.
B
i
b
=
+
i
a
A
i
R
.0
0
i
d

(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

aa a




A
B
R
=
x xx
yy y
z zz
uvw
uvw
uvw





;
A
i
R
=
ix ix ix
iy iy iy
iz iz iz
uvw
uvw

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

2


2
3


Hình 4.2.3
Các ma trận cosin chỉ hớng A
i
R
đợc xác định bởi các phép quay liên tiếp:
Ma trận
A
1
R
đợ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
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 (
2
/2

+
).
Ma trận
A
3
R
đợ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
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

α α
αα
−−
⎡⎤⎡ ⎤
⎢⎥⎢ ⎥
−−
⎢⎥⎢ ⎥
⎢⎥⎢ ⎥
⎣⎦⎣ ⎦

α
=
i
A
sin cos 0
001
cos sin 0
ii
ii
α α
αα
−−
⎡⎤
⎢⎥
⎢⎥
⎢⎥
−
⎣⎦
(2.8)
§Æt :

(/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
γ γαα
γγ

αα
−−−
⎡⎤⎡ ⎤
⎢⎥⎢ ⎥
⎢⎥⎢ ⎥
⎢⎥⎢ ⎥
−
⎣⎦⎣ ⎦


1
=
A
R
11 1 1 1
11 1 1 1
11
cos sin cos cos sin
sin sin sin cos cos
cos sin 0
γ αγα γ
γ αγα γ
αα
−−
⎡⎤
⎢⎥
−
⎢⎥
⎢⎥
⎣⎦

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
η ηγγ αα
ηη γ γ
αα
−−−−
⎡⎤⎡⎤⎡ ⎤
⎢⎥⎢⎥⎢ ⎥
⎢⎥⎢⎥⎢ ⎥
⎢⎥⎢⎥⎢ ⎥
−

⎣⎦⎣⎦⎣ ⎦


14
2
=
A
R
2 2 22 2 22 2 2 2 22 2 2
22 22 2 22 22 2 22 22
22
()()
()()
0



+


+ + +




CC SS S CS SC C CS SC
SC CS S SC CS C SS CC
CS

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


3333
().().
A
zz
RA A A


=

3
=
A
R
33 3 3 3 3
33 3 3
33
cos sin 0 cos sin 0 sin cos 0
sin cos 0 sin cos 0 0 0 1
001001cossin0

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



+ + +

+ + +



CC SS S CC SS C CS SC
SC CS S SC CS C SS CC
CS

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
i
R
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 A
i
thuộc mặt phẳng X
0
Y
0
nên
3
i
a
= 0 (i=1,2,3)
A
1
trên trục X
0
nên
12
0
a
=

Và B
i
thuộc mặt phẳng X
0

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

1
11 2
1
11 3
1
+−
⎧
=
⎪
⎪
⎪
−
=
⎨
⎪
⎪
−
=
⎪
⎩
x
x
x
y
y
x
z
z
x
audp

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
⎧
++=+
⎪
++=+
⎨
⎪
++=

⎩
(2.10)
+Víi i=3

13 33133
23 33233
33 333
(a)
(b)
(c)
xx xy x
yx yy y
zx zy z
pub vb a ud
pub vb a ud
pub vb ud
⎧
++=+
⎪
++=+
⎨
⎪
++=
⎩
(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 ))

λ
λ
λ
−
⎧
⎪
−
⎨
⎪
−
⎩
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) )
λλλ
λλλ
λλλ
⎧
−+ = + − +
⎪
−+ = + − +
⎨

⎪
−+ = −
⎩
xxx
yyy
zzz
p uaudaud
p uaudaud
puudud
(2.12)


16
Víi
3
2
y
y
b
b
λ
=
,
12 3
x x
bb
λ λ
=−

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
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.13)
MÆt kh¸c, dùa vµo kÕt cÊu cña bµn di ®éng B ta cã :
B
1
B
2
3
B
b
1
3
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

2
23
=
uuuur
BB
23 23
()()
T
bb bb
−−
=b
1
2
víi :
=+
ii
ba
A
.
i
R
0
0
i
d
⎡⎤
⎢⎥
⎢⎥
⎢⎥
⎣⎦

(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

aud
ud
+
⎡ ⎤
⎢ ⎥
+
⎢ ⎥
⎢ ⎥
⎣ ⎦
;
3
=b
31 3 3
32 3 3
33
x
y
z
aud
aud
ud
+
⎡ ⎤
⎢ ⎥
+
⎢ ⎥
⎢ ⎥
⎣ ⎦

11 21 1 1 2 2 11 21 1 1 2 2

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

aaudud aaudud
ud a ud ud a ud b
ud ud ud ud
−+ − −+ −
⎡⎤⎡⎤
⎢⎥⎢⎥
−− −− =
⎢⎥⎢⎥
⎢⎥⎢⎥
−−
⎣⎦⎣⎦


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
22 33 22 33
.
T
xx xx
yy yy
zz zz
aaudud aaudud
a a ud ud a a ud ud b
ud ud ud ud
−+ − −+ −
⎡⎤⎡⎤
⎢⎥⎢⎥
−+ − −+ − =
⎢⎥⎢⎥

⎢⎥⎢⎥
−−
⎣⎦⎣⎦

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
⎧
−+ − + −− + − =
⎪
⎪
−+ − + −− + − =
⎨
⎪
−+ − + −+ − + − =
⎪
⎩

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ố
123123123
,,,,,,,,
ddd pp p

. Các
thành phần
,,
ix iy iz
uuu
đã xác định đợc, các thành phần
,,
x yz
uuu
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 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à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ị của P vào hệ (2.14), ta sẽ đợc hệ 6 phơng trình với 6 ẩn là :
123123
,,,,,
ddd

.
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 A
1
A
2
A
3

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

i

=

Khi đó các đại lợng trong công thức (4.6) trở thành :
1
B
b
=
0
0





h
;
2
B
b
=

2
3
2
0

h
h
;
3
B
b
=

2
3
2
0

h
h
(2.15)

1
a =

0
0





g
;
2
a =

2
3
2
0

g
g
;
3
a =

2
3
2
0











g
g
(2.16)
Do
/2
i

=
nên
i

=
(/2 )
i


= 0
Khi đó các ma trận cosin chỉ hớng
A
R
i
trở thành:
1
A
R
=

11 1 1 1
11 1 1 1
11
cos sin cos cos sin
sin sin sin cos cos
cos sin 0

1
A
R
=

11
11
sin cos 0
001
cos sin 0








(2.17)

20

2
A
R
=
222 2222 2
222 222 2 2
22
13 13 13
( cos sin )sin ( cos sin )cos sin cos
22 22 22
31 31 31
( cos sin )sin ( cos sin )cos sin cos
22 22 22
cos sin 0
γ γα γ γα γ γ
γ γα γ γα γ γ
αα
⎡ ⎤
+−+ −
⎢ ⎥
⎢ ⎥
⎢ ⎥
−+ − −−
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎣ ⎦

2

A
R
=

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

A
R
=
333 3333 3
333 333 3 3
33
13 13 13
( cos sin )sin ( cos sin ) sin cos
22 22 22
31 31 31
( cos sin )sin ( cos sin )cos sin cos
22 22 22
cos sin 0
γ γα γ γα γ γ
γ γα γ γ α γ γ
αα
⎡⎤
−−+ +
⎢⎥
⎢⎥
⎢⎥
+−− −
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎣⎦
cos

3

A
R
=

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

3
2
y
y
b
b
λ
=
= -1 ;
12 3
x x
bb
λ λ
= −
= h;
Thay vµo hÖ (2.14) ta ®−îc :

21
1223311
22233
31 12 23 3
2 22
11 2 2121212
1
3 ( sin sin ) sin
2
3
3(sinsin)
2
3 cos cos cos (4.20)

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

=


+ ++ =


+ ++ =

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 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ệ (2.20), 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.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à :

123123
,,,,,
ddd

.
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
ib
i
Fnj ff

=
=+


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

hớng kéo dài của
i
OA
và trục
i
y
dọc theo trục các khớp quay, còn trục
i
z

song song với z. Góc
i

đo từ x đến
i
x
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
i
x
và
ii
AC
.


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
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.21)
Tọa độ điểm
i
C

[]
,, .
T
ci xci yci zci ai Ri ciA==+
(2.22)
Trong đó:
Ri là ma trận cosin chỉ hớng của hệ trục
iiii
Ax yz
so với hệ Oxyz. Để
tính R
i
ta chỉ việc xoay hệ Oxyz quanh trục z một góc
i

, khi đó:
cos( ) sin( ) 0
(, ) sin( ) cos( ) 0
001
ii
ii i i
RRz

==



(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)