Robot Yaskawa EP4000D

CHƯƠNG I
GIỚI THIỆU VỀ ROBOT YASKAWA EP4000D
 CHƯƠNG II

XÂY DỰNG ĐỘNG HỌC THUẬN VỊ TRÍ-PHẦN MỀM TÍNH TOÁN

2.1. Xác định số khớp và các hệ trục tọa độ

- Robot Yaskawa EP4000D là robot có 6 bậc tự do, gồm 6 khớp quay và 7 thanh
nối.
- Ta sẽ lần lượt xác định các trục tọa độ như sau:

+ Xác định trục Zi: vì tất cả các khớp đều là khớp quay, nên Zi là trục mà khớp i+1
quay xung quanh nó.

+ Xác định trục Xi, Yi: xác định như hình bên dưới.
2.2. Xác định các thông số động học của robot Yaskawa EP4000D thông qua bảng
DH

- Ta xác định các thông số của bảng D-H như sau:

+di: khoảng cách Oi-1 và Oi theo trục zi-1(độ lệch với thanh nối i-1).

+i: góc quay của thanh nối i. Là góc quay quanh trục zi-1 để trục xi-1 chuyển đến
trục xi theo qui tắc bàn tay phải.

+i: góc xoay đưa trục zi-1về zi quanh xi theo quy tắc bàn tay phải (góc vặn của
thanh nối i).

+ai: Chiều dài thanh nối I (chiều dài đường vuông góc chung giữa trục zi và

zi-1; ai>=0).

Như vây ta lập được bảng DH như sau:

Khâu i di ai i
1 1 d1 0 90o
2 2 0 a2 0
3 3 d3 0 90o
4 4 d4 0 -90o
5 5 d5 0 90o
6 6 0 0 0
2.3. Các ma trận A của Robot Yaskawa EP4000D

Các ma trận A được xác định bằng ma trận biến đổi tọa độ thuần nhất tổng quát:

C i  S i C i S i S i aC i 
S C i C i C i S i aS i 
Ai    i
0 S i C i d  (2.1)
 
0 0 0 1 

Sử dụng Matlab ta được:

>> syms theta d a anpha
A = [ cos(theta) -sin(theta)*cos(anpha) sin(theta)*cos(anpha) a*cos(theta);
sin(theta) cos(theta)*cos(anpha) -cos(theta)*sin(anpha) a*sin(theta);
0 sin(anpha) cos(anpha) d ;
0 0 0 1];
+ Ma trận A1
>> syms q1 d1
A1= subs(A,{theta,d,a,anpha},{q1,d1,0,pi/2})

A1 =

[ cos(q1), 0, 0, 0]
[ sin(q1), 0, -cos(q1), 0]
[ 0, 1, 0, d1]
[ 0, 0, 0, 1]

+ Ma trận A2
>> syms q2 a2
A2= subs(A,{theta,d,a,anpha},{q2,0,a2,0})

A2 =

[ cos(q2), -sin(q2), sin(q2), a2*cos(q2)]

[ sin(q2), cos(q2), 0, a2*sin(q2)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

+ Ma trận A3
>> syms q3 d3
A3= subs(A,{theta,d,a,anpha},{q3,d3,0,pi/2})

A3 =

[ cos(q3), 0, 0, 0]
[ sin(q3), 0, -cos(q3), 0]
[ 0, 1, 0, 0]
[ 0, 0, 0, 1]

+ Ma trận A4
>> syms q4 d4
A4= subs(A,{theta,d,a,anpha},{q4,d4,0,-pi/2})

A4 =
[ cos(q4), 0, 0, 0]
[ sin(q4), 0, cos(q4), 0]
[ 0, -1, 0, d4]
[ 0, 0, 0, 1]

+ Ma trận A5
>> syms q5 d5
A5= subs(A,{theta,d,a,anpha},{q5,d5,0,pi/2})

A5 =

[ cos(q5), 0, 0, 0]
[ sin(q5), 0, -cos(q5), 0]
[ 0, 1, 0, d5]
[ 0, 0, 0, 1]

+ Ma trận A6
>> syms q6
A6= subs(A,{theta,d,a,anpha},{q6,0,0,0})

A6 =

[ cos(q6), -sin(q6), sin(q6), 0]

[ sin(q6), cos(q6), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

2.4. Tính toán ma trận T06 (hàm truyền RTH của robot hay ma trận xác định tọa độ
vị trí và hướng của khâu cuối)

Từ các ma trận biến đổi giữa các trục, ta sẽ xác định được hàm truyền RTH của
robot (hay chính là ma trận chuyển đổi giữa trục 0 và trục 6 của robot)

 nx sx ax px 
n sy ay p y 
T6   y
0
 A1 A2 A3 A4 A5 A6
 nz sz az pz  (2.2)
 
0 0 0 1

Trong đó:
nx =sin(q6)*(cos(q4)*sin(q1) - sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) +
cos(q6)*(cos(q5)*(sin(q1)*sin(q4)+cos(q4)*(cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3)))
- sin(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))).

