TÍNH TOÁN THIẾT KẾ ROBOT

ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN CƠ KHÍ

BÁO CÁO BTL:
TÍNH TOÁN THIẾT KẾ ROBOT

Đề tài:
Tính toán thiết kế robot gắp thả vật

Sinh viên thực hiện:

Nhóm 8

Giáo viên hướng dẫn: PGS PHAN BÙI KHÔI


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

MỤC LỤC
Phân chia và đánh giá công việc của các thành viên trong nhóm: ...................................... 4
1. PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC ................................................................ 5
1.1. Mục đích ứng dụng của Robot ................................................................................ 5
1.2. Phân tích số bậc tự do cần thiết ............................................................................... 6
1.3. Không gian làm việc: ............................................................................................ 10
1.4. Tham số động học Robot....................................................................................... 12
2. PHÂN TÍCH KHẢO SÁT ĐỘNG HỌC ..................................................................... 13
2.1. Khảo sát bài toán động học ................................................................................... 13
2.1.1. Tính các ma trận truyền .................................................................................. 13
2.1.2. Vị trí, hướng khâu thao tác ............................................................................. 14

2.1.3. Vận tốc, gia tốc điểm tác động cuối................................................................ 15
2.1.4. Vận tốc góc, gia tốc góc khâu thao tác ........................................................... 16
2.1.5. Không gian làm việc ....................................................................................... 17
2.2. Thiết kế quỹ đạo .................................................................................................... 20
2.2.1. Tổng quát ........................................................................................................ 22
2.2.2. Giải bài toán quỹ đạo trên từng đoạn cụ thể ................................................... 24
2.2.3. Bài toán động học ngược quỹ đạo .................................................................. 31
2.2.4. Bài toán động học ngược vận tốc, gia tốc ....................................................... 33
3. BÀI TOÁN TĨNH HỌC ROBOT ................................................................................ 33
3.1. Phân tích trạng thái tĩnh của robot ........................................................................ 33
3.2. Tính toán lực/momen lớn nhất ở trạng thái tĩnh ................................................... 39
3.3. Tính lực và momen dẫn động lớn nhất.................................................................. 40
4. BÀI TOÁN ĐỘNG LỰC HỌC ROBOT .................................................................... 40
4.1. Các thông số động lực học .................................................................................... 40
4.2. Phương trình vi phân chuyển động của robot ....................................................... 40
5. Thiết kế hệ dẫn động Robot ......................................................................................... 44
5.1. Các thông số đầu vào hệ dẫn động các khâu ......................................................... 44
NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

2


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

5.2. Thiết kế hệ dẫn động cho khâu 1........................................................................... 45
5.3. Kiểm nghiệm bền các khâu ................................................................................... 47
6. THIẾT KẾ BỘ ĐIỀU KHIỂN PID ............................................................................. 50
6.1. Bộ điều khiển PID ................................................................................................. 50

6.2. Mô phỏng bằng Simulink – điều khiển robot trong không gian thao tác ............. 51
6.3. Xây dựng mô hình trên Matlab Simulink.............................................................. 52
6.4. Sử dụng công cụ simMechanics của Matlab để mô phỏng – điều khiển trong
không gian khớp ............................................................................................................. 55
7. Tài liệu tham khảo: ...................................................................................................... 58

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

3


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

Phân chia và đánh giá công việc của các thành viên trong nhóm:

STT

1

2

Họ và tên
Nguyễn Văn
Huy

MSSV

Công việc

 Phân tích và lựa chọn cấu trúc
20131782  Bài toán động học thuận,
nghịch
 Thiết kế hệ thống điều khiển

 Tính toán tĩnh học
Võ Trọng Đức 20131078  Động lực học robot
 Thiết kế hệ dẫn động

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

Đánh giá

95%

90%

4


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

TÍNH TOÁN THIẾT KẾ ROBOT GẮP THẢ VẬT
1. PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC
1.1. Mục đích ứng dụng của Robot
Yêu cầu đặt ra là thiết kế Robot để hỗ trợ việc bốc vận chuyển hàng từ băng
chuyền ra xe đẩy để vận chuyển ra đến nơi lưu trữ như hình:

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

5


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

