TÌM HIỂU ROBOT OMRON VIPER 650

ĐẠI HỌC BÁCH KHOA HÀ NỘI
TRƯỜNG ĐIỆN - ĐIỆN TỬ

BÁO CÁO BÀI TẬP LỚN MƠN KỸ THUẬT ROBOT
NHĨM 10: TÌM HIỂU ROBOT OMRON VIPER 650

Giảng viên hướng dẫn:

PGS. TS.

Sinh viên thực hiện:

Hà Nội – 7/2023

1


Mục Lục
CHƯƠNG I. GIỚI THIỆU ROBOT OMRON VIPER 650................................................3
1. Giới thiệu về hãng Omron..........................................................................................3
2. Giới thiệu về Robot Omron Viper 650.......................................................................3
2.1 Thơng số kỹ thuật.................................................................................................4
2.2 Kích thước và vùng làm việc................................................................................4
2.3 Phụ kiện đi kèm....................................................................................................5
2.4 Hình ảnh Robot Omron Viper 650.......................................................................5
CHƯƠNG II. ĐỘNG HỌC THUẬN ROBOT OMRON VIPER 650................................7
1. Xác định các khớp quay và trục tọa độ.......................................................................7
2. Xác định các ma trận A..............................................................................................8
3. Xây dựng phần mềm tính tốn động lực học thuận..................................................11
CHƯƠNG III. XÂY DỰNG MA TRẬN JACOBY.........................................................13
1. Xác định ma trận T in................................................................................................................................................................................ 13

2. Xác định ma trận JH.................................................................................................. 13
3. Xác định ma trận J....................................................................................................15
4. Xây dựng giao diện..................................................................................................16
CHƯƠNG IV. ĐỘNG HỌC NGƯỢC ROBOT VIPER 650....................................20
1. Bài tốn tìm vị trí giao điểm các trục cổ tay (tâm cổ tay).........................................21
2. Bài tốn tìm hướng của cổ tay..................................................................................23
CHƯƠNG V. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT VIPER 650
........................................................................................................................................... 26 1.Giới
thiệu và cơ sở thiết kế quỹ đạo........................................................................... 26
2. Tính tốn thiết kế quỹ đạo chuyển động dạng bậc 3..................................................26

CHƯƠNG VI. XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG
TRÊN TOOLBOX SIMSCAPE/MATLAB.....................................................................31


CHƯƠNG I. GIỚI THIỆU ROBOT OMRON VIPER 650
1. Giới thiệu về hãng Omron
Omron là một công ty của Nhật Bản đứng hàng đầu thế giới về cơng nghệ tự động
hóa, được thành lập vào năm 1933.
 Các thiết bị tự động của Omron chất lượng cao được sản xuất bằng công
nghệ hiện đại rất đa dạng với hơn 100.000 mặt hàng.
 Nhiều sản phẩm chiếm thị phần đứng đầu trên thế giới như Biến tần, Rơ le,
Điểu khiển nhiệt, Công tắc hành trình, Cảm biến quang…
 Ngày 01/04/2016 Omron Adept giới thiệu Robot cơng nghiệp mới nhất
của mình ra thị trường và đáp ứng đầy đủ các tiêu chuẩn vệ sinh của Bộ
Nơng nghiệp Hoa Kỳ cho phịng chống ơ nhiễm sản phẩm.
2. Giới thiệu về Robot Omron Viper 650
Robot Viper 650 phù hợp với dây chuyền lắp ráp và đóng gói sản phẩm. Với thiết kế
nhỏ gọn có thể làm việc được trong diện tích nhỏ. Phù hợp để lắp ráp các linh kiện
nhỏ của trong các ngành sản xuất ơ-tơ, xe máy. Ngồi ra chúng cũng có thể phân loại,

kiểm định chất lượng sản phẩm.
Robot khớp nối để gia công, lắp ráp và xử lý vật liệu
 Khả trình thơng qua phần mềm ACE và ngơn ngữ eV+ hoặc thông qua IEC
61131-3 quen thuộc khi sử dụng kết nối ePLC.
 Hiển thị chẩn đoán cho phép xử lý sự cố nhanh hơn.
 Encoder tuyệt đối, độ phân giải cao để cung cấp độ chính xác, theo dõi tốc độ
chậm và dễ dàng hiệu chỉnh.
 Hiệu suất cao, hộp số Harmonic quán tính thấp và cánh tay nhẹ cho phép thực