ny=- sin(q6)*(cos(q1)*cos(q4)-sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))-

cos(q6)*(cos(q5)*(cos(q1)*sin(q4)+cos(q4)*(sin(q1)*sin(q2)*sin(q3)- cos(q2)*cos(q3)*sin(q1)))
+ sin(q5)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))).

nz=cos(q6)*(cos(q2+ q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5)) - sin(q2 + q3)*sin(q4)*sin(q6).

sx=cos(q6)*(cos(q4)*sin(q1) - sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) -

sin(q6)*(cos(q5)*(sin(q1)*sin(q4)+cos(q4)*(cos(q1)*cos(q2)*cos(q3)- cos(q1)*sin(q2)*sin(q3)))
- sin(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))).

sy=sin(q6)*(cos(q5)*(cos(q1)*sin(q4)+cos(q4)*(sin(q1)*sin(q2)*sin(q3)-
cos(q2)*cos(q3)*sin(q1)))+sin(q5)*(cos(q2)*sin(q1)*sin(q3)+cos(q3)*sin(q1)*sin(q2)))-
cos(q6)*(cos(q1)*cos(q4) - sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))).

sz=- sin(q6)*(cos(q2+q3)*sin(q5)+ sin(q2 + q3)*cos(q4)*cos(q5)) - sin(q2 + q3)*cos(q6)*sin(q4).

ax=sin(q5)*(sin(q1)*sin(q4) + cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) +

cos(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)).

ay=cos(q5)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - sin(q5)*(cos(q1)*sin(q4) +

cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))).

az=sin(q2 + q3)*cos(q4)*sin(q5) - cos(q2 + q3)*cos(q5).

px=d3*sin(q1)+d5*(cos(q4)*sin(q1)-sin(q4)*(cos(q1)*cos(q2)*cos(q3)-
cos(q1)*sin(q2)*sin(q3)))+d4*(cos(q1)*cos(q2)*sin(q3)+cos(q1)*cos(q3)*sin(q2))+a2*cos(q1)*
cos(q2).

py=d4*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) - d3*cos(q1) - d5*(cos(q1)*cos(q4) -

sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) + a2*cos(q2)*sin(q1).

pz=d1 - d4*cos(q2 + q3) + a2*sin(q2) - d5*sin(q2 + q3)*sin(q4).

CHƯƠNG III

MA TRẬN JACOBI

CHƯƠNG IV

XÂY DỰNG ĐỘNG HỌC ĐẢO VỊ TRÍ ROBOT

Bài toán động học đảo (động học ngược):

Xuất phát từ phương trình động học cơ bản:

 nx sx ax px 
n sy ay p y 
T6   y
0
 A1 A2 A3 A4 A5 A6 (4.1)
 nz sz az pz 
 
0 0 0 1

Ta có:

Tn  Ti .Tni  Ti 1Tn  Tni  Ai1... A21 A11Tn  Tni

 nx sx ax px 
 p y 
1 1 1  n y sy ay
 Tn  Ai ... A2 A1
i
(4.2)
 nz sz az pz 
 
0 0 0 1

Ứng với mỗi giá trị của i, khi so sánh các phần tử tương ứng của 2 matrix ở 2
vế của biểu thức (4.2) ta có 6 phương trình độc lập để xác định biến khớp qi. Vậy
lần lượt ta có:

A11T60  T61 (4.3)

