Đồ án Cơ điện tử
Đồ án Cơ điện tử
1. GIỚI THIỆU
Ra đời cách đây nửa thế kỷ, robot công nghiệp đã có những phát triển
vượt bậc. Nhiều nước trên thế giới sớm áp dụng mạnh mẽ kỹ thuật robot vào
sản xuất và nó đã đem lại những hiệu quả to lớn về kinh tế và kỹ thuật, nâng
cao năng suất lao động, tăng chất lượng và khả năng cạnh tranh của sản phẩm,
cải thiện điều kiện làm việc của công nhân
Đối với nước ta, kỹ thuật robot vẫn còn là vấn đề khá mới mẻ, nhất là
việc nghiên cứu thiết kế, chế tạo robot. Nội dung nghiên cứu nhằm chế tạo
một robot sáu bậc tự do, kiểu robot PUMA, sử dụng card điều khiển LAB-
PC+ để điều khiển các động cơ bước dẫn động các khớp. Sản phẩm được sử
dụng trong nghiên cứu và giảng dạy môn robot công nghiệp tại trường Đại
học Bách khoa Hà Nội.
2. SƠ ĐỒ ĐỘNG VÀ HỆ TỌA ĐỘ CỦA ROBOT PUMA
Robot PUMA là robot có 6 bậc tự do, cấu hình RRRRRR.

Sơ đồ động và hệ tọa độ gắn trên các khâu của robot như sau:
Hình 1: Hệ tọa độ gắn trên các khâu của robot.
Đồ án Cơ điện tử
BẢNG THÔNG SỐ DENAVIT - HARTENBERG (DH)
Khâu
θ
i
α
i
a
i
d
i
1

π
−
0 D
4
5
θ
5
*
2
π
0 0
6
θ
6
*
0 0 d
6
+ Chọn a
1
= 00 mm; a
2
= 0 mm;
a
3max
= 0mm.
+ Giới hạn chuyển động cuả các khâu:
- Khâu 1: -90
0
≤ θ
1

). Trans(a,0,0). Trans(0,0,d). Rot(x, α)
Hay
A
i
=












−
−
1000
cossin0
sinsincoscoscossin
cossinsincossincos
d
a
a
αα
θαθαθθ
θαθαθθ
Qui ước viết tắt các hàm lượng giác như sau:

0 1 0
0 0 0 1
C S
S C
d
−
 
 
 
 
−
 
 
; A
2
=
2 2 2 2
2 2 1 2
2
0
0
0 0 1
0 0 0 1
C S a C
S C a S
d
−
 
 
 

0 1 1
0 0 0 1
C S
S C
d
−
 
 
 
 
−
 
 
;
A
5
=
5 5
5 5
0 0
0 0
0 1 0 0
0 0 0 1
C S
S C
 
 
−
 
 

6;
4
T
6
= A
5
. A
6
;
3
T
6
= A
4
.A
5
.A
6;
2
T
6
= A
3
.A
4
.A
5
.A
6 ;
1

6 ;
Ta còn có ma trận trạng thái cuối:
T
E
=
0 0 0 1
x x x x
y y y y
z z z z
n s a p
n s a p
n s a p
 
 
 
 
 
 
Ma trận T
E
mô tả hướng và vị trí của hệ toạ độ gắn trên khâu chấp hành
cuối đối với hệ tọa độ gốc. Trong đó:
, s, an
r r
r
là các véctơ chỉ phương của hệ
tọa độ gắn trên khâu chấp hành cuối,
p

là véctơ điểm chỉ vị trí của gốc hệ tọa

−
 
 
 
 
 
 
;
4
T
6
= A
5
. A
6 =
5 6 5 6 5 5 6
5 6 5 6 5 5 6
6 6
. .
. .
1 0
0 0 0 1
C C C S S S d
S C S S C C d
S C
−
 
 
− −
 

[1,4]=
[2,1]=
[2,2]=

[2,3]=

[2,4]=

[3,1]=
[3,2]=
[3,3]=
[3,4]=
1
T
6= A
2.
A
3
.A
4
.A
5
.A
6
[1,1]=

(CHƯA CHỈNH SỬA XONG)

= - C
1
S
24
- S
1
C
24
= - S
124
o
y
= - S
1
S
24
+ C
1
C
24
= C
124
o
z
= 0
a
x
= 0
a
y

.
4. PHƯƠNG TRÌNH ĐỘNG HỌC NGƯỢC ROBOT PUMA
Trong thực tế, thường ta biết trước vị trí và hướng mà khâu chấp hành
cuối của robot cần đạt đến. Điều ta cần biết là giá trị của các biến khớp (góc
quay) tại mỗi thời điểm đó. Giải hệ phương trình (1), khi biết trước hướng và
vị trí của hệ tọa độ gắn trên khâu chấp hành cuối, ta sẽ xác định được tệp
nghiệm (θ
1
, θ
2
, θ
3
, θ
4,
θ
5,
θ
6
) là giá trị của các biến khớp. Các phương trình xác
định giá trị các biến khớp thông qua các véctơ
p ,a ,o ,


n
được gọi là hệ phương
trình động học ngược của robot.
(1)
Đồ án Cơ điện tử
Tham khảo bài giảng của GS.TSKH Nguyễn Văn Khang trong bài giải về
việc giải một bài toán động học ngược bằng phương pháp số.