Trong giới hạn của dự án là gắp thả vật có dạng hình khối lập phương (như là hộp mì
tôm, hộp bánh kẹo…) từ cuối dây chuyền sang xe đẩy. Robot cần phải đảm bảo gắp
thả đúng vị trí và hướng của đối tượng cần gắp.
1.2. Phân tích số bậc tự do cần thiết
Để khâu thao tác có thể đưa vật di chuyển điểm-điểm trong không gian cần 3
bậc tự do để đảm bảo với tới các điểm trong không gian thao tác. Nhưng ở đây cần
đảm bảo hướng của khâu thao tác để có thể gắp và thả được vật đúng hướng trên
mặt phẳng xy thì cần thêm một bậc tự do nữa;
 Phải có ít nhất 4 bậc tự do cho mô hình thiết kế.

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

6


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

Dưới đây là một số cơ cấu có thể dùng cho mô hình thiết kế Robot trên:

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

7


GVHD: PGS.TS PHAN BÙI KHÔI

Mô hình 1:

Mô hình 3

ROBOTICS DESIGN

Mô hình 2:

Mô hình 4

Mô hình 5

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

8


GVHD: PGS.TS PHAN BÙI KHÔI

-

-

-

-

-

ROBOTICS DESIGN

Mô hình 1,2,3 là mô hình robot scara 4 dof, gồm 3 khớp quay và một khớp tịnh
tiến, 3 khớp quay đảm bảo vị trí trong mặt phẳng và hướng của khâu thao tác trong
mặt phẳng.
Mô hình 1: khớp quay 1,2 đảm bảo vị trí trong mặt phẳng, khớp tịnh tiến 3 đảm
bảo với tới vị trí theo phương z, khớp quay 4 đảm bảo hướng của khâu thao tác để
có thể gắp vả thả vật đúng hướng, quay theo trục z.
Mô hình 2: khớp quay 1,2,3 đảm bảo đưa khâu thao tác đến vị trí và hướng của
khâu thao tác trong mặt phẳng xy, khớp tịnh tiến 4 đưa khâu thao tác đến vị trí theo
trục z.
Mô hình 3: tương tự mô hình 2, nhưng thay đổi nhiệm vụ của khâu 2 và khâu 4 cho
nhau, toàn bộ khâu 2,3,4 hoạt động trong không gian vừa theo trục z vừa trên mặt
phẳng xy.
Mô hình 4: gổm 5 khớp quay, linh hoạt, đảm bảo gắp được vật và hướng đặt vật,
mô hình toàn khớp xoay và 5 bậc tự do nên nên việc thiết kế và điều khiển phức
tạp.
Mô hình 5: gồm 3 khớp tịnh tiến 1,2,3 đưa khâu thao tác đến điểm xác định trong
không gian, khớp quay 4 đảm bảo hướng của vật theo trục z. mô hình gồm 3 khớp
tịnh tiến, đơn giản trong việc điều khiển nhưng lại chiếm nhiều không gian.
…

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

9


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

1.3. Không gian làm việc:

Dựa vào việc phân tích một số mô hình trên, nhóm quyết định chọn mô hình số 1
vì những ưu điểm sau:
o Mô hình có 2 khâu 1,2 để xác định vị trí trên mp xy chỉ hoạt động trên 1 mp
-> tiết kiệm không gian, không bị vướng với các vật nằm gần thân robot.
o Có sự độc lập phân chia nhiệm vụ của từng khớp rõ ràng, độc lập -> mô hình
điều khiển đơn giản.
o Kết cấu đơn giản đảm bảo tính linh hoạt.

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

10


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

 Mô hình 3D thiết kế sơ bộ như sau:

 Với mô hình này, khớp quay 1,2 sẽ giúp Robot có thể với tới những điểm trong

mặt phẳng xy nằm trong vùng làm việc của nó, khớp tịnh tiến 1 giúp Robot có
thêm dịch chuyển trong không gian theo trục z. Như vậy, với khớp 1,2,3 giúp
Robot có thể với tới mọi điểm trong không gian nằm trong vùng làm việc của
nó. Và trục z của khâu thao tác trùng với trục z0, đến đây Robot cần phải sắp
xếp được thùng hàng theo một trật tự (các cạnh kề nhau như hình vẽ dưới). Khi
đó, khớp quay 4 quay quanh trục z và đảm bảo xếp được hàng theo trật tự nhất
định. Như vậy, Robot với 4 bậc tự do này đã đảm bảo được yêu cầu của bài
toán đó là gắp, thả vật đúng vị trí, đúng hướng cần thiết.

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

