Báo cáo bài tập lớn học phần Robotics

BÁO CÁO BÀI TẬP LỚN
Học phần Robotics

Sinh viên thực hiện: Nguyễn Văn Huy
MSSV: 20131782

1


MỤC LỤC
ĐỀ BÀI................................................................................................................................ 3
BÀI LÀM ............................................................................................................................ 4
1.

BÀI TOÁN ĐỘNG HỌC ROBOT ....................................................................... 4
1.1 Vẽ hệ trục tọa độ theo quy tắc D-H và lập bảng thông số D-H .................... 4
1.2 Tính các ma trận D-H: ..................................................................................... 4
1.3 Xác định vị trí điểm tác động cuối khâu thao tác, hướng của khâu thao
tác: ............................................................................................................................ 5
1.4 Vận tốc, gia tốc điểm tác động cuối................................................................. 6
1.5 Vận tốc góc, gia tốc góc khâu cuối. ................................................................. 6
1.6 Bài toán động học ngược ................................................................................ 10

2.

TĨNH HỌC ROBOT. .......................................................................................... 11
2.1 Lực dẫn động khâu 3: .................................................................................... 12
2.2 Lực dẫn động khâu 2: .................................................................................... 14
2.3 Lực dẫn động khâu 1: .................................................................................... 15

3.

ĐỘNG LỰC HỌC ROBOT ................................................................................ 16
3.1.

Động năng .................................................................................................... 17

3.1.1 Ma trận Jacobi tịnh tiến trọng tâm các khâu .......................................... 17
3.1.2 Ma trận Ten-xơ quán tính ......................................................................... 18
3.1.3 Ma trận khối lượng..................................................................................... 18
3.2.

Thế năng ....................................................................................................... 18

3.3.

Lực suy rộng ................................................................................................ 18

3.4.

Tính toán bằng phần mềm MAPLE .......................................................... 19

4.

THIẾT KẾ QUỸ ĐẠO ........................................................................................ 26

5.

ĐIỀU KHIỂN ROBOT ....................................................................................... 28

2

ĐỀ BÀI
Chọn một mô hình robot nghiên cứu theo nội dung dưới đây:
1. Tính số bậc tự do của robot.
2. Vẽ các hệ trục tọa độ gắn liền với các khâu theo quy tắc Denavit – Hartenberg (DH).
3. Lập bảng D-H. Tính các ma trận D-H: i-1Ai, i=1,2…
4. Tìm vị trí điểm thao tác biểu diễn theo các tọa độ khớp. Xác định hướng của khâu
thao tác.
5. Tính vận tốc điểm tác động cuối E. Tính vận tốc góc các khâu.
6. Tính vận tốc điểm tác động cuối. Tính gia tốc góc khâu thao tác.
7. Cho vị trí, vận tốc, gia tốc điểm tác động cuối, hướng, vận tốc góc, gia tốc góc
khâu thao tác.
8. Tính các tọa độ khớp.
9. Tính vận tốc, gia tốc dài của các khâu tương ứng với các khớp tịnh tiến.
10. Tính vận tốc góc, gia tốc góc của các khâu tương ứng với các khớp quay.
11. Khảo sát bài toán tĩnh học robot theo một trong các câu: 13, 14, 15.
12. Thiết kế quỹ đạo chuyển động như câu 16.
13. Khảo sát động lực học theo nội dung một trong các câu 17, 18.
14. Thiết kế mô hình điều khiển robot theo một trong các câu 19, 20, 21.
15. Lập trình tính toán, vẽ đồ thị bằng phần mềm MAPLE, MATLAB, mô phỏng điều
khiển robot bằng MATLAB, SIMULINK.

3


BÀI LÀM

Robot có 3 bậc tự do.
Có thông số như sau:

L2=0.6 ; a3 = 0.4; d1 = 0.6 (m)
m1 = 16; m2 = 12; m3 = 6 (kg)
1. BÀI TOÁN ĐỘNG HỌC ROBOT
1.1 Vẽ hệ trục tọa độ theo quy tắc D-H và lập bảng thông số D-H
Bảng thông số D-H của tay máy
robot:
Joint
1

θi
θ1

di
d1

ai
0

2


2

d2

0

3

θ3

0

a3

αi


2

2

0

1.2 Tính các ma trận D-H:

4


ci
 s
i 1
Ai   i
0

0

c i si

s i si