hiện chuyển động với gia tốc tối đa.
 Tầm với: 653mm
 Tải trọng tối đa: 5kg
 Khối lượng: 34kg


2.1 Thơng số kỹ thuật

2.2 Kích thước và vùng làm việc


2.3 Phụ kiện đi kèm

2.4 Hình ảnh Robot Omron Viper 650



CHƯƠNG II. ĐỘNG HỌC THUẬN ROBOT OMRON VIPER
650
1. Xác định các khớp quay và trục tọa độ

Hình 1. Xác định các gốc tọa độ tại các khớp

Bảng1. Bảng DH

𝒊

𝜶 𝒊°

𝒂𝒊

𝜽𝒊 °

𝒅𝒊

1

-90

0.075

𝜽𝟏

-0.335

2

0

0.27

𝜽𝟐

0

3

90

-0.09

𝜽𝟑

0

4

-90

0

𝜽𝟒

0.295

5

90

0

𝜽𝟓

0

6

0

0

𝜽𝟔

0.08

2. Xác định các ma trận A


>> syms a1 a2 a3 a4 a5 a5 a6;
>> a4=0; a5=0; a6=0;
>> syms i1 i2 i3 i4 i5 i6;
>> syms d1 d2 d3 d4 d5 d6;
>> d2=0; d3=0; d5=0;
>> A1=[cos(i1) -0*sin(i1) -1*sin(i1) a1*cos(i1); sin(i1) 0*cos(i1) --1*cos(i1) a1*sin(i1); 0 -1
0 d1; 0 0 0 1]
A1 =
[cos(i1), 0, -sin(i1), a1*cos(i1)]
[sin(i1), 0, cos(i1), a1*sin(i1)]
[

0, -1,

0,

d1]

[

0, 0,

0,

1]

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

0,

0, 1,

0]

[

0,

0, 0,

1]

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


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

0, 1,

0,

0]

[

0, 0,

0,

1]

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

[

0, -1,

0, d4]

[

0, 0,

0, 1]

>> A5=[cos(i5) -0*sin(i5) 1*sin(i5) a5*cos(i5); sin(i5) 0*cos(i5) -1*cos(i5) a5*sin(i5); 0 1
0 d5; 0 0 0 1]
A5 =
[cos(i5), 0, sin(i5), 0]
[sin(i5), 0, -cos(i5), 0]
[

0, 1,

0, 0]

[

0, 0,

0, 1]

