Chương IV.

Động lực học Robot

(Dynamics of Robots)

d3/F3 5/M5

4/M4 6/M6
d2/F2

M  M 1 , F2 , F3 , M 4 , M 5 , M 6 
T
1/M1
Q  1 , d 2 , d 3 ,  4 ,  5 ,  6 
T

Hệ phương trình động lực học của Robot nDOF (Dynamic equation
system
M  f (Q, Q , Q
 )
Các phương pháp thiết lập phương trình động lực học Robot

-Phương pháp Newton-Euler

-Phương pháp Euler- Lagrange
I. Phương pháp Euler- Lagrange

Động năng K
Năng lượng của hệ thống Robot nDOF

Thế năng P
I. Phương pháp Euler- Lagrange

Động năng K
Năng lượng của hệ thống Robot nDOF

Thế năng P

+ Động năng K

n
K   Ki
i 1

+ Thế năng P

n
P   Pi
i 1
1. Động năng của thanh nối i
1. Động năng của thanh nối i mi: khối lượng thanh nối i
Ci: trọng tâm của thanh nối i
0
pci: vị trí Ci trong {0}
pci  xci , yci , zci 
0 T

Ci
Ii: mô men quán tính của thanh nối tại Ci
 I xx I xy I xz 
Ci  
I i   I yx I yy I yz 
 I zx I zy I zz 

O
Tốc độ dài của Ci trong {0}
p ci  x ci , y ci , z ci 
0 T

Tốc độ quay
ci   xi ,  yi ,  zi 
0 T

Động năng của thanh nối i

1 1
K i  ( 0 p ci )T mi ( 0 p ci )  iT I ii
2 2
2. Thế năng của thanh nối i

với

T
Pi   mi G 0pci

G  0,0,1 9,8
T

O
3. Hàm Lagrange

LK P
4. Thiết lập phương trình động lực học

+ Nếu i là khớp quay (i), mô men đặt trên trục khớp i

 L L
i  

t  i  i

+ Nếu i là khớp trượt (ri), lực đặt trên trục khớp Fi

d L L
Fi  
dt ri ri
II. Thiết lập hệ động lực học cho Robot 2DOF
Ví dụ: Thiết lập phương trình động lực học của Robot -r
a. Động năng thanh nối 1
 a. Động năng thanh nối 1

1
K1  [ p cT1m1 p c1  1T ( c1I1 )1 ]
2
 xc1   Lc   x c1   Ls 
pc1        p c1   y    
y
 c1   Ls    c1   Lc 

 x 
p cT1m1 p c1  m1 x c1 y c1  c1   m1 ( x c21  y c21 )  L2 s 2 2  L2 c 2 2  m1 L2 2
 y c1 
 I xx1 0 0 
c1
I1   0 I yy1 0 
 I xx1 0 0  0
 0 0 I zz1 
 
1T I11  0 0   0 I yy1 0   0   I zz1 2

1  0 0  T
  0 0 I zz1   

1 1 1
K1  m1[ xc1  yc1 ]  I zz1  m1 L2  I zz1  2
2
 2
 2

2
 2

2
 
b. Động năng thanh nối 2
1
K 2  [ p cT2 m2 p c 2  2T ( c 2I 2 )2 ]
2

 xc 2  rc    x c 2   rs  rc 

pc 2        p c 2      
y
 c2   rs   y
 c2   rc   
r s  
b. Động năng thanh nối 2
1
K 2  [ p cT2 m2 p c 2  2T ( c 2I 2 )2 ]
2
 xc 2  rc    x c 2   rs  rc 
pc 2        p c 2      
 yc 2   rs   
 yc 2   rc   rs 
p cT2 m2 p c 2  m2 ( x c22  y c22 )  m2 (r 2 s 2 2  2rrsc  r 2 c 2
 r 2 c 2 2  2rrsc  r 2 s 2 )
 m2 (r 2 2  r 2 )

c2
 I xx 2
I 2   0
0
I yy 2
0 
0 

2  0 0  
T T c2
2 2
1
 ( I )2  I zz 2 2
2
 0 0 I zz 2 

1 1 1 1
2
2 2

2
 2

2
2
 
K 2  m2 [ x c 2  y c 2 ]  I zz 2  m2 r  I zz 2   m2 r 2
2

2

 c.Thế năng thanh nối 1 G=[0,-1]T

P1  m1G T pc1
 Lc 
  m1 0  1g    m1 Lsg
 Ls 
 d. Thế năng thanh nối 2

P2   m2G T pc 2
rc  
  m2 0  1g    m2 rsg
 rs 
e. Tổng động năng K

1 1 2
2
 2
 2 2
K  K1  K 2  m1 L  I zz1  I zz 2 m2 r   mr
2
e. Tổng động năng K

1 1
 2 2

K  K1  K 2  m1 L  I zz1  I zz 2 m2 r   m2 r 2
2
2

g. Tổng thế năng P

P  P1  P2  (m1 L  m2 r ) sg

h. Hàm Lagrange L

1 1
L  K  P  m1 L  I zz1  I zz 2 m2 r   m2 r 2  m1 L  m2 r sg
2 2 2

2 2
i. Mô men đặt lên khớp thứ nhất

d L L
1  

dt  

1 1 2
2
2
 2 2

L  K  P  m1 L  I zz1  I zz 2 m2 r   mr  m1 L  m2 r sg
2

L
  m1 L2
 I zz1  I zz 2 m2 r 
2 


d L
dt 
 m1 L2
 I zz1  I zz 2 m2 r 
2 
  2 m2 r 
r 

dL
 m1 L  m2 r cg
d
i. Mô men đặt lên khớp thứ nhất

d L L
1  

dt  

L
 m1 L2  I zz1  I zz 2 m2 r 2 


d L
 m1 L2  I zz1  I zz 2 m2 r 2   2m2 rr
dt 

dL
 m1 L  m2 r cg
d

 1  m1 L2  I zz1  I zz 2 m2 r 2   2m2 rr  m1 L  m2 r cg

k. Lực đặt lên khớp thứ hai

 L L
F2  
t r r
1 1 2
2
2

L  K  P  m1 L  I zz1  I zz 2 m2 r   mr  m1 L  m2 r sg
2 2

L
 m2 r
r
d L
 m2 r
dt r
L  2
 m2 r  m2 sg
r
L
F2   m2 r  m2 r 2  m2 sg
r
 lII. Dạng tổng quát của phương trình động lực học
Hệ phương trình động lực học Robot -r

 1  m1 L2  I zz1  I zz 2 m2 r 2   2m2 rr  m1 L  m2 r cg


 F2  m2 r  m2 r 2  m2 sg

Dạng ma trận

  1  m1 L2  I zz1  I zz 2 m2 r 2 0   2m2 r    0   2 m1 L  m2 r c 

         r      g
 F2   0 m2   r   0   m2 r   m2 s 

T/p quán tính T/p T/p ly T/p trọng

tương tâm trường
hỗ
Ma trận quán tính H(Q) G (Q)
V (Q, Q )

Dạng tổng quát   V (Q, Q )  G (Q)

M  H (Q)Q
Tương tự, thiết lập hệ phương trình động lực học của Robot Planar

C2 2

l1

lc2
C1
1
x
l c1

  1  (m1  m2 )a12  m2 a22  2m2 a1a2 c 2 m2 a22  m2 a1a2 c 2  1 

       

 2  m a
2 2
2
 m a
2 1 2a c 2 m a
2 2
2
  2 
 m 2 a1a2 (212  22 ) s 2  m1  m2 a1c1  m2 a2 c12 
   g
 2
m2 a1a21 s 2 m2 a2 c12
   
Thiết lập hệ phương trình động lực học của Robot hình trụ

Q   , h, r 
T

 1   J  m2 r
2
0 0    2m2 rr   0 
F    0    
 2  (m1  m2 ) 0   h    0   (m1  m2 ) gh
 F3   0 0 m2   r   m2 r 2   0 
 III. Tính chất của hệ phương trình động lực

1. Các tính chất của ma trận quán tính H(Q)

-Ma trận vuông cấp n

-Đối xứng:
hij  h ji (i  j )

a T H (Q)b  bT H (Q)a

-Xác định dương

-H(Q) bị chặn trên và dưới

M 1  H (Q)  M 2
2. Tính chất của vector tương hỗ + ly tâm V(Q,Q’)
Biểu diễn khác của V

1 
V (Q, Q )   H (Q)  S (Q, Q )Q  C (Q, Q )Q
2 
S: ma trận nghiêng đối S  ST  0 sij  s ji  0
a T Sa  0

Tính ma trận C ( công thức Christoffel)

1  hkj hki hij 

cijk (Q)     
2  qi q j qk 

n
ckj   cijk (Q)q i
i 1
Ví dụ: Động lực học Robot Planar 2DOF có bổ sung thêm
thành phần ma sát
V. Mô phỏng động lực học

Phương trình động lực học   V (Q, Q )  G (Q)

M  H (Q)Q
Mô phỏng chuyển động

  H (Q) 1 M  V (Q, Q )  G (Q)
Q 
Q   Q
 dt Q   Q dt


Q Q
 
Các phần mềm mô phỏng thông dụng : - MatLab
- Simnon

Mô phỏng trên MatLab

-Sử dụng Simulink để tạo sơ đồ khối tổng quát

-Các hàm tính toán sử dụng M-funtion hoặc S-function