TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

MỤC LỤC
LỜI MỞ ĐẦU ........................................................................................................... 3
PHẦN 1: PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC ........................................ 3
1.1 Số bậc tự do cần thiết ........................................................................................... 4
1.2 Các phương án thiết kế cấu trúc các khâu khớp ................................................... 4
1.3 Chọn, thiết kế cấu trúc được chọn ........................................................................ 7
PHẦN 2: GIẢI BÀI TOÁN ĐỘNG HỌC ............................................................ 10
2.1. Xây dựng cấu trúc, thiết lập hệ phương trình động học của robot .................... 10
2.1.1 Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg ...................................... 11
2.2.1 Xác định vận tốc điểm tác động cuối và vận tốc góc khâu thao tác ............... 13
2.2.2 Xây dựng quy luật chuyển động từng khâu .................................................... 14
2.3 Giải bài toán động học ngược ............................................................................ 17
2.3.1 Giải bằng phương pháp giải tích ..................................................................... 17
2.3.2 Xây dựng quy luật chuyển động khâu thao tác E và giải động học ngược ..... 18
PHẦN 3: TÍNH TOÁN LỰC ................................................................................ 22
3.1 Phân tích trạng thái tĩnh ..................................................................................... 22
3.2 Tính toán lực/moment lớn nhất ở trạng thái tĩnh ............................................... 24
PHẦN 4: TÍNH TOÁN ĐỘNG LỰC HỌC.......................................................... 25
4.1 Xây dựng cấu trúc động lực học và các thành phần cần thiết ........................... 25
4.2 Thiết lập phương trình Lagrang của robot ......................................................... 27
4.3 Động lực học thuận ............................................................................................ 29
4.4 Động lực học ngược ........................................................................................... 33
PHẦN 5: THIẾT KẾ HỆ DẪN ĐỘNG ROBOT ................................................ 36
5.1. Thiết kế một hệ dẫn động cho một khớp ........................................................... 36
5.2. Tính chọn động cơ ............................................................................................. 36

PHẦN 6: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN .............................................. 39
6.1. Hệ thống điều khiển phản hồi không bù trọng lực G(q) ................................... 39
6.1.1. Luật điều khiển ............................................................................................... 39
1


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

6.1.2. Mô Phỏng điều khiển bằng Matlab ................................................................ 41
6.2. Hệ thống điều khiển phản hồi có bù trọng lực G(q).......................................... 46
6.2.1. Luật điều khiển ............................................................................................... 46
6.2.2. Mô Phỏng điều khiển bằng Matlab ................................................................ 46
Tài Liệu tham khảo ................................................................................................ 51
Phục Lục .................................................................................................................. 52
1) Code trương trình con (nhom6.text) .................................................................... 52
2) Chương trình maple tính động học thuận ............................................................ 64
3) Chương trình maple tính động học ngược ........................................................... 65
4) Chương trình maple tính tĩnh học ........................................................................ 66
5) Chương trình maple tính động lực học thuận ...................................................... 67
6) Chương trình maple tính động lực học ngược ..................................................... 71
7) Chương trình con matlab động học thuận ............................................................ 71
8) Chương trình con matlab động học ngược........................................................... 71
9) Chương trình con matlab điều khiển không bù trọng lực .................................... 72
10) Chương trình con matlab điều khiển có bù trọng lực ........................................ 72
11) Chương trình con matlab quỹ đạo ...................................................................... 73
12) Chương trình con matlab phương trình động lực học robot .............................. 73

13) Chương trình matlab mô phỏng với bộ điều khiển PD không bù trọng lực ...... 74
14) Chương trình matlab mô phỏng với bộ điều khiển PD có bù trọng lực ............ 76

2


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

