Robotics 9.2 Dynamics - Serial Robot - Lagrange 2-4s

29/03/2023
-2-
Đặt vấn đề
ĐỘNG LỰC HỌC ROBOT
Phương trình Lagrange loại 2

Nguyễn Quang Hoàng
Email: hoang.nguyenquang@hust.edu.vn
Nguyen Quang Hoang - Department of Applied Mechanics Nguyen Quang Hoang - Department of Applied Mechanics
1 2
-3- -4-
Phương trình Lagrange loại 2 Phương trình Lagrange loại 2 – Ví dụ
Ví dụ 1: Tay máy 1 dof y
Phương trình Lagrange loại 2 cho hệ nhiều vật hô lô nôm O Tính các đạo hàm
Động năng và thế năng hệ: a1=OC, IO
u T
d T T   1 1  0,
  Qi     Q ikt T  I  2  I O  2


dt  qi qi qi  qi 2 O 2 C
T d T
x  I O  ,  I O 
  mga 1 cos  mg   dt  

 mga 1 sin 
Lực suy rộng không thế 
d T T   Q kt  u
    Qi  ui phương trình vi phân chuyển động
dt  qi qi  qi qi d T T 
  Qi    Q ikt d T T 
   Q kt
dt  qi q i qi dt    
 I O   mga 1 sin   u
3 4
29/03/2023
-5- -6-
Phương trình Lagrange loại 2 – Ví dụ Phương trình Lagrange loại 2 – Ví dụ
Sử dụng PT Lagrange 2 thiết lập PT Ví dụ 2: tay máy TR, cho m1, m2, a2, I2z, u1, u2.
động lực cho tay máy 1 bậc tự do Lập PTVP CĐ cho tay máy. B
m2, I2z
chuyển động trong mặt phẳng đứng. y0 g
x d a2 C2
Biết các thông số: I 1 , r I 2,O , R, m , g A 
u1 x0
O C1
1 1 1 m1 u2
T  m 1vC2 1  m 2 vC2 2  I 2 z  22
2 2 2
5r C2
2 vC 1  x ,
g d T T 
M
vC2 2  xC2 2  yC2 2 ,  2     Qi    Q ikt , i  1, 2
  dt  qi q i qi
A O
x C 2  x  d  a 2 cos   xC 2  x   a 2 sin 
1 yC 2  a 2 sin   yC 2   a 2 cos 
r R
vC2 2  x 2  2a 2 x  sin   a 22  2 1 1
T  (m  m 2 )x 2  m 2a 2 x  sin   I 2 z  m 2a 22  2
2 1 2
 
5 6
-7- -8-
1 1
T 
1 1

(m  m 2 )x 2  m 2a 2 x  sin   I 2 z  m 2a 22  2
2 1 2

m2, I2z
B T  (m  m 2 )x 2  m 2a 2x  sin   I 2 z  m 2a 22  2
2 1 2
 
y0 g
  m 2 gyC 2  m 2 ga 2 sin 
x d a2 C2   m 2 gyC 2  m 2 ga 2 sin 
A
 A  u 1  x  u 2   Q1kt  u 1 , Q 2kt  u 2
O
u1 C1
 x0 Q 1kt  u 1 , Q 2kt  u 2
Tính các đạo hàm
m1 u2
T T
 T 
 m 2a 2 x  cos ,
 

 m 2a 2 x sin   I 2 z  m 2a 22  
 0,  0,
x x d T T  d T
T dt  qi

q i
 Qi  
q i
 Q ikt , i  1, 2
dt  
 
 m 2a 2 x sin   m 2a 2 x cos   I 2 z  m 2a 22 
 (m 1  m 2 )x  m 2a 2 sin ,
 x

d T  m 2 ga 2 cos 
 (m 1  m 2 )x  m 2a 2 sin   m 2a 2 2 cos  
dt  x
Phương trình VPCĐ đối với tọa độ 
Phương trình VPCĐ đối với tọa độ x
(m 1  m 2 )x  m 2a 2  sin   m 2a 2  2 cos   u 1  

m 2a 2 x sin   I 2 z  m 2a 22   m 2 ga 2 cos   u 2
7 8
29/03/2023
-9- -10-
(m 1  m 2 )x  m 2a 2 sin  B

Ví dụ 3: Tay máy RR 2DOF,
C2
cho m1, a1 = OC1, IO, m2, a2= AC2, I2z, u1, u2, g. y
m2, I2z B
2
m 2a 2 cos   u 1 y0 g Lập PTVP CĐ cho tay máy. A q2
x d a2 C2

m 2a 2 x sin   I 2 z  m 2a 22   O
u1 C1
A  x0 T 
1
2 O1 1
1
2
1
I  2  m 2vC2 2  I C 2  22
2
C1 u2
u1
m 2 ga 2 cos   u 2 m1 u2
 1  q1 , q1