c i ci

s i ci

s i

c i

0

0

c1
 s
0
A1   1
0

0

0

 s1

0

c1

1

0

0

0

c3
 s
2
A3   3
 0

 0

 s3
c3

 s1s3
 c s
0
A3   1 3
 c3

 0

0
0

ai ci 

ai si 
di 

1 

0 0
 1 0
1
A2  
 0 1

0 0

0
0 
d1 

1

0
0 0 
0 d2 

0 1

1

0 a3c3 
0 a3 s3 
1

0 

0
1 

s1c3

c1

c1c3

s1

 s3

0

0

0

a3 s1s3  d 2 s1 
a3c1s3  d 2c1 

a3c3  d1

1


1.3 Xác định vị trí điểm tác động cuối khâu thao tác, hướng của khâu thao

tác:
Vị trí điểm tác động khâu thao tác E:
 a3 s1s3  d 2 s1 
0
rE  r3   a3c1s3  d 2c1 


a3c3  d1
Hướng khâu thao tác:
s1c3 c1 
 s1s3

0
RE  R3   c1s3 c1c3 s1 
 c3
 s3
0 

5


So sánh với ma trận Cardan RCD

c c


  s s  c  c s
 c s c  s s

c s

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

s

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

có hệ phương trình:
 f1  c c  0

và tìm được bộ 3 góc Cardan.
 f 2  s  c1  0
 f  c s  s c  0
1
3
 3
1.4 Vận tốc, gia tốc điểm tác động cuối.
Vận tốc điểm tác động cuối:
vE  rE 0 r3  J TE .q trong đó JTE là ma trận Jacobi tịnh tiến của khâu cuối E
JTE

 a3c1s3  d 2c1
  a3 s1s3  d 2 s1

0

 s1
c1

0

a3 s1c3 
a3c1c3  ;
a3 s3 

 1 
 
q  d2 
 3 
 

 a3c1s31  d 2 c11  s1d 2  a3 s1c33 


vE   a3 s1s31  d 2 s11  c1d 2  a3c1c33 


a3 s33



Gia tốc điểm cuối E:
Với JTE

J TE

 J11
  J 21
 J 31

J12
J 22
J 32

aE  vE  rE  J TE q  JTE q
 J11
J13 


J 23  thì J TE   J 21
 J 31
J 33 


J12
J 22
J 32

 a3 s 1s31  d 2 s11  c1d 2  a3c1c33

  a3c1s31  d 2 c11  s1d 2  a3 s1c33

0


J13 

J 23 
J 33 

c11
 s11
0

a3c1c31  a3 s1s33 

a3c1c31  a3c1s33 

a3c33


 1 
 
Và q   d 2  thay vào ta được gia tốc aE:
 3 
 

1.5 Vận tốc góc, gia tốc góc khâu cuối.
6


-

Vận tốc góc khâu cuối:

s1c3 c1 
 s1s3

RE  R3   c1s3 c1c3 s1  ;

 c3
 s3
0 
c1s31  s1c33 c1c31  s1s33

RE   s1s31  c1c33 s1c31  c1s33

s33
c31

0

 s1s3
R   s1c3
 c1
 s11 

c11 
0 
T
E

c1s3

c3 
 s3 
0 

c1c3
s1

Vận tốc góc khâu cuối biểu diễn trong hệ tọa độ khâu cuối:
c1s31  s1c33 c1c31  s1s33  s11   s1s3 c1s3 c3 


E 0 RE .RET   s1s31  c1c33 s1c31  c1s33 c11   s1c3 c1c3 s3 

s33
c31
0   c1
s1
0 

 0

   1
  s13


c13 
s13 



0
c13   E   s13  ; E  c13
 1 
c13
0 



0 0 c3 
Ma trận Jacobi quay khâu cuối: J RE  0 0  s3 
1 0
0 
Gia tốc     J RE q  J RE q
1

Trong đó: J RE
  s33

    c33
 0


T

s13 1   J RE .q

  s33

  c33
 0


0 0

0 0
0 0 
0 0   1   c3

 
0 0   d 2     s3
0 0  3   0

0 0   1    s313  1c3 
  

0 0   d 2    c313  1s3 

0 1  3  
3


 Cho quy luật chuyển động của các khâu như sau:
Ta vẽ có đồ thị biểu diễn vị trí, vận tốc, gia tốc của điểm thao tác E:

7