LỜI MỞ ĐẦU
Robot là một ngành khoa học kỹ thuật liên quan đến vấn đề thiết kế, xây
dựng mô hình, điều khiển và ứng dụng của robot. Ngày nay, robot đồng hành
với cuộc sống thường ngày của con người và phạm vi ứng dụng ngày càng
rộng rãi từ đồ chơi trẻ em, thiết bị văn phòng đến những robot trong công
nghiệp và robot thăm dò không gian.
Trong công nghiệp, từ khi mới ra đời thì robot được áp dụng trong nhiều
lĩnh vực dưới góc độ thay thế sức lao động của con người. Nhờ vậy các dây
chuyền sản xuất được tổ chức lại, năng suất lao động tăng cao.
Với nước ta, một nước đang phát triển, và đang trên con đường trở này
một nước công nghiệp tiên tiến. Robot lại là một trong những mảng không thể
thiếu trong công nghiệp hiện đại. Thế nên việc đào sâu nghiên cứu về robot đã
được tiến hành từ rất sớm. Tuy ra đời muộn hơn nhưng robot Việt Nam đã và
đang dần bắt kịp với các nước trên thế giới.
Chính vì vậy trong báo cáo môn học tính toán thiết kế Robot, chúng em
đã được thầy giao cho tìm hiểu, phân tích cũng như tính toán thiết kế robot
khắc chữ trên mặt phẳng nghiêng. Tuy còn sai sót, nhưng những phần cơ bản
về động học, tĩnh học, động lực học và điều khiển robot đều được trình bày

trong tiểu luận này. Mong thầy và các bạn có thể đóng góp ý kiến để nhóm
chúng em hoàn thiện hơn nữa bài tập này.
Nhóm sinh viên thực hiện

3


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

PHẦN 1: PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC
1.1 Số bậc tự do cần thiết
Nguyên tắc di chuyển trong mặt phẳng chỉ cần 2 bậc tự do nếu robot cũng nằm trong
mặt phẳng đó, nhưng với trường hợp viết chữ thì robot không thể nằm trong mặt phẳng này
được. Vì vậy, cần 3 bậc tự do để di chuyển trong không gian. Mặt khác, do đề bài không yêu
cầu về hướng khâu cuối mà chỉ tập trung vào điểm tác động cuối cho lên ta chỉ cần thiết kế
cấu hình robot chuỗi hở 3 bậc tự do trở lên.

1.2 Các phương án thiết kế cấu trúc các khâu khớp
Dựa trên việc phân tích số bậc tự do cần thiết là 3. Dưới đây là một số phương án cấu
trúc robot đề ra (cấu trúc dưới dạng thanh):

Hình 1.2.1: Cấu trúc 1

4



TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình 1.2.2: Cấu trúc 2

Hình 1.2.3: Cấu trúc 3
5


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình 1.2.4: Cấu trúc 4

6


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình 1.2.5: Cấu trúc 5


1.3 Chọn, thiết kế cấu trúc được chọn
Với các phương án đưa ra trên, cả nhóm đã chọn cấu trúc 4 làm cấu trúc để khảo sát,
tính toán và thiết kế. Đây là robot 3 DOF dạng RRT. Do không gian làm việc là mặt phẳng
nghiêng có kích thước 30x50 cm và góc nghiêng của mặt phẳng là 300 nên để cho robot với
tới điểm xa nhất thì tổng chiều dài (trên mặt phẳng song song với mặt phẳng nằm ngang)


302  502 . cos(30)  50 (cm) . Từ đó ta thiết kế: d1 = 0,7 (m); a1 = 0,3 (m); a2 = 0,3 (m); d3

= 0,6 (m).
Mô hình tổng thể của robot:

7


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình 1.3.1: Mô hình robot khắc chữ 3 bậc RRT
Mô hình các khâu, khớp:

Hình 1.3.2: Khâu 0 (đế)

8



TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình 1.3.3: Khâu 1

Hình 1.3.4: Khâu 2

9


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình 1.3.5: Khâu 3

PHẦN 2: GIẢI BÀI TOÁN ĐỘNG HỌC
Ta thiết lập phương trình động học theo phương pháp ma trận Denavit Hartenberg.

2.1. Xây dựng cấu trúc, thiết lập hệ phương trình động học của robot

Z1
q2
X2


a2
a1

O2
X1
X0'

d1

q3

Z2

O1
q1

X2

Z0

X3

O3

Z3

Y0
O0
X0


Hình 2.1: Hệ tọa độ robot khắc chữ cấu trúc RRT