O
x
vC2 2  xC2 2  yC2 2 ,  2  q2
Phương trình VPCĐ ở dạng ma trận q1  x , q2  
 m m
xC 2  l1 cos q 1  a 2 cos q 2  xC 2  l1q1 sin q 1  a 2q2 sin q 2 d T T 
 m 2a 2 sin q 2  q1  m 2a 2q22 cos q 2  u    Qi    Qikt ,
1 2     1  yC 2  l1 sin q 1  a 2 sin q 2  yC 2  l1q1 cos q 1  a 2q2 cos q 2 dt qi qi qi
m a sin q I 2 z  m 2a 22  q2   m 2ga 2 cos q 2  u
 2 2 2      2  i  1,2
vC2 2  (l1q1 )2  2l1a 2q1q2 cos(q 1  q 2 )  a 22q2 2
M(q )q  h(q, q )  u T 

1 1

(I  m 2l 12 )q12  m 2l1a 2q1q2 cos(q 1  q 2 )  I C 2  m 2a 22 q2 2 
2 O1 2
9 10
-11- -12-
Phương trình Lagrange loại 2 – Ví dụ Bài tập
  m 1ga 1 sin q 1  m 2 g (l1 sin q 1  a 2 sin q 2 ) C2 Sử dụng PT Lagrange 2 thiết lập PT động C2
y y
 B
lực cho tay máy 2 bậc tự do chuyển động q2 B
 A  u 1q 1  u 2 , A q2 trong mặt phẳng đứng. u2
q1  q2      q1  q2  A  u 1q 1  u 2 (q 2  q 1 ) C1 u2 Khâu 1: m1, L1, IC1, OC1 = lC1. A

C1
  q 1  q 2
u1 Khâu 2: m2, L2, IC2, AC2 = lC2.
 A  (u 1  u 2 )q 1  u 2 q 2 q1 u1
O
x
Phương trình VPCĐ  A  u 1 q 1  u 2 q 2 q1
O
x
I  m a 2  m l 2 m 2a 2l 1 cos(q 2 1 ) q1  m 2a 2l 1 sin(q 2 1 )q22  (m 1a 1  m 2l 1 )g cos q 1  u  u  Q 1kt  u 1, Q 2kt  u 2
 C1 1 1 2 1
   1 2
 m a l cos(q ) I C 2  m 2a 22  q2   m 2a 2l 1 sin(q 2 1 )q12   m 2a 2 g cos q 2   u 
 2 2 1 2 1        2 
u   0   u 1 
 1   1  Bu
u  0 1  u 2 
u  u   1  1  u    
M(q )q  h(q, q )  Bu  
2
 1 2    1
 u    0 1  u   Bu
 2     2 
11 12
29/03/2023
-13- -14-
Phương trình Lagrange loại 2 dạng ma trận
Xét hệ nhiều vật f dof với các liên kết giữ, dừng
Véc tơ tọa độ suy rộng q  [q 1 , q 2 ,..., q f ]T

Phương trình Lagrange Vị trí khối tâm, ma trận cosin chỉ hướng của các khâu :
loại 2 dạng ma trận rCk  rCk (q ), R k  R k (q ) 

g
Vận tốc khối tâm vật k
(0) d
T
vCk  r (q )  JTk q
d   T    T T   T dt Ck
         Q kt
dt   q    q    q  ma trận Jacobi tịnh tiến:  x
 Ck  x Ck  x Ck 
 q 
 1
q 2  q f 
  y  yCk  yCk 
JTk  rCk (q , t )   Ck 
 q f 
M(q )q  C(q, q )q  g(q )  Q kt q  q1
 z
q 2
 Ck  z Ck  z Ck 
 q 
 q 2  q f 
1 
13 14
-15- -16-
Phương trình Lagrange loại 2 dạng ma trận Phương trình Lagrange loại 2 dạng ma trận
ta đưa vào ký hiệu véc tơ quay một cách hình thức Cách tính ma trận Jacobi quay
 k   k (q ) d
R k (q )  R (q ),  k(0)  R k (q )R Tk (q )

dt k
Vận tốc góc vật k
 k(0) 
d
 (q )  J Rk q    k(0)  S 1 (
 k(0) )  J Rk q 
g g
dt k
    kx   kx 
ma trận Jacobi quay  kx 
  q  q2  q f 
   1
 kx      kx   
 kx   kx
  kx  kx
  (0)    ky   ky 
 q q2  q f    q  q2  q f  J Rk   (q )   ky

 
 q k   q1  q2  q f 
1 1
  k ( q, t )     ky  ky    k ( q, t )    ky   ky  ky 
       kz 
ky
J Rk           kz
q   q1 q2 q f   q   q1  q2  q f   kz 
      
  kz   kz  kz     kz  kz   kz  q1  q2  q f 
       
 q1 q2 q f    q1  q2  q f 
  
15 16
29/03/2023
-17- -18-
(0)
Biểu thức động năng của hệ vCk  JTk q Biểu thức động năng của hệ
1 N
     k(0)  J Rk q 1
T  q T M(q )q
T  
 v  m k vCk
2 k 1 Ck
  k  lCk  2
(0)
ICk (k )
 R k0 ICk R k0T
N 1 f f

1

 v (0)T  m k vCk(0)  k(0)T ICk(0) k(0)     m ij (q )qi q j
2 k 1 Ck 2 i  1 j 1 
g
N
1 T 1

2
q  JTkT

m k JTk  JTRk R k0 ICk
(k )

R k0T J Rk q  q T M(q )q
2
k 1 Biểu thức thế năng của hệ do trọng lực
 
 i (q )  m i g  rCi (q )  m i g (0)T rCi(0) (q )
Ma trận khối lượng suy rộng n
 
 (q )    i (q )   m i g  rCi (q )
N g i 1 T
M (q )   J T
(k )

R k0T J Rk  MT (q )   
g(q)   
Tk n
k 1   m i g (0)T (0)
r (q )
Ci  q 
i 1
17 18
-19- -20-
Tính lực suy rộng của các lực không thế: uk uk
Các mômen/lực dẫn động, Cản nhớt qk Động năng, thế năng và lực suy rộng qk
f f  1 T 
A    q
i 1
i i
  qT  A    q d i
  qT  d F T 
2
q M(q )q    (q ) F
i 1
E E
  qT Bu   qT Dq Q kt  Bu  Dq  JTE F(0)
q1 q1
u1  u1 
g g
Lực tác dụng tại điểm E bất kỳ Phương trình Lagrange loại 2
    r 
 A(F )  F  rE ( q )  F (0)T  rE  F(0)T  E  q 
  q  d   T 
T
  T T   T
       Bu  Dq     JTE (q )  F (0)
dt   q    q    q 
 F (0)T  J E  q    J E  q  F(0)
T
  qT JTE F(0)   qT Q F*
Q kt  Bu  Dq  JTE F(0) M(q )q  C(q, q )q  Dq  g(q )  Bu  JTE (q )F (0)
T T
    
(0) hình chiếu lực F trong hệ cố định g(q )    ,  (q )q    T 
C(q , q )q  M
  A   q T (Bu  Dq  JTE F (0) ) F   q    q 
19 20
29/03/2023
-21- -22-
Phương trình Lagrange loại 2 Như vậy phương trình vi phân thứ i của hệ sẽ là
  T    T      T    T    
d        d
       
kt
  q     q      q   Q k , k  1, 2,..., f  kt
dt  k  k  k dt   qi    q i    q   Q i , i  1, 2, ..., f
 i
f f f
1 T 1 f f
T 
2
q M(q )q  
2 j 1
m jk
(q )q j qk    (q ) m ij
(q )qj    [ q m ij (q ) 
k
1 
2 qi
m jk (q )] qk q j  g i ( q )  Q ikt
k 1 j 1 j  1 k 1

Đưa vào ký hiệu
f f
T f f  m jk (q ) bi (q, q )   [ 
 qk
m ij (q )  1 
2  qi
m jk (q )] qk q j
qi
 1
2  [ q i
]q j qk j 1 k 1
j 1 k 1 
gi   , g  [g 1 , g 2 , ..., g n ]T Chú ý đến tính chất đối xứng của ma trận khối lượng m ij (q )  m ji (q )
T f qi
 qi
 m ij
(q )q j ,
j 1  m ij (q ) 1  m ij (q ) 1  m ji (q )
 m ij (q )
m ij (q )  m ji (q )  12 [ m ij (q )  m ji (q )]   
d T f f f f f
q k 2 q k 2 qk
dt  qi
 m ij
(q )qj   [ dtd m ij (q )]q j  m ij
(q )qj    [
q k
qk ] q j
j 1 j 1 j 1 j 1 k 1
21 22
-23- -24-
Biến đổi số hạng
1 f f  m  m ik  m jk  f f

f f m
 1  m  1 f f  m
 m  m  bi ( q, q )      q ij
   q q 
 q i  k j
 c qk q j
    q ij  2 q jk  qk q j  2    q ij   q ji  q jk  qk q j
ijk
bi ( q, q )  2 j 1 k 1  k
q j j 1 k 1
j 1 k 1  k i  j 1 k 1  k k i 
trong đó ta định nghĩa
1   m ij 
Bằng cách thay đổi chỉ số chạy trong tổng sigma kép như sau:
 m ik  m jk
c ijk     
f f   m ( q ) 

f  
  m (q ) 
f f f  
  m (q )  2   q k q j q i 
   qji  qk q j     kiq  q j qk     ikq  qk q j
j  1 k 1   j 1 k 1   j 1 k 1  
k j j số hạng này có tên gọi là Christoffel symbols ở dạng thứ nhất.
Véc tơ
1 f f  m  m ji  m jk  T f f
b  b1 b2  b f  ,
bi (q, q ) 
2
    q ij

q k
  q q
 q i  k j
 
bi (q, q )   c
j 1 k 1
q q j
ijk k
 b  C(q, q )q
j 1 k 1  k
1 f f  m  m ik  m jk  f f f

    q  q q   c c
ij
   qk q j C(q, q )  { C ij (q, q )}, C ij (q, q )  qk
2 j 1 k 1  k
q j  q i  k j j 1 k 1
ijk
k 1
ijk
23 24
29/03/2023
-25- -26-
Tóm lại: 1 T 1 f f uk Tinh ma tran C tu ma tran khoi luong M(q)
T  q M(q )q   m (q )q j qk qk
2 2 j 1 k 1
jk Christoffel:=proc(M::matrix, q::vector, qd::vector, n::integer)::matrix;

   (q ) Q kt  Bu  Dq  JTE F(0) F local CC, i,j,k;
CC:=matrix(n,n,0);
E
for i from 1 to n do
Phương trình Lagrange loại 2 q1
u1  for j from 1 to n do
  T T   T T   T g
d  for k from 1 to n do
     Bu  Dq     JTE (q )  F ( 0)
dt   q    q    q  CC[i,j]:=CC[i,j]+
f
1/2*(diff(M[i,j],q[k])+diff(M[i,k],q[j])-diff(M[j,k],q[i]))*qd[k];
C ij (q , q )  c ijk
qk
od
k 1
T (0) od
M (q )q  C( q, q )q  Dq  g(q )  Bu  J (q )F E od;
RETURN(evalm(CC));
  
T
 
T 1   m ij  m ik  m jk 

g(q )    ,  ( q )q    T  cijk     end proc:
C(q, q )q  M 2   q k q j  q i 
  q    q  mtC:=Christoffel(M, q, qdot, n));
if M(q )  const  C(q, q )  0
25 26
-27- -28-
Phương trình Lagrange loại 2 dạng ma trận Tính chất của các ma trận M và C
uk Ma trận khối lượng đối xứng, xác định dương uk
  qk qk
 i (q )  m i g  rCi (q )  m i g (0)T rCi(0) (q ) T T
 M(q )  M (q )  0, V(s)  1
2
s M(q )s  0,  s  0 
n F F
 
(q )    i (q )   m i g  rCi (q ) N
i 1
q1
E M(q )   J
k 1
T
Tk
(k )

R k0T J Rk  MT (q )
q1
E
n
 
   m i g (0)T rCi(0) (q )
u1 u1
g g
i 1 Ma trận sau đây phản đối xứng (skew-symmetry)
Lực suy rộng do trọng lực g(q) từ các jacobi tịnh tiến JT
 (q )  2C(q, q )  NT (q, q )
N(q, q )  M
T T
 (q )  n 
 (0)  1  (q )  2C(q, q )]s  0,  s
sT [M
g(q )      m i g (0)T r (q ) 2
  q  i 1

  q Ci 
n n
 (q )  M
M  T (q ), M
 (q )  2C(q, q )  (M
 (q )  2C(q, q ))T
 
T
  m i g (0)T JTi (q )   m i JTi
T
(q )g (0)
i 1 i 1  (q )  C(q, q )  CT (q, q )
M
27 28
29/03/2023
-29- -30-
Tính chất của các ma trận M và C PT Lagrange loại 2 dạng ma trận
Chứng minh  (q )  2C(q, q )  NT ( q, q )

N( q, q )  M
Các bước thực hiện thiết lập PTVPCĐ cho hệ nhiều vật sd Lagrange 2
uk
dạng ma trận (n dof, tọa suy rộng đủ) qk
N ij (q, q )  M ij (q )  2C ij (q, q ) Cj

1. Chọn tọa độ khớp q F
m   m  m  m
 m ik  m jk  2. Đưa vào các hệ tọa độ:
    q   
ij ij Ck
  q 

k   q   q   q  qk
 1. hệ cơ sở Fr0 => g(0), (Fr = Frame) E
k 1  k  k 1  k j i  q1
2. hệ gắn liền khâu k : Frk => FrCk => (k)ICk; [ (k)Ck ] u1 
m  m  m ij  m ik  m jk  m   m ik  m jk  g
    q ij

q k

q j
  q 
 q i  k
  q j
  q
 q i  k
3. Các tọa độ suy rộng đủ q => rCk(q), Rk(q) => JTk(q), JRk(q),
k 1  k k 1  4. Tính ma trận khối lượng M(q);
Bằng cách hoán vị hai chỉ số i và j ta được 5. Tính ma trận Coriolis và ly tâm C( q, q ) từ ma trận M(q);
 Tính véc-tơ g(q) từ các mk, JTk, g(0);
 m ik 
6.
m  m jk
N ji (q, q )    q i
  q
 q  k
7. Lực suy rộng của các lực dẫn động Bu;
k 1  j 8. Lực suy rộng của các lực khác (cản, ngoại lực tại điểm E, ...)
Như thế tính chất phản đối
 m m  m jk  xứng của ma trận N(.,.) được
   ik
  q  N (q , q ) chứng minh. M(q )q  C(q, q )q  Dq  g(q )  Bu  JTE (q )F (0)
k 1 
 q j  q i  k ij
29 30
-31- -32-
PT Lagrange loại 2 dạng ma trận - Ví dụ PT Lagrange loại 2 dạng ma trận - Ví dụ
Ví dụ 1. Tay máy Đề các hai khâu Bảng thông số DH Tay máy Đề các hai khâu 0  0 0
   
 1  J R 1 q   0  , JR 1  0 0
y1 q2 y2 khâu i di ai i y1 q2 y2
0  0 0
z1 z2 z1 z2    
1 /2 q1 + c1 0 /2
u2 m2 0  u2 m2 0 0 0
m1 2 0 q2 + c2 0 0 m1    
(0)    2  J R 2 q   0  , JR 2   0 0
g   0 , 0 0 0
u1
q  q  q  u1 g    
g  g  q   1, q   1  ,    1 
q
q1   q 2  q2  q2  q1
z0 z0 m  m 2 0  0 0
x0 0 0 0 0 x0 M  m 1 JTT 1 JT 1  m 2 JTT 2 JT 2   1  C   
 0 m2   0 0
       
rC(0)1  0 , vC(0)1  JT 1 q   0  , JT 1  0 0
q  q  1 0  n
 1  1   g(q )   m i JTTi g (0)
 q  q  0 1  i 1
 2   2   m 1  m 2 0  q1  (m 1  m 2 )g   u 1 
rC(0)2   0  vC(0)2  JT 2 q   0  , JT 2  0 0        
 0 m 2  q2   0  u 2 
q  c  q  1 0    (m 1  m 2 )gq 1  cosnt   
 1 1  1  
31 32
29/03/2023
-33- -34-
Ví dụ 2. Tay máy phẳng hai khâu, 2dof (TR) Bảng thông số DH, 2 dof (TR) Tọa độ khối tâm trong hệ cố định
khâu i di ai i
y2 y2
g x2 1
x2 0  lC 2 sin  
m2, IC2 0 q1 + const 0 -/2 m2, IC2
g    
x0 2 -/2+q2 0 l2 0 x0 rC(0)1   0  , rC(0)2   0 
q1 lC2 C2 q1 lC2 C2
x1 x1 x  c  x  l cos  
y1 y1    C2 
z0 C1
q2
g (0) T
 [ g , 0, 0] q  [q1 q 2 ]T  [x  ]T z0 u 1 C1
q2
u1 m1 u2 m1 u2 q  [q1 q 2 ]T  [x  ]T
Tọa độ khối tâm trong hệ gắn liền khâu Các ma trận Jacobi tịnh tiến
u C(1)1  [0, c , 0]T ,  sin  cos  0
JT 1  ?,
0 0 0 lC 2 cos  
 
u C(2)2  [ (l 2  lC 2 ), 0, 0]T , R 20   0 0 1    
JT 2  ? JT 1  0 0 , JT 2  0 0 
IC(2)2  diag([I 2 x , I 2y , I C 2 ])  cos   sin  0 1 0 1 lC 2 sin  
     
33 34
-35- -36-
véc tơ vận tốc góc Tính ma trận khối lượng IC(0)2  R 20 IC(2)2 R 20T ,
y2
x2       y2
x2 M  m JT J  m JT J  JT R 0 I(2) R 0T J
m2, IC2 1  0,  2  0i0  q2 j 0  0k 0 m2, IC2 1 T1 T1 2 T2 T2 R2 2 C2 2 R2
g g
x0
lC2 C2 0 0 x0
lC2 C2
q1 x1     q1 x1
y1   1(0)  0   2(0)    y1  m1  m2 m 2lC 2 sin  
z0 q2 z0 q2 M   
u1 C1 0 0 u 1 C1
 m 2lC 2 sin  I C 2  m 2lC2 2 
m1     m1 
u2 u2
 1 m l2 0 0 
 12 1 1  Biểu thức thế năng hệ Véc tơ do trọng lực
IC(1)1   0 0 0  Từ đó suy ra được các ma trận Jacobi quay:
 0 T
 0 1
m 1l12  P  m 2 glC 2 sin   C  P   0 
12  g( q )  
0 0 0 0   m gl cos  
 0 0 0       q   2 C 2 
  JR 1  0 0 , JR 2  0 1 , n
IC(2)2   0 1
m 2l 22 0 
12
0 Hoặc sử dụng công thức g(q )   m i JTTi (q )g (0)
 0 0 1 Coi khâu như thanh
m 2l 22  thẳng đồng chất 0 0 0
 12      i 1
35 36
29/03/2023
-37- -38-
 m1  m 2 m 2lC 2 sin   Ma trận liên quan đến lực ly tâm, lực quán tính coriolis
M  
m 2lC 2 sin  I C 2  m 2lC2 2    m ij  m ik m jk 
  1 m
y2 y2 C ij (q , q )       q
x2
Tính các số hạng theo công thức Christoffel dạng thứ nhất.
x2 2 k 1  q k q j  q i  k
m2, IC2 m2, IC2
g g
x0
lC2 C2 1  m ij m ik  m jk 
x0
lC2 C2 0 m 2lC 2 cos  
q1 x1 cijk
q1 x1 C(q, q )   
y1      y1 0 0
z0 u1 C1
q2 2   q k q j q i  z0 u 1 C1
q2  
m1 u2 m1 u2 Phương trình vi phân chuyển động dạng ma trận
1   m 11  m 11  m 11  1  m 11 1  m 21 m 21 m 11 
c 111      0 c 211     0
2   q 1 q 1  q 1  2  q 1 2   q 1 q 1  q 2 
  C(q, q )q  g(q )  u
M(q )q
1   m 11  m 12  m 12  1  m 22 m 21 m 21 
c112  0 c 221  0

2   q 2

q 1

 q 1 

2   q 1

q 2

 q 2   m1  m2 m2lC 2 sin   x 0 m2lC 2 cos   x   0  ux 
 2          
1   m 12  m 11  m 21  1  m 21 m 22 m 12  m2lC 2 sin  IC 2  m2lC 2   0 0    m2glC 2 cos   u 
c121       0 c 212       0
2   q 1 q 2 q 1  2   q 2 q 1 q 2 
(m 1  m 2 )x  m 2lC 2 sin    m 2l 2 cos   u1,
1   m 12  m 12  m 22  1  m 22 m 22 m 22 
c122       m 2lC 2 cos  c 222     0
2   q 2 q 2  q 1  2   q 2 q 2  q 2 
m 2lC 2 sin  x  (I C 2  m 2lC2 2 )  m 2glC 2 cos   u 2 .
37 38
-39- -40-
Ví dụ 3. Tay máy 2 dof - RR Ma trận cosin chỉ hướng các khâu so với hệ cố định
Bảng thông số DH, rôbốt phẳng
x2 x2
y2 Frk 2 dof (RR) y2  cos q  sin q 0 
1 1
g g  
O2 khâu i di ai i O2 R 10   sin q 1 cos q 1 0  ,
l2 l2  0
y0 y1
C2 FrCk 1 q1 0 l1 0 y0 y1
C2
0 1
q2 q2  
2 q2 0 l2 0  cos q
O1
x1
O1
x1
1 2
 sin q 1  2 0 
l1 l1  
u1
q1 u2 0  u1
q1 u2 R 20   sin q 1  2 cos q 1  2 0 
O0   O0  0 0 1
x0 q  [q 1 q 2 ]T g (0)   g  x0  
0 
 
Tọa độ khối tâm trong hệ gắn liền khâu Frk Tọa độ khối tâm các khâu trong hệ cố định
u C(1)1  [ (l1  a 1 ), 0, 0]T , u C(2)2  [ (l 2  a 2 ), 0, 0]T , a 1  O 0C 1 a cos q  l cos q  a cos q 

Frk  1 1
 1 1 2 1 2

rC  a 1 sin q 1  , rC   l1 sin q 1  a 2 sin q 1  2  ,
1 2
FrCk IC 1  diag([I 1x , I 1y , I C 1 ]), IC 2  diag([I 2 x , I 2y , I C 2 ]) a 2  O1C 2  0   0 
   
39 40
29/03/2023
-41- -42-
Véc tơ vận tốc góc trong hệ cố định
a cos q  l cos q  a cos q 
0  0   1 1
 1 1 2 1 2

y2
x2
    y2
x2 rC   a 1 sin q 1  , rC   l 1 sin q 1  a 2 sin q 1  2  ,
g 1   0  , 2   0  g
1
 
2
 
O2 q  q  q  O2 0 0
   
l2 C2  1  1 2 l2 C2
y0 y1 y0 y1
q2 q2
O1 O1
Các ma trận Jacobi tịnh tiến
x1 Các Jacobi tịnh tiến và Jacobi quay x1
l1 l1
u1 u1  a sin q 0 
q1 u2 0 0 0 0 q1 u2 rC 1 1 1

O0
 1    2   O0 JT 1    a 1 cos q 1 0  ,
x0
JR1   0 0 , JR 2   0 0 . x0
q 
 q q 0 0
1 0 1 1  
     l sin q  a sin q a 2 sin q 1  2 
1 1 2 1 2
r  
 0
JT 2  C 2   l1 cos q 1  a 2 cos q 1  2 a 2 cos q1  2  ,
0 0   0 0 0  q
     0 0 
IC(1)1   0 1
12
m 1l12 0  IC(2)2   0 1
12
m 2l 22 0   
 0 0 1 2
m 1l1   0 0 1 2
m 2l 2 
 12   12 
41 42
-43- -44-
Tính ma trận khối lượng Từ hàm thế năng của tay máy
M  m1 JTT 1 JT 1  JTR1R10 IC 1R10T JR1  m2 JTT 2 JT 2  JTR 2 R 20 IC 2 R 20T JR 2 2
y2
x2
y2
x2   g  m i yCi  m 1ga 1 sin q 1  m 2 g [l1 sin q 1  a 2 sin(q 1  q 2 )];
g g i 1
O2 m m 12  T
O2
l2 C2 M(q )   11   M (q ) l2 C2
T
m 21 m 22 
y0 y1 y0 y1
q2 q2    m 1ga 1 cosq 1  m 2g [l1 cos q 1  a 2 cos(q 1  q 2 )] 
O1  O1 g     
l1
x1
l1
x1
 q   m 2ga 2 cos(q 1  q 2 ) 
u1
q1 u2 m 11  I C 1  m 1a 12  I C 2  m 2 (l12  a 22  2l1a 2 cos q 2 ) u1
q1 u2
O0 2 O0 n
x0 m 21  I C 2  m 2 (a  l1a 2 cos q 2 )  m 21
2
x0
g(q )   m i JTTi (q )g (0) Lực điều khiển
2
m 22  I C 2  m a 2 2
i 1 T
u  u 1 , u 2 
Áp dụng công thức Christoffel  
 m 2l1a 2q2 sin q 2 m 2l1a 2 (q1  q2 ) sin q 2 
C(q, q )      C(q, q )q  g(q )  Bu
M(q )q
 m 2l1a 2q1 sin q 2 0 PTVP CĐ

43 44
29/03/2023
-45- -46-
PT Lagrange loại 2 dạng ma trận - Ví dụ PT Lagrange 2 dạng ma trận: các bước thực hiện
Bài tập: Tay máy SCARA 4 dof
0  0 
 
1   0  , 2  

0 
 Bước 1. …??
q  q  q 
 1  1 2
 0   0  Bước 2. …??
   
3  2   0 , 4   0 
q  q  q  q  q 
 1 2  1 2 4
Bước 3. …??
……
……
45 46
-47- -48-
Sử dụng Matlab/Simulink giải PTVP CĐ Sử dụng Matlab/Simulink giải PTVP CĐ
Viết m-file mô tả PTVP CĐ của robot.

47 48
29/03/2023
-49- -50-
Sử dụng Matlab/Simulink giải PTVP CĐ Sử dụng Matlab/Simulink giải PTVP CĐ
function q2dot = tinhq2dot(ins)

% Damping matrix
x2 x2
y2 y2 % system parameter, see page 16,
% ins = [1 2 3 1 2 3]';
g g PhD Thesis of Renwei Hu
O2 n=2; O2 % body 1
l2 C2 l2 C2 m1=21.20; %[kg]
y0 y1 % state variables y0 y1
q2 q2 Ic1=0.21; %[kg m2]
O1 x1 O1 x1 l1=0.50; %[m]
l1 qdot = ins(1:n); l1
u1 u1 a1=0.15; %[m]
q1 u2 q = ins(n+1:2*n); q1 u2 % body 2
O0 u = ins(2*n+1:3*n); O0
x0 x0 m2=15.22; %[kg]
q1_dot=qdot(1);
Ic2=0.18; %[kg m2]
q2_dot=qdot(2);
l2=0.45; %[m]
q1=q(1);
a2=0.19; %[m]
q2=q(2);
gr=9.81;
49 50
-51- -52-
Sử dụng Matlab/Simulink giải PTVP CĐ Bài tập
% mass matrix of system
m11 = Ic1+Ic2+m1*a1^2+m2*l1^2+m2*a2^2+2*m2*l1*a2*cos(q2);
m12 = Ic2+m2*a2^2+m2*l1*a2*cos(q2);
x2
y2 m21 = m12;
g m22 = Ic2+m2*a2^2;
O2
M=[m11, m12; m21, m22];
l2 C2
y0 y1 % Coriolis forces
q2 c11 = -2*m2*l1*a2*sin(q2)*q2_dot; c12 = m2*l1*a2*sin(q2)*q2_dot;
c21 = m2*l1*a2*sin(q2)*q1_dot; c22 = 0;
O1
x1 c11 = -m2*l1*a2*q2_dot*sin(q2)’;
l1
u1 c12 = -m2*l1*a2*(q1_dot+q2_dot)*sin(q2);
q1 u2 c21 = m2*l1*a2*q1_dot*sin(q2); a3 P
O0 c22 = 0;
x0 3
C = [c11, c12; c21, c22]; a2
% gravitational forces
g = gr*[m1*a1*sin(q1)+m2*(l1*sin(q1)+a2*sin(q1+q2));
m2*a2*sin(q1+q2)]; 2
D = 60*diag([0.1, 0.1]); % Damping matrix d1 z0
B = [1, 0; 0, 1]; % B matrix
q2dot=inv(M)*(B*u-C*qdot-D*qdot-g); % q2dot
1
Nguyen Quang Hoang - Department of Applied Mechanics
x0
Nguyen Quang Hoang - Department of Applied Mechanics
51 52
29/03/2023
-53- -54-
Bài tập:
y3 x3 y3 x3
y2 y2
y1 y1
a3 a3
3 z3 z3
a2 x2 a2 x2
2 x1 z2 x1 z2
d1 z1 d1 z1
z0
z0
1 1
x0 x0
53 54
-55- -56-
Sử dụng tích Kronecker xác định ma trận C từ M Ví dụ tích Kronecker
T
1 M 1 M 1  M  Tích Kronecker: là phép nhân hai ma trận có cỡ tùy ý theo luật sau đây
C(q, q )  (I  q )  (q  In )  (q T  In )   .
2 q n 2 q 2  q  A   m n , B   p q : C  A  B, C   mp nq
a B a 12 B .. a 1n B 
 11 
- Đạo hàm ma trận theo biến véc-tơ: C  A  B   .. .. .. .. 
a B am 2B .. a mn B 
 m1  mp nq
 M   m m 2 m n 
M  m1 m 2 ... m n  ,    1 ... 
 
 q n n 2  q q q  Ví dụ a a 12  b11 b12 b13 
A   11 , B   
a a 22  b21 b22 b23 
 22  
- Tích Kronecker hai ma trận
 b11 b12 b13  b11 b12 b13  
a 11   a 12  
A   m n , B   p q : C  A  B, C   mp nq
a B a 12 B   b21 b22 b23 
 b21 b22 b23  

C  A  B   11    
a B a B b11 b12 b13  b11 b12 b13  
 21 22  46 
a   a 22  
a B a 12 B .. a 1n B   21 b b b23  b b22 b23  
  21 22   21   4 6
 11 
C  A  B   .. .. .. .. 
Tích Kronecker không có tính chất giao hoán A  B  B  A
a B a m 2B .. a mn B 
 m1  mp nq
55 56

Robotics 9.2 Dynamics - Serial Robot - Lagrange 2-4s

Uploaded by

Copyright:

Available Formats

You might also like

Robotics 9.2 Dynamics - Serial Robot - Lagrange 2-4s

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robotics 9.2 Dynamics - Serial Robot - Lagrange 2-4s

Uploaded by

Copyright:

Available Formats

29/03/2023

ĐỘNG LỰC HỌC ROBOT

Phương trình Lagrange loại 2

(m 1  m 2 )x  m 2a 2  sin   m 2a 2  2 cos   u 1  

(m 1  m 2 )x  m 2a 2 sin  B

M(q )q  h(q, q )  u T 

q1  q2      q1  q2  A  u 1q 1  u 2 (q 2  q 1 ) C1 u2 Khâu 1: m1, L1, IC1, OC1 = lC1. A

Véc tơ tọa độ suy rộng q  [q 1 , q 2 ,..., q f ]T

loại 2 dạng ma trận rCk  rCk (q ), R k  R k (q ) 

Tính các đạo hàm

Chứng minh  (q )  2C(q, q )  NT ( q, q )

u C(1)1  [ (l1  a 1 ), 0, 0]T , u C(2)2  [ (l 2  a 2 ), 0, 0]T , a 1  O 0C 1 a cos q  l cos q  a cos q 

Viết m-file mô tả PTVP CĐ của robot.

function q2dot = tinhq2dot(ins)

You might also like