Tải bản đầy đủ

TLRoboticsDHBKHNRobot RRT

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN CƠ KHÍ
******

TIỂU LUẬN BÁO CÁO
Robotics
GVHD: PGs. Nguyễn Quang Hoàng
SVTH: Trần Đình Hùng
Lớp: KT Cơ điện tử 4 K59
MSSV: 20142121

Hà Nội, tháng 5 năm 2017


I. Tổng quan
3 Dof Robot, RRT

Mô hình thực tế:

1



II. Tính toán
1. Tính số bậc tự do của robot

Theo định lý Gruebler
- số khâu động: 𝑝 = 3
- số khớp động: 𝑛𝐽 = 3, 𝑓𝐽1 = 1 (𝑅), 𝑓𝐽2 = 1 (𝑅), 𝑓𝐽3 = 1 (𝑇)
- số bậc tự do thừa: 𝑓𝑡ℎ = 0
⇒ số bậc tự do của robot:
3

f  6  p  nJ    f Ji  fth  6  3  3  1  1  1  0  3
i 1

Chọn tọa độ suy rộng: q  [q1 q2 q3]T (như hình vẽ).

2


2. Vẽ các hệ trục tọa độ gắn liền các khâu theo quy tắc Denavit-Hartenberg

3


3. Lập bảng D-H. Tính các ma trận D-H: 𝑻𝒊−𝟏
𝒊 , 𝐢 = 𝟏, 𝟐 …
Bảng D-H:
Link
1
2
3

𝜽𝒊
𝑞1
𝑞2
0

𝒅𝒊
𝑑1
0

−𝑞3

𝒂𝒊
𝑎1
𝑎2
0

𝜶𝒊
0
0
π

Tính các ma trận:
cq1  sq1
 sq1 cq1
T10  
 0
0

0
 0
cq 2  sq 2
 sq 2 cq 2
T21  
 0
0

0
 0

0 a1cq1
0 a1sq1
1
d1 

0
1 
0 a 2cq 2 
0 a 2sq 2 
1
0 

0
1 

0 
1 0 0
0 1 0
0 
2

T3 
0 0 1 q3


1 
0 0 0



cq1  sq1

T30  T10 *T21 *T32   sq1 cq1
 0
0

0
 0

0 a1cq1
0 a1sq1
1
d1 

0
1 

cq 2  sq 2
 sq 2 cq 2

 0
0

0
 0

cq12 sq12 0 a1cq1  a 2cq12 
 sq12 cq12 0 a1sq1  a 2sq12 

⇒ T30  
 0
0
1
q3  d1 


0
0
1
 0

̇ = 𝑞1̇ + 𝑞2̇
𝑞12

