BÀI TẬP LỚN KT Robot: TÌM HIỂU ROBOT EPX2050

ĐẠI HỌC BÁCH KHOA HÀ NỘI
TRƯỜNG ĐIỆN – ĐIỆN TỬ
KHOA TỰ ĐỘNG HOÁ
====o0o====

HỌC PHẦN: KĨ THUẬT ROBOT

BÁO CÁO BÀI TẬP LỚN
ĐỀ TÀI:

TÌM HIỂU ROBOT EPX2050
Giảng viên hướng dẫn:

PGS. TS. Nguyễn Phạm Thục Anh

Thành viên nhóm:

Đào Mạnh Dương
Nguyễn Văn Lộc
Nguyễn Tuấn Hoàng
Nguyễn Văn Nghĩa
Nguyễn Quốc Huy

20173796
20174025
20173919
20174095
20181532

Lê Hoàng Hưng
Đinh Quang Minh

Nguyễn Hoàng Nam
Nguyễn Trọng Đức
Nguyễn Quàng Linh

20173943
20174069
20174082
20164798
20174022

Hà Nội, 1-2022


MỤC LỤC
CHƯƠNG 1: GIỚI THIỆU VỀ ROBOT EPX2050 ....................................................... 4
1.1. Tổng quan Robot công nghiệp Yaskawa Motoman ................................................. 4
1.2. Giới thiệu Robot EPX2050 ...................................................................................... 5
1.3. Kết cấu cơ khí........................................................................................................... 6
1.4. Thông số kĩ thuật ...................................................................................................... 8
CHƯƠNG 2: ĐỘNG HỌC THUẬN VỊ TRÍ ROBOT ................................................. 10
2.1. Cơ sở lý thuyết ....................................................................................................... 10
2.2. Tính tốn động học thuận robot: ............................................................................ 15
2.3. Giao diện tính tốn động học thuận trên GUI Matlab ............................................ 20
CHƯƠNG 3: MA TRẬN JACOBY ............................................................................... 22
3.1. Tính tốn ma trận Jacoby ....................................................................................... 22
3.2. Chương trình Matlab .............................................................................................. 24
3.3. Xây dựng giao diện GUI_JACOBI trên MATLAB ............................................... 30
CHƯƠNG 4: ĐỘNG HỌC ĐẢO VỊ TRÍ ROBOT ...................................................... 32
4.1. Tính tốn các góc 𝜭𝒊 .............................................................................................. 32
4.1.1. Tính góc 𝜭𝟏, 𝜭𝟓, 𝜭𝟔 ..................................................................................... 32

4.1.2. Tính góc 𝜭𝟐 , 𝜭𝟑 ............................................................................................ 33
4.1.3. Tính góc 𝜭𝟒 .................................................................................................... 34
4.2. Chương trình m.file Matlab.................................................................................... 34
4.2.1. Chương trình Matlab ....................................................................................... 34
4.2.2. Kết quả............................................................................................................. 36
4.2.3. Code Matlab tính các góc: ............................................................................... 40
CHƯƠNG 5: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CÁC KHỚP CỦA ROBOT
THEO QUỸ ĐẠO DẠNG BẬC 3................................................................................... 42
CHƯƠNG 6: XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG TRÊN
TOOLBOX SIMSCAPE/MATLAB............................................................................... 45
6.1. Cài đặt và sử dụng Toolbox ................................................................................... 45
6.2. Thiết kế hệ thống điều khiển .................................................................................. 49
6.3. Kết quả mô phỏng .................................................................................................. 50

1


