Tính toán thiết kế cho robot elbow

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN ĐIỆN
BỘ MÔN TỰ ĐỘNG HOÁ CÔNG NGHIỆP
====o0o====

BÀI TẬP LỚN
ĐỀ TÀI:
TÍNH TOÁN THIẾT KẾ CHO ROBOT ELBOW

Giáo viên hướng dẫn

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

Hà nội, 11-2015


MỤC LỤC
LỜI MỞ ĐẦU..............................................................................................................1
Chương 1...................................................................................................................... 2
GIỚI THIỆU ROBOT NHÓM NGHIÊN CỨU........................................................2
1.1Kết cấu cơ khí robot Elbow.....................................................................................2
1.2. Cổ tay robot...........................................................................................................3
1.3. Bàn tay robot (Cơ cấu tác động cuối)...................................................................3
1.4. Ứng dụng...............................................................................................................4
Chương 2...................................................................................................................... 6
XÂY DỰNG PHẦN MỀM TÍNH TOÁN ĐỘNG .....................................................6
HỌC THUẬN ROBOT ELBOW................................................................................6
2.1. Xác định số khớp và các hệ trục tọa độ................................................................6
2.2 Xác định các thông số động học của robot Elbow thông qua bảng DH................7
2.3 Các ma trận A của robot Elbow.............................................................................8
2.4 Tính toán ma trận T06 ( hàm truyền RTH của robot hay ma trận xác định tọa

độ vị trí và hướng của khâu cuối).................................................................................9
2.5. Xây dựng phần mềm tính toán ( sử dụng GUI trong Matlab)............................10
Chương 3.................................................................................................................... 12
VIẾT CHƯƠNG TRÌNH MATLAB TÍNH MA TRẬN JACOBY .......................12
TRỰC TIẾP VÀ THÔNG QUA MA TRẬN ..........................................................12
3.1. Tính ma trận Jacoby trực tiếp..............................................................................12
3.1.1 Tính toán........................................................................................................12
3.1.2 Viết chương trình Matlab tính ma trận Jacoby trực tiếp...............................14
3.2 Tính ma trận Jacoby thông qua ma trận JH.........................................................15
3.2.2 Viết chương trình Matlab tính ma trận Jacoby thông qua ma trận HJ.........20
Chương 4.................................................................................................................... 19
THIẾT KẾ QUỸ ĐẠO TRONG KHÔNG GIAN KHỚP ......................................19
4.1. Giới thiệu và cơ sở thiết kế quỹ đạo....................................................................19
4.2. Tính toán thiết kế quỹ đạo chuyển động.............................................................19
4.2.2 Phương pháp quỹ đạo dạng đa thức 2-1-2....................................................22
KẾT LUẬN................................................................................................................. 22
TÀI LIỆU THAM KHẢO.........................................................................................23


Lời nói đầu

LỜI MỞ ĐẦU

Từ lâu, con người đã không còn xa lạ với từ “Robot” nữa. Bởi lẽ, Robot đã xuất
hiện nhiều trong cuộc sống của chúng ta và nếu chưa tận mắt chứng kiến những Robot
này hoạt động thì cũng được xem qua các phương tiện thông tin đại chúng.
Có thể thấy Robot đã đi vào mọi lĩnh vực của đời sống con người. Vậy, Robot
được ứng dụng như thế nào? Có thể nói, Robot được ứng dụng rất phong phú trong đời
sống con người, cả trong trường học và trong nông nghiệp. Đặc biệt trong công nghiệp,
Robot được sử dụng rộng rãi để làm một số công việc: vận chuyển nguyên vật liệu, sản

phẩm, máy móc hay làm những công việc nặng nhọc, hoặc trong những môi trường làm
việc có nhiệt độ cao, độc hại…
Đầu tiên, Robot được dùng trong một số ngành công nghiệp kỹ thuật cao như công
nghiệp xe hơi, công nghiệp máy bay…để làm những công việc như hàn thân xe, phun
sơn…Những Robot này là những cánh tay máy mô phỏng con người, mỗi tay máy bao
gồm nhiều khâu liên kết với nhau bằng các khớp. Các khâu này có thể chuyển động
tương đối với nhau và làm thay đổi tầm với của Robot. Thông thường những Robot này
được đặt cố định một chỗ và chỉ thao tác được trong khả năng tầm với của chúng.
Với những kiến thức đã học và sau một thời gian tìm hiểu cùng với sự giúp đỡ tận
tình của cô PGS. TS. Nguyễn Phạm Thục Anh thuộc bộ môn Tự động hóa công nghiệp
trường Đại học bách khoa Hà Nội, và sự đóng góp trao đổi xây dựng của bạn bè nhóm
em đã hoàn thành nhiệm vụ được giao.
Song với những hiểu biết còn hạn chế nên nhóm em không tránh khỏi những thiếu
sót. Chúng em rất mong được sự chỉ bảo của cô giáo và sự góp ý kiến của các bạn trong
lớp Kỹ thuật Robot để bài tập của nhóm em được hoàn thiện hơn.

