Báo cáo robot EP4000d Môn Kỹ thuật Robot

MỤC LỤC


Chương 1: Giới thiệu tổng quan về robot YASKAWA EP4000D

Giới thiệu hãng hãng YASKAWA

1.1

Robot EP4000D do hãng YASKAWA tại Nhật Bản thiết kế và chế tạo. Hãng
được thành lập năm 1915 với lịch sử trên 100 năm, là tập đoàn hàng đầu thế giới
trong sản xuất và cung cấp các sản phẩm trong lĩnh vực robot công nghiệp, biến
tần, truyền động điện...
Các sản phẩm của Yaskawa là sự kết hợp giữa thành tựu nghiên cứu phát triển về
cơ và điện tử kết tinh trong một sản phẩm. Yaskawa là công ty đầu tiên trên thế
giới đưa ra khái niệm “Mechatronics – cơ điện tử” nổi tiếng và giờ đây đã trở
thành một lĩnh vực mũi nhọn thu hút được rất nhiều sự quan tâm vì nó tạo ra các
sản phẩm tự động hóa đáp ứng được các yêu cầu ngày càng cao trong đời sống sản
xuất

Giới thiệu về Robot EP4000D

1.2

Robot EP4000D được phát triển để ứng dụng vào các dây truyền sản xuất công
nghiệp với phù hợp với nhiều công việc khác nhau như:
•
•
•
•
•

Robot gắp (Handling)
Robot nâng bốc, đóng gói ( Picking/packing, palletizing)
Robot hàn, hàn điểm ( Arc handling, spot welding)
Robot sơn ( Painting)
Robot lắp ráp ( Assembly/distributing)

Robot YASKAWA EP4000D

Page 2


Do vậy việc sử dụng sản phẩm robot EP4000D của Yaskawa sẽ đảm bảo tính
tương thích, độ nhất quán và chất lượng dịch vụ cao... trong việc tự động hóa các
máy móc, dây chuyền sản xuất vì Yaskawa có thể cung cấp tát cả các loại robot để
giải quyết các bài toán và đáp ứng các yêu cầu khác nhau của khách hàng.
Các robot EP4000D có trọng tải 200 kg, chiều ngang ngang 3,505 mm và tầm
với thẳng đứng 2,629 mm. Thiết kế gọn nhẹ của robot cho phép làm việc trong
không gian chật hẹp một cách kinh hoạt
Đây là loại robot có sáu trục quay vận hành bằng khí nén kết hợp với động cơ
điện, giúp robot vận hành một cách linh hoạt, di truyển dễ dàng dến từng vị trí đảm
bảo độ chính xác của robot trong công việc.
Hình ảnh bên dưới cho ta thấy các kích thước của robot, cũng như vùng làm việc
của robot là vùng có màu xanh.

Robot YASKAWA EP4000D

Page 3

Thông số kĩ thuật của robot

•

Bộ điều khiển Robot

Robot YASKAWA EP4000D

Page 4


Robot dùng bộ diều khiển DX100, giúp người vận hành có thể dễ dàng lập trình
và điều khiển robot, cũng như dễ dàng kết nối robot với hệ thống trong dây chuyền
sản xuất.
Dưới đây là các thông số kĩ thuật của bộ điều khiển.

Chương 2: Động học thuận vị trí
Robot YASKAWA EP4000D

Page 5


2.1 Xây dựng phương trình động học thuận
2.1.1 Gắn trục tọa độ lên các khâu

Hình 2.1 Lựa chọn các hệ tọa độ cho từn khớp robot
Bảng 2.1 Bảng thông số Denavit-Hartenberg
i
1
2

3
4
5
6

ai
550
1100
250
0
0
0

Robot YASKAWA EP4000D

αi
90
0
90
-90
90
0

Page 6

θi
θ1*
θ2*
θ3*
θ4*

θ5*
θ6*

di
740
0
2135
0
255
0