DANH MỤC HÌNH VẼ
Hình 1.1: Các dịng robot hãng Yaskawa. ....................................................................... 4
Hình 1.2: Robot EPX2050. ................................................................................................ 5
Hình 1.3: Bản vẽ cơ khí kiểu Lemma wrist. .................................................................... 6
Hình 1.4: Bản vẽ cơ khí kiểu Hollow slim arm. .............................................................. 7
Hình 1.5: Thơng số kĩ thuật Robot EPX2050. ................................................................ 8
Hình 1.6: Thơng số bộ điều khiển NX100-FM ................................................................ 9
Hình 2.1: Số khớp và thanh nối của Robot. .................................................................. 10
Hình 2.2: Trường hợp chéo nhau. .................................................................................. 11
Hình 2.3: Trường hợp song song. ................................................................................... 11
Hình 2.5: Cách đặt vị trí tâm 𝑂0..................................................................................... 11
Hình 2.4: Trường hợp cắt nhau. .................................................................................... 12
Hình 2.6: Trường hợp khớp i là khớp tịnh tiến. ........................................................... 12

Hình 2.7: Trường hợp nếu i là khớp quay. ................................................................... 13
Hình 2.8: Trường hợp nếu i là khớp tịnh tiến. ............................................................. 13
Hình 2.9: Xác định các thơng số động học của thanh nối. ........................................... 14
Hình 2.10: Tên các phần và các trục làm việc của robot. ............................................ 15
Hình 2.11: Đồ thị vecto các trục toạ độ. ........................................................................ 16
Hình 2.12: Giao diện tính tốn động học thuận............................................................ 21
Hình 2.13: Kết quả tính tốn với số liệu nhập trước. ................................................... 21
Hình 3.1: Giao diện tính tốn ma trận Jacoby. ............................................................ 30
Hình 3.2: Kết quả tính tốn với số liệu nhập trước. ..................................................... 31
Hình 5.1: Đồ thị quỹ đạo, vận tốc, gia tốc của khớp quay. .......................................... 44
Hình 6.1: Thư mục cần tải. ............................................................................................. 45
Hình 6.2: Giao diện Matlab. ........................................................................................... 46
Hình 6.3: Mơ hình robot 3D trên Solidworks ............................................................... 46
Hình 6.4: Thao tác khởi tạo mơ phỏng. ......................................................................... 47
Hình 6.5: Thao tác xuất file mơ phỏng. ......................................................................... 48
Hình 6.6: Mơ hình mơ phỏng robot. .............................................................................. 49
Hình 6.7: Mơ phỏng điều khiển robot EPX2050 bằng bộ điều khiển PI trên Simulink.
........................................................................................................................................... 49
Hình 6.8: Bộ điều khiển PI.............................................................................................. 49
2


Hình 6.9: Khối tạo tín hiệu đặt cho góc quay 𝜽𝒊 của các khớp ................................... 50
Hình 6.10: Vị trí ban đầu các biến khớp có giá trị bằng 0........................................... 50
Hình 6.11: Kết quả mô phỏng với bộ điều khiển PI và tham số đầu vào như hình 6.7.
........................................................................................................................................... 51
Hình 6.12: Tín hiệu đặt và giá trị đầu ra khớp 1. ......................................................... 51
Hình 6.13: Tín hiệu đặt và giá trị đầu ra khớp 2. ......................................................... 52
Hình 6.14: Tín hiệu đặt và giá trị đầu ra khớp 3. ......................................................... 52
Hình 6.15: Tín hiệu đặt và giá trị đầu ra khớp 4. ......................................................... 53

Hình 6.16: Tín hiệu đặt và giá trị đầu ra khớp 5. ......................................................... 53
Hình 6.17: Tín hiệu đặt và giá trị đầu ra khớp 6. ......................................................... 54

3


CHƯƠNG 1: GIỚI THIỆU VỀ ROBOT EPX2050
1.1. Tổng quan Robot công nghiệp Yaskawa Motoman
Yaskawa Motoman cung cấp cho các nhà sản xuất một loạt các sản phẩm và công nghệ
Robot bao gồm Robot cánh tay kép có trọng tải cao, tốc độ cao, phạm vi tiếp cận mở rộng,
siêu cơ động và cấu tạo có thể lên đến 15 trục cánh tay kép độc đáo. Với bộ điều khiển tiên
tiến, hệ thống thị giác, thiết bị ngoại vi và các thành phần tốt nhất khác, Robot của hãng
Yaskawa có thể định cấu hình giải pháp phù hợp với nhu cầu sản xuất của hầu hết các cơng
việc trong cơng nghiệp.