Chúng em xin chân thành cảm ơn !

1


Chương 1. Giới thiệu robot nhóm nghiên cứu

Chương 1
GIỚI THIỆU ROBOT NHÓM NGHIÊN CỨU

1.1 Kết cấu cơ khí robot Elbow
Robot Elbow ở vị trí ban đầu

Robot Elbow ở vị trí bất kỳ

Hình 1.1. Kết cấu cơ khí robot Elbow.
+ Cấu hình Robot Elbow :
Là một cấu hình robot 6 DOF (6 bậc tự do) tức là gồm 6 khớp chuyển động độc
lập.

2


Chương 1. Giới thiệu robot nhóm nghiên cứu

1.2. Cổ tay robot

Hình 1.2. Cơ cấu cổ tay ba bậc tự do.
Cổ tay có nhiệm vụ định hướng chính xác bàn tay robot (cơ cấu tác động cuối)
trong không gian làm việc. Ví dụ bàn tay robot cần định hướng chính xác so với chi tiết
để gắp chi tiết.
Thông thường cổ tay robot có 3 bậc tự do tương ứng với 3 chuyển động có cấu tạo
điển hình như hình 1.2: Cổ tay xoay xung quanh trục thanh nối cuối cùng (Roll), cổ tay
quay xung quanh trục thẳng đứng tạo chuyển động lắc phải, trái của bàn tay (Pitch), cổ
tay quay xung quang trục nằm ngang tạo chuyển động lên xuống của bàn tay (Yaw).
1.3. Bàn tay robot (Cơ cấu tác động cuối)
Bàn tay được gắn lên cổ tay robot đảm bảo cho robot thực hiện các nhiệm vụ khác
nhau trong không gian làm việc. Cơ cấu bàn tay có hai dạng khác nhau tùy theo chức
năng của robot trong dây chuyền sản xuất: cơ cấu bàn kẹp (gripper) và cơ cấu dụng cụ
(tool).
Một số dạng bàn tay robot của Robot Unimate (Mỹ, 1961):

3

Chương 1. Giới thiệu robot nhóm nghiên cứu

Hình 1.3. Một số hình ảnh về các cơ cấu kẹp.
1.4. Ứng dụng

Hình 1.4.Video ứng dụng cấu hình robot Elbow điều khiển chuyển động theo cánh tay
trong công nghiệp.
4


Chương 1. Giới thiệu robot nhóm nghiên cứu

Hình 1.5.Ứng dụng cấu hình robot Elbow ngoài trạm không gian vũ trụ.

Hình 1.6. Ứng dụng cấu hình robot Elbow trong
công nghiệp sản xuất và lắp ráp xe hơi.

5


Chương 2. Xây dựng phần mềm tính toán động học thuận Robot Elbow

Chương 2
XÂY DỰNG PHẦN MỀM TÍNH TOÁN ĐỘNG
HỌC THUẬN ROBOT ELBOW
2.1. Xác định số khớp và các hệ trục tọa độ
- Robot Elbow 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 Xi, Yi : xác định như hình bên dưới. ( Lưu ý : để làm đơn giản cho việc
tính toán, ta sẽ chỉnh sửa một số trục tọa độ như sau. O 0 sẽ trùng với O1 . O4 trùng với O5,
O6 . Như vậy : a1=a5=a6=0, di = 0, dễ dàng hơn trong việc tính toán).

Hình 2.1. Vị trí ban đầu của robot Elbow và các hệ tọa độ
6


Chương 2. Xây dựng phần mềm tính toán động học thuận Robot Elbow

