BÀI TẬP LỚN MÔN KỸTHUẬT ROBOT: TÌM HIỂU ROBOT OMRON VIPER 650

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

BÁO CÁO BÀI TẬP LỚNMƠ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. Nguyễn Phạm Thục Anh

Sinh viên thực hiện:

1. Vũ Văn Tú

20181810

2. Vũ Mạnh Thọ

20181770

3. Bùi Quang Minh

20181644

4. Phạm Duy Thi

20181765

5. Vũ Huy Hùng

20181512

6. Phạm Văn Đồng

20181397

7. Đặng Đình Dũng

20181418

8. Trương Thanh Hằng 20181459
9. Vũ Văn Long

20181633

10. Trần Văn Dương

20181442

11. Đoàn Đức Anh

20181315

Hà Nội – 01/2022


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 i n ................................................................................................. 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 ĐỐITƯỢ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,
1]

0,

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 46 ,… bằng cách:
T 56  A6
T 4 6  A5  A6
T 36  A4  A5  A6

…
-

Để 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:


J

H
6n

 H px


 q1
 H p
y

 q1
 H
 pz
H
 X  q1

H
Q
  x
 q
1

H
  y

 q1
 H 
z


q
1


px
q2

H

p y

H

q2

...

pz
q2

...

 x
q2

...

H

H

H

 y

q2

 z
q2

px 

qn 
H
p y 

qn 

H
pz 
qn 

H
 x 
qn 

H
 y 

qn 
H
 z 

qn 
H

...

...

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ì:
px i
 nz
ri 1

H

H

p y

ri 1

 i oz

pz i
 az
ri 1

H

H
 y H  y
 x


0
ri 1
ri 1
ri 1

H



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

px

i 1
H

p y

i 1
H

pz

i 1

-

 i n y i p x  i nx i p y

 x i
 nz
i 1

H

H

 o y p x  ox p y
i

i

i

i

 i a y i px  i ax i p y

 y

i 1

 i oz

 z i
 az
i 1

H

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 khi tìm được ma trận JH, ta tìm được ma trận J theo công thức:
 R0
J  n
0

-

0 H
 J
Rn0 

Đoạn code trên matlab m.file:
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);
a44=-sin(t2 + t3)*cos(t1);
a45=cos(t4)*sin(t1) + cos(t1)*cos(t2)*cos(t3)*sin(t4) - cos(t1)*sin(t2)*sin(t3)*sin(t4);
a46=sin(t5)*(sin(t1)*sin(t4) + cos(t4)*(cos(t1)*sin(t2)*sin(t3) - cos(t1)*cos(t2)*cos(t3)))
- cos(t5)*(cos(t1)*cos(t2)*sin(t3) + cos(t1)*cos(t3)*sin(t2));


a51= 0;
a52= -cos(t1);
a53= -cos(t1);
a54= -sin(t2 + t3)*sin(t1);
a55= cos(t2)*cos(t3)*sin(t1)*sin(t4) - cos(t1)*cos(t4) - sin(t1)*sin(t2)*sin(t3)*sin(t4);
a56= -sin(t5)*(cos(t1)*sin(t4) - cos(t4)*(sin(t1)*sin(t2)*sin(t3) cos(t2)*cos(t3)*sin(t1))) - cos(t5)*(cos(t2)*sin(t1)*sin(t3) + cos(t3)*sin(t1)*sin(t2));
a61= (cos(t2 + t3)*cos(t5) - sin(t2 + t3)*cos(t4)*sin(t5))^2 + (sin(t6)*(cos(t2 +
t3)*sin(t5) + sin(t2 + t3)*cos(t4)*cos(t5)) + sin(t2 + t3)*cos(t6)*sin(t4))^2 +
(cos(t6)*(cos(t2 + t3)*sin(t5) + sin(t2 + t3)*cos(t4)*cos(t5)) - sin(t2 +
t3)*sin(t4)*sin(t6))^2;
a62= 0;
a63= 0;