Trong đó: q12  q1  q2 ⇒ { ̈
𝑞12 = 𝑞1̈ + 𝑞2̈

4

0 a 2cq 2 
0 a 2sq 2 
1
0 

0
1 

0 
1 0 0
0 1 0
0 

0 0 1 q3


1 
0 0 0


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
Ta có:
cq12 sq12 0 a1cq1  a 2cq12 
 sq12 cq12 0 a1sq1  a 2sq12 

T30  
 0
0
1
 q3  d1 


0
0
1
 0


⇒ Tọa độ điểm cuối E:
 a1cq1  a 2cq12 
rE   a1sq1  a 2sq12 
 q3  d1 

5


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
a, Vận tốc điểm tác động cuối E:
.

̇
 a1cq1  a 2cq12 
−𝑎1𝑠𝑞1𝑞1̇ − 𝑎2𝑠𝑞12𝑞12
̇ ]
𝑣𝐸 = 𝑟𝐸̇ =  a1sq1  a 2 sq12  = [ 𝑎1𝑐𝑞1𝑞1̇ + 𝑎2𝑐𝑞12𝑞12


−𝑞3̇
 q3  d1 

b, Vận tốc góc các khâu:
Khâu 1:
cq1  sq1 0 
A   sq1 cq1 0 
 0
0
1 
0
1

  sq1 cq1 0   cq1 sq1 0 
0 −𝑞1̇ 0
̃
(0)




0𝑇
0
̇
⇒ 𝑤1 = 𝐴1 ∗ 𝐴1 = 𝑞1̇  cq1  sq1 0    sq1 cq1 0  =[𝑞1̇
0
0]
 0
0
0
0
0
0 1 
0   0

⇒ 𝑤1 = [0 0 𝑞1̇]𝑇
Khâu 2:
cq1  sq1 0  cq 2  sq 2 0  cq12  sq12 0 
cq1 0   sq 2 cq 2 0  =  sq12 cq12 0 

 

 0
0
1   0
0
1   0
0
1 

A20  A10 * A21   sq1


  sq12 cq12 0   cq12 sq12 0 
0



̇
 sq12 0   sq12 cq12 0 =[𝑞12



0
 0
0
0   0
0
1 

̃
(0)
0𝑇

̇ 
⇒𝑤
2 = 𝐴2 ∗ 𝐴2 = 𝑞12 cq12

̇
−𝑞12
0
0
0]
0
0

̇ ]𝑇
⇒ 𝑤2 = [0 0 𝑞12

Khâu 3:
cq12  sq12 0  1 0 0  cq12 sq12 0 
cq12 0  0 1 0  =  sq12 cq12 0 

 

 0
0
1  0 0 1  0
0
1

A30  A20 * A32   sq12


̇
  sq12 cq12 0  cq12 sq12 0 
0
−𝑞12
0
̃
(0)
0
0𝑇
̇




̇
⇒ 𝑤3 = 𝐴3 ∗ 𝐴3 = 𝑞12  cq12 sq12 0 sq12 cq12 0 =[𝑞12
̇
0
0]


 0

0

0   0

̇ ]𝑇
⇒ 𝑤3 = [0 0 𝑞12
6

0

1

0

0

0


6. Tính gia tốc điểm tác động cuối. Tính gia tốc góc khâu thao tác
a, Gia tốc điểm tác động cuối
̇ ′
−𝑎1𝑠𝑞1𝑞1̇ − 𝑎2𝑠𝑞12𝑞12
̇ ]
𝑎𝐸 = 𝑣𝐸̇ = [ 𝑎1𝑐𝑞1𝑞1̇ + 𝑎2𝑐𝑞12𝑞12
−𝑞3̇
̈ − 𝑎2𝑐𝑞12𝑞12
̇ 2
−𝑎1𝑠𝑞1𝑞1̈ − 𝑎1𝑐𝑞1𝑞1̇2 − 𝑎2𝑠𝑞12𝑞12
̈ − 𝑎2𝑠𝑞12𝑞12
̇ 2 ]
= [ 𝑎1𝑐𝑞1𝑞1̈ − 𝑎1𝑠𝑞1𝑞1̇2 + 𝑎2𝑐𝑞12𝑞12
−𝑞3̈

b, Gia tốc góc khâu thao tác (khâu 3)
0 ′
0
𝛼3 = 𝑤3̇ = [ 0 ] = [ 0 ]
̇
̈
𝑞12
𝑞12

7


7. Giải bài toán động học ngược. Cho vị trí, vận tốc, gia tốc điểm tác động cuối
Sử dụng phương pháp hình học:

Ta có:
 xE  a1cq1  a 2cq12

 yE  a1sq1  a 2sq12
 z   q3  d1
 E

1
 2
 3

Từ (1) ⇒ q3  d1  zE
Áp dụng định lý cosin cho tam giác OMN có
̂ = 𝜋 − 𝑞2
𝑂𝑀 = 𝑎1, 𝑀𝑁 = 𝑎2, 𝑂𝑁 = 𝑟, 𝑂𝑀𝑁

⇒ r 2  a12  a22  2a1a2cq2
⇒ cq 2 

r 2  a12  a 22
D
2a1a 2

⇒ sq2   1  cq22   1  D2
⇒ 𝑞2 = 𝑎𝑡𝑎𝑛2(  1  D2 , 𝐷)
Lại có:
̂
̂
𝑞1 = 𝑁𝑂𝑋
0 − 𝑁𝑂𝑀 = 𝑎𝑡𝑎𝑛2( 𝑌𝐸 , 𝑋𝐸 ) − 𝑎𝑡𝑎𝑛2( 𝑎2𝑠𝑞2, 𝑎1 + 𝑎2𝑐𝑞2)
8


8. Tính các tọa độ khớp
Có 3 khớp:
𝐴 (0, 0, 𝑑1)
𝐵 (𝑎1𝑐𝑞1, 𝑎1𝑠𝑞1, 𝑑1)
𝐶 (𝑎1𝑐𝑞1 + 𝑎2𝑐𝑞12, 𝑎1𝑠𝑞1 + 𝑎2𝑠𝑞12, 𝑑1)

9


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
Có 1 khâu tịnh tiến là khâu 3, tương ứng với khớp C
Xét tại điểm tác động cuối E ta có:
̇
−𝑎1𝑠𝑞1𝑞1̇ − 𝑎2𝑠𝑞12𝑞12
̇ ]
𝑣3 = 𝑣𝐸 = [ 𝑎1𝑐𝑞1𝑞1̇ + 𝑎2𝑐𝑞12𝑞12
−𝑞3̇
̈ − 𝑎2𝑐𝑞12𝑞12
̇ 2
−𝑎1𝑠𝑞1𝑞1̈ − 𝑎1𝑐𝑞1𝑞1̇2 − 𝑎2𝑠𝑞12𝑞12
̈ − 𝑎2𝑠𝑞12𝑞12
̇ 2 ]
𝑎3 = 𝑎𝐸 = [ 𝑎1𝑐𝑞1𝑞1̈ − 𝑎1𝑠𝑞1𝑞1̇2 + 𝑎2𝑐𝑞12𝑞12
−𝑞3̈

10


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
Có 2 khâu quay là khâu 1 tương ứng với khớp A, khâu 2 tương ứng với khớp B
Ta có:
0
0
0
𝑤1 = [ ] ⇒ 𝛼1 = 𝑤1̇ = [ 0 ]
𝑞1̇
𝑞1̈
0
0
𝑤2 = [ 0 ] ⇒ 𝛼2 = 𝑤2̇ = [ 0 ]
̇
̈
𝑞12
𝑞12

11


11. Khảo sát bài toán tĩnh học
Cho vị trí, chiều dài, khối lượng các khâu. Cho lực/ momen tác động vào khâu cuối
𝑓⃗, 𝑛⃗⃗ trong hệ cố định 0. Bỏ qua ma sát. Tính giá trị lực/ momen động cơ tại các
khớp để robot cân bằng tĩnh.
Bảng tham số:
Khâu

1
2
3

Khối
lượng
m1
m2
m3

Vị trí trọng tâm

Momen quán tính khối từng khâu

(so với gốc tọa độ trên mỗi khớp)

(đối với hệ tọa độ đặt tại trọng tâm và song song tọa độ khớp)

Xc
-(a1-l1)
-(a2-l2)
0

Ixx

Yc
0
0
0

Zc
0
0
-l3

Iyy

Izz

Ixy
0
0
0

Ixz
0
0
0

Iyz
0
0
0

Sử dụng phương pháp cân bằng khâu, tách khâu và đặt lực/ momen như hình vẽ:

12


Trong đó:
 𝑓⃗ℎ,𝑘 , 𝑛⃗⃗ℎ,𝑘 là lực/ momen tác dụng lên khâu h tại 𝑂𝑘 từ khâu k
𝑓⃗ℎ,𝑘 = −𝑓⃗𝑘,ℎ , 𝑛⃗⃗ℎ,𝑘 = −𝑛⃗⃗𝑘,ℎ
 𝑟⃗𝐶𝑖𝑖 = ⃗⃗⃗⃗⃗⃗⃗⃗
𝑂𝑖 𝐶𝑖 là vector có gốc tại Oi và mút tại Ci
𝑖
⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗
 𝑟⃗𝑖−1 = 𝑂
𝑖 𝑂𝑖−1 là vector có gốc tại 𝑂𝑖 và mút tại 𝑂𝑖−1


Ta có:  Pi , fi ,i 1 , ni ,i 1 ,  fi 1,i , ni 1,i   0
⇒  Fk  fi ,i 1  fi 1,i  Pi  0
 mOi  ni ,i 1  ni 1,i  rCii * Pi  ri i 1 * fi,i 1  0

Sử dụng công thức truy hồi:
fi ,i 1  fi 1,i  Pi
ni ,i 1  ni 1,i  rCii * Pi  ri i 1 * fi ,i 1

Với khâu cuối: 0 f 4,3  f   fx fy fz  , 0 n4,3  n  nx ny nz 
⇒ 3 f4,3  A03 * 0 f4,3

3

cq12 sq12 0 


 ( A30 )T * f =  sq12 cq12 0 
 0
0
1

n4,3  A03 * 0 n4,3

T

cq12 sq12 0 


 ( A30 )T * n =  sq12 cq12 0 
 0
0
1

 fx 
 fxcq12  fysq12
 fy  = 

 
 fxsq12  fycq12 
 fz 


 fz
T

 nx   nxcq12  nysq12 
 ny  =  nxsq12  nycq12 
  

 nz  

nz

Chiếu trong hệ gắn liền khâu i ta có:
i

fi ,i 1  i fi 1,i  mi * i g

i

ni ,i 1  i ni 1,i  S ( i rCii )* mi * i g  S ( i rii1 )* i fi ,i 1

Chuyển i fi ,i 1 , i ni ,i 1 từ hệ (i) sang hệ (i-1) cho bước tính tiếp theo
i 1

i

fi ,i 1  Aii 1 * i fi ,i 1 ,

i 1

ni ,i 1  Aii 1 * i ni ,i 1

g  ( Ai0 )1 * 0 g  ( Ai0 )T * 0 g

13


Triển khai tính toán:
a, Khâu 3, chiếu trong hệ gắn liền khâu 3
cq12 sq12 0 


3
0 T
0
g  ( A3 ) * g =  sq12 cq12 0 
 0
0
1

T

 0  0
 0  = 0
   
  g   g 

rC33 * P3  0

 0 
 0 q3 0 


3 3
r  EC   0   S ( r2 )   q3 0 0 
 q3
 0
0 0

3 3
2



3

3

n3,2

 fxcq12  fysq12
 0   fxcq12  fysq12
f3,2  3 f 4,3  m3 * 3 g =  fxsq12  fycq12  - m3  0  =  fxsq12  fycq12 



  


 g    fz  m3 g 
 fz

 nxcq12  nysq12   0 q3 0   fxcq12  fysq12
 n4,3  S ( r )* f3,2 =  nxsq12  nycq12  -  q3 0 0   fxsq12  fycq12 





  0
0 0    fz  m3 g 
nz
3

3 3
2

3

 nxcq12  nysq12  q3 fxsq12  q3 fycq12 
=  nxsq12  nycq12  q3 fxcq12  q3 fysq12 


nz

b, Khâu 2, chiếu trong hệ gắn liền khâu 2
2

2

2

fxcq12  fysq12 
 fxcq12  fysq12 

 fxsq12  fycq12  =  fxsq12  fycq12 


 

fz  m3 g
  fz  m3 g  

f3,2

1 0 0 
 A * f3,2 = 0 1 0 
0 0 1

n3,2

 nxcq12  nysq12  q3 fxsq12  q3 fycq12 
 A * n3,2 =  nxsq12  nycq12  q3 fxcq12  q3 fysq12 


nz

2
3

2
3

3

3

cq12  sq12 0   0   0 
g  ( A ) * g =  sq12 cq12 0   0  =  0 
 0
0
1    g    g 
0 T
2

0

14


0
 a 2 
0 0



2 2
r  CB   0   S ( r1 )  0 0 a 2 
 0 
0 a 2 0 

2 2
1

0
 l 2
0 0



2 2
r  CC2   0   S ( rC 2 )  0 0 l 2 
 0 
0 l 2 0 

2 2
C2


2

2

 fxcq12  fysq12 
 0   fxcq12  fysq12 


f 2,1  f3,2  m2 * g =  fxsq12  fycq12 - m 2  0  =   fxsq12  fycq12



  


  g   fz  m2 g  m3 g 
fz  m3 g
2

2

n2,1  2 n3,2  S ( 2 rC22 )* m2 * 2 g  S ( 2 r12 )* 2 f 2,1
0   0  0 0
0   fxcq12  fysq12 






0 l 2  m2  0  0 0 a 2    fxsq12  fycq12
 


 0 l 2 0    g  0 a 2 0   fz  m2 g  m3 g 

 nxcq12  nysq12  q3 fxsq12  q3 fycq12  0

= nxsq12  nycq12  q3 fxcq12  q3 fysq12 - 0



nz

0

nxcq12  nysq12  q3 fxsq12  q3 fycq12



=  nxsq12  nycq12  q3 fxcq12  q3 fysq12  a 2 fz  (a 2  l 2)m2 g  a 2m3 g 


nz  a 2 fxsq12  a 2 fycq12

c, Khâu 1, chiếu trong hệ gắn liền khâu 1
1

1

cq 2  sq 2 0   fxcq12  fysq12   fxcq1  fysq1 
f 2,1  A * f 2,1 =  sq 2 cq 2 0    fxsq12  fycq12 =  fxsq1  fycq1 
 



 0
0
1   fz  m2 g  m3 g   fz  m2 g  m3 g 
1
2

2

 nxcq1  nysq1  q3 fxsq1  q3 fycq122  a 2 fzsq 2  (( a2  l 2) m2 g  l 2m3 g ) sq 2 
n2,1  A * n2,1 =  nxsq1  nycq1  q3 fxcq1  q3 fysq122  a 2 fzcq2  (( a2  l 2) m2 g  l 2m3 g )cq2 




nz  a 2 fxsq12  a 2 fycq12
1
2

1

2

cq1  sq1 0   0   0 
g  ( A ) * g =  sq1 cq1 0   0  =  0 

   
 0
0
1    g    g 
0 T
1

0

0
 a1
0 0



1 1
r  BA   0   S ( r0 )  0 0 a1
 0 
0 a1 0 

1 1
0

15


 l1
0 0 0 


1 1
r  BC1   0   S ( rC1 )  0 0 l1
 0 
0 l1 0 

1 1
C1



1

fxcq1  fysq1
 fxcq1  fysq1 

0 






f1,0  f 2,1  m1 * g = fxsq1  fycq1 - m1 0 =
fxsq1  fycq1



  
 fz  m2 g  m3 g 
  g   fz  m1 g  m2 g  m3 g 
1

1

n1,0  1n2,1  S ( 1rC11 )* m1 * 1 g  S ( 1r01 )* 1 f1,0 =

1

 nxcq1  nysq1  q3 fxsq1  q3 fycq122  a 2 fzsq 2  (( a2  l 2) m2 g  l 2m3 g ) sq 2 
 nxsq1  nycq1  q3 fxcq1  q3 fysq122  a 2 fzcq2  (( a2  l 2) m g  l 2m g )cq2 
2
3




nz  a 2 fxsq12  a 2 fycq12
fxcq1  fysq1

0 
0 0 0   0  0 0








- 0 0 l1 m1 0 - 0 0 a1
fxsq1  fycq1


   

0 l1 0    g  0 a1 0   fz  m1 g  m2 g  m3 g 


nxcq1  nysq1  q3 fxsq1  q3 fycq122  a 2 fzsq 2  ((a 2  l 2)m2 g  l 2m3 g ) sq 2




nz  (a1sq1  a 2sq12) fx  (a1cq1  a 2cq12) fy




m
)
g
2
3