2.2 Xác định các thông số động học của robot Elbow 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.
+ θ i : góc vặn của thanh nối i . Là góc quay quanh trục z i-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.
+ ai : khoảng dịch chuyển giữa 2 trục khớp động kề nhau theo trục x.
- Như vậy ta có :
+ a1 = 0 do trục Z0 và Z1 vuông góc với nhau.
+ a2 là khoảng cách giữa Z1 và Z2
+ a3 là khoảng cách giữa Z2 và Z3
+ a4 là khoảng cách giữa Z3 và Z4
+ a5 = 0 do trục Z4 và Z5 vuông góc với nhau.
+ a6 = 0 do trục Z5 trùng với trục Z6
0
+ α1 = 90 là góc quay quanh trục x1 để biến z0 thành z1 .

+ α 2 = 0 là góc quay quanh trục x2 để biến z1 thành z2 .
+ α3 = 0 là góc quay quanh trục x3 để biến z2 thành z3 .

0
+ α 4 = −90 là góc quay quanh trục x4 để biến z3 thành z4 .

+ α5 = 900 là góc quay quanh trục x5 để biến z4 thành z5 .
+ α 6 = 0 là góc quay quanh trục x6 để biến z5 thành z6 .
+ d1 là khoảng cách đo dọc trục giữa O0 và O1
d
+ 6 là khoảng cách đo dọc trục giữa O5 và O6
(Như đã nói ở trên ta sẽ cho: d1=d6=0)

7


Chương 2. Xây dựng phần mềm tính toán động học thuận Robot Elbow

Như vậy ta được bảng D-H như sau:
Khâu

θi

αi

ai

di

1

θ1

900

0

0

2

θ2

0

a2

0

3

θ3

0

a3

0

4

θ4

-900

a4

0

5

θ5

900

0

0

6

θ6

0

0

0

2.3 Các ma trận A của robot Elbow
Ký hiệu :
C1 = cos θ1 , C2 = cos θ 2 , …, C6 = cos θ 6
S1 = sin θ1 , S 2 = sin θ 2 …, S6 = sin θ 6

C12 = cos(θ1 + θ 2 ) , C23 = cos(θ 2 + θ3 ) , ….
S12 = sin(θ1 + θ 2 ) , S 23 = sin(θ 2 + θ3 ) ,….

Các ma trận A được xác định bằng ma trận biến đổi tọa độ thuần nhất tổng quát :

Cθ i

S
Ai =  θ i
 0

 0

− S Cα i
θi
C Cα i
θi
Sα i

S Sα i
θi
−C Sα i
θi
Cα i

0

0

8

aC 
θi 
aS 
θi 
d 


1 


Chương 2. Xây dựng phần mềm tính toán động học thuận Robot Elbow
C1 0

0
S
A = 1
1
0 1

0 0

S
0
1

−C 0 
1  ;
0 0

C3

S
A = 3
3
0

0

−S
3
C
3
0
0

C5

S
A = 5
5
 0

 0

0



0

1
0 C a 
3 3
0 S a 
3 3 ;
1
0 

0
1 

S5 0

0 −C 0
5
;
1
0
0

0
0
1

C2

S
A = 2
2

 0

 0

−S

0

0

C4

S
A = 4
4
 0

 0

0

−S

0
−1

C
4
0

C a 
4 4
S a 
4 4
0 

0

0

1 

2

C
2
0

−S
6
C
6
0
0

C6

S
A = 6
6

 0

 0

0 C a 
2 2
0 S a 
2 2
1
0 


1 
4



0 0


0 0


1 0

0 1

2.4 Tính toán ma trận T06 ( hàm truyền RTH của robot hay ma trận xác định tọa độ
vị trí và hướ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 RTH 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)

 nx
n
0
T6 =  y
 nz

0

ox
oy
oz

ax
ay
az

0

0

px 
py 
 = A1. A2 . A3 . A4 . A5 . A6
pz 

1

Sử dụng công cụ matlab để nhập và nhân các ma trận. Sau khi dùng hàm simplify để rút

gọn, ta sẽ được thành phần của ma trận như sau :

9


Chương 2. Xây dựng phần mềm tính toán động học thuận Robot Elbow

nx = C (C C C − S
S )−S S S
1 234 5 6 234 6
1 5 6
n y = S (C C5C − S
S ) + C S5S
1 234
6 234 6
1 6
nz = C C5C + S
S
234
6 234 6
Ox = −C (C C5S + S
C ) + S S5S
1 234
6 234 6
1 6
O y = − S (C C S + S
C )−C S S
1 234 5 6 234 6
1 5 6
Oz = − S