Hình 1.1: Các dòng robot hãng Yaskawa.
Cánh tay Robot của Yaskawa có đa dạng các dịng sản phẩm phù hợp cho nhiều ứng
dụng cơng nghiệp. Có thể lựa chọn Robot dựa theo thông số kỹ thuật về tải trọng và phạm
vi tiếp cận yêu cầu của từng công việc. Mỗi mô hình tương thích với một hoặc nhiều mơ
hình điều khiển rơ bốt, cho phép người lập trình và điều khiển các tác vụ của một Robot
duy nhất hoặc phối hợp nhiều Robot.

4


1.2. Giới thiệu Robot EPX2050
Đây là dòng Robot được dùng phổ biến trong cơng nghiệp sơn, có thể được ứng dụng
vào nhiều cơng đoạn khác nhau như sơn lót, sơn lớp cơ sở, sơn phủ lớp cuối...với khả năng
sử dụng nhiều loại sơn khác nhau như sơn bột, sơn nước, ... Robot rất linh hoạt với hỗ trợ
3 trục, kèm theo 5 chế độ kiểm soát độ dày của lớp sơn, khả năng làm việc liên tục ổn định

giúp chất lượng, cơng suất sơn của dây chuyền tăng đáng kể.

Hình 1.2: Robot EPX2050.

5


1.3. Kết cấu cơ khí

Hình 1.3: Bản vẽ cơ khí kiểu Lemma wrist.

6


Hình 1.4: Bản vẽ cơ khí kiểu Hollow slim arm.
Robot sơn EPX2050 có 6 trục tốc độ cao, linh hoạt rất lý tưởng cho các ứng dụng sơn
phủ oto và nhiều ứng dụng khác trong công nghiệp.
Thiết kế cổ tay lõm là lý tưởng cho việc sơn các phần có đường viền như bề mặt bên
trong/bên ngoài; phù hợp để lắp đặt thiết bị phun sơn. Đường kính ống bên trong là 50mm
tránh sự can thiệp giữa ống và bộ phận/thiết bị, đảm bảo thời gian chu kỳ tối ưu và Robot
tiếp cận/truy cập.
7


Trọng tải 10 kg với kiểu Lemma wrist, 15 kg với kiểu Hollow slim arm. Robot có thiết
kế nhỏ gọn, linh hoạt được chứng nhận dùng trong môi trường nguy hiểm.
Robot có bộ điều khiển nâng cao NX100-FM:
•
•

Bao gồm phần mềm ứng dụng cụ thể cho các ứng dụng sơn.
Phối hợp hoạt động của Robot và thiết bị sơn, kể cả súng phun.

•
•

Hỗ trợ hướng dẫn kiểm sốt súng như khởi động / dừng phun và điều kiện sơn.
Tất cả các thơng số vị trí vẽ có thể được nộp và lưu.

•

Hỗ trợ các mạng chuẩn (như DeviceNet, ControlNet, Profibus-DP và InterbusS), cho phép kết nối với bộ điều khiển thiết bị sơn và bộ điều khiển dây chuyền
sản xuất.

1.4. Thơng số kĩ thuật

Hình 1.5: Thơng số kĩ thuật Robot EPX2050.

8


Hình 1.6: Thơng số bộ điều khiển NX100-FM

9


CHƯƠNG 2: ĐỘNG HỌC THUẬN VỊ TRÍ ROBOT
2.1. Cơ sở lý thuyết
Các phương pháp giải bài toán động học thuận vị trí:
•

•

Đối với những robot có số bậc tự do nhỏ: Sử dụng phương pháp hình học để
tính tốn trực tiếp.
Khi robot có bậc tự do > 2 sẽ gây khó khăn trong việc tính tốn trực tiếp.
 Sử dụng phương pháp Denavit – Hartenberg.