10


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

2.1.1 Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg
Khâu 0 (đế): ta chọn hệ tọa độ O0XoYoZo có trục Zo chọn trùng với trục khớp 1,
trục Xo ta chọn tùy ý sao cho phù hợp nhất như hình vẽ , trục Yo chọn theo quay tắc tam
diện thuận.
Khâu 1: ta chọn hệ tọa độ O1X1Y1Z1 có trục Z1 trùng với trục khớp 2, trục X1 ta
chọn theo đường vuông góc chung Zo và Z1 có hướng từ Zo sang Z1, trục Y1 chọn theo
quay tắc tam diện thuận.
Khâu 2: ta chọn hệ tọa độ O2X2Y2Z2 có trục Z2 trùng với trục khớp 3, trục X2 ta
chọn theo đường vuông góc chung Z1 và Z2 có hướng từ Z1 sang Z2, trục Y2 chọn theo quy
tắc tam diện thuận.
Khâu 3: ta chọn hệ tọa độ X3Y3Z3 có trục Z3 trùng với Z2, trục X3 song song với
trục X2, trục Y3 chọn theo quy tắc tam diện thuận.
2.1.2 Thiết lập hệ phương trình động học của robot
Với hệ tọa độ đã chọn ta có bảng DH sau:
Khâu

di

θi


ai

αi

1

d1

q1

a1

0

2

0

q2

a2

π

3

q3

0


0

0

Ta có:

0

A

1

=

11


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

1

2

A


3

A

2

=

=

C12
S
0
0
1
 12
 A 2 = A1 . A 2 =
 0

 0

S12
C12
0
0

C12
S
0
0

1
2
 12
 A 3( q ) = A1 . A 2 . A 3 =
 0

 0

a2C12  a1C1 
0 a2 S12  a1 S1 


1
d1

0
1

0

S12
C12
0
0

a2C12  a1C1 
0 a2 S12  a1 S1 

1
 q3  d1 


0
1

0

Với: C1=cos(q1); S1=sin(q1); C12=cos(q1+q2); S12= sin(q1+q2); q=[q1, q2, q3]T
Mặt khác ta dùng ma trận sau mô tả hướng và vị trí robot thông qua vector
p=[xE, yE, zE, α, β, η]T với α, β, η là 3 góc Cardan.
cos( ) cos( )
cos(  )sin( )
sin(  )


sin( )sin(  )cos( )  cos( )sin( )  sin( )sin(  )sin( )  cos( )cos( )  sin( )cos(  )
0
A3( p ) =  cos( )sin(  )cos( )  sin( )sin( ) cos( )sin(  )sin( )  sin( )cos( ) cos( )cos(  )

0
0
0


xE 

yE 
zE 

1 


12


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Từ hai ma trận

0

0

A (q ) , A ( p)
3

3

ta thiết lập được hệ phương trình động học sau:

 f1  0 ( p )[1, 4]  0 ( q )[1, 4]  xE   a2 cos( q1  q2 )  a1cos( q1 )   0
A3
A3

0
0
 f2 
A3( p)[2, 4]  A3(q)[2, 4]  yE   a2 sin(q1  q2 )  a1 sin(q1 )   0


 f 3  0 A ( p )[3, 4]  0 A ( q )[3, 4]  z E    q3  d1   0
3
3

0
0
 f 4  A3( p )[1,1]  A3( q )[1,1]  cos( )cos( )  cos( q1  q2 )  0

0
0
 f 5  A3( p )[2, 2]  A3( q )[2, 2]   sin( ) sin(  ) sin( )  cos( ) cos( )  cos( q1  q2 )  0

0
0
 f 6  A3( p )[3, 3]  A3( q )[3, 3]  cos( )cos(  )  1  0

2.2. Giải bài toán động học thuận
Để tính toán ta chọn: d1=0,7 (m); a2=0,3 (m); a3=0,3 (m); d3=q3=0,6 (m).

2.2.1 Xác định vận tốc điểm tác động cuối và vận tốc góc khâu thao tác
Theo hệ phương trình động học ở trên ( với q=[q1, q2, q3]T ) ta có:
 xE   a2 cos( q1  q2 )  a1cos( q1 ) 
  