C S +C C
234 5 6
234 6
ax = C C
S5 + S C5
1 234
1
ay = S C
S −C C
1 234 5 1 5
az = S
S
234 5
p x = C (C
a +C a +C a )
1 234 4
23 3
2 2
p y = S (C
a +C a +C a )
1 234 4
23 3
2 2
pz = S
a +S a +S a
234 4 23 3 2 2

2.5. Xây dựng phần mềm tính toán ( sử dụng GUI trong Matlab)
Giao diện phần mềm tính toán động học thuận vị trí robot
- Từ ma trận hàm truyền ta có thể thấy rằng các giá trị của các thành phần

phụ thuộc vào các giá trị của các tham số đầu vào là:
+ Các góc: θ1 , θ 2 , θ3 , θ 4 , θ5 , θ 6 , ( tương ứng với dữ liệu đầu vào : từ ‘Goc Theta1’ đến
‘Góc Theta6’)
+ Các giá trị a2, a3, a4.
- Sau khi thiết kế giao diện và viết code ta sẽ được như hình bên dưới :

10


Chương 2. Xây dựng phần mềm tính toán động học thuận Robot Elbow

Hình 2.2. Giao diện tính toán động học thuận.
Ta thử một ví dụ và chạy chương trình sẽ được hình như sau:

Chương trình code : Tham khảo phụ lục 1

11


Chương 3. Viết chương trình matlab tính ma trân Jacoby

Chương 3
VIẾT CHƯƠNG TRÌNH MATLAB TÍNH MA TRẬN JACOBY
TRỰC TIẾP VÀ THÔNG QUA MA TRẬN

H

J

3.1. Tính ma trận Jacoby trực tiếp

3.1.1 Tính toán
- Ta có : X= [ px py pz Φx Φy Φz]T
- Với Φx Φy Φz : tương ứng là các góc quay vi sai của tay xung quanh các trục x, y, z.
Tuy nhiên, không tồn tại phương trình duy nhất mô tả sự quay xung quanh các trục, do
đó sẽ không có phương trình duy nhất cho tính toán Φx Φy Φz.
Như vậy ta có: X= [ px py pz ]T
- Ta đã xác định được:
p x = C (C
a +C a +C a )
1 234 4
23 3
2 2
p y = S (C
a +C a +C a )
1 234 4
23 3
2 2
pz = S
a +S a +S a
234 4 23 3 2 2
- Tiếp theo thực hiện đạo hàm theo biến khớp:
+ Tính đạo hàm px, nhận được phương trình sai phân sau:

dpx =

∂px
∂p
∂p
dθ1 + x dθ 2 + ... + x dθ 6
∂θ1

∂θ 2
∂θ 6

dpx = − S (C234 a4 + C23a3 + C2 a2 )dθ1 + C1 (− S 234 a4 − S 23a3 − S 2 a2 )dθ 2
+C1 (− S234 a4 − S 23a3 )dθ3 + C1 (− S 234 a4 )dθ 4
Từ đó nhận được các thành phần hàng 1 của ma trận Jacoby:

12


Chương 3. Viết chương trình matlab tính ma trân Jacoby

∂px

J
=
= − S1 (C234 a4 + C23a3 + C2 a2
11

∂θ1

∂px

 J12 = ∂θ = C1 (− S 234 a4 − S 23a3 − S 2 a2 )
2


∂px
= C1 (− S 234 a4 − S 23a3 )
 J13 =

∂
θ

3

 J = ∂px = C (− S a )
1
234 4
 14 ∂θ 4

 J = ∂px = 0
 15 ∂θ5

 J = ∂px = 0
 16 ∂θ
6

Tương tự với Py, Pz.
Ta được hàng 2:
∂p y

 J 21 =
∂θ1


∂p y
 J 22 =
∂θ 2



∂p
 J 23 = y
∂θ3


 J = ∂p y
 24 ∂θ 4

∂p y

 J 25 = ∂θ
5


∂p y
 J 26 =
∂θ6


= C1 (C234 a4 + C23 a3 + C2 a2
= S1 (− S 234 a4 − S23 a3 − S 2 a2 )
= S1 (− S 234 a4 − S23 a3 )
= S1 (− S 234 a4 )
=0
=0

Ta được hàng 3:

13

Chương 3. Viết chương trình matlab tính ma trân Jacoby
∂pz

 J 31 = ∂θ = 0
1

∂pz

 J 32 = ∂θ = C234 a4 + C23 a3 + C2 a2 )
2


∂pz
 J 33 = ∂θ = C234 a4 + C23 a3 )