Phương pháp D-H (Denavit-Hartenberg):
❖ Bước 1: Xác định số khớp và thanh nối của Robot.
o n khớp (i=1n): khớp quay + tịnh tiến.
o (n+1) thanh nối: 0n.
o Thanh nối 0 cố định.
o Thanh nối n gắn với khâu tác động cuối.
o Khớp i nối giữa thanh nối (i-1) và i.

Hình 2.1: Số khớp và thanh nối của Robot.
❖ Bước 2: Khai báo các hệ trục tọa độ trên mỗi thanh nối OnXiYi Zi (i=0n).
o Xác định trục Zi:
10


▪ Khớp (i+1) tịnh tiến: Zi là trục mà theo nó khớp (i+1) trượt.
▪ Khớp (i+1) quay: Zi là trục mà xung quanh nó khớp (i+1) quay.
o Xác định trục Xi:
▪ Trường hợp 1: Zi-1 và Zi chéo nhau.

Hình 2.2: Trường hợp chéo nhau.
▪ Trường hợp 2: Zi-1 // Zi.

Hình 2.3: Trường hợp song song.

▪ Trường hợp 3: Zi-1 cắt Zi.

Hình 2.5: Cách đặt vị trí tâm 𝑂0.
11


❖ Tâm On đặt tại vị trí chọn trên khâu tác động cuối.
❖ Nếu khớp i là khớp tịnh tiến, tâm Oi đặt ở điểm cuối thanh nối i.

Hình 2.4: Trường hợp cắt nhau.

➢ Chú ý:
❖ Tâm O0 có thể đặt ở vị trí cố định bất kỳ trên đế, hoăc điểm cố định trên Robot.

Hình 2.6: Trường hợp khớp i là khớp tịnh tiến.

❖ Bước 3: Xác định các biến khớp.
o Nếu i là khớp quay.

12


Hình 2.7: Trường hợp nếu i là khớp quay.
o Nếu i là khớp tịnh tiến.

Hình 2.8: Trường hợp nếu i là khớp tịnh tiến.
❖ Bước 4: Xác định các thông số động học của thanh nối.
➢ Các tham số:
❖ ai: chiều dài thanh nối i, ai  0.
❖ i: góc vặn của thanh nối i.

❖ di: độ lệch với thanh nối i (có thể âm).
❖ qi: góc quay của thanh nối i.
➢ Lập bảng D-H: i = 1n
❖ Nếu i là khớp tịnh tiến: biến di.
❖ Nếu i là khớp quay: biến qi.

13


Hình 2.9: Xác định các thơng số động học của thanh nối.
❖ Bước 5: Xác định ma trận Ai.
Ti i −1 = Ai = Rot ( Z , i )Trans ( Z , d i )Trans ( X i , ai ) Rot ( X ,  i )
cos i
 sin 
i
=
 0

 0
ci
 s
= i
0

0

− sin i
cos i
0
0

−c i si
c i ci
s
0

0
0
1
0

0  1
0  0
0  0

1  0

s i si
− s i ci
c
0

0
1
0
0

0 0  1
0 0  0
1 di  0


0 1  0

0
1
0
0

0 ai  1
0


0 0  0 cos  i
1 0  0 sin  i

0 1  0
0

0
− sin  i
cos  i
0

0
0 
0

1

ai ci 
ai si 

di 

1 

❖ Bước 6: Tính tốn ma trận Tn0 .

14


 nx
n
Tn0 = A1 A2 A3 ... An =  y
 nz

0

ox
oy
oz

ax
ay
az

0

0

px 
p y 

pz 

1

2.2. Tính tốn động học thuận robot:
Mục đích: Từ các biến khớp thành phần, xác định vị trí bàn tay máy.
Ta xác định các hệ trục tọa độ gắn với Robot như sau:

Z0

Oo
X0