11


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

1.4. Tham số động học Robot
Tại vị trí đặt Robot, ta đặt hệ tọa độ gốc O0xyz. Gốc hệ tọa độ Oixyz gắn với xe
đẩy và gốc hệ tọa độ Oiixyz gắn với băng tải có tọa độ lần lượt là:
0
Oi
X1=1800
Y1=480
Z1=180
0
Oii
X2= -490
Y2=1480

Z1=610

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

12


GVHD: PGS.TS PHAN BÙI KHÔI
Joint
1
2
3
4

i
1
2

0
4

di
d1
d2
d3
d4

ai
a1
a2

0
0

ROBOTICS DESIGN

i

0
0
0
0

Ta có sơ bộ thông số động học của Robot như sau:
Chiều dài các khâu:

d1 = 1700; d2 = 50;

d4 = 350;

l3 = 1300.

a1 = 1000; a2 = 800;
Giới hạn các biến khớp:  1 = -60o ÷150o;

 2 = ±120o;  4 = ±180o ;

d3 = -50÷-1050 mm;
Tham số động lực học Robot
Khối lượng các khâu: m1= 42kg, m2= 35kg, m3= 10kg, m4 = 5kg;
Đối tượng làm việc:

 Hình dạng: khối hộp chữ nhật
 Kích thước: 300x300x200
 Khối lượng: M = 10kg

2. PHÂN TÍCH KHẢO SÁT ĐỘNG HỌC
2.1. Khảo sát bài toán động học
2.1.1. Tính các ma trận truyền
Theo quy tắc D-H ta có ma trận biến đổi tọa độ thuần nhất như sau:

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

13


GVHD: PGS.TS PHAN BÙI KHÔI

cos i
 sin 
i
i 1
Ai  
 0

 0

ROBOTICS DESIGN

 cos  i s in i

s in  i s in i

c os  i c os i

s in  i c os i

sin  i

c os  i

0

0

ai c os i 
ai s in i 
di 

1 

Lần lượt thay các hàng của bảng thông số động học vào ma trận trên ta được các
ma trận truyền tương ứng với các khâu:

C1
 S1
0
A1  
0

0
1

0
2
A3  
0

0

 S1 0 a1C1
C1 0 a1S1
0 1 d1 

0 0
1 
0 0 0
1 0 0 
0 1 d3 

0 0 1

C 2  S 2
 S 2 C2
1
A2  
0
0

0
0
C 4  S 4
 S 4 C4

3
A4  
0
0

0
0

0 a2C 2 
0 a2 S 2 
1
d2 

0
1 
0 0
0 0 
1 d4 

0 1

Ma trận biến đổi tọa độ thuần nhất của khâu thao tác EE là:

C124  S124
 S124 C124
0
0
A4  AE  
 0
0


0
 0

a2C12  a1C1 
0
a2 S12  a1S1 
1 d1  d 2  d3  d 4 

0
1

0

2.1.2. Vị trí, hướng khâu thao tác
 0 RE  3x3 0 rE  3 x1 
Ma trận trên có dạng AE  
 trong đó:
0
1


0
 RE  3x3 là ma trận cosin chỉ hướng của khâu thao tác so với hệ tọa độ
0

gốc.


0

rE  3x1 là ma trận chỉ vị trí của khâu thao tác so với hệ tọa độ gốc.

 Ta có:

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

14


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

 xE   a2C12  a1C1 
o Vị trí khâu thao tác: rE   yE    a2 S12  a1S1 
 zE   d1  d 2  d3  d 4 
0

C124  S124 0
0
RE   S124 C124 0
o Hướng khâu thao tác:
 0
0
1 
So sánh với ma trận Cardan ta được:

c c
C124  S124 0  

0
RE   S124 C124 0    s s  c  c s
 0
0
1   c s c  s s

c s
 s s  s  c c
c s s  s c

s


s c 
c c 

giải ra ta được bộ 3 góc Cardan chỉ hướng của khâu thao tác là:
   k
  0


Hoặc   k
với k=1,2,3…
  0
        k
      
1
1
2
2