2.1.2 Xác định các ma trận A
Ma trận An có dạng :

Thực hiện trên matlab, ta đặt θ = i.
>> syms a1 d1 a2 a3 d3 d5;
>> syms i1 i2 i3 i4 i5 i6;
>> c1=cos(i1);c2=cos(i2);c3=cos(i3);c4=cos(i4); c5=cos(i5);c6=cos(i6);
>> s1=sin(i1);s2=sin(i2);s3=sin(i3);s4=sin(i4); s5=sin(i5);s6=sin(i6);
>> A1 = [c1 0 s1 a1*c1; s1 0 -c1 a1*s1; 0 1 0 d1 ;0 0 0 1];
>> A2=[c2 -s2 0 a2*s2; s2 c2 0 a2*s2; 0 0 1 0 ;0 0 0 1];
>> A3=[c3 0 s3 a3*c3; s3 0 -c3 a3*s3; 0 1 0 d3; 0 0 0 1];
>> A4=[c4 0 -s4 0; s4 0 c4 0; 0 -1 0 0; 0 0 0 1];
>> A5=[c5 0 s5 0; s5 0 -c5 0; 0 1 0 d5; 0 0 0 1];
>>A6=[c6 -s6 0 0; s6 c6 0 0; 0 0 1 0;0 0 0 1];

Bảng 2.2 Ma trận định vị theo khớp

Robot YASKAWA EP4000D

Page 7


A1=
[cos (i1), 0
[sin(i1), 0
[0
,1
[0
, 0

, sin(i1), a1*cos(i1)]
, -cos(i1), a1*sin(i1)]
,0
, d1
]
, 0
, 1
]

A2=
[cos(i2), -sin(i2), 0, a2*sin(i2)]
[sin(i2), cos(i2), 0, a2*sin(i2)]
[
0, 0,
1, 0 ]
[
0, 0,
0, 1 ]

A3=
[cos(i3), 0, sin(i3), a3*cos(i3) ]
[sin(i3), 0, -cos(i3), a3*sin(i3)]
[
0, 1, 0,
d3 ]
[
0, 0, 0,
1 ]

A4=
[cos(i4), 0, -sin(i4), 0 ]
[sin(i4), 0, cos(i4), a4*sin(i4) ]
[ 0, -1, 0,
0 ]
[ 0, 0, 0,
0 ]

A5=
[cos(i5), 0, sin(i5), 0 ]
[sin(i5), 0, -cos(i5), 0 ]
[
0, 1, 0,
d5 ]
[
0, 0,
0,
1 ]

A6=
[cos(i6), -sin(i6), 0,
[sin(i6), cos(i6), 0,
[
0,
0,
1,
[
0,
0,
0,

0 ]
0]
0]
1]

2.1.3 Xác định các ma trận T theo các hệ tọa độ lần lượt từ khâu
cuối về.
* =
>> T56 = A6;
T56 =
[ cos(i6), -sin(i6), 0, 0]
[ sin(i6), cos(i6), 0, 0]
[

0,

0, 1, 0]

[

0,

0, 0, 1]

*=

Robot YASKAWA EP4000D

Page 8


>> T46=A5*A6
T46 =
[ cos(i5)*cos(i6), -cos(i5)*sin(i6), sin(i5), 0]
[ cos(i6)*sin(i5), -sin(i5)*sin(i6), -cos(i5), 0]
[

sin(i6),

[

cos(i6),

0,

0,

0, d5]

0, 1]

* = T46
T36 =
[
cos(i4)*cos(i5)*cos(i6)
sin(i4)*sin(i6),
cos(i4)*cos(i5)*sin(i6), cos(i4)*sin(i5), -d5*sin(i4)]

-