a64= cos(t2+t3);
a65=sin(t2 + t3)*sin(t4);
a66=cos(t2 + t3)*cos(t5) - sin(t2 + t3)*cos(t4)*sin(t5);
a11=num2str(a11);
a12=num2str(a12);
a13=num2str(a13);
a14=num2str(a14);
a15=num2str(a15);
a16=num2str(a16);
a21=num2str(a21);
a22=num2str(a22);
a23=num2str(a23);
a24=num2str(a24);
a25=num2str(a25);
a26=num2str(a26);
a31=num2str(a31);
a32=num2str(a32);
a33=num2str(a33);
a34=num2str(a34);
a35=num2str(a35);
a36=num2str(a36);
a41=num2str(a41);
a42=num2str(a42);
a43=num2str(a43);
a44=num2str(a44);
a45=num2str(a45);
a46=num2str(a46);
a51=num2str(a51);
a52=num2str(a52);
a53=num2str(a53);

a54=num2str(a54);
a55=num2str(a55);


a56=num2str(a56);
a61=num2str(a61);
a62=num2str(a62);
a63=num2str(a63);
a64=num2str(a64);
a65=num2str(a65);
a66=num2str(a66);
%Xuat ketqua
set(handles.a11,'string',a11);
set(handles.a12,'string',a12);
set(handles.a13,'string',a13);
set(handles.a14,'string',a14);
set(handles.a15,'string',a15);
set(handles.a16,'string',a16);
set(handles.a21,'string',a21);
set(handles.a22,'string',a22);
set(handles.a23,'string',a23);
set(handles.a24,'string',a24);
set(handles.a25,'string',a25);
set(handles.a26,'string',a26);
set(handles.a31,'string',a31);
set(handles.a32,'string',a32);
set(handles.a33,'string',a33);
set(handles.a34,'string',a34);
set(handles.a35,'string',a35);
set(handles.a36,'string',a36);

set(handles.a41,'string',a41);
set(handles.a42,'string',a42);
set(handles.a43,'string',a43);
set(handles.a44,'string',a44);
set(handles.a45,'string',a45);
set(handles.a46,'string',a46);
set(handles.a51,'string',a51);
set(handles.a52,'string',a52);
set(handles.a53,'string',a53);
set(handles.a54,'string',a54);
set(handles.a55,'string',a55);
set(handles.a56,'string',a56);
set(handles.a61,'string',a61);
set(handles.a62,'string',a62);
set(handles.a63,'string',a63);
set(handles.a64,'string',a64);
set(handles.a65,'string',a65);
set(handles.a66,'string',a66);


CHƯƠNG IV. ĐỘNG HỌC NGƯỢC ROBOT VIPER 650
Bài toán động học ngược là rất khó, đối với tay máy 6 khớp có ba khớp cuối
đồng quy tại một điểm, ta có thể tách bài tốn động học ngược thành hai bài tốn đơn
giản hơn là động học ngược vị trí và động học ngược hướng
- Tìm vị trí giao điểm các trục cổ tay (tâm cổ tay)
-

Tìm hướng của cổ tay.

Ta sẽ biểu diễn hướng và vị trí của cổ tay, cổ tay sẽ được biểu diễn đối với hệ tọa

độ cố định bên ngoài (world coordinate system – Hệ tọa độ 𝑂0 ). Ta phải giải bài toán
trên đối với các ẩn 𝜃1 , … , 𝜃6 .
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 RC xác định qua vị trí cơng cụ (The given tool position – so với
hệ tọa độ 𝑂0 ) và phương của Tool pointing ( Z 6 ). Do đó vị trí của tâm cổ tay phụ thuộc
vào 3 biến khớp đầu tiên.
Các biến khớp 𝜃1 , 𝜃2 , 𝜃3 xác định từ ma trận định hướng cổ tay (The arm
orientation) 𝑇30 và ma trận định hướng công cụ (The given tool orientation) 𝑇60 . Vị trí C
so với 𝑂0 sẽ bằng vị trí của C so với hệ tọa độ 𝑂3 nhân với ma trận biến đổi đồng nhất
của 𝑂3 trong tọa độ 𝑂0 (𝑃𝑐0 = 𝑇30 𝑃𝑐3 ). Tương tự vị trí C so với 𝑂0 sẽ bằng vị trí C so với
hệ tọa độ 𝑂6 nhân với ma trận biến đổi đồng nhất của 𝑂6 trong tọa độ 𝑂0 (𝑃𝑐0 = 𝑇60 𝑃𝑐6 )
Các biến khớp 𝜃4 , 𝜃5 , 𝜃6 xác định từ ma trận 𝑇63 (𝜃1 , 𝜃2 , 𝜃3 ) và ma trận
−1
𝑇63 = 𝑇30 (𝜃1 , 𝜃2 , 𝜃3 ) ∗ 𝑇60
Như vậy bài tốn của chúng ta sẽ được giải quyết thơng qua hai bước sau
 Thiết lập ma trận vị trí 𝑃𝑐 = cột cuối cùng của 𝑇30 (1 , 2 ,3 ) và 𝑇60 để tìm ra các biến
