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 ục Xi, Yi: xác định như hình bên dướ i.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 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 2.3. Các ma tr ận A của Robot Yaskawa EP4000D
0
Các ma tr ận ận A được xác định bằng ma tr ận biến đổi tọa độ thuần nhất tổng quát:
Ai
C i S i 0 0
Sử dụng Matlab ta đượ c: c:
S i C i
S i S i
C i C i
C i S i
aS i
S i
C i
d
0
0
1
aC i
(2.1)
>> syms theta d a anpha A = [ cos(theta) -sin(theta)*cos(anpha) -sin(theta)*cos(anpha) sin(theta) cos(theta)*cos(anpha) cos(theta)*cos(anpha) 0 sin(anpha) 0 0 + Ma tr ận A1
sin(theta)*cos(anpha) sin(theta)*cos(anpha) -cos(theta)*sin(anpha) -cos(theta)*sin(anpha) cos(anpha) 0
>> 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 =
a*cos(theta); a*sin(theta); d ; 1];
[ 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]
ận xác định tọa độ 2.4. Tính toán ma tr ận T06 (hàm truyền R TH của robot hay ma tr ận vị trí và hướ ng 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 R TH 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) n x n y T n z 0 0
6
Trong đó:
sx
ax
px
sy
ay
py
sz
az
0
0
AA A A A A pz 1 1
2
3
4
5
6
(2.2)
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) 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) 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) 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): c):
Xuất phát từ phương trình độ ng học cơ bản: n x n y T n z 0 0
6
sx
ax
px
sy
ay
py
sz
az
0
0
AA A A A A (4.1) pz 1 1
2
3
4
5
6
Ta có: Tn Ti .Tni Ti 1Tn Tni Ai1... A21 A11Tn Tni
n x i n y Tn Ai ...A A n z 0 1
1
2
1
1
sx
ax
sy
ay
sz
az
0
0
px
py
pz 1
(4.2)
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 2 Ứ ng 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 q i. V ậy lần lượ t ta có: 1
0
A1 T6
1
1
1
1
1
1
T 6 (4.3) 0
A2 A1 T6
1
1
1
1
2
T 6 0
A3 A2 A1 T6
(4.4)
1
1
1
3
T 6 0
A4 A3 A2 A1 T6
1
(4.5)
1
4
T 6 0
A5 A4 A3 A2 A1 T6
(4.6)
5
T 6
(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 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 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 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 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 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 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 , , , , , tại A và B. 1
2
3
4
5
6
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: (t ) a i
i
bi t ci t
2
3
dit
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 đến i
giá tr ị cuối f trong thờ i gian tf . Ta có: '
i
''
i
bi
2ci
2ci t
3di t
2
6di t
Suy ra: ai
( t 0) 0
( t t
f
) f
3
ci
d i
Ta đượ c hệ phương trình:
t f2
( f 2 t f3
; (' t
; bi
' t f ) f
0
( f
' ' ( t 0 ) 0
)
0
2 tf
)
'
0
1 t 2f
1 t f
'
f
( 0' 'f )
1 (t ) a1 b1t c1t 2 d1t 3 2 3 2 (t ) a2 b2t c2t d t 2 3 3 (t ) a3 b3t c3t d 3t 2 3 4 (t ) a4 b4t c4t d 4t 2 3 ( t ) a5 b5t c5t d 5t 5 6 (t ) a6 b6t c6t 2 d 6t 3 2