= nxsq1  nycq1  q3 fxcq1  q3 fysq122  (a1  a2cq2) fz  ((a2  l 2)m g  l 2m g )cq2  (a1  l1)m g  a1(m
2

3

1

d, Khâu 0, chiếu trong hệ gắn liền khâu 0


0

f1,0



fxcq1  fysq1
cq1  sq1 0  
 
fxcq12 

 A * f1,0 =  sq1 cq1 0 


=
fxsq1  fycq1


  fxsq12  fy 

 0
0
1   fz  m1 g  m2 g  m3 g  
3
0
1

1

 fz   mi g 


i 1

0

n1,0  A10 * 1n1,0 =

 nx  q3 fycq1122  (a1sq1  a 2sq12) fz  ((a 2  l 2)m2 g  l 2m3 g ) sq12  ((a1  l1)m1 g  a1(m2  m3 ) g ) sq1
 ny  q3 fxsq1122  (a1cq1  a 2cq12) fz  ((a 2  l 2)m g  l 2m g )cq12  ((a1  l1)m g  a1(m  m ) g )cq1
2
3
1
2
3




nz  (a1sq1  a 2sq12) fx  (a1cq1  a 2cq12) fy

rC00 * P0  0
0
 0  d1 0 


0 0
r  OA   0   S ( r1 )   d1 0 0 
 d1
 0
0 0 

0 0
1

16




0

0

f0  0 f1,0




0 
 fxcq12 
 fxcq12 
 

 - m0 0 = 
 m0 * 0 g = 
   fxsq12  fy 
fxsq
12

fy


  g  



3
3
 fz   mi g 
 fz   mi g 


i 0
i 1



n0  0 n1,0  S ( 0 r01 )* 0 f0 =

 nx  q3 fycq1122  (a1sq1  a 2sq12) fz  ((a 2  l 2)m2 g  l 2m3 g ) sq12  ((a1  l1)m1 g  a1(m2  m3 ) g ) sq1
 ny  q3 fxsq1122  (a1cq1  a 2cq12) fz  ((a 2  l 2)m g  l 2m g )cq12  ((a1  l1)m g  a1(m  m ) g )cq1
2
3
1
2
3




nz  (a1sq1  a 2sq12) fx  (a1cq1  a 2cq12) fy
0

-  d1

 0


 d1 0  
 fxcq12 

=
0 0 
 fxsq12  fy 
0 0  

3
 fz   mi g 


i 0

 nx  d1sq12 fx  (d1  q3cq1122) fy  (a1sq1  a 2sq12) fz  ((a 2  l 2)m2 g  l 2m3 g ) sq12  ((a1  l1)m1 g  a1(m2  m3 ) g )sq1
 ny  (d1cq12  q3sq1122) fx  (a1cq1  a 2cq12) fz  ((a 2  l 2)m g  l 2m g )cq12  ((a1  l1)m g  a1(m  m ) g )cq1 
2
3
1
2
3




nz  (a1sq1  a 2sq12) fx  (a1cq1  a 2cq12) fy

17


12. Thiết kế quỹ đạo chuyển động
Cho điều kiện về vị trí, vận tốc của 2 điểm trong không gian thao tác, quỹ đạo
thẳng. Thiết kế quỹ đạo chuyển động theo đa thức bậc 3.

Cho kích thước: 𝑑1 = 10, 𝑎1 = 7, 𝑎2 = 7
Tọa độ điểm tác động cuối: 𝐸1 (14, 0, 5), 𝐸2 (10, 5, 7)
Vận tốc tại điểm tác động cuối:
𝑥𝐸̇ 2 = 0.00, 𝑦𝐸̇ 2 = 0.00, 𝑥𝐸̇ 2 = 0.00; 𝑥𝐸̇ 2 = 0.50, 𝑦𝐸̇ 2 = 0.00, 𝑥𝐸̇ 2 = 0.87

Thời gian chuyển động: 𝑡 = 2 𝑠, 𝑡𝑖 = 0
a, Xác định luật chuyển động s(t ) bằng đa thức bậc 3: s(t )  a3t 3  a2t 2  a1t  a0
Các điều kiện:
s(ti )  s(0)  OE1  142  02  52  14.87

s(t f )  s(2)  OE2  102  52  72  13.19
̇ = 0, 𝑠(𝑡̇𝑓 ) = 𝑠(2)
̇ = √0.502 + 0.002 + 0.872 = 1.00
𝑠(𝑡̇ 𝑖 ) = 𝑠(0)
18


Hệ phương trình xác định các hệ số:
1
0

1

0

0   a0  14.87 
1 0 0   a1   0 

2 4 8   a2  13.19 
  

1 4 12   a3   1.00 
0 0

⇒ 𝑎0 = 14.87, 𝑎1 = 0, 𝑎2 = −1.76, 𝑎3 = 0.67
⇒ s(t )  0.67t 3 1.76t 2  14.87
Vẽ đồ thị:

b, Xác định hàm di chuyển tọa độ điểm tác động cuối:
 x(t): x(t )  b3t 3  b2t 2  b1t  b0
Các điều kiện:
x(ti )  x(0)  14 , x(t f )  x(2)  10
̇ = 0, 𝑥(𝑡̇ 𝑓 ) = 𝑥(2)
̇ = 0.50
𝑥(𝑡̇ 𝑖 ) = 𝑥(0)
19


Hệ phương trình xác định các hệ số:
1
0

1

0

0  b0   14 
1 0 0   b1   0 

2 4 8  b2   10 
  

1 4 12  b3  0.50 
0 0

⇒ 𝑏0 = 14, 𝑏1 = 0, 𝑏2 = −3.25, 𝑏3 = 1.13
⇒ x(t )  1.13t 3  3.25t 2  14
⇒ x '(t )  3.39t 2  6.5t

 y(t): y(t )  c3t 3  c2t 2  c1t  c0
Các điều kiện:
y(ti )  y(0)  0 , y(t f )  y(2)  5
̇ = 0, 𝑦(𝑡̇ 𝑓 ) = 𝑦(2)
̇ =0
𝑦(𝑡̇ 𝑖 ) = 𝑦(0)

Hệ phương trình xác định các hệ số:
1
0

1

0

0  c0  0 
1 0 0   c1  0 

2 4 8  c2  5 
   
1 4 12   c3  0 
0 0

⇒ 𝑐0 = 0, 𝑐1 = 0, 𝑐2 = 3.75, 𝑐3 = −1.25
⇒ y(t )  1.25t 3  3.75t 2
⇒ y '(t )  3.75t 2  7.5t

 z(t): z(t )  d3t 3  d2t 2  d1t  d0
Các điều kiện:
z (ti )  z (0)  5 , z (t f )  z (2)  7
̇ = 0, 𝑧(𝑡̇𝑓 ) = 𝑧(2)
̇ = 0.87
𝑧(𝑡̇ 𝑖 ) = 𝑧(0)

20


Hệ phương trình xác định các hệ số:
1
0

1

0

0   d0   5 
1 0 0   d1   0 

2 4 8  d2   7 
  

1 4 12   d3  0.87 
0 0

⇒ 𝑑0 = 5, 𝑑1 = 0, 𝑑2 = 1.07, 𝑑3 = −0.28
⇒ z(t )  0.28t 3  1.07t 2  5
⇒ z '(t )  0.84t 2  2.14t

c, Giải bài toán ngược xác định các tọa độ khớp:
q3  z(t )  zE1  0.28t 3  1.07t 2

q2 = ±2𝑎𝑡𝑎𝑛2

(a1  a 2) 2  ( x 2  y 2 )
( x 2  y 2 )  (a1  a 2) 2

196  ((1.13t 3  3.25t 2  14)2  (1.25t 3  3.75t 2 )2 )
= ±2𝑎𝑡𝑎𝑛2
((1.13t 3  3.25t 2  14)2  (1.25t 3  3.75t 2 )2 )
𝑞1 = 𝑎𝑡𝑎𝑛2

x(a1  a 2cq 2)  ya 2sq 2
y (a1  a 2cq 2)  xa 2sq 2

= 𝑎𝑡𝑎𝑛2

(1.13t 3  3.25t 2  14))(1  cq 2)  (1.25t 3  3.75t 2 )sq 2
(1.25t 3  3.75t 2 )(1  cq 2)  (1.13t 3  3.25t 2  14)sq 2