Vị trí:

Vận tốc:

8


Gia tốc:

Và đồ thị vận tốc góc, gia tốc góc của khâu 3:
Vận tốc góc:

9


Gia tốc góc:

1.6 Bài toán động học ngược
 xE  0.5
 xE  0.3
 xE  0.1








Cho rE   yE   0.8  m  ; vE   yE   0.2  m / s  ; aE   yE   0   m / s 2 
 zE  0.6
 zE  0.2
 zE  0.1

Ta có phương trình
 xE  a3 s1s3  d 2 s1
 xE 
 a3 s1s3  d 2 s1 
 y   r   a c s  d c    y  a c s  d c
 E
2

1
3
1
3
2
1
 E E  3 1 3
 z  a c  d
 zE 


a3c3  d1
3
3
1
 E

(*)

Sử dụng sự hỗ trợ của phần mềm MAPLE:
>

>
>

10


 q1 
Ta được 4 bộ nghiệm  q2  như sau:

 q3 
-0.5585993153  2.582993338  -.5585993153 2.582993338 
0.5433981132  ; -1.343398113  ; 1.343398113  ; -.5433981132

 
 
 

-1.570796327  -1.570796327  1.570796327  1.570796327 

Với q2 = d2 ≤ l2 = 0.6(m) ta chỉ chọn được 1 bộ nghiệm duy nhất thỏa mãn là:
1  -0.5585993153
q   d 2   0.5433981132
3  -1.570796327





Đạo hàm 2 vế của phương trình (*) ta được:
 a3c1s 3  d 2c1  s1 a 3s 1c 3    1   xE 
 
 a s s  d s
c 1 a 3c 1c 3   d 2    yE   J q q  vE
2
1
 3 1 3

0
0

a3 s 3   3   z E 
 1  -.1573033707 
 
 q  J q1vE ta tính được: q   d 2   0.3285993428 (m / s )
3  0.5

 

Tiếp tục đạo hàm 2 vế của phương trình: J q q  vE ta được:
J q q  J q q  vE  J q q  vE  J q q  q  J q1  vE  J q q 
 q  J q1  aE  J q q 

Với sự hỗ trợ của phần mềm MAPLE ta tính được:
 1  -0.0197058310 
 
q   d 2   0.1995750313
3  0.2500000000
 

 (m / s 2 )



2. TĨNH HỌC ROBOT.

11


Bổ sung các tham số động học, động lực học:
q1 = 120o; q2 = 0.4 m; q3 = 45o;

Fx = 80N; Fz = 60N; Mx = 20Nm.
Lấy g = 9.8 m/s2
2.1 Lực dẫn động khâu 3:
 0 F   F , F , F T
 3,2  32 x 32 y 32 z 

T
0
 M 32 x , M 32 y , M 32 z 

M

3,2


Lực tác dụng từ RB lên đối tượng công nghệ F, M:
F   Fx , 0, Fz  0 F4,3   Fx , 0, Fz 
T

T

M   M x , 0, 0 z  0 M 4,3   M x , 0, 0
T

T

Áp dụng công thức đệ quy:
12

0
0
0

 F3,2  F4,3  P3
0
0
0 3 0
0 3 0

 M 3,2  M 4,3  r2 . F3,2  rc3 . P3

Trong đó:
T
T
T
 0 g3  0, 0,  g  0 P3  0, 0, m3 g   0, 0, 6 g 




s1c3 c1 
 s1s3

3 3
r2   a3 , 0, 0 ; r23 0 R3 .3 r23 với RE 0 R3   c1s3 c1c3 s1 
 c3
 s3
0 
0

a3c3
a3c1s3 
 a3 s1s3 




0 3
0 3
 r2   a3c1s3   r2   a3c3
0
a3 s1s3 
 a3c3 
 a3c1s3 a3 s1s3
0 
s1c3 c1 
 s1s3
T
 a3
 0 3 0 3 3

0
3 3
rc 3    , 0, 0 ; rc 3  R3 . rc3 với RE  R3   c1s3 c1c3 s1 
 2

 c3
 s3
0 
a3c3

a3c1s3 
 a3 s1s3 

0



2
2
2 




a3c1s3  0 3  a3c3
a3 s1s3 
0 3

 rc 3 
 rc 3 
0
 2


2
2 





 a3c3 
 a3c1s3 a3 s1s3
0 
 2