[
cos(i4)*sin(i6)
+
cos(i5)*cos(i6)*sin(i4),
-cos(i5)*sin(i4)*sin(i6), sin(i4)*sin(i5), d5*cos(i4)]
[ -cos(i6)*sin(i5),

sin(i5)*sin(i6),

[

0,

0,

0,

cos(i5),

cos(i6)*sin(i4)

-

cos(i4)*cos(i6)
0]

1]

* = T36
T26 =
[ - cos(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)) - cos(i6)*sin(i3)*sin(i5),
sin(i3)*sin(i5)*sin(i6) - cos(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)),
cos(i5)*sin(i3) + cos(i3)*cos(i4)*sin(i5), a3*cos(i3) - d5*cos(i3)*sin(i4)]
[ cos(i3)*cos(i6)*sin(i5) - sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)), sin(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) - cos(i3)*sin(i5)*sin(i6),
cos(i4)*sin(i3)*sin(i5) - cos(i3)*cos(i5), a3*sin(i3) - d5*sin(i3)*sin(i4)]
[cos(i4)*sin(i6) + cos(i5)*cos(i6)*sin(i4), cos(i4)*cos(i6) - cos(i5)*sin(i4)*sin(i6),
sin(i4)*sin(i5), d3 + d5*cos(i4)]
[

0,

0,

0,

1]

* = T26

Robot YASKAWA EP4000D

Page 9


T16 =
[ sin(i2)*(sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)) cos(i3)*cos(i6)*sin(i5)) - cos(i2)*(cos(i3)*(sin(i4)*sin(i6) cos(i4)*cos(i5)*cos(i6)) + cos(i6)*sin(i3)*sin(i5)),
sin(i2)*(sin(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) +
cos(i3)*sin(i5)*sin(i6)) - cos(i2)*(cos(i3)*(cos(i6)*sin(i4) +
cos(i4)*cos(i5)*sin(i6)) - sin(i3)*sin(i5)*sin(i6)), cos(i2)*(cos(i5)*sin(i3) +
cos(i3)*cos(i4)*sin(i5)) + sin(i2)*(cos(i3)*cos(i5) - cos(i4)*sin(i3)*sin(i5)),
cos(i2)*(a3*cos(i3) - d5*cos(i3)*sin(i4)) + a2*sin(i2) - sin(i2)*(a3*sin(i3) d5*sin(i3)*sin(i4))]
[ - cos(i2)*(sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)) cos(i3)*cos(i6)*sin(i5)) - sin(i2)*(cos(i3)*(sin(i4)*sin(i6) cos(i4)*cos(i5)*cos(i6)) + cos(i6)*sin(i3)*sin(i5)), cos(i2)*(sin(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) +
cos(i3)*sin(i5)*sin(i6)) - sin(i2)*(cos(i3)*(cos(i6)*sin(i4) +
cos(i4)*cos(i5)*sin(i6)) - sin(i3)*sin(i5)*sin(i6)), sin(i2)*(cos(i5)*sin(i3) +
cos(i3)*cos(i4)*sin(i5)) - cos(i2)*(cos(i3)*cos(i5) - cos(i4)*sin(i3)*sin(i5)),
sin(i2)*(a3*cos(i3) - d5*cos(i3)*sin(i4)) + a2*sin(i2) + cos(i2)*(a3*sin(i3) d5*sin(i3)*sin(i4))]
[
cos(i4)*sin(i6) + cos(i5)*cos(i6)*sin(i4),
cos(i4)*cos(i6) - cos(i5)*sin(i4)*sin(i6),
sin(i4)*sin(i5), d3 + d5*cos(i4)]
[ 0,

0,

0, 1 ]