d, Vẽ cấu hình tay máy theo thời gian chuyển động:

21


13. Khảo sát động lực học
a, Tính các ma trận Jacobi tịnh tiến, ma trận Jacobi quay của các khâu robot
Khâu 1:
l1cq1
 l1sq1
r 


rC1  l1sq1 ⇒ J T 1 
  l1cq1 
q
 d1 
 0 
0 
0
w  
 0
𝑤1 = [ 0 ] ⇒ J R1 
q '  
𝑞1̇
1 

Khâu 2:
 a1cq1  l 2cq12 
 a1sq1  l 2sq12 l 2sq12 
r 


rC 2   a1sq1  l 2sq12  ⇒ JT 2 
  a1cq1  l 2cq12 l 2cq12 
q



0
d1
0 

0 0 
0
w  
 00
𝑤2 = [ 0 ] ⇒ J R 2 
q '  
̇
𝑞12
1 1 

Khâu 3:
 a1cq1  a 2cq12 
 a1sq1  a 2sq12 a 2sq12 0 
r 


  a1cq1  a 2cq12 a 2cq12 0 
rC 3   a1sq1  a 2sq12  ⇒ JT 3 
q

 q3  d1 
0
0
1

0 0 0 
0
w 
 0 0 0 
𝑤3 = [ 0 ] ⇒ J R 3 
q '
̇
𝑞12
1 1 0 