2
2
T 0

 Thay tất cả vào hệ phương trình ban đầu ta được:

 Fx

0


 F3,2  0


 m3g+Fz 

 1


  2 a3 m3gc1s3  a3 FZ c1s3  M x






 0 M   1 a  -m gs s  2F s s  2c F  
3
1
3
Z
1
3
3 x

 3,2  2 3



 a3 Fx c1s3






Thay số vào ta được:

13



80 
0


 N
 F3,2  0
 

118.8


8.94 2+20


 0 M  16 2-8.94
 3,2 

-8 2




6   Nm 



2.2 Lực dẫn động khâu 2:
 0 F   F , F , F T
 2,1  21x 21 y 21z 

T
0
 M 21x , M 21 y , M 21z 


M

2,1


Lực tác dụng từ khâu 3 lên khâu 2:

80 
0

 N
 F3,2  0
 

118.8


8.94 2+20


 0 M  16 2-8.94
3,2



-8 2





6   Nm 



Áp dụng công thức đệ quy:
0
0
0

 F2,1  F3,2  P2
0
0
0 2 0
0 2 0

 M 2,1  M 3,2  r1 . F2,1  rc 2 . P2

Trong đó:
T
T
T
 0 g3   0, 0,  g  0 P2  0, 0, m2 g   0, 0, 12 g 


0 s1 c1 
r   0, q2 , 0 ; r  R2 . r với R2  0 c1 s1 
1
0
0 

d2 
 3d 2 

0
0



2 
 2 


d
3
d



2
0 r12   2  0 r23   0
0

2
2 




3d 2
0


 d2

0 


 2
2




2

2
1

T 0

2
1

0

2

2
1

0

14




T

2

 l 
rc22  0, 2 , 0 ;0 rc22 0 R2 .2 rc22
 2 

 3l2 



 0
4



 l2  0 2 
0 2
 rc 2  
 rc 2   0
4 




0

 l2


 4




0
0
3l2
4

l2 
4 

3l2 

4 


0 


 Thay tất cả vào hệ phương trình ban đầu ta được:

80 

0

 N
 F2,1  0
 

 236.4 

3.003 

0
 M  52.067   Nm 

 2,1 


4.686



2.3 Lực dẫn động khâu 1:
 0 F   F , F , F T
 1,0  10 x 10 y 10 z 

T
0
 M 10 x , M 10 y , M 10 z 

M


10


Lực tác dụng từ khâu 3 lên khâu 2:

80 
0

 N
 F3,2  0
 

 236.4 

3.003 

 0 M  52.067   Nm 

 3,2 


4.686



Áp dụng công thức đệ quy:
0
0
0


 F1,0  F2,1  P1
0
0
0 1 0
0 1 0

 M 1,0  M 2,1  r0 . F1,0  rc1. P1

Trong đó:
T
T
T
 0 g1  0, 0,  g  0 P1  0, 0, m1 g   0, 0, 16 g 

15




r   0, d1 , 0 ; r  R . r với
T 0

1 1
0

1
0

0

1 1
1 0

0 
 0 d1


0 3
 r  0   r2   d1 0
 d1 
 0
0
0



1
1

0

c1
R1   s1
 0
0
0
0

 s1 
0 c1 

1 0 
0




 0


0

T

 0 1  d1
 d1  0 1 0 1 1
1 1
0 1
rc1  0, , 0 ; rc1  R1. rc1  rc1  0   rc1 
 2
 2 
 d1 
 0
 

 2


d1
2
0

0


0

0

0 


 Thay tất cả vào hệ phương trình ban đầu ta được:

80 
0

 N
 F1,0  0
 

392.2 

3.003 

0
 M  100.067   Nm 

 1,0 


6.686




3. ĐỘNG LỰC HỌC ROBOT
Phương trình động lực học Lagrange loại II:
d  T

dt  qi

 T


 Qi  U i , i  1..n

qi
 qi

Trong đó:
o
o
o
o
o
o

T – Động năng của cả hệ robot
Π – Thế năng của cả hệ robot
qi – tọa độ suy rộng thứ i
Qi – Lực suy rộng của các lực không thế ứng với tọa độ suy rộng qi
Ui – Lực/Momen điều khiển ứng với tọa độ suy rộng qi.

n – Số bậc tự do của robot.

Phương trình động lực học dạng ma trận:
M ( q ) q  C ( q, q ) q  G ( q )  Q  U