=
K
K
(4.5)
trong đó
O
và
R
là hướng và vị trí của dụng cụ, được biểu diễn đối với hệ tọa
độ cố định bên ngoài (world coordinate system). Ta phải giải bài toán trên
đối với các ẩn
61
,, qq 
.
Đồ án Cơ điện tử
Pỉeper’Solution ứng dụng cho cơ cấu 6 khớp quay có 3 khớp cuối giao
nhau có phương pháp giải như sau:
Vị trí của tâm cổ tay,
c
p
xác định qua vị trí công cụ (The given tool
position) và phương của Tool pointing (
6
Z
).Do đó vị trí của tâm cổ tay phụ
thuộc vào 3 biến khớp đầu tiên.
The relative wrist oriention
6
3
R

θ θ θ
.
+ Định vị trí của tâm cổ tay
c
O
có tọa độ cho trước như sau:
Đồ án Cơ điện tử
0
6
0
. . 0
1
C
O O d R
 
 
= −
 
 
 
trong đó
O
và
R
là hướng và vị trí của dụng cụ, được
biểu diễn đối với hệ tọa độ cố định bên ngoài (world coordinate system).
c
P
as
6

Tính
1
3 0 0
6 3 6
R R R
−
=
sau khi đã thay giá trị của các biến khớp vào
1 2 3
, ,
θ θ θ
vào
0
3
R
1 2 3
( , , )
θ θ θ
+So sánh
3
6
R
và
3
6 4 5 6
( , , )R
θ θ θ
để rút ra
4 5 6
, ,

T
.
6
r
E
=
6
6
6
.
0
0
.
.
6
1
1
0 0 0 1
x p
y p
z p
n s a p
a d x
x x x x
n s a p
a d y
y y y y
d
a d z
n s a p

Ma trận chuyển từ hệ tọa độ 0
→
3:
0
R
3=
A
1.
A
2.
A
3
=
23
3 23 2 2 1 2
3 23 2 2 1 2
3 23 2 2
.
( . . ) .
1 23
1 1
1
.
( . . ) .
1 1 23
1
1
. .
0
23

R
:
r
C
=
0
R
3
.
4
r
C =
4 3 23 2 1 2 1 2
4 3 23 2 1 2 1 2
4 23 3 23 2 2 1
. . . . . .
1 23 1
. . . . . .
1 23 1
. . .
C S d a C C a C C S d
S S d a S C a S C C d
d C a S a S d
t
 
+ + −
 
 
+ + +
 

1 6 6
2 2
6 6
( ) os ( )sin
cos tan 2( , )
( ) ( )
p y p x
p x p y
p y p x
d y a d c x a d
d
Ar A x a d y a d
y a d x a d
θ θ
θ
= − − −
 
 ÷
⇒ = ± − − −
 ÷
− + −
 
Nhân (2) với sin
1
θ
cộng với (1) nhân vói cos
1
θ
và kết hợp với (3) ta có
hệ [@]

2 4 3
( )
cos tan 2( , )
2
x y
P P a a d
Arc A d a a a
a d a
θ
 
+ − + +
 ÷
⇒ = ± +
 ÷
+
 
Khai triển hệ phương trình liên kết [@] ta được:
Đồ án Cơ điện tử
3 3 2 3 4 2 3 4 3 3 2
3 4 3 3 2 2 3 4 3 3 2
( . . ) ( . .sin )sin
( . . ) os ( . . os )sin
x
y
a C a S d C C d a P
C d a S c a S d a c P
θ θ
θ θ θ
+ + + − =
− − + + =

tan 2 ( ) ( ) ,( ) ( )
x y x
A d C a S P a d S a C P a d S a C P d C a S
θ
 
⇒ = − − + + + + + −
 
Ta có
3
6
R
=
4 5 6
A A A
=
4 4
4 4
4
0 0
0 0
0 1 1
0 0 0 1
C S
S C
d
−
 
 
 
 

 
 
 
 
 
=
Mặt khác:
1 2 3
A A A
4 5 6
A A A
=
E
T
0 3
3 6
.
E
R R T⇔ =
3 0( 1)
6 3
.
E
R R T
−
⇒ =
1 23 1 23 23 1 23 1 23 23 1 23 1 23 23 14
1 1 1 1 1 1 24
1 23 1 23 23 1 23 1 23 23 1 23 1 23 23 34
0 0 0 1

5
θ
=
23 1 1 23
cos ( )
x y z
Arc S C a S a C a
 
± + −
 
Đồ án Cơ điện tử
Nếu sin
5
0
θ
≠
⇒
[ ]
[ ]
3
6 4 5 1 1
1 1
4
5
3
6 4 5 23 1 1 23
23 1 1 23
4
5
1 1 23 1 1 23

θ
θ
θ θ
⇒ = − +
− +
⇒ =
⇒ = + −
+ −
⇒ =
− + + −
=
[ ]
[ ]
3
6 6 5 1 23 1 23 23
1 23 1 23 23
6
5
3
6 6 5 1 23 1 23 23
1 23 1 23 23
6
5
23 1 1 23 23 1 1
6
5
3,2 sin *sin
sin
sin
3,1 os *sin

⇒ = −
+ + + +
= −
23
5
)
sin
z
n
θ
Đồ án Cơ điện tử
TÀI LIỆU THAM KHẢO
[1] Nguyễn Thiện Phúc, Robot công nghiệp, NXB Khoa học và Kỹ thuật,
Hà Nội, 2004.
[2] Nguyễn Văn Khang, Động lực học hệ nhiều vật ,NXB Khoa học và Kỹ
thuật
[3] www.google.com