* = T16
T06 =
[sin(i1)*(cos(i4)*sin(i6)+cos(i5)*cos(i6)*sin(i4))

cos(i1)*(cos(i2)*(cos(i3)*(sin(i4)*sin(i6)
cos(i4)*cos(i5)*cos(i6))
+
cos(i6)*sin(i3)*sin(i5)) - sin(i2)*(sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6))

Robot YASKAWA EP4000D

Page 10


- cos(i3)*cos(i6)*sin(i5))),
sin(i1)*(cos(i4)*cos(i6) - cos(i5)*sin(i4)*sin(i6)) cos(i1)*(cos(i2)*(cos(i3)*(cos(i6)*sin(i4)
+
cos(i4)*cos(i5)*sin(i6))
sin(i3)*sin(i5)*sin(i6)) - sin(i2)*(sin(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6))
+
cos(i3)*sin(i5)*sin(i6))),
cos(i1)*(cos(i2)*(cos(i5)*sin(i3)
+
cos(i3)*cos(i4)*sin(i5)) + sin(i2)*(cos(i3)*cos(i5) - cos(i4)*sin(i3)*sin(i5))) +
sin(i1)*sin(i4)*sin(i5), cos(i1)*(cos(i2)*(a3*cos(i3) - d5*cos(i3)*sin(i4)) +
a2*sin(i2) - sin(i2)*(a3*sin(i3) - d5*sin(i3)*sin(i4))) + a1*cos(i1) + sin(i1)*(d3 +
d5*cos(i4))]
[ - sin(i1)*(cos(i2)*(cos(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)) +
cos(i6)*sin(i3)*sin(i5)) - sin(i2)*(sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6))
- cos(i3)*cos(i6)*sin(i5))) - cos(i1)*(cos(i4)*sin(i6) + cos(i5)*cos(i6)*sin(i4)), cos(i1)*(cos(i4)*cos(i6)-cos(i5)*sin(i4)*sin(i6))sin(i1)*(cos(i2)*(cos(i3)*(cos(i6)*sin(i4)
+
cos(i4)*cos(i5)*sin(i6))
sin(i3)*sin(i5)*sin(i6)) - sin(i2)*(sin(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6))
+

cos(i3)*sin(i5)*sin(i6))),
sin(i1)*(cos(i2)*(cos(i5)*sin(i3)
+
cos(i3)*cos(i4)*sin(i5)) + sin(i2)*(cos(i3)*cos(i5) - cos(i4)*sin(i3)*sin(i5))) cos(i1)*sin(i4)*sin(i5), sin(i1)*(cos(i2)*(a3*cos(i3) - d5*cos(i3)*sin(i4)) +
a2*sin(i2) - sin(i2)*(a3*sin(i3) - d5*sin(i3)*sin(i4))) - cos(i1)*(d3 + d5*cos(i4)) +
a1*sin(i1)]
[-cos(i2)*(sin(i3)*(sin(i4)*sin(i6)-cos(i4)*cos(i5)*cos(i6))-cos(i3)*cos(i6)*sin(i5))
-sin(i2)*(cos(i3)*(sin(i4)*sin(i6)-cos(i4)*cos(i5)*cos(i6))+cos(i6)*sin(i3)*sin(i5)),
-cos(i2)*(sin(i3)*(cos(i6)*sin(i4)+cos(i4)*cos(i5)*sin(i6))+cos(i3)*sin(i5)*sin(i6))
-sin(i2)*(cos(i3)*(cos(i6)*sin(i4)+cos(i4)*cos(i5)*sin(i6))- sin(i3)*sin(i5)*sin(i6)),
sin(i2)*(cos(i5)*sin(i3) + cos(i3)*cos(i4)*sin(i5)) - cos(i2)*(cos(i3)*cos(i5) cos(i4)*sin(i3)*sin(i5)), d1 + sin(i2)*(a3*cos(i3) - d5*cos(i3)*sin(i4)) + a2*sin(i2)
+ cos(i2)*(a3*sin(i3) - d5*sin(i3)*sin(i4))]
[ 0, 0, 0, 1 ]

2.2 Xây dựng phần mềm tính toán động lực học thuận
Sử dụng công cụ guide trong matlab, đầu tiên ta xây dựng mennu

Robot YASKAWA EP4000D