0
r E =  y E    a2 sin( q1  q2 )  a1cos( q1 ) 
 z E    q3  d1



Mà:

0

v

E



Ta có : J E 



0

v

E

d(

0

r

E

)


dt
(

0

r

E

)

q



(

0

r

E

)

q

  a2 sin( q1  q2 )  a1 sin( q1 )
  a2 cos( q1  q2 )  a1cos( q1 )



0

 xE 
 
  y E   J E .q 
 z E 

Từ ma trận

C12
S
0
=
R 3  12
 0

0

A

3

.q  J E .q
 a2 sin( q1  q2 )
a2 cos( q1  q2 )
0

0
0


1

 (  a2 .S12  a1S1 ) q1  (  a2 S12 ) q2 


(
a
.
C

a
C
)
q

(
a
C
)
q
2
12
1
1
1
2
12
2





 q3

ta rút ra ma trận cosin chỉ hướng:

S12
C12
0

0
0  =>

1

0

3 

0

R 3.

0

R

T
3


 0 
 0 

= 
 q1  q2 
13


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

2.2.2 Xây dựng quy luật chuyển động từng khâu từ đó vẽ quỹ đạo điểm E, vận tốc
điểm E và vận tốc góc
Để khảo sát kết quả trên ta xây dựng quy luật chuyển động của các biến khớp q như

 q1  3t sin(3t)

 q2  2 cos(5t)
sau: 
 q3  4 sin(2t) cos(3t)

Hình 2.2.1: Đồ thị điểm tác động cuối (trong 2s)

xE = – 0,3 sin(3t sin(3t) + 2 cos(5t)) (3 sin(3t) + 9t cos(3t) – 10 sin(5t))
– 0,3 sin(3t sin(3t)) (3 sin(3t) + 9t cos (3t))


yE = – 0,3 cos(3t sin(3t) + 2 cos(5t)) (3 sin(3t) + 9t cos(3t) – 10 sin(5t))
– 0,3 cos(3t sin(3t)) (3 sin(3t) + 9t cos (3t))

14


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

z E = – 8 cos(2t) cos(3t) + 12 sin(2t) sin(3t)

 vE 

x 2E  y 2E  z 2E  aE 

x 2E  y 2E  z 2E

x  0
y  0
z  3sin(3t )  9t.cos(3t )  10sin(5t )
Dùng maple ta vẽ được các đồ thị sau:

Hình 2.2.2: Đồ thị ba góc cardan khâu cuối

15



TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình 2.2.3: Đồ thị vận tốc, gia tốc điểm cuối

Hình 2.2.4: Đồ thị vận tốc góc khâu cuối

16


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

2.3 Giải bài toán động học ngược
2.3.1 Giải bằng phương pháp giải tích
Khi giải bài toán động học thuận người ta xác định được quan hệ dước dạng ma trận
như sau: x=f(q). Từ quan hệ đó ta suy ra một cách hình thức:

q=f-1(x) trong đó:

x=[x1,x2,…,xm]T , q=[q1,q2,…,qn]T.
Từ hệ phương trình:
 xE  a1cos( q1 )  a2 cos( q1  q2 )


 y E  a1 sin( q1 )  a2 sin( q1  q2 )
z  d  q
1
3
 E

(2.1)

Kí hiệu: C1 = cos(q1); S1 = sin(q1); C2 = cos(q2); S2 = sin(q2);
C12 = cos(q1+q2); S12 = sin(q1+q2).
2
2
2
2
2
2
=> xE  yE  a1  a2  2a1a2 (C1C12  S1S12 )  a1  a 2  2a1a 2C 2


xE2  y E2  a12  a22
cos(q2 ) 
2 a1a2
=> 

2
sin( q2 )   1  cos ( q2 )

Vậy: q2  a tan 2(sin( q2 ), cos( q2 )) .

 xE  ( a1  a2C2 )C1  a2 S 2 S1


Ta viết lại hệ (2.1) như sau  y E  a2 S 2C1  ( a1  a2C 2 ) S1
q  d  z
1
E
 3