b, Tính tensor quán tính của các khâu robot
Coi khâu 0 có dạng trụ tròn, kích thước 𝑅0 , 𝐿0
̅̅̅̅
Các khâu 1,2,3 có dạng khối hộp chữ nhật, kích thước 𝐿𝑖 ∗ 𝑊𝑖 ∗ 𝐻𝑖 , 𝑖 = 1,3

22


Khâu 0:
I xx  I 0 x 

1
m0 (3R0 2  L0 2 )
12

I yy  I 0 y 

1
m0 (3R0 2  L0 2 )
12

1
I zz  I 0 z  m0 R0 2
2

I xy  I yz  I zx  0

⇒I

(0)
C0

 I0 x
  0
 0

0
I0 y
0

1
2
2
0
12 m0 (3R0  L0 )
0 
1
0   
0
m0 (3R0 2  L0 2 )

12
I 0 z  

0
0


Khâu i, 𝑖 = ̅̅̅̅
1,3
1
I xx  I ix  mi ( Li 2  H i 2 )
2
1
I yy  I iy  mi (Wi 2  H i 2 )
2
23




0 


1
m0 R0 2 

2
0


1
I zz  I iz  mi ( Li 2  Wi 2 )
2

I xy  I yz  I zx  0

⇒ I Ci(i )

 I ix
  0
 0