4
4



 Vị trí và hướng của robot đặc trưng bởi:
 a2C12  a1C1 


 rE   a2 S12  a1S1 
   d  d  d  d 
2
3
4
pE      1

  
0

  

0
  

 1   2   4 

Do bản thân kết cấu robot đã luôn cố định góc α và β luôn bằng 0 nên ta có
thể hiểu ngắn gọn vị trí của robot qua 4 thông số còn lại như sau:
 a2C12  a1C1 



 rE   a2 S12  a1S1 
pE    
⇔  xE , yE , zE ;E 
   d1  d 2  d3  d 4 


 1   2   4 

Và từ đây về sau khi nói về vị trí và hướng của robot ta chỉ cần quan tâm đến
4 thông số trên.
2.1.3. Vận tốc, gia tốc điểm tác động cuối
Ma trận tọa độ điểm tác động cuối E:
NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

15


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

 a2C12  a1C1 
rE  r4   a2 S12  a1S1 
 d1  d 2  d3  d 4 
0

Vận tốc:





 a    S12  a11S1
 xE   2 1 2




vE  xE   yE   a2 1   2 C12  a11C1   J TE .q


 z E  

d3







Trong đó JTE là ma trận Jacobi tịnh tiến của khâu thao tác.
Gia tốc:













   a    S12     2 C12   a  S1   2C1 
1
2
1

 1  1
  2 1 2


2
aE  vE  rE    a2 1   2 C12  1   2 S12   a1 1C1  12 S1 

 



d3







2.1.4. Vận tốc góc, gia tốc góc khâu thao tác
Vận tốc góc, gia tốc góc của khâu thao tác trong hệ tọa độ cố định
Ma trận quay R:
C124  S124 0 
RE  R4   S124 C124 0 
 0
0
1 
 C124 S124 0 
0
T
o RE   S124 C124 0
 0
0
1 
0

0



o










  1   2   4 S124  1   2   4 C124 0 


0
RE   1   2   4 C124  1   2   4 S124 0



0
0
0







Tích hai ma trận:

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

16


GVHD: PGS.TS PHAN BÙI KHÔI

 0 RE .0 RE T


0

  1   2   4


0




ROBOTICS DESIGN



 1   2   4



0
0



0

0

0



Ta suy ra vận tốc góc của khâu thao tác theo hệ tọa độ cố định từ ma trận  trên:
  0 0 1   2  3 

T

Gia tốc góc :     0 0 1  2  4 

T

 a2C12  a1C1 
rE  r4   a2 S12  a1S1 
 d1  d 2  d3  d 4 
0

2.1.5. Không gian làm việc
Với tọa độ điểm tác động cuối như trên và giới hạn chuyển động của từng khớp, ta
tìm được không gian làm việc của Robot như hình dưới đây:

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

17


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

Là vùng mà điểm tác động cuối của Robot có thể với tới được.

Chọn quỹ đạo điểm tác động cuối của Robot để thực hiện việc gắp thả vật như
hình dưới đây:
 Vị trí ban đầu ở điểm M
 Di chuyển đến điểm G để gắp vật
 Di chuyển qua G1 để đến vị trí thả vật A,B,C,D lần lượt qua
A1,B1,C1,D1

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

18


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

Với quỹ đạo như vậy, khảo sát trong không gian làm việc ta thấy không gian làm việc của
Robot thỏa mãn yêu cầu bài toán (mọi điểm trên quỹ đạo đó nằm trong không gian làm
việc của robot)

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

19


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

2.2. Thiết kế quỹ đạo

Yêu cầu bài toán là gắp thả vật, không có yêu cầu gì thêm về đường đi, vận tốc,
gia tốc
trên đường đi nên nhóm chọn quỹ đạo điểm – điểm như phần trên và quy luật chuyển
động là hàm bậc 3 của vị trí.
Ta có tọa độ các điểm cần đi qua:
M(-700, 960, 1100);
G(50,1470,810); G1(50, 1470, 1300);
A(1610, 127, 380); A1(1610, 127, 1300);
B(1610, -192, 380); B1(1610, -192, 1300);
C(1290, 127, 380); C1(1290, 127, 1300);
D(1290, -192,380); D1(1290, -192, 1300);
Quỹ đạo như hình sau:

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