Giải hệ phương trình đại số tuyến tính này ta được :



a1  a2C2

1 

a2 S 2

 a2 S 2
a1  a2C2

xE

 a2 S 2

yE

a1  a2C2

 a12  a22  2a1a2C2  xE2  y E2

 a1 xE  a2 ( xE C2  y E S 2 )


17


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

2 

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

a1  a2C2

xE

a2 S 2

yE

 a1 yE  a2 ( y E C2  xE S 2 )

a1 xE  a2 ( xE C2  y E S 2 )

cos(
q
)

1


xE2  y E2


=> q1  a tan 2  sin( q1 ), cos( q1 ) 
sin( q )  a1 y E  a2 ( y E C2  xE S 2 )
1

xE2  y E2

2.3.2 Xây dựng quy luật chuyển động khâu thao tác E và giải động học ngược
bằng phương pháp số Newton-Raphson
Ta có:
 f1  xE  [a1cos( q1 )  a2 cos( q1  q2 )] = 0

 f 2  y p  a1 sin( q1 )  a2 sin( q1  q2 )  0

 f 3  z p  d1  q3  0
 f1 
 
F   f2   0
 f 3 
Bài toán khi biết được xE(t), yE(t), zE(t) tại mỗi thời điểm t ta sẽ tìm được vector q=[q1, q2,

q3]T tại mỗi thời điểm đó.
Ta lấy giá trị sát giá trị đầu để tiến hành quá trình lặp Newton-Raphson
pk 1  pk  ( J ( pk )) 1 F ( pk )

Quá trình lặp dừng lại khi sai số ở lần k+1 với lần k nhỏ hơn giá trị cho phép.
Ta viết chương trình bằng maple sử dụng phương pháp Newton-Raphson lưu kết quả tính
toán ra file(.txt) và 1 đoạn vẽ đồ thị từ file txt được viết bằng matlab. Với quỹ đạo điểm tác

động cuối là đường xoắn ốc có phương trình như sau:

 xE  0, 2  0,1 cos(5t )

 y E  0, 2  0,1sin(5t )
 z  0,5t
 E

18


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình 2.3.1: Quỹ đạo điểm tác động cuối

Hình 2.3.2: Đồ thị q1 theo t
19


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình 2.3.3: Đồ thị q2 theo t


Hình 2.3.4: Đồ thị q3 theo t
20


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

Hình
2.3.5:
Đồ thị
kiểm
nghiệm
lại quỹ
đạo điểm
tác động
cuối

21


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI


PHẦN 3: TÍNH TOÁN LỰC
3.1 Phân tích trạng thái tĩnh
Lực tác động vào khâu cuối coi như bằng không vì robot thiết kế là robot viết chữ
trên mặt phẳng nghiêng nên:
0
T

 F3 E  [ 0, 0, 0]
0
T

 M 3 E  [ 0, 0, 0]

Z1

q2
X2

a2
X1

Từ phần động học ta xác định
được các ma trận cosin chỉ hướng sau:

Z2

0

C12
R 3 =  S12

 0

S12
C12
0

O1
q1

X0'

d1

q3

C1  S1 0 


0
R 1 =  S1 C1 0 
 0 0 1 
C12 S12 0 
0
R 2 =  S12 C12 0 
 0
0  1 

a1

O2


Z0

X3

X2

O3

0

0
1 

Z3

Y0
O0
X0

Trong đó: C1=cos(q1); S1=sin(q1); C12=cos(q1+q2); S12=sin(q1+q2).
Gọi xc1; xc2; xc3 lần lượt là khoảng cách từ gốc tọa độ của khâu 1; 2; 3 đến khối tâm
của từng khâu 1; 2; 3.
a) Xét khâu 3:


0

0
3

 rc 3  R 3. rc 3  


 3 rc 3  [0, 0, -z c 3 ]T



3

T
 r3  [ 0, 0, q3 ]


 0 r  0 .3r  
R3 3 
 3





0 
zc 3 
0
, và P 3  [ 0, 0,  m3g]T
0


0


 q3 
0

Thay vào công thức đã được đã được thiết lập trong bài giảng robot:

22


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