0
I iy
0

1
2
2
 2 mi ( Li  H i )
0 
0   
0

I iz  

0


0
1
mi (Wi 2  H i 2 )
2
0





0


1
mi ( Li 2  Wi 2 ) 

2
0

c, Tính động năng, thế năng. Viết PTVPCĐ
Thế năng của hệ:
3

P   mi * g0T * rCi   g (m0l 0  m1d1  m2 d 2  m3 (d1  q3  l 3))
i 0

 0 
T
3
 P 
T
(0)
⇒ g      mi * JTi * g   0 
i 0
 q 
 m3 g 

Ma trận khối lượng:
3

T
M (q)   ( JTkT mk JTk  J Rk
Ak0 I Ci(i ) Ak0T J Rk )
k 1

 JTT1m1 JT 1  J RT1 A10 I C(1)1 A10T J R1 + JTT2 m2 JT 2  J RT 2 A20 IC(2)2 A20T J R 2 + JTT3m3 JT 3  J RT3 A30 IC(2)3 A30T J R3
 a12  l 22  2a1l 2cq 2 l 22  a1l 2cq2   I 2 z I 2 z 
 m1l12  I1z + m2 

+
2
l 22
 l 2  a1l 2cq 2
 I2z I2z 
 a12  a 22  2a1a 2cq 2 a 22  a1a 2cq 2 0   I 3 z I 3 z 0 


m3  a 22  a1a 2cq 2
a 22
0    I 3 z I 3 z 0  =

0
0
1   0
0 0 

 I1z  I 2 z  I 3 z  m1l12  m2 (a12  l 22  2a1l 2cq 2)  m3 (a12  a 22  2a1a 2cq 2) I 2 z  I 3 z  m2 (l 22  a1l 2cq 2)  m3 (a 2 2  a1a 2cq 2) 0 


I 2 z  I 3 z  m2 (l 22  a1l 2cq 2)  m3 (a 22  a1a 2cq 2)
I 2 z  I 3 z  m2l 22  m3a 22
0


0
0
m3 


24


Tài liệu bạn tìm kiếm đã sẵn sàng tải về

Tải bản đầy đủ ngay

×