20


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

Để đơn giản bài toán, đầu tiên nhóm em xin được thực hiện quỹ đạo chuyển động theo
M-G-G1-A1-A để thực hiện việc di chuyển khâu thao tác của Robot từ điểm ban đầu M,
đến điểm G để gắp vật, sau đó di chuyển qua G1, A1 để tới điểm A là điểm đặt vật.

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

21

GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

2.2.1. Tổng quát
Robot di chuyển từ vị trí (i) đến vị trí (j), phương trình quỹ đạo có dạng
p  a0  a1s  a2 s 2  a3 s 3 hay là:

 x  ax 0  ax1s  ax 2 s 2  ax 3 s 3

2
3
 y  a y 0  a y1 s  a y 2 s  a y 3 s

2
3
 z  a z 0  a z1 s  a z 2 s  a z 3 s
  a  a s  a s 2  a s 3
0
1
2
3

Với s = t – ti (ti: là thời gian tại vị trí (i); ti ≤ t ≤ tj)
o t = ti :

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

22

GVHD: PGS.TS PHAN BÙI KHÔI

 x  xi
y  y

i

 z  zi ;
  i

ROBOTICS DESIGN

 x  xi
y  y

i

 z  zi
  i

o t = tj :

x  xj

y  yj

z  z j ;
  

j


x  xj

y  yj

z  z j
  
j


Theo phương x: x  a0  a1  t  ti   a2  t  ti   a3  t  ti 
2

o t = ti :

x = xi ;

x  xi

o t = ti :

x = xj ;

x  xj

3

 xi  a0

x  a  0
1
 i
 x  a  a t  t  a t  t 2  a t  t 3
0
1 j
i
2 j
i
3 j
i
 j
2

 x j  a1  2a2  t j  ti   3a3  t j  ti   0

Đặt:
1
0

Tij  
1


0

t

j

0

0

1

0

 ti 
1

t  t 
2 t  t 
2

j

i

j



0

3  ;
 t j  ti  
2
3  t j  ti  
0

i

 xi 
 ax 0 
x 
a 
x1 

; X ij   i 
Axij 
 ax 2 
xj
 
 
 x j 
 ax 3 

 TAxij  X ij  Axij  T 1 X ij
Tương tự với phương y, z ta được như sau:

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

23


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

ay 0 
 yi 
 
y 
i
 a y1 
Ayij    ; Yij     TAyij  Yij  Ayij  T 1Yij
yj 
a
 y2 
 
 a y 3 
 y j 

 zi 
 az 0 
z 
a 
i
z
1
Azij    ; Z ij     TAzij  Z ij  Azij  T 1Z ij
z j 
 az 2 
 
 
a
 z j 
 z3 
 a 0 

i 
 
 
i
 a1 
Aij    ; ij     TAij  ij  Aij  T 1ij
 j 
a
 2 
 
 a 3 
 j 

2.2.2. Giải bài toán quỹ đạo trên từng đoạn cụ thể
2.2.2.1. Đoạn MG (hay là đoạn (1)-(2))
t1 = 0; t2 = 1(s)
Tọa độ các điểm: M(-700, 960, 1100;0);
G(50,1470,810;0);
Với sự hỗ trợ của phần mềm Maple ta được kết quả như sau:
2
 x  1500t 3  2250t 2  700 vx  4500t  4500t

ax  9000t  4500

2
3
2
 y  1020t  1530t  960 v y  3060t  3060t a  6120t  3060
 y



;
;
3
2
2

vz  1740t  1740t
 z  580t  870t  1100
az  3480t  1740

  0
  0


  0
Đồ thị vị trí, vận tốc, gia tốc của Robot trên đoạn MG như sau:

NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

24


GVHD: PGS.TS PHAN BÙI KHÔI

ROBOTICS DESIGN

Tương tự như trên, với sự hỗ trợ của phần mềm Maple, ta dễ dàng tìm được
phương trình chuyển động của Robot trong các đoạn còn lại như sau:
2.2.2.2. Đoạn GG1 (hay là đoạn (2)-(3))

t2 = 1; t3 = 3(s)
Tọa độ các điểm: G(50,1470,810;0); G1(50, 1470, 1300;0);
NGUYỄN VĂN HUY – VÕ TRỌNG ĐỨC

25