3

 J = ∂pz = C a
234 4
 34 ∂θ 4

 J = ∂pz = 0
 35 ∂θ5

 J = ∂pz = 0
 36 ∂θ 6

3.1.2 Viết chương trình Matlab tính ma trận Jacoby trực tiếp
a. Thiết kế giao diện

Hình 3.1. Giao diện phần mềm tính ma trận Jacoby trực tiếp.
- Nhập thử số và chạy thử ta sẽ được kết quả như sau:

14


Chương 3. Viết chương trình matlab tính ma trân Jacoby

Chương trình code: tham khảo phụ lục 2.

3.2 Tính ma trận Jacoby thông qua ma trận JH
3.2.1 Tính toán
- Ta xác định các ma trân iTn theo các hệ tọa độ lần lượt từ khâu cuối trở về gốc:
C6

5T =  S6
6 
 0

 0

−S

6

C
6
0
0

0 0


0 0


1 0

0 1

C5C6

4T = A A =  S5C6
5 6  S
6
 6
 0

−C S
5 6
−S5C
6
C
6
0

S
0
5


−C5 0


0

0

0

1 



15


Chương 3. Viết chương trình matlab tính ma trân Jacoby
C4C5C6 − S4S6

3T = A A A =  S4C5C6 + C4 S6
6
4 5 6 
− S5C
6


0

−C C5S − S C C S5 C a 

4
6 4 6
4
4 4
−S C5S + C C
S S5 S a 
4
6
4 6
4
4 4
S5S
C5
0 
6

0
0
1 

 C34C5C6 − S34 S6 − C34C5S6 − S34C6 C34S5 C34a4 + C3a3 


S C C + C S −S C S + C C S S S a + S a 
2T = A A A A =  34 5 6 34 6
34 5 6 34 6 34 5 34 4 3 3 
6 3 4 5 6 

−S C
S S

C
0
5 6
56
5




0
0
0
1

 C234C5C6 − S234 S6 − C234C5S6 − S234C6 C234 S5 C234 a4 + C23a3 + C2a2 


S
C
C
+
C
S
−
S
C
S
+
C
C

S
S
S
a
+
S
a
+
S
a

1T = A A A A A =  234 5 6 234 6
234 5 6 234 6 234 5 234 4 23 3 2 2 
6 2 3 4 5 6 

−S C
SS
C
0
5 6
56
5




0
0
0
1

 nx
n
0
T6 =  y
 nz

0

ox

ax

oy

ay

oz

az

0

0

px 
p y 
= A1. A2 . A3 . A4 . A5 . A6
pz 


1

Các hệ số của 0T6 được xác định ở phần động lực thuận vị trí.
- Tiếp theo ta sẽ tính toán ma trận JH bằng việc tính các ma trận theo từng cột, tương ứng
với từng khớp của robot.
0
+ Với khớp θ1 là khớp quay. Sử dụng ma trận T6 :
H

J11 = n y1. px1 − nx1. p y1
− [ C1 (C234C5C6 − S 234 S6 ) − S1S5C6 ] .[ S1 (C234 a4 + C23a3 + C2 a2 )]

+ [ S1 (C234C5C6 − S 234 S 6 ) − C1S5C6 ] .[ C1 (C234 a4 + C23a3 + C2 a2 ) ]
= S5C6 (C234 a4 + C23a3 + C2 a2 )

16


Chương 3. Viết chương trình matlab tính ma trân Jacoby
H

J 21 = o y1. px1 − ox1. p y1 = −S5C6 (C23a3 + C2 a2 + C234 a4 )

H

J 31 = a y1. px1 − a x1. p y1 = −C5 (C23a3 + C2 a2 + C234 a4 )

H

J 41 = nz1 = S 234C5C6 + C234 a4

H

J 51 = oz1 = − S 234C5C6 + C234C6

H

J 61 = az1 = S 234 S5

1
+ Với khớp θ 2 là khớp quay. Sử dụng ma trận T6 , ta được cột 2 của ma trận H J ::
H