>> A6=[cos(i6) -1*sin(i6) 0*sin(i6) a6*cos(i6); sin(i6) 1*cos(i6) -0*cos(i6) a6*sin(i6); 0 0

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


[ 0, 0, 1, d6]
[ 0, 0, 0, 1]

>> T06=A1*A2*A3*A4*A5*A6

T06 =
[- cos(i6)*(sin(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) + cos(i5)*(sin(i1)*sin(i4) cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3)))) - sin(i6)*(cos(i4)*sin(i1) +
sin(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))),
sin(i6)*(sin(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) + cos(i5)*(sin(i1)*sin(i4)
- cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3)))) - cos(i6)*(cos(i4)*sin(i1) +
sin(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))), cos(i5)*(cos(i1)*cos(i2)*sin(i3) +
cos(i1)*cos(i3)*sin(i2)) - sin(i5)*(sin(i1)*sin(i4) - cos(i4)*(cos(i1)*cos(i2)*cos(i3) cos(i1)*sin(i2)*sin(i3))), d6*(cos(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) sin(i5)*(sin(i1)*sin(i4) - cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3)))) +
a1*cos(i1) + d4*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) + a2*cos(i1)*cos(i2) +
a3*cos(i1)*cos(i2)*cos(i3) - a3*cos(i1)*sin(i2)*sin(i3)]
[- cos(i6)*(sin(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) - cos(i5)*(cos(i1)*sin(i4) +
cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)))) sin(i6)*(sin(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)) - cos(i1)*cos(i4)),
sin(i6)*(sin(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) - cos(i5)*(cos(i1)*sin(i4)
+ cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)))) cos(i6)*(sin(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)) - cos(i1)*cos(i4)),
cos(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) + sin(i5)*(cos(i1)*sin(i4) +
cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3))), d6*(cos(i5)*(cos(i2)*sin(i1)*sin(i3) +
cos(i3)*sin(i1)*sin(i2)) + sin(i5)*(cos(i1)*sin(i4) + cos(i4)*(cos(i2)*cos(i3)*sin(i1) sin(i1)*sin(i2)*sin(i3)))) + a1*sin(i1) + d4*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) +
a2*cos(i2)*sin(i1) + a3*cos(i2)*cos(i3)*sin(i1) - a3*sin(i1)*sin(i2)*sin(i3)]
[cos(i6)*(sin(i5)*(sin(i2)*sin(i3) - cos(i2)*cos(i3)) - cos(i4)*cos(i5)*(cos(i2)*sin(i3) + cos(i3
*sin(i2))) +sin(i4)*sin(i6)*(cos(i2)*sin(i3) + cos(i3)*sin(i2)),

cos(i6)*sin(i4)*(cos(i2)*sin(i3) + cos(i3)*sin(i2)) - sin(i6)*(sin(i5)*(sin(i2)*sin(i3) cos(i2)*cos(i3)) - cos(i4)*cos(i5)*(cos(i2)*sin(i3) + cos(i3)*sin(i2))),
- cos(i5)*(sin(i2)*sin(i3) - cos(i2)*cos(i3)) - cos(i4)*sin(i5)*(cos(i2)*sin(i3) + cos(i3)*sin(i2)),
d1 - d4*(sin(i2)*sin(i3) - cos(i2)*cos(i3)) - d6*(cos(i5)*(sin(i2)*sin(i3) - cos(i2)*cos(i3))
+ cos(i4)*sin(i5)*(cos(i2)*sin(i3) + cos(i3)*sin(i2))) - a2*sin(i2) - a3*cos(i2)*sin(i3) a3*cos(i3)*sin(i2)]


[0,
0,
0,
1]

3. Xây dựng phần mềm tính tốn động lực học thuận

Hình 3.1. Giao diện phần mềm tính tốn động học thuận

Ta xây dựng hàm callback cho nút Tính Tốn 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 pushbutton1_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')); i1

= i1*pi/180;
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=0.075; a2=0.27; a3=-0.09; d1=-0.335; d4=0.295; d6=0.08;


%tinh toan va hien thi
nx= num2str(- cos(i6)*(sin(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) +
cos(i5)*(sin(i1)*sin(i4) - cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))))
sin(i6)*(cos(i4)*sin(i1) + sin(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))));
set(handles.nx,'string',nx);
ny= num2str(- cos(i6)*(sin(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) cos(i5)*(cos(i1)*sin(i4) + cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3))))
sin(i6)*(sin(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)) cos(i1)*cos(i4))); set(handles.ny,'string',ny);
nz= num2str( cos(i6)*(sin(i5)*(sin(i2)*sin(i3) - cos(i2)*cos(i3)) cos(i4)*cos(i5)*(cos(i2)*sin(i3) + cos(i3)*sin(i2))) + sin(i4)*sin(i6)*(cos(i2)*sin(i3)
+ cos(i3)*sin(i2)));
set(handles.nz,'string',nz);
ox= num2str(sin(i6)*(sin(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) +
cos(i5)*(sin(i1)*sin(i4) - cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))))

cos(i6)*(cos(i4)*sin(i1) + sin(i4)*(cos(i1)*cos(i2)*cos(i3) cos(i1)*sin(i2)*sin(i3)))); set(handles.ox,'string',ox);
oy= num2str(sin(i6)*(sin(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) cos(i5)*(cos(i1)*sin(i4) + cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3))))
cos(i6)*(sin(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)) cos(i1)*cos(i4))); set(handles.oy,'string',oy);
oz= num2str(cos(i6)*sin(i4)*(cos(i2)*sin(i3) + cos(i3)*sin(i2)) - sin(i6)*(sin(i5)*(sin(i2)*sin(i3)
- cos(i2)*cos(i3)) - cos(i4)*cos(i5)*(cos(i2)*sin(i3) + cos(i3)*sin(i2))));
set(handles.oz,'string',oz);
ax= num2str(cos(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) - sin(i5)*(sin(i1)*sin(i4)
- cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3))));
set(handles.ax,'string',ax);
ay= num2str(cos(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) +
sin(i5)*(cos(i1)*sin(i4) + cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3))));
set(handles.ay,'string',ay);
az= num2str(- cos(i5)*(sin(i2)*sin(i3) - cos(i2)*cos(i3)) - cos(i4)*sin(i5)*(cos(i2)*sin(i3)
+ cos(i3)*sin(i2)));
set(handles.az,'string',az);
px= num2str(d6*(cos(i5)*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) sin(i5)*(sin(i1)*sin(i4) - cos(i4)*(cos(i1)*cos(i2)*cos(i3) - cos(i1)*sin(i2)*sin(i3)))) +
a1*cos(i1) + d4*(cos(i1)*cos(i2)*sin(i3) + cos(i1)*cos(i3)*sin(i2)) + a2*cos(i1)*cos(i2) +
a3*cos(i1)*cos(i2)*cos(i3) - a3*cos(i1)*sin(i2)*sin(i3));
set(handles.px,'string',px);
py= num2str(d6*(cos(i5)*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) +
sin(i5)*(cos(i1)*sin(i4) + cos(i4)*(cos(i2)*cos(i3)*sin(i1) - sin(i1)*sin(i2)*sin(i3)))) +
a1*sin(i1) + d4*(cos(i2)*sin(i1)*sin(i3) + cos(i3)*sin(i1)*sin(i2)) + a2*cos(i2)*sin(i1) +
a3*cos(i2)*cos(i3)*sin(i1) - a3*sin(i1)*sin(i2)*sin(i3));
set(handles.py,'string',py);