Trong đó:
16






o M (q)    JTiT mi JTi  J RiT cir J Ri 
n

 i 1

 nn

o C (q, q)q  c j (q, q)q  n1

; c j (q, q)q 

o G j (q)   g j (q)  n 1

; g j (q ) 

n

  k , l; j  q q

k ,l 1

k l


q j

o Q  Bq  Dsignq  Q a trong đó: Bq là ma sát nhớt, Dsignq là ma sát khô,
Q a là lực suy rộng của các lực không thế
3.1. Động năng
T

1 T
1  3

q M (q)q  qT   J TiT mi J Ti  J RiT cir J Ri   q
2
2  i 1


3.1.1 Ma trận Jacobi tịnh tiến trọng tâm các khâu
Ma trận Jacobi tịnh tiến các trọng tâm các khâu:
0

Aci 0 Ai .i Aci 0 rci 0 JTi 

r
q

Ta tính được:
1
 2 l2 c1  d 2 c1
0
0
0



1


JT 1  0 0 0  ; JT 2   l2 s1  d 2 s1
2
0 0 0 

0


1
 2 a3c1s3  d 2 c1

1
JT 3   a3 s1s3  d 2 s1
2


0


 s1
c1
0


 s1 0 

c1 0  ;

0
0 


1

a3 s1c 3 
2

1
 a3c1c 3 

2

1
 a3 s 3 

2

Ma trận Jacobi quay các khâu:

0

i 0 Ri 0 RiT 0 i  JRi 


q

Ta tính được:

17


0 0 0
0 0 0
0 0 c1 




JR1  0 0 0  ; JR 2  0 0 0  ; JR1  0 0 s1  .
1 0 0 
1 0 0 
1 0 0 

3.1.2 Ma trận Ten-xơ quán tính
Coi các khâu của robot là các thanh đồng chất tiết diện đều và bề ngang không đáng
kể.
Ta có:
 m1d12


 12
1
c1   0

 0


0
0
0

 m2l2 2

0 

 12
 2
0  ; c 2   0


m1d12 
 0

12 

0
0
0




0
0 

 3
0  ; c 3  0



m2l2 2 
0
12 


0
m3a32
12
0


0 

0 

2
m3a3 
12 

3.1.3 Ma trận khối lượng
Với sự hỗ trợ của phần mềm MAPLE ta tính được M:

 Động năng T:

3.2. Thế năng
3

   g.mi .zci với m   m1 m2
i 1

m3  và zc1 

d1
1
; zc 2  d1; zc3  a3c3  d1
2
2

 Π=

3.3. Lực suy rộng
T
T
Với lực không thế tác dụng lên RB là F   Fx 0 Fz  ; M   Mx 0 0

18


Lực suy rộng Q    JTiT Fi  J RiT M i  trong đó F1 = F2 = 0; M1 = M2 = 0; F3 = F; M3
3

i 1

= M ta tính được Q:

3.4. Tính toán bằng phần mềm MAPLE
Chương trình MAPLE tính toán động lực học thuận:
>

Ma tran Denavit Hartembeg: DH(theta, d, a, alpha)
Nhan 2 ma tran: nhan2(A,B)
Nhan 3 ma tran: nhan3(A,B,C)
ma tran Jacobi 3dof : J(A,q1,q2,q3)
ma tran Jacobi 2dof: J(A,q1,q2)
Rut gon: rutgon(A)
tao ma tran song: w2ws(w)
van toc goc tu ma tran song: ws2w(ws)
> #--------Nhập các biến khớp của robot:-------#

> #--------các ma trận D-H---------#

19


> #------Nhập tọa độ trọng tâm các khâu của rb-----#

> #-----Ma trận Jacobi tịnh tiến trọng tâm các khâu------#

20

>

tao ma tran omega tu ma tran DH: w(A)
>

21


>

ma tran Jacobi quay tu ma tran omega: jr(w)
>

> #---- Tính ma trận khối lượng:----#
#---ma trận quán tính của các khâu đối với hệ tọa độ qua trọng
tâm của khâu đó:

22


>

> #--=>ĐỘNG NĂNG---

23


> #----Tính ma trận C----#----hàm tính ma trận C----#

>

24


>

> #------Tính thế năng PI, ma trận G--->

>

> #-----Tính lực suy rộng Q----

> #----lực dẫn động U-----

>

25