0
0
0

 F32  FE 3  P3
0
0
0
0
0
0

 M 32  M E 3  r3 F32  rc 3 P3


 0 

0


 F32   0 

 m3 g 
Ta tính được: 

 0

0
 M   0
32
 

 0 



b) Xét khâu 2 (lấy kết quả của khâu 3):

 a2C12 
0
0


2
 rc 2  R 2. rc 2   a2 S12 
 0


 2 rc 2  [ -x c 2 , 0, 0]T
0


, và P 2  [ 0, 0,  m2 g]T
2

T
 C12 xc 2 

 r2  [ a 2 , 0, 0]
0


0
2
 r 
. r2    S12 xc 2 
2
R
2



0

0
0
0


 F21  F32  P2
Thay vào công thức:  0
0
0
0
0
0

 M 21  M 32  r2 F21  rc 2 P2


0


0


0
 F21  


 ( m3  m2 ) g 
Ta tính được: 
 gS12 ( a2 m3  a2 m2  xc 2 m2 ) 

 0 M    gC ( a m  a m  x m ) 
12
2 3
2 2
c2 2 

 21 


0



c) Xét khâu 1 (lấy kết quả của khâu 2):

23


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6

ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI


0
 r1 
 1r1  [ a1 , 0, d1 ]T


1

T

 rc1  [ -x c1 , , 0]
 0r 

 c1


 a1C1 


R 1. r1   a1S1 
 d1 
  xc1C1
0

1
R 1. rc1    xc1S1
 0

0

1






, và

0

P


1

 [ 0, 0,  m1g]T

0
0
0

 F10  F21  P1
Thay vào công thức:  0
0
0
0
0
0

 M 10  M 21  r1 F10  rc1 P1

Ta tính được :

0


0


0
 F10  



 ( m3  m2  m1 ) g 

 g ( a2 m3S12 +a 2 m2 S12  xc 2 m2 S12  a1m3 S1  a1m2 S1  a1m1S1  xc1m1S1 )


 0 M    g (a m C  a m C  x m C  a m C  a m C  a m C  x m C ) 
2 3 12
2 2 12
c 2 2 12
1 3 1
1 2 1
1 1 1
c1 1 1 
 10 


0



3.2 Tính toán lực/moment lớn nhất ở trạng thái tĩnh
Tính các moment động cơ cần cho robot cân bằng từ các kết quả tính toán trên:
– Mdc1 = 0M10 [3] = 0 (Nm).
– Mdc2 = 0M21 [3] = 0 (Nm).
– Fdc3 = 0F32 [3] = m3g = 2,1 . 9,81 = 20,601 (N)

24


TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6


ĐH Bách Khoa Hà Nội

GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI

PHẦN 4: TÍNH TOÁN ĐỘNG LỰC HỌC
4.1 Xây dựng cấu trúc động lực học và các thành phần cần thiết để viết phương
trình động lực học
Gọi xc1; xc2; xc3 lần lượt
khoảng cách từ gốc tọa độ của
2; 3 đến khối tâm của từng
2; 3.

là
khâu 1;
khâu 1;

Z1
q2
X2

a2
a1

O2
X1
X0'

d1


q3

Z2

O1
q1

X2

Z0

X3

O3

Z3

Y0
O0
X0

Ta có:
sin( q1 ) a1 cos( q1 )    xc1   ( a1  xc1 ) cos( q1 ) 
 cos( q1 ) 0




sin( q1 ) 0 cos( q1 ) a1 sin( q1 )   0   ( a1  xc1 ) sin( q1 ) 
0

0
1



C1  A1 C1 


 0
 0  
1
0
d1
d1



 
0
0
1
1
 0
 1  

 ( a1  xc1 ) cos( q1 ) 


0
 rc1   ( a1  xc1 ) sin( q1 ) 



d1
C12
S
0
C2  0 A2 2C2   12
 0

 0

S12
C12
0
0

a2 C12  a1C1    xc 2    xc 2C12  a2C12  a1C1 


0 a2 S12  a1 S1   0    xc 2 S12  a2 S12  a1 S1 



 0  
1
d1
d1

 


0
1
1
 1  

0

25