pz= num2str(d1 - d4*(sin(i2)*sin(i3) - cos(i2)*cos(i3)) - d6*(cos(i5)*(sin(i2)*sin(i3)
- cos(i2)*cos(i3)) + cos(i4)*sin(i5)*(cos(i2)*sin(i3) + cos(i3)*sin(i2))) - a2*sin(i2) a3*cos(i2)*sin(i3) - a3*cos(i3)*sin(i2));

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

CHƯƠNG III. XÂY DỰNG MA TRẬN JACOBY
1. Xác định ma trận T i n

-

Từ phần động học thuận, ta tính được ma trận T bằng cách sử dụng đoạn code
matlab như sau:
%% Ma tran dong hoc thuan
A1 = [cos(t1) 0 sin(t1) a1*cos(t1); sin(t1) 0 -cos(t1) a1*sin(t1); 0 1 0 d1 ; 0 0 0 1];
A2 = [cos(t2) -sin(t2) 0 a2*cos(t2); sin(t2) cos(t2) 0 a2*sin(t2); 0 0 1 0; 0 0 0 1];
A3 = [cos(t3) 0 -sin(t3) a3*cos(t3); sin(t3) 0 cos(t3) a3*sin(t3); 0 -1 0 0; 0 0 0 1];
A4 = [cos(t4) 0 sin(t4) 0; sin(t4) 0 -cos(t4) 0; 0 1 0 d4; 0 0 0 1];
A5 = [cos(t5) 0 -sin(t5) 0; sin(t5) 0 cos(t5) 0; 0 -1 0 d5; 0 0 0 1];
A6 = [cos(t6) -sin(t6) 0 0; sin(t6) cos(t6) 0 0; 0 0 1 0; 0 0 0
1]; T = simplify(A1*A2*A3*A4*A5*A6);

-

Từ đó ta tìm được các ma trận T 56, T 4 ,…
bằng cách:
6
T 56  A6
T 46  A5  A6
T3 A A A
6

4

5

6

…
-

Để thuận tiện cho việc tính tốn, ta sử dụng đoạn code trên matlab m.file như sau:
%% Tinh toan ma tran
Jacoby T5 = simplify(A6);
T4
=
simplify(A5*T5); T3
= simplify(A4*T4);
T2
=
simplify(A3*T3); T1
= simplify(A2*T2);
T0 = simplify(T);

2. Xác định ma trận JH

-

Theo lý thuyết, ta có thể tìm ma trận JH theo công thức sau:
H
 H p
p

x

x

q2
 q1

H

...

p 
x

qn 




HX

H

 H p
y

y

q1
 H
p
z

 q1

H

H


p
q

...

p 
y 
qn 
H
p

H2

p
...

z

z



qn 

q2

J 6n 

...
 H  x H 
Q
q
q
1
2

 H  H 
...
y

y

q
q2
H
 H 1

z
z
...

q
q



1
2


x 
q 
n

H
 
y

qn 
H
 
z 
qn 
H

Trong đó:
Các phần tử ở cột thứ i+1 sẽ được tính theo ma trận Ti như tính tốn ở trên


Khi khớp thứ i+1 là khớp trượt thì:
H
i

p x  nz

ri
1
H

p

y

 ioz

r

i1
H
z

 iaz

p
ri1

H