Hình 2.10: Tên các phần và các trục làm việc của robot.
Trên Hình 2.10 là miêu tả các phần và các trục làm việc của Robot. Dựa theo chiều
quay thuận và nghịch của các khớp quay, ta xác định hệ trục các hệ trục tọa độ ứng với
từng khớp là: (Các góc 𝜽𝑺 , 𝜽𝑳 , 𝜽𝑼 , 𝜽𝑹 , 𝜽𝑩 , 𝜽𝑻 lần lượt xoay quanh các trục:
𝒁𝟎 , 𝒁𝟏 , 𝒁𝟐 , 𝒁𝟑 , 𝒁𝟒 , 𝒁𝟓 ).

15


Đồ thị vecto cho các trục tọa độ:

Hình 2.11: Đồ thị vecto các trục toạ độ.
Bảng D-H:
i

𝛼𝑖

𝑎𝑖

𝑑𝑖

𝜃𝑖

1

90𝑜

a1

0

𝜃1

2

0

a2

0

𝜃2

3

0

a3

0

𝜃3

16


4

90𝑜

0

d4

𝜃4

5

−90𝑜

0

d5

𝜃5

6

0

0

d6

𝜃6

Trong đó:
• 𝛂𝐢 là góc lệch giữa Zi và Zi−1 (Góc quay từ Zi−1 sang Zi thơng qua X i ).
• 𝛉𝐢 là góc lệch giữa X i và X i−1 (Góc quay từ X i−1 sang X i thơng qua Zi−1 ).
• 𝐚𝐢 là khoảng cách giữa Zi và Zi−1 .
• 𝐝𝐢 là khoảng cách giữa X i−1 và X i .
➢ Xác định ma trận 𝐀𝐢 :
Từ công thức tổng quát, ta xác định các ma trận 𝐴𝑖 (i=1÷6):
A1 =
cos(t1)
( )
[ sin t1
0
0

0
0
1
0

sin(t1)
−cos(t1)

0
0

L1 ∗ cos(t1)
L1 ∗ sin(t1) ]
0
1

A2 =
cos(t2)
[ sin(t2)
0
0

−sin(t2)
cos(t2)
0
0

0
0
1
0

L2 ∗ cos(t2)
L2 ∗ sin(t2) ]
0
1

−sin (t3)

cos (t3)
0
0

0
0
1
0

L3 ∗ cos(t3)
L3 ∗ sin(t3) ]
0
1