J12 = n y 2 . px 2 − nx 2 . p y 2
= a4 S 6 + a3C4 S 6 + a2C3C4 S 6 + a3C5C6 S 4 − a2 S3 S 4 S6 + a2C3C5C6 S 4 + a2C4C5C6 S3

H

J 22 = o y 2 . px 2 − ox 2 . p y 2
= a4C6 + a3C4C6 + a2C3C4C6 − a3C5 S 6 S 4 − a2 S3 S 4C6 − a2C3C5 S 6 S 4 − a2C4C5 S 6 S3

H

J 32 = a y 2 . px 2 − ax 2 . p y 2 = S5 .( S34 a2 + S 4 .a3 )

H

J 42 = nz 2 = − S5 .C6

H

J 52 = oz 2 = S5 .S6

H

J 62 = az 2 = C5

2
+ Với khớp θ3 là khớp quay. Sử dụng ma trận T6 , ta được cột 3 của ma trận H J ::
H

J13 = n y 3 . px 3 − nx 3 . p y 3 = a4 S6 + a3C4 S 6 + a3C5C6 S 4

H

J 23 = o y 3 . p x 3 − ox 3 . p y 3 = a4C6 + a3C4C6 − a3C5 S 6 S 4

H

J 33 = a y 3 . p x 3 − ax 3 . p y 3 = a3 S 4 S5

H

J 43 = nz 3 = − S5C6

H

J 53 = oz 3 = S5 S6

H

J 63 = az 3 = C5

+ Với khớp θ 4 là khớp quay. Sử dụng ma trận 3T6 , ta được cột 4 của ma trận H J :
H

J14 = n y 4 . px 4 − nx 4 . p y 4 = a4 S 6

H

J 24 = o y 4 . px 4 − ox 4 . p y 4 = a4C6

17


Chương 3. Viết chương trình matlab tính ma trân Jacoby
H

J 34 = a y 4 . px 4 − ax 4 . p y 4 = 0

H

J 44 = nz 4 = − S5C6

H

J 54 = oz 4 = S5 S6

H

J 64 = az 4 = C5

+ Với khớp θ5 là khớp quay. Sử dụng ma trận 4T6 , ta được cột 5 của ma trận H J :
H

J15 = n y 5 . p x 5 − nx 5 . p y 5 = 0

H

J 25 = o y 5 . px 5 − ox 5 . p y 5 = 0

H

J 35 = a y 5 . px 5 − ax 5 . p y 5 = 0

H

J 45 = nz 5 = S 6

H

J 55 = oz 5 = C6

H

J 65 = az 5 = 0

+ Với khớp θ 6 là khớp quay. Sử dụng ma trận 5T6 , ta được cột 6 của ma trận H J :
H

J16 = n y 6 . px 6 − nx 6 . p y 6 = 0

H

J 26 = o y 6 . px 6 − ox 6 . p y 6 = 0

H

J 36 = a y 6 . px 6 − ax 6 . p y 6 = 0

H

J 46 = nz 6 = 0

H

J 56 = oz 6 = 0

H

J 66 = az 6 = 0

- Như vậy ta đã xác định được ma trận

H

J 6 x 6 như trên. Tiếp theo, để xác định được ma

trận Jacoby, ta tiếp tục áp dụng công thức sau:

18


Chương 3. Viết chương trình matlab tính ma trân Jacoby





J =
0
0

0

 nx
n
 y
n
J = z
0
0

 0

0

0
0
0

Rn 3 x 3
0
0
0

ox
oy
oz
0
0
0

0
0
0

ax
ay
az
0
0
0

0
0
0
0

0

0
0
nx
ny
nz

Rn3 x 3

0
0
0
ox
oy
oz

0
0 
0 H
. J





0   H J11

0   H J 21
0   H J 31
=
ax   H J 41

a y   H J 51
 
az   H J 61

H

J12
H
J 22
H
J 32
H
J 42
H
J 52
H
J 62

H

J13
H
J 23
H
J 33
H
J 43
H
J 53
H

J 63

H

J14
H
J 24
H
J 34
H
J 44
H
J 54
H
J 64

H

J15
H
J 25
H
J 35
H
J 45
H
J 55
H
J 65

J16 

J 26 
H
J 36 

H
J 46 
H
J 56 

H
J 66 
H

H