Page 11


-Nút ĐỘNG HỌC THUẬN mở giao diện tương ứng.
function donghocthuan_Callback(hObject, eventdata, handles)
close
donghocthuan
- Nút MA TRẬN JACOBY mở giao diện tương ứng.
function jacobi_Callback(hObject, eventdata, handles)
close
jacobi

- Nút ĐỘNG HỌC NGHỊCH mở giao diện tương ứng.

Robot YASKAWA EP4000D

Page 12


function donghocnghich_Callback(hObject, eventdata, handles)
close
IF.m
- Nút THOAT có chức năng đóng giao diện.
function thoat_Callback(hObject, eventdata, handles)
close
- Tiếp theo, ta xây dựng được giao diện tính toán động học thuận:

Hình 2.3: Giao diện phần mềm tính toán động học thuận

Ta xây dựng hàm callback cho nút TINH TOAN sẽ thực hiện chương trình đọc dữ
liệu từ bảng nhập dữ liệu, sau đó hiển thị ma trận T06.
function tinhtoan_Callback(hObject, eventdata, handles)
i1 = str2num(get(handles.theta1,'string'));
i2 = str2num(get(handles.theta2,'string'));
i3 = str2num(get(handles.theta3,'string'));
i4 = str2num(get(handles.theta4,'string'));
i5 = str2num(get(handles.theta5,'string'));
i6 = str2num(get(handles.theta6,'string'));
% Doi tu rad sand do
i1 = i1*pi/180;

Robot YASKAWA EP4000D

Page 13


i2 = i2*pi/180;
i3 = i3*pi/180;
i4 = i4*pi/180;
i5 = i5*pi/180;
i6 = i6*pi/180;
% Khai bao a,d
a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255;
%tinh toan va hien thi
nx=
num2str(sin(i1)*(cos(i4)*sin(i6)
+
cos(i5)*cos(i6)*sin(i4))
cos(i1)*(cos(i2)*(cos(i3)*(sin(i4)*sin(i6)
cos(i4)*cos(i5)*cos(i6))
+
cos(i6)*sin(i3)*sin(i5)) - sin(i2)*(sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6))
- cos(i3)*cos(i6)*sin(i5))));
set(handles.nx,'string',nx);

sin(i1)*(cos(i2)*(cos(i3)*(sin(i4)*sin(i6)
cos(i4)*cos(i5)*cos(i6))
+
cos(i6)*sin(i3)*sin(i5))
sin(i2)*(sin(i3)*(sin(i4)*sin(i6)
cos(i4)*cos(i5)*cos(i6))
cos(i3)*cos(i6)*sin(i5)))

cos(i1)*(cos(i4)*sin(i6)
cos(i5)*cos(i6)*sin(i4)));
ny=

num2str(-

+

set(handles.ny,'string',ny);
nz= num2str(- cos(i2)*(sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6))

- cos(i3)*cos(i6)*sin(i5)) - sin(i2)*(cos(i3)*(sin(i4)*sin(i6)
cos(i4)*cos(i5)*cos(i6)) + cos(i6)*sin(i3)*sin(i5)));

-

set(handles.nz,'string',nz);

- cos(i5)*sin(i4)*sin(i6)) cos(i1)*(cos(i2)*(cos(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) sin(i3)*sin(i5)*sin(i6))
sin(i2)*(sin(i3)*(cos(i6)*sin(i4)
+
cos(i4)*cos(i5)*sin(i6)) + cos(i3)*sin(i5)*sin(i6))));
ox=

num2str(sin(i1)*(cos(i4)*cos(i6)

set(handles.ox,'string',ox);

Robot YASKAWA EP4000D

Page 14


cos(i1)*(cos(i4)*cos(i6) - cos(i5)*sin(i4)*sin(i6)) sin(i1)*(cos(i2)*(cos(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) sin(i3)*sin(i5)*sin(i6))
sin(i2)*(sin(i3)*(cos(i6)*sin(i4)
+
cos(i4)*cos(i5)*sin(i6)) + cos(i3)*sin(i5)*sin(i6))));
oy=

num2str(-

set(handles.oy,'string',oy);
oz= num2str(- cos(i2)*(sin(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6))

+ cos(i3)*sin(i5)*sin(i6)) - sin(i2)*(cos(i3)*(cos(i6)*sin(i4)
cos(i4)*cos(i5)*sin(i6)) - sin(i3)*sin(i5)*sin(i6)));

+

set(handles.oz,'string',oz);
ax= num2str (cos(i1)*(cos(i2)*(cos(i5)*sin(i3) + cos(i3)*cos(i4)*sin(i5))

+
sin(i2)*(cos(i3)*cos(i5)
sin(i1)*sin(i4)*sin(i5));

-

cos(i4)*sin(i3)*sin(i5)))

+

set(handles.ax,'string',ax);
ay= num2str (sin(i1)*(cos(i2)*(cos(i5)*sin(i3) + cos(i3)*cos(i4)*sin(i5)) +

sin(i2)*(cos(i3)*cos(i5)
cos(i1)*sin(i4)*sin(i5));

-

cos(i4)*sin(i3)*sin(i5)))

-

set(handles.ay,'string',ay);

+ cos(i3)*cos(i4)*sin(i5))
cos(i2)*(cos(i3)*cos(i5) - cos(i4)*sin(i3)*sin(i5)));
az=

num2str(sin(i2)*(cos(i5)*sin(i3)

-

set(handles.az,'string',az);

- d5*cos(i3)*sin(i4)) +
a2*sin(i2) - sin(i2)*(a3*sin(i3) - d5*sin(i3)*sin(i4))) + a1*cos(i1) +
sin(i1)*(d3 + d5*cos(i4)));
px=

num2str(cos(i1)*(cos(i2)*(a3*cos(i3)

set(handles.px,'string',px);

- d5*cos(i3)*sin(i4)) +
a2*sin(i2) - sin(i2)*(a3*sin(i3) - d5*sin(i3)*sin(i4))) - cos(i1)*(d3 +
d5*cos(i4)) + a1*sin(i1));
py=

num2str(sin(i1)*(cos(i2)*(a3*cos(i3)

set(handles.py,'string',py);

Robot YASKAWA EP4000D

Page 15


pz= num2str(d1 + sin(i2)*(a3*cos(i3) - d5*cos(i3)*sin(i4)) + a2*sin(i2) +

cos(i2)*(a3*sin(i3) - d5*sin(i3)*sin(i4)));
set(handles.pz,'string',pz);
Nút THOAT có chức năng đóng giao diện.
function thoat_Callback(hObject, eventdata, handles)
close
- Nút QUAY LAI có chức năng trở về menu chính.
function back_Callback(hObject, eventdata, handles)
close
mennu

Chương 3. Ma trận Jacoby
3.1 Xây dựng ma trận Jacoby
Các bước thực hiện


Bước 1:Xác định các ma trận (với i= 0 -> 5)

Robot YASKAWA EP4000D

Page 16




Ta đã xác định được ma trận từ chương 2
Bước 2: các định ma trận
=

Khi (i+1) là biến quay ,biến khớp
= . -.

=

= . -.

=

= . -.

=



Bước 3: xác định ma trận J
J=.

3.2 Thực hiện trên matlap
-Xác định :
>> JH11 = T06(2,1)*T06(1,4)- T06(1,1)*T06(2,4);
>> JH21 = T06(2,2)*T06(1,1)- T06(1,2)T06(2,4);
>> JH31 = T06(2,3)*T06(1,4)- T06(1,3)*T06(2,4);
>> JH41 = T06(3,1);JH51=T06(3,2); JH61= T06(3,3);
>> JH12 = T16(2,1)*T16(1,4)- T06(1,1)*T16(2,4);
>> JH22 = T16(2,2)*T16(1,1)- T16(1,2)T16(2,4);
>> JH32 = T16(2,3)*T16(1,4)- T16(1,3)*T16(2,4);
>> JH42 = T16(3,1);JH52=T16(3,2); JH62= T16(3,3);

Robot YASKAWA EP4000D

Page 17


>> JH13 = T26(2,1)*T26(1,4)- T26(1,1)*T26(2,4);
>> JH23 = T26(2,2)*T26(1,1)- T26(1,2)T26(2,4);
>> JH33 = T26(2,3)*T26(1,4)- T26(1,3)*T26(2,4);
>> JH43 = T26(3,1);JH53=T26(3,2); JH63= T26(3,3);

>> JH14 = T36(2,1)*T36(1,4)- T36(1,1)*T36(2,4);
>> JH24 = T36(2,2)*T36(1,1)- T36(1,2)T36(2,4);

>> JH34 = T36(2,3)*T36(1,4)- T36(1,3)*T36(2,4);
>> JH44 = T36(3,1);JH54=T36(3,2); JH64= T36(3,3);
>> JH15 = T46(2,1)*T46(1,4)- T46(1,1)*T46(2,4);
>> JH25 = T46(2,2)*T46(1,1)- T46(1,2)T46(2,4);
>> JH35 = T46(2,3)*T46(1,4)- T46(1,3)*T46(2,4);
>> JH45 = T46(3,1);JH55=T46(3,2); JH65= T46(3,3);
>> JH16 = T56(2,1)*T56(1,4)- T56(1,1)*T56(2,4);
>> JH26 = T56(2,2)*T56(1,1)- T56(1,2)T56(2,4);
>> JH36 = T56(2,3)*T56(1,4)- T56(1,3)*T56(2,4);
>> JH46 = T56(3,1);JH56=T56(3,2); JH66= T56(3,3);
- Xác định J:
>> T061 = T06;
>> T061(4,:)=[]; T061(:,4)=[];
>> R06 = T061;
>> X =[R06 zeros(3,3); zeros(3,3) R06];
>> J = X*JH;

3.3 Xây dựng phần mềm giao diện tính toán Jacoby

Robot YASKAWA EP4000D

Page 18


- Sử dụng công cụ guide trong matlab, ta xây dựng được giao diện:

- Ta xây dựng hàm callback cho nút TINH TOAN sẽ thực hiện chương trình đọc dữ liệu
từ bảng nhập dữ liệu, sau đó hiển thị ma trận Jacobi.
function tinhtoan_Callback(hObject, eventdata, handles)
i1 = str2num(get(handles.theta1,'string'));

i2 = str2num(get(handles.theta2,'string'));
i3 = str2num(get(handles.theta3,'string'));
i4 = str2num(get(handles.theta4,'string'));
i5 = str2num(get(handles.theta5,'string'));
i6 = str2num(get(handles.theta6,'string'));
% Doi tu rad sand do
i1 = i1*pi/180;
i2 = i2*pi/180;
i3 = i3*pi/180;
i4 = i4*pi/180;
i5 = i5*pi/180;

Robot YASKAWA EP4000D

Page 19


i6 = i6*pi/180;
% Khai bao a,d

a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255;
% Tinh toan va hien thi
J11 =;J12 =; J13 =; J14 =; J15 =; J16 =;
J21 =;J22 =; J23 =; J24 =; J25 =; J26 =;
J31 =;J32 =; J33 =; J34 =; J35 =; J36 =;
J41 =;J42 =; J43 =; J44 =; J45 =; J46 =;
J51 =;J52 =; J53 =; J54 =; J55 =; J56 =;
J61 =;J62 =; J63 =; J64 =; J65 =; J66 =;
J11 = set(handles.J11,'string',num2str(J11));
J12 = set(handles.J12,'string',num2str(J12));

J13 = set(handles.J13,'string',num2str(J13));
J14 = set(handles.J14,'string',num2str(J14));
J15 = set(handles.J15,'string',num2str(J15));
J16 = set(handles.J16,'string',num2str(J16));

J21 = set(handles.J21,'string',num2str(J21));
J22 = set(handles.J22,'string',num2str(J22));
J23 = set(handles.J23,'string',num2str(J23));
J24 = set(handles.J24,'string',num2str(J24));
J25 = set(handles.J25,'string',num2str(J25));
J26 = set(handles.J26,'string',num2str(J26));
J31 = set(handles.J31,'string',num2str(J31));
J32 = set(handles.J32,'string',num2str(J32));
J33 = set(handles.J33,'string',num2str(J33));
J34 = set(handles.J34,'string',num2str(J34));
J35 = set(handles.J35,'string',num2str(J35));
J36 = set(handles.J36,'string',num2str(J36));

Robot YASKAWA EP4000D

Page 20


J41 = set(handles.J41,'string',num2str(J41));
J42 = set(handles.J42,'string',num2str(J42));
J43 = set(handles.J43,'string',num2str(J43));
J44 = set(handles.J44,'string',num2str(J44));
J45 = set(handles.J45,'string',num2str(J45));
J46 = set(handles.J46,'string',num2str(J46));
J51 = set(handles.J51,'string',num2str(J51));

J52 = set(handles.J52,'string',num2str(J52));
J53 = set(handles.J53,'string',num2str(J53));
J54 = set(handles.J54,'string',num2str(J54));
J55 = set(handles.J55,'string',num2str(J55));
J56 = set(handles.J56,'string',num2str(J56));
J61 = set(handles.J61,'string',num2str(J61));
J62 = set(handles.J62,'string',num2str(J62));
J63 = set(handles.J63,'string',num2str(J63));
J64 = set(handles.J64,'string',num2str(J64));
J65 = set(handles.J65,'string',num2str(J65));
J66 = set(handles.J66,'string',num2str(J66));

Nút THOÁT có chức năng đóng giao diện.
function thoat_Callback(hObject, eventdata, handles)
close

Nút QUAY LAI có chức năng trở về menu chính.
function back_Callback(hObject, eventdata, handles)
close

Chương 4 : Động học đảo vị trí

Robot YASKAWA EP4000D

Page 21


4.1 Lập phương trình tính toán động học đảo vị trí robot
4.1.1 Tính ma trận nghịch đảo của các ma trận vị trí khớp
Từ các ma trận vị trí khớp đã xây dựng ở phần động học thuận, sử dụng lệnh inv

trong matlab ta tìm được các ma trận nghịch đảo:
B1=simplify(inv(A1));
Ta thu được kết quả:

B2=simplify(inv(A2));

B3=simplify(inv(A3));

B4=simplify(inv(A4));

Robot YASKAWA EP4000D

Page 22


B5=simplify(inv(A5));

B6=simplify(inv(A6));

4.1.2 Tính toán động học ngược robot theo phương pháp phân ly biến
Do robot Yaskawa có 6 bậc tự do việc tính toán trực tiếp theo phương pháp phân ly
biến số rất khó để tính toán, nên nhóm em chọn phương án chia robot ra thành từng
phần 3 x 3 để thuận lợi cho việc tính toán động học ngược.
Ma trận định vị tổng quát có dạng:

Tính toán góc theta 1
Phương trình tính toán góc theta 1: B1*T=T13
Ma trận vế trái là:

Robot YASKAWA EP4000D

Page 23


Ma trận vế phải là:

Đồng nhất kết quả hai vế ta có các phương trình:

Suy ra:

Vậy:
-Tính

:

Robot YASKAWA EP4000D

Page 24


ta có:

Vì:
Nên:

Và:

Vì
Nên

Vậy:
-Tính

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

Robot YASKAWA EP4000D

Page 25