A3 =
cos(t3)
[ sin(t3)
0
0
A4 =
17


cos(t4)
sin(t4)
[
0
0

0

0
1
0

sin(t4)
0
−cos(t4) 0
]
0
d4
0
1

A5 =
cos(t5)
sin(t5)
[
0
0

0
0
−1
0

−sin(t5)
cos(t5)
0
0

0
0
]
d5
1

A6 =
cos(t6)
sin(t6)
[
0
0

−sin(t6)
cos(t6)
0
0

0
0
1
0

0
0
]
d6
1

T06 =

nx
ny
[
nz
0

ox
oy
oz
0

ax
ay
az
0

px
py
]
pz
1

➢ Tính ma trận 𝐓𝟔𝟎
Tính 𝐓𝟔𝟎 theo công thức tổng quát với n=6:
Sử dụng m.file MATLAB để tính tốn như sau:
• File dong_hoc_thuan.m:
clear all
clc
syms L1 L2 L3 d4 d5 d6 S L U R B T nx ny nz ox oy oz ax ay az px
py pz

18


T60=[nx,ox,ax,px;ny,oy,ay,py;nz,oz,az,pz;0,0,0,1]
A1=[cos(S),0,sin(S),L1*cos(S);sin(S),0,cos(S),L1*sin(S);0,1,0,0;0,0,0,1]
A2=[cos(L),sin(L),0,L2*cos(L);sin(L),cos(L),0,L2*sin(L);0,0,1,0;0,0,0,1]
A3=[cos(U),sin(U),0,L3*cos(U),;sin(U),cos(U),0,L3*sin(U);0,0,1,0;0,0,0,1]
A4=[cos(R),0,sin(R),0;sin(R),0,-cos(R),0;0,1,0,d4;0,0,0,1]
A5=[cos(B),0,-sin(B),0;sin(B),0,cos(B),0;0,-1,0,d5;0,0,0,1]
A6=[cos(T),-sin(T),0,0;sin(T),cos(T),0,0;0,0,1,d6;0,0,0,1]
T60=simplify(A1*A2*A3*A4*A5*A6)
nx=T60(1,1)
ox=T60(1,2)
ax=T60(1,3)
px=T60(1,4)
ny=T60(2,1)
oy=T60(2,2)
ay=T60(2,3)
py=T60(2,4)
nz=T60(3,1)
oz=T60(3,2)
az=T60(3,3)
pz=T60(3,4)

Kết quả:
𝑛𝑥
𝑛
𝑇60 = 𝐴1 𝐴2 𝐴3 𝐴4 𝐴5 𝐴6 = [ 𝑦
𝑛𝑧

0

𝑜𝑥
𝑜𝑦
𝑜𝑧
0

𝑎𝑥
𝑎𝑦
𝑎𝑧
0

𝑝𝑥
𝑝𝑦
]
𝑝𝑧
1

nx = cos(T)*(sin(B)*sin(S) + cos(B)*cos(L + R + U)*cos(S)) cos(S)*sin(L + R + U)*sin(T)

19


ox = - sin(T)*(sin(B)*sin(S) + cos(B)*cos(L + R + U)*cos(S)) cos(S)*cos(T)*sin(L + R + U)
ax = cos(B)*sin(S) - sin(B)*cos(L + R + U)*cos(S)
px = L1*cos(S) + d6*(cos(B)*sin(S) - sin(B)*cos(L + R +
U)*cos(S)) + d4*sin(S) + L2*cos(L)*cos(S) + d5*cos(S)*sin(L + R
+ U) + L3*cos(L)*cos(S)*cos(U) - L3*cos(S)*sin(L)*sin(U)
ny = - cos(T)*(sin(B)*cos(S) - cos(B)*cos(L + R + U)*sin(S)) sin(L + R + U)*sin(S)*sin(T)
oy = sin(T)*(sin(B)*cos(S) - cos(B)*cos(L + R + U)*sin(S)) cos(T)*sin(L + R + U)*sin(S)

ay = - cos(B)*cos(S) - sin(B)*cos(L + R + U)*sin(S)
py = L1*sin(S) - d6*(cos(B)*cos(S) + sin(B)*cos(L + R +
U)*sin(S)) - d4*cos(S) + L2*cos(L)*sin(S) + d5*sin(L + R +
U)*sin(S) + L3*cos(L)*cos(U)*sin(S) - L3*sin(L)*sin(S)*sin(U)
nz = cos(L + R + U)*sin(T) + cos(B)*cos(T)*sin(L + R + U)
oz = cos(L + R + U)*cos(T) - cos(B)*sin(L + R + U)*sin(T)
az = -sin(B)*sin(L + R + U)
pz = L3*sin(L + U) - d5*(cos(L + U)*cos(R) - sin(L + U)*sin(R))
+ L2*sin(L) - d6*sin(B)*(cos(L + U)*sin(R) + sin(L + U)*cos(R))

2.3. Giao diện tính tốn động học thuận trên GUI Matlab
Dưới đây là bảng động học thuận vị trí được thiết kế trên GUI của matlab giúp người
sử dụng có thể tính tốn các ma trận dựa trên các góc theta của các khớp một cách dễ dàng.
Trên giao diện GUI ta tạo 6 ô trống “Edit text” 1-6 để nhập góc quay mong muốn bằng
một số chính xác trực tiếp vào đây ứng với các góc theta 1-6.
20


Nút “TINH TOAN” là nút lệnh để phần mềm thực hiện tính tốn ma trận T06.
Bảng “T06” hiển thị kết quả tính tốn động học thuận thơng qua các khối “Static text”
chỉ có khả năng hiển thị chứ khơng thể nhập số.

Hình 2.12: Giao diện tính tốn động học thuận.
Ta thực hiện tính tốn với các góc quay được nhập vào phần mềm:

Hình 2.13: Kết quả tính tốn với số liệu nhập trước.

21

CHƯƠNG 3: MA TRẬN JACOBY
3.1. Tính tốn ma trận Jacoby
Ta có:
nx
ny
n
J= z
0
0
[0

ox
oy
oz
0
0
0

ax
ay
az
0
0
0

0
0
0
nx
ny

nz

0
0
0
ox
oy
oz

0
0
0 H
. J
ax
ay
az ]

nx ox ax 0 0 0
JH11 JH12 JH13 JH14 JH15
ny oy ay 0 0 0
JH21 JH22 JH23 JH24 JH25
JH31 JH32 JH33 JH34 JH35
n oz az 0 0 0
= z
.
0 0 0 nx ox ax
JH41 JH42 JH43 JH44 JH45
JH51 JH52 JH53 JH54 JH55
0 0 0 ny oy ay
[ 0 0 0 nz oz az ] [JH61 JH62 JH63 JH64 JH65

Các bước tính tốn ma trận Jacoby sử dụng thuật toán ma trận JH
Bước 1: Xác định ma trận 𝑇𝑛𝑖 (𝑖 = 0 → 𝑛 − 1)

JH16
JH26
JH36
JH46
JH56
JH66 ]

22


Bước 2: Xác định ma trận 𝐽𝐻

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

 q1

px
q2

px 

qn 
H
p y 

qn 

H
pz 

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

H

H

...

p y

H

...

q2
pz
q2

...

 x
q2

...

H

H

H

 y

...

q2
 z
q2

H

...

-

Khi (i+1) là khớp trượt, biến khớp 𝑟𝑖+1

-

Sử dụng ma trận 𝑇𝑛𝑖
px i
= nz
ri +1

H

H

p y

ri +1
H
 y H  y
 x
=
=
=0
ri +1
ri +1
ri +1
,

= i oz

pz i
= az
ri +1

H

H

Khi (i+1) là khớp quay, biến khớp 𝜃𝑖+1
H

px

i +1

H

= i ny i px − i nx i p y

 x i
= nz
i +1
,

H

p y

, i +1
H

= i o y i p x − i ox i p y

 y

i +1

H

pz

, i +1

= i a y i px − i ax i p y

,

 z i
= az
i +1

H

= oz
i

,

Bước 3: Tính J
 Rn0
J =
0

0 H
 J
Rn0 

23


3.2. Chương trình Matlab
syms theta d alpha a
A = [cos(theta) -cos(alpha)*sin(theta) sin(alpha)*sin(theta) a*cos(theta)
sin(theta)

cos(alpha)*cos(theta) -sin(alpha)*cos(theta) a*sin(theta)

0

sin(alpha)

cos(alpha)

d

0

0

0

1

];

% tinh A1
syms theta1 a1
A1= subs(A,{a,alpha,d,theta},{a1,pi/2,0,theta1});
% tinh A2
syms theta2 a2
A2= subs(A,{a,alpha,d,theta},{a2,0,0,theta2});
% tinh A3
syms theta3 a3
A3= subs(A,{a,alpha,d,theta},{a3,0,0,theta3});
% tinh A4
syms theta4 d4
A4= subs(A,{a,alpha,d,theta},{0,pi/2,d4,theta4});
% tinh A5
syms theta5 d5
A5= subs(A,{a,alpha,d,theta},{0,-pi/2,d5,theta5});
% tinh A6
syms theta6 d6
A6= subs(A,{a,alpha,d,theta},{0,0,d6,theta6});
T56=A6;
T46=simplify(A5*A6);
T36=simplify(A4*A5*A6);
T26=simplify(A3*A4*A5*A6);
T16=simplify(A2*A3*A4*A5*A6);
T06=simplify(A1*A2*A3*A4*A5*A6);
%Tính JH
dpx1=simplify(T06(2,1)*T06(1,4)-T06(1,1)*T06(2,4));
dpy1=simplify(T06(2,2)*T06(1,4)-T06(1,2)*T06(2,4));

24