+ Với nx, ny, nz, ox, oy, oz, ax, ay, az được xác định từ ma trận hàm truyền khi tính toán
động lực học thuận vị trí:

nx = C (C C5C − S
S ) − S S5S
1 234
6 234 6
1 6
n y = S (C C C − S
S )+C S S
1 234 5 6 234 6
1 5 6
nz = C C5C + S

S
234
6 234 6
Ox = −C (C C5S + S
C ) + S S5S
1 234
6 234 6
1 6
O y = − S (C C5S + S
C ) − C S5S
1 234
6 234 6
1 6
Oz = − S
C S +C C
234 5 6
234 6
ax = C C
S +S C
1 234 5 1 5
ay = S C
S −C C
1 234 5 1 5
az = S
S
234 5
+ Sử dụng matlab để nhập và nhân các ma trận. Sau khi dùng hàm simplify ta sẽ được
ma trận Jacoby thông qua ma trận HJ ( Vì biểu thức tính quá dài, để biết thêm về Ma trận
Jacoby, xin xem thêm phần phụ lục 3)

19


Chương 3. Viết chương trình matlab tính ma trân Jacoby

3.2.2 Viết chương trình Matlab tính ma trận Jacoby thông qua ma trận HJ

a. Thiết kế giao diện

Hình 3.2. Giao diện tính ma trận Jacoby thông qua ma trận

- Nhập thử số và chạy thử ta sẽ được kết quả như sau:

20


Chương 3. Viết chương trình matlab tính ma trân Jacoby

Chương trình code: tham khảo phụ lục 3.

21


Chương 4. Thiết kế quỹ đạo trong không gian khớp : bậc 3 quỹ đạo 2-1-2

Chương 4
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 ( quỹ đạo bậc 1, 2, 3, 5..)
4.1. Giới thiệu và cơ sở 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 đ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 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 lien 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 cong có dạng:
+ Đa thức bậc 2: x(t ) = a + bt + ct 2
+ Đa thức bậc 3 : x(t ) = a + bt + ct 2 + dt 3
+ Đa thức bậc cao: x(t ) = a + bt + ct 2 + ... + kt n
Ta sẽ sử dụng quỹ đạo dạng đa thức bậc 3 để thiết kế cho quỹ đạo 2-1-2
4.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 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 ta xác định được các biến khớp θ1 , θ 2 ,θ3 ,θ 4 ,θ5 , θ 6 tại A và B.

Chọn quỹ đạo thiết kế là hàm đa thức bậc 3 theo thời gian như sau:

19


Chương 4. Thiết kế quỹ đạo trong không gian khớp : bậc 3 quỹ đạo 2-1-2

θi (t ) = ai + bi t + ci t 2 + di t 3

i =1..6 tương ứng với 6 biến khớp
Ta được hệ phương trình sau:
θ1 (t ) = a1 + b1t + c1t 2 + d1t 3

2
3
θ 2 (t ) = a2 + b2t + c2t + d 2t
θ (t ) = a + b t + c t 2 + d t 3
 3
3
3
3
3

2
3
θ 4 (t ) = a4 + b4t + c4t + d 4t
θ (t ) = a + b t + c t 2 + d t 3
5
5
5
5
 5
2
θ 6 (t ) = a6 + b6t + c6t + d 6t 3

Giả sử thời gian robot đi từ điểm A đến điểm B là trong t(s) và vân tốc tại 2 điểm đó là
bằng 0.
Từ đó ta có hệ phương trình:
θi (t ) = θi ( A) = ai


2
3
θi (t ) = ai + bi t + ci t + d it = θi ( B)
.
.
θ i (t ) = θ i ( A) = bi = 0
.
.
2
θ i (t ) = θ i ( B ) = bi + 2cit + 3d1t = 0
ai = θi ( A)
b = 0
 i

3[θ ( B ) − θi ( A)]
ci = i
t2


2[θi ( B) − θi ( A)]
di =
t3


Giả sử robot Enbow cần dịch chuyển từ vị trí A(0.1536, 0.2660, 0.2747) đến điểm
B(0.1541, -0.1120, 0.4048) là 5s tương ứng với tọa độ khớp là A (
(

9π 2π 3π

,
, )
5 9 5

Ta suy ra hệ số phương trình của quỹ đạo cho khâu 1 là:

20

4π 3π π
, , ) , đến B
3 4 5