A21 A11T60  T62 (4.4)
A31 A21 A11T60  T63 (4.5)
A41 A31 A21 A11T60  T64 (4.6)
A51 A41 A31 A21 A11T60  T65 (4.7)
Ta so sánh phần tử hàng 3 cột 4 của (4.3):

d3 + d5c4 = pxs1 - pyc1 (4.8)

Ta so sánh phần tử hàng 1 và hàng 2 cột 4 của (4.4):

(pz-d1).s2 + (pxc1+pys1).c2 - a2 = d4s3 - d5c3s4 (4.9)

 (pz-d1).c2 - (pys1+pxc1).s2= -d4c3 - d5s3c4 (4.10)
Ta so sánh hàng 3 cột 1, cột 2 và cột 3 của (4.4):

axs1 - ayc1 = s4s5 (4.11)

sxs1 - syc1 = c4c6 - c5s4s6 (4.12)
nxs1 - nyc1 = s4c5c6 + c4s6 (4.13)
Từ (4.8),(4.9),(4.10),(4.11),(4.12),(4.13) ta sẽ giải ra được các giá trị qi với
(i=1 - 6).

Ta thấy cũng tùy thuộc vào cơ cấu loại robot cụ thể mới xác định được biểu
thức tính qi dạng công thức cụ thể.
CHƯƠNG V

THIẾT KẾ QUỸ ĐẠO TRONG KHÔNG GIAN KHỚP

Chọn hai điểm bất kỳ trong không gian làm việc của robot. Sau đó đi thiết kế quỹ

đạo chuyển động Robot theo quỹ đạo bậc 3

5.1. Giới thiệu và cơ sở thiết kế quỹ đạo

Thiết kế quỹ đạo chuyển động của robot có lien quan mật thiết đến bài toán điều

khiển robot di chuyển từ vị trí này sang vị trí khác trong không gian làm việc. Đường
đi và quỹ đạo được thiết kế là đại lượng đặc trưng cho hệ thống điều khiển vị trí của
robot. Do đó độ chính xác của quỹ đạo sẽ ảnh hưởng đến chất lượng di chuyển của
robot.

Yêu cầu thiết kế quỹ đạo chuyển động của Robot là:

+ Khâu chấp hành phải đảm bảo đi qua lần lượt các điểm trong không gian làm việc

hoặc di chuyển theo một quỹ đạo xác định.

+ Quỹ đạo của Robot phải là đường liên tục về vị trí trong một khoảng các nhất

định.
+ Không có bước nhảy về vận tốc, gia tốc.

Quỹ đạo là các đường cong có dạng đa thức bậc 3: x(t )  a  bt  ct 2  dt 3

5.2 Tính toán thiết kế quỹ đạo chuyển động

Thiết kế quỹ đạo trong không gian khớp: Chọn hai điểm A, B bất kỳ trong không
gian làm việc, biết tọa độ (xE, yE, zE) và hướng của các khâu thao tác. Thiết kế quỹ
đạo chuyển động bất kỳ từ A đến B. Theo bài toán động học đảo ta xác định được
các biến khớp 1 ,2 ,3 ,4 ,5 ,6 tại A và B.

Chọn quỹ đạo thiết kế là hàm đa thức bậc 3 theo thời gian như sau:

Giả sử khớp thứ i: i (t )  ai  bi t  cit  dit

2 3

Với i=1-6 tương ứng với 6 biến khớp. Ta giả sử khớp di chuyển từ giá trị đầu  i đến
giá trị cuối  f trong thời gian tf. Ta có:

i'  bi  2cit  3dit 2

i''  2ci  6dit
Suy ra:

ai   (t 0)  0 ; bi  ('t 0)  0'

(t t )   f ;('t t )   f'
f f

3 2 1
ci  2
( f  0 )   0'   f'
tf tf tf
2 1 '
di   3
( f   0 )  2
( 0   'f )
tf tf

Ta được hệ phương trình:

1 (t )  a1  b1t  c1t 2  d1t 3

 2 (t )  a2  b2t  c2t  d 2 t
2 3


3 (t )  a3  b3t  c3t  d3t
2 3


 4 (t )  a4  b4t  c4t  d 4t
2 3


 5 (t )  a5  b5 t  c5 t 2
 d 5 t 3

 (t )  a  b t  c t 2  d t 3
 6 6 6 6 6