−1

khớp 1 , 2 ,3 . Tính 𝑇63 = 𝑇30 . 𝑇60 sau khi đã thay giá trị của các biến khớp vào
1 , 2 ,3 vào 𝑇30 (1 , 2 ,3 ) .

 So sánh 𝑇63 và 𝑇63 (𝜃4 , 𝜃5 , 𝜃6 ) để rút ra 4 ,5 ,6 .
Đưa từ ý tưởng thực hiện ta Giả sử C là điểm cổ tay máy


Ta chia bài toán thành 2 bước
𝑇60 = 𝐴
⏟1 𝐴2 𝐴3 𝐴
⏟4 𝐴5 𝐴6
𝑇30

𝑇63

1. Bài tốn tìm vị trí giao điểm các trục cổ tay (tâm cổ tay)
Tọa độ C trong hệ tọa độ O6 là:
0
0
]
𝑃𝑐6 = [
−𝑑6
1
Tọa độ C trong hệ tọa độ O0 𝑣ớ𝑖 (𝑇𝑟𝑒𝑓 = 𝑇60 đặ𝑡) là:
𝑛𝑥 𝑜𝑥
𝑛
𝑜𝑦
𝑃𝑐0 = 𝑇𝑟𝑒𝑓 . 𝑃𝑐6 = [ 𝑦
𝑛𝑧 𝑜𝑧
0 0

𝑎𝑥 𝑝𝑥
−𝑎𝑥 𝑑6 + 𝑝𝑥
0
𝑎𝑦 𝑝𝑦
−𝑎𝑦 𝑑6 + 𝑝𝑦
0
𝑎𝑧 𝑝𝑧 ] . [−𝑑6 ] = [ −𝑎𝑧 𝑑6 + 𝑝𝑧 ] (4.1)
1
0 1
1

Ma trận biến đổi đồng nhất của O3 trong O0 là:
𝑇30 = 𝐴1 𝐴2 𝐴3


𝑐1
𝑐2
𝑐
Đặt 3
𝑐4
𝑐5
{𝑐6

= cos(𝜃1 )
= cos(𝜃2 )
= cos(𝜃3 )
= cos(𝜃4 )
= cos(𝜃5 )
= cos(𝜃6 )

𝑠1
𝑠2
𝑠3
và
𝑠4
𝑠5
{𝑠6

𝑠12 = sin(𝜃1 + 𝜃2 )
𝑠23 = sin(𝜃2 + 𝜃3 )
𝑠34 = sin(𝜃3 + 𝜃4 )

và
𝑐12 = cos(𝜃1 + 𝜃2 )
𝑐23 = cos(𝜃2 + 𝜃3 )
{𝑐34 = cos(𝜃3 + 𝜃4 )

= sin(𝜃1 )
= sin(𝜃2 )
= sin(𝜃3 )
= sin(𝜃4 )
= sin(𝜃5 )
= sin(𝜃6 )

Ta có:
𝑐1 𝑐23
𝑠𝑐
𝑇30 = [ 1 23
−𝑠23
0

−𝑠1
𝑐1
0
0

𝑐1 𝑠23
𝑠1 𝑠23
𝑐23
0

𝑎1 𝑐1 + 𝑎2 𝑐1 𝑐2 + 𝑎3 𝑐1 𝑐23

𝑎1 𝑠1 + 𝑎2 𝑠1 𝑐2 + 𝑎3 𝑠1 𝑐23
]
𝑑1 − 𝑎2 𝑠2 − 𝑎3 𝑠23
1

Mặt khác ta có tọa độ của C trong tọa độ O0 là:

𝑐1 𝑐23
𝑠 𝑐
𝑃𝑐0 = [ 1 23
−𝑠23
0

0
0
𝑃𝑐0 = 𝑇30 𝑃𝑐3 𝑣ớ𝑖 𝑃𝑐3 = [ ]
𝑑4
1
−𝑠1 𝑐1 𝑠23 𝑎1 𝑐1 + 𝑎2 𝑐1 𝑐2 + 𝑎3 𝑐1 𝑐23
0
𝑐1 𝑠1 𝑠23 𝑎1 𝑠1 + 𝑎2 𝑠1 𝑐2 + 𝑎3 𝑠1 𝑐23
0
].[ ]
𝑐23
𝑑4
0
𝑑1 − 𝑎2 𝑠2 − 𝑎3 𝑠23
0
1
0

1

𝑑4 𝑐1 𝑠23 + 𝑎1 𝑐1 + 𝑎2 𝑐1 𝑐2 + 𝑎3 𝑐1 𝑐23
𝑑 𝑠 𝑠 + 𝑎1 𝑠1 + 𝑎2 𝑠1 𝑐2 + 𝑎3 𝑠1 𝑐23
] (4.2)
↔ 𝑃𝑐0 = [ 4 1 23
𝑑4 𝑐23 + 𝑑1 − 𝑎2 𝑠2 − 𝑎3 𝑠23
1
Đồng nhất 2 vế của (4.1) và (4.2) ta được hệ phương trình sau:
−𝑎𝑥 𝑑6 + 𝑝𝑥 = 𝑑4 𝑐1 𝑠23 + 𝑎1 𝑐1 + 𝑎2 𝑐1 𝑐2 + 𝑎3 𝑐1 𝑐23 (1)
{ −𝑎𝑦 𝑑6 + 𝑝𝑦 = 𝑑4 𝑠1 𝑠23 + 𝑎1 𝑠1 + 𝑎2 𝑠1 𝑐2 + 𝑎3 𝑠1 𝑐23 (2)
−𝑎𝑧 𝑑6 + 𝑝𝑧 = 𝑑4 𝑐23 + 𝑑1 − 𝑎2 𝑠2 − 𝑎3 𝑠23
(3)
 Ta lấy (1)𝑠1 − (2)𝑐1 ta thu được:
𝑠1 (−𝑎𝑥 𝑑6 + 𝑝𝑥 ) − 𝑐1 (−𝑎𝑦 𝑑6 + 𝑝𝑦 ) = 0
=> 𝜃1 = 𝐴𝑇𝐴𝑁2(−𝑎𝑦 𝑑6 + 𝑝𝑦 , −𝑎𝑥 𝑑6 + 𝑝𝑥 )
ℎ𝑜ặ𝑐 𝜃1 = 𝐴𝑇𝐴𝑁2(𝑎𝑦 𝑑6 − 𝑝𝑦 , 𝑎𝑥 𝑑6 − 𝑝𝑥 )
 Ta lấy (1)𝑐1 + (2)𝑠1 và kết hợp phương trình 3 ta có hệ:


{

𝑐1 (−𝑎𝑥 𝑑6 + 𝑝𝑥 ) + 𝑠1 (−𝑎𝑦 𝑑6 + 𝑝𝑦 ) = 𝑑4 𝑠23 +𝑎1 + 𝑎2 𝑐2 + 𝑎3 𝑐23 = 𝑀
−𝑎𝑧 𝑑6 + 𝑝𝑧 = 𝑑4 𝑐23 + 𝑑1 − 𝑎2 𝑠2 − 𝑎3 𝑠23 = 𝑁

↔{

𝑑4 𝑠23 +𝑎2 𝑐2 + 𝑎3 𝑐23 = 𝑀 − 𝑎1
𝑑4 𝑐23 − 𝑎2 𝑠2 − 𝑎3 𝑠23 = 𝑁 − 𝑑1

Bình phương hai vế của phương trình trên và cộng lại ta được
(𝑑4 𝑠23 +𝑎2 𝑐2 + 𝑎3 𝑐23 )2 + (𝑑4 𝑐23 − 𝑎3 𝑠23 − 𝑎2 𝑠2 )2 = (𝑀 − 𝑎1 )2 + (𝑁 − 𝑑1 )2
↔ 𝑑42 + 𝑎22 + 𝑎32 + 2𝑑4 𝑎2 𝑠3 + 2𝑎3 𝑎2 𝑐3 = (𝑀 − 𝑎1 )2 + (𝑁 − 𝑑1 )2
↔ 2𝑑4 𝑎2 𝑠3 + 2𝑎3 𝑎2 𝑐3 = (𝑀 − 𝑎1 )2 + (𝑁 − 𝑑1 )2 −𝑑42 − 𝑎22 − 𝑎32
Đặt 𝑋 = (𝑀 − 𝑎1 )2 + (𝑁 − 𝑑1 )2 −𝑑42 − 𝑎22 − 𝑎32
→ 𝜃3 = 𝐴𝑇𝐴𝑁2(2𝑑4 𝑎2 , 2𝑎2 𝑎3 ) + 𝐴𝑇𝐴𝑁2(±√(2𝑎3 𝑎2 )2 + (2𝑑4 𝑎2 )2 − 𝑋 2 , 𝑋)
Ta khai triển M và N ta được hệ
{

𝑀 − 𝑎1 = 𝑑4 𝑠2 𝑐3 + 𝑑4 𝑠3 𝑐2 + 𝑎2 𝑐2 + 𝑎3 𝑐3 𝑐2 − 𝑎3 𝑠2 𝑠3
𝑁 − 𝑑1 = 𝑑4 𝑐2 𝑐3 − 𝑑4 𝑠2 𝑠3 − 𝑎2 𝑠2 − 𝑎3 𝑠2 𝑐3 − 𝑎3 𝑠3 𝑐2

↔{

𝑀 − 𝑎1 = 𝑠2 (𝑑4 𝑐3 − 𝑎3 𝑠3 ) + 𝑐2 (𝑑4 𝑠3 + 𝑎2 + 𝑎3 𝑐3 )
𝑘 = 𝑑4 𝑐3 − 𝑎3 𝑠3
đặt: { 1
𝑘2 = 𝑑4 𝑠3 + 𝑎2 + 𝑎3 𝑐3
𝑁 − 𝑑1 = 𝑐2 (𝑑4 𝑐3 − 𝑎3 𝑠3 ) − 𝑠2 (𝑑4 𝑠3 + 𝑎2 + 𝑎3 𝑐3 )

↔{

𝑀 − 𝑎1 = 𝑐2 𝑘2 +𝑠2 𝑘1
𝑁 − 𝑑1 = 𝑐2 𝑘1 − 𝑠2 𝑘2

→ 𝑛𝑔ℎ𝑖ệ𝑚 𝑑𝑢𝑦 𝑛ℎấ𝑡: 𝜃2 = 𝐴𝑇𝐴𝑁2(𝑘1 (𝑀 − 𝑎1 ) − 𝑘2 (𝑁 − 𝑑1 ), 𝑘1 (𝑁 − 𝑑1 ) + 𝑘2 (𝑀 −
𝑎1 ))
2. Bài tốn tìm hướng của cổ tay
Sau khi giải xong bài tốn tìm vị trí giao điểm của các trục cổ tay ta đi đến bài tốn tìm
hướng sử dụng ma trận 𝑇63 để tính tốn.

Ta có:
𝑐4 𝑐5 𝑐6 − 𝑠4 𝑠6
𝑇63 = 𝐴4 𝐴5 𝐴6 = [𝑠4 𝑐5 𝑐6 + 𝑐4 𝑠6
−𝑠5 𝑐6
0
Mặt khác

−𝑐4 𝑐5 𝑠6 − 𝑠4 𝑐6
−𝑠4 𝑐5 𝑠6 + 𝑐4 𝑐6
𝑠5 𝑠6
0

𝑐4 𝑠5 𝑑6 𝑐4 𝑠5
𝑠4 𝑠5 𝑑6 𝑠4 𝑠5
] (4.3)
𝑐5 𝑑4 + 𝑑6 𝑐5
0
1

𝐴1 𝐴2 𝐴3 𝐴4 𝐴5 𝐴6 = 𝑇60 ↔ 𝑇30 𝑇63 = 𝑇60 ↔ 𝑇63 = 𝑇30

−1 0
𝑇6


−1

Mà
𝑇30 𝑇60 =
𝑛𝑥 𝑐1 𝑐23 + 𝑛𝑦 𝑠2 𝑐23 − 𝑛𝑧 𝑠23 𝑜𝑥 𝑐1 𝑐23 + 𝑜𝑦 𝑠1 𝑐23 − 𝑜𝑧 𝑠23

−𝑛𝑥 𝑠1 + 𝑛𝑦 𝑐1
−𝑜𝑥 𝑠1 + 𝑜𝑦 𝑐1
𝑛𝑥 𝑐1 𝑠23 + 𝑛𝑦 𝑠1 𝑠23 + 𝑛𝑧 𝑐23 𝑜𝑥 𝑠23 𝑐1 + 𝑜𝑦 𝑠1 𝑠23 + 𝑜𝑧 𝑐23
[
𝑓𝑎
𝑓𝑏

𝑎𝑥 𝑐1 𝑐23 + 𝑎𝑦 𝑠1 𝑐23 − 𝑎𝑧 𝑠23 𝑓𝑥
−𝑎𝑥 𝑠1 + 𝑎𝑦 𝑐1
𝑓𝑦
(4.4)
𝑎𝑥 𝑠23 𝑐1 + 𝑎𝑦 𝑠1 𝑠23 + 𝑎𝑧 𝑐23 𝑓𝑧
𝑓𝑐
1]

Chỉ cần quan tâm tới phần định hướng tức là ma trận [3,3]. Đồng nhất 2 vế của 2 ma trận
(4.3) và (4.4) ta thu được
Đặt: 𝑘3 = 𝑎𝑥 𝑠23 𝑐1 + 𝑎𝑦 𝑠1 𝑠23 + 𝑎𝑧 𝑐23

𝑐5 = 𝑎𝑥 𝑠23 𝑐1 + 𝑎𝑦 𝑠1 𝑠23 + 𝑎𝑧 𝑐23

𝜃5 = 𝐴𝑇𝐴𝑁2(±√1 − 𝑘32 , 𝑘3 )

Ta có:
 Nếu 𝑠𝑖𝑛𝜃5 ≠ 0 có:
{

𝑐4 𝑠5 = 𝑎𝑥 𝑐1 𝑐23 + 𝑎𝑦 𝑠1 𝑐23 − 𝑎𝑧 𝑠23
𝑠4 𝑠5 = −𝑎𝑥 𝑠1 + 𝑎𝑦 𝑐1

−𝑎𝑥 𝑠1 + 𝑎𝑦 𝑐1
𝑠5
↔
𝑎𝑥 𝑐1 𝑐23 + 𝑎𝑦 𝑠1 𝑐23 − 𝑎𝑧 𝑠23
𝑐4 =
{
𝑠5
𝑠4 =

→ 𝜃4 = 𝐴𝑇𝐴𝑁2(𝑠4 , 𝑐4 )
Mặt khác:
−𝑠5 𝑐6 = 𝑛𝑥 𝑐1 𝑠23 + 𝑛𝑦 𝑠1 𝑠23 + 𝑛𝑧 𝑐23
{ 𝑠 𝑠 =𝑜 𝑠 𝑐 +𝑜 𝑠 𝑠 +𝑜 𝑐
5 6
𝑥 23 1
𝑦 1 23
𝑧 23
𝑜𝑥 𝑠23 𝑐1 + 𝑜𝑦 𝑠1 𝑠23 + 𝑜𝑧 𝑐23
𝑠5
↔
𝑛𝑥 𝑐1 𝑠23 + 𝑛𝑦 𝑠1 𝑠23 + 𝑛𝑧 𝑐23
𝑐 =−
{ 6
𝑠5
𝑠6 =

→ 𝜃6 = 𝐴𝑇𝐴𝑁2(𝑠6 , 𝑐6 )
 Nếu sin 𝜃5 = 0
Với cos 𝜃5 = 1 ta có 𝜃5 = 0
𝑐46

𝑠
𝑇63 = [ 46
0
0
Ta chỉ tính được 𝜃4 + 𝜃6

−𝑠46
𝑐46
0
0

0
0
0
0
]
1 𝑑4 + 𝑑6
0
1


𝜃4 + 𝜃6 = 𝐴𝑇𝐴𝑁2(𝑛𝑦 𝑐1 − 𝑛𝑥 𝑠1 , 𝑛𝑥 𝑐1 𝑐23 − 𝑛𝑧 𝑠23 + 𝑛𝑦 𝑠1 𝑐23 )
Với cos 𝜃5 = −1 ta có 𝜃5 = 𝜋
−𝑠𝑡6 −𝑡4
𝑠
𝑇63 = [ 𝑡6−𝑡4
0
0

𝑠𝑡6−𝑡4

𝑠𝑡6−𝑡4
0
0

0
0
−1
0

0
0
]
𝑑4 −𝑑6
1

Ta chỉ tính được 𝜃6 − 𝜃4
𝜃6 − 𝜃4 = 𝐴𝑇𝐴𝑁2(𝑜𝑋 𝑐1 𝑐23 − 𝑜𝑧 𝑠23 + 𝑜𝑦 𝑠1 𝑐23 , 𝑜𝑦 𝑐1 − 𝑜𝑥 𝑠1 )
Giải xong 2 bài toán trong động học ngược ta tìm ra các kết quả của 𝜃1 , 𝜃2 𝜃3 𝜃4 𝜃5 𝜃6 như
sau:
Góc

𝜃2
𝜃3
𝜃4

Giá trị
𝜃1 = 𝐴𝑇𝐴𝑁2(−𝑎𝑦 𝑑6 + 𝑝𝑦 , −𝑎𝑥 𝑑6 + 𝑝𝑥 )
ℎ𝑜ặ𝑐 𝜃1 = 𝐴𝑇𝐴𝑁2(𝑎𝑦 𝑑6 − 𝑝𝑦 , 𝑎𝑥 𝑑6 − 𝑝𝑥 )
𝜃2 = 𝐴𝑇𝐴𝑁2(𝑘1 (𝑀 − 𝑎1 ) − 𝑘2 (𝑁 − 𝑑1 ), 𝑘1 (𝑁 − 𝑑1 ) + 𝑘2 (𝑀 − 𝑎1 ))
𝜃3 = 𝐴𝑇𝐴𝑁2(2𝑑4 𝑎2 , 2𝑎2 𝑎3 ) + 𝐴𝑇𝐴𝑁2(±√(2𝑎3 𝑎2 )2 + (2𝑑4 𝑎2 )2 − 𝑋 2 , 𝑋)

𝐴𝑇𝐴𝑁2(𝑠4 , 𝑐4 )

𝜃5

𝐴𝑇𝐴𝑁2(±√1 − 𝑘32 , 𝑘3 )

𝜃6

𝐴𝑇𝐴𝑁2(𝑠6 , 𝑐6 )

𝜃1

Từ những kết quả trên ta thay thông số thực tế của robot VIPER 650:
Với ma trận định hướng công cụ đặt là Tref:
0
0
0 −1
𝑇60 = 𝑇𝑟𝑒𝑓 = [
1
0
0 0

1 0.3
0 0.2
]
0 0.2
0 1

ta được bảng kết quả như sau:
Góc

𝜃1
𝜃2
𝜃3
𝜃4
𝜃5
𝜃6

TH1
TH2
TH3
42.30
42.30
42.30
-139.39 -139.39
4.45
235.28 235.28 -21.24
-96.43
83.65
136.55
42.62
-42.62 -102.38
-81.40
98.69
-11.47

TH4
42.30
4.45
-21.24
-43.53

102.38
168.62

TH5
TH6
TH7
TH8
-137.80 -137.80 -137.80 -137.80
-176.42 -176.42 -73.37 -73.37
201.81 201.81 12.23
12.23
134.89 -45.20 118.05 -62.05
108.54 -108.54 49.64
-49.64
162.39 -17.70 -129.42 50.68