H





H


y

x

ri1


ri1





y

ri1

0

Khi khớp thứ i+1 là khớp quay thì:
H

p
x

i1
H

p

y

i1
H
p

i

y

 io
p

x
y

i

y



i

H

i

 n p  pn
i

x
y
i

 io
p
i

i



x

 in z

i1

x

i

H

  io

z

y

x

i1
H



z

i


z

i1

-

ay px  ax py

i1  az

Theo lý thuyết, ta có thể xây dựng đoạn code trên matlab m.file để tính tốn như
sau:
J_h(1,1) = simplify(T0(2,1)*T0(1,4)-T0(1,1)*T0(2,4));

J_h(2,1) = simplify(T0(2,2)*T0(1,4)-T0(1,2)*T0(2,4));
J_h(3,1) = simplify(T0(2,3)*T0(1,4)-T0(1,3)*T0(2,4));
J_h(4,1) = simplify(T0(3,1));
J_h(5,1) = simplify(T0(3,2));
J_h(6,1) = simplify(T0(3,3));
J_h(1,2) = simplify(T1(2,1)*T1(1,4)-T1(1,1)*T1(2,4));
J_h(2,2) = simplify(T1(2,2)*T1(1,4)-T1(1,2)*T1(2,4));
J_h(3,2) = simplify(T1(2,3)*T1(1,4)-T1(1,3)*T1(2,4));
J_h(4,2) = simplify(T1(3,1));
J_h(5,2) = simplify(T1(3,2));
J_h(6,2) = simplify(T1(3,3));
J_h(1,3) = simplify(T2(2,1)*T2(1,4)-T2(1,1)*T2(2,4));
J_h(2,3) = simplify(T2(2,2)*T2(1,4)-T2(1,2)*T2(2,4));
J_h(3,3) = simplify(T2(2,3)*T2(1,4)-T2(1,3)*T2(2,4));
J_h(4,3) = simplify(T2(3,1));
J_h(5,3) = simplify(T2(3,2));
J_h(6,3) = simplify(T2(3,3));
J_h(1,4) = simplify(T3(2,1)*T3(1,4)-T3(1,1)*T3(2,4));
J_h(2,4) = simplify(T3(2,2)*T3(1,4)-T3(1,2)*T3(2,4));
J_h(3,4) = simplify(T3(2,3)*T3(1,4)-T3(1,3)*T3(2,4));
J_h(4,4) = simplify(T3(3,1));
J_h(5,4) = simplify(T3(3,2));
J_h(6,4) = simplify(T3(3,3));
J_h(1,5) = simplify(T4(2,1)*T4(1,4)-T4(1,1)*T4(2,4));
J_h(2,5) = simplify(T4(2,2)*T4(1,4)-T4(1,2)*T4(2,4));
J_h(3,5) = simplify(T4(2,3)*T4(1,4)-T4(1,3)*T4(2,4));
J_h(4,5) = simplify(T4(3,1));
J_h(5,5) = simplify(T4(3,2));
J_h(6,5) = simplify(T4(3,3));
J_h(1,6) = simplify(T5(2,1)*T5(1,4)-T5(1,1)*T5(2,4));

J_h(2,6) = simplify(T5(2,2)*T5(1,4)-T5(1,2)*T5(2,4));
J_h(3,6) = simplify(T5(2,3)*T5(1,4)-T5(1,3)*T5(2,4));
J_h(4,6) = simplify(T5(3,1));
J_h(5,6) = simplify(T5(3,2));
J_h(6,6) = simplify(T5(3,3));
3. Xác định ma trận J

-

Sau khi0 tìm được ma trận JH, ta tìm được ma trận J theo cơng thức:

-

Đoạn code trên matlab m.file:

R
J   n0


0 H
 J
R0
n

R = simplify(T0(1:3,1:3)); %ma tran quay
J = simplify([R,zeros(3,3);zeros(3,3),R]*J_h);% ma tran Jacoby


4. Xây dựng giao diện

-

Sử dụng công cụ Guide trên matlab, ta xây dựng được giao diện như sau:

-

Code giao diện trên matlab (code phần tính tốn để hiển thị kết quả sau khi tính
tốn trên giao diện):
function tinhtoan_Callback(hObject, eventdata, handles)
% hObject handle to tinhtoan (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see
GUIDATA) t1=get(handles.slider1,'value');
t1= t1*pi/180;
t2=get(handles.slider2,'value');
t2= t2*pi/180;
t3=get(handles.slider3,'value');
t3= t3*pi/180;
t4=get(handles.slider4,'value');
t4= t4*pi/180;
t5=get(handles.slider5,'value');
t5= t5*pi/180;
t6=get(handles.slider6,'value');
t6= t6*pi/180;
d1=get(handles.slider7,'value');
d4=get(handles.slider8,'value');
d5=get(handles.slider9,'value');
a1=get(handles.slider10,'value');

a2=get(handles.slider11,'value');
a3=get(handles.slider12,'value');
%Tinh toan
a11= d5*cos(t1)*cos(t4) - a1*sin(t1) - a2*cos(t2)*sin(t1) - a3*cos(t2)*cos(t3)*sin(t1) +
d4*cos(t2)*sin(t1)*sin(t3) + d4*cos(t3)*sin(t1)*sin(t2) + a3*sin(t1)*sin(t2)*sin(t3) d5*cos(t2)*cos(t3)*sin(t1)*sin(t4) + d5*sin(t1)*sin(t2)*sin(t3)*sin(t4);
a12= -cos(t1)*((d5*cos(t2 + t3 - t4))/2 + d4*cos(t2 + t3) + a3*sin(t2 + t3) + a2*sin(t2)
- (d5*cos(t2 + t3 + t4))/2);
a13= -cos(t1)*((d5*cos(t2 + t3 - t4))/2 + d4*cos(t2 + t3) + a3*sin(t2 + t3) - (d5*cos(t2
+ t3 + t4))/2);
a14= d5*cos(t1)*cos(t2)*cos(t3)*cos(t4) - d5*sin(t1)*sin(t4)
- d5*cos(t1)*cos(t4)*sin(t2)*sin(t3);
a15=0
;
a16=0
;
a21= a1*cos(t1) + a2*cos(t1)*cos(t2) + d5*cos(t4)*sin(t1) +
a3*cos(t1)*cos(t2)*cos(t3) - d4*cos(t1)*cos(t2)*sin(t3) - d4*cos(t1)*cos(t3)*sin(t2)
- a3*cos(t1)*sin(t2)*sin(t3) + d5*cos(t1)*cos(t2)*cos(t3)*sin(t4) d5*cos(t1)*sin(t2)*sin(t3)*sin(t4);
a22= -sin(t1)*((d5*cos(t2 + t3 - t4))/2 + d4*cos(t2 + t3) + a3*sin(t2 + t3) + a2*sin(t2)
- (d5*cos(t2 + t3 + t4))/2);
a23= -sin(t1)*((d5*cos(t2 + t3 - t4))/2 + d4*cos(t2 + t3) + a3*sin(t2 + t3) - (d5*cos(t2
+ t3 + t4))/2);
a24= d5*cos(t1)*sin(t4) + d5*cos(t2)*cos(t3)*cos(t4)*sin(t1)
- d5*cos(t4)*sin(t1)*sin(t2)*sin(t3);
a25=0;
a26=0;
a31=
0;
a32= a3*cos(t2 + t3) - (d5*sin(t2 + t3 - t4))/2 - d4*sin(t2 + t3) + a2*cos(t2) +
(d5*sin(t2 + t3 + t4))/2;

a33= (cos(t6)*(cos(t2 + t3)*sin(t5) + sin(t2 + t3)*cos(t4)*cos(t5)) - sin(t2 +
t3)*sin(t4)*sin(t6))*(a3*cos(t6)*sin(t5) + d4*sin(t4)*sin(t6) d4*cos(t4)*cos(t5)*cos(t6)
+ d5*cos(t6)*sin(t4)*sin(t5)) - (sin(t6)*(cos(t2 + t3)*sin(t5) + sin(t2 +
t3)*cos(t4)*cos(t5)) + sin(t2 + t3)*cos(t6)*sin(t4))*(d4*cos(t6)*sin(t4) a3*sin(t5)*sin(t6) + d4*cos(t4)*cos(t5)*sin(t6) - d5*sin(t4)*sin(t5)*sin(t6)) + (cos(t2
+ t3)*cos(t5) - sin(t2 + t3)*cos(t4)*sin(t5))*(a3*cos(t5) + d4*cos(t4)*sin(t5) +
d5*cos(t5)*sin(t4));
a34= d5*sin(t2 +
t3)*cos(t4); a35=0;
a36=0;
a41=
0;
a42=
sin(t1);
a43=
sin(t1);