Robotics 6-1 Kinematics - Velocity - Jacobian Matrix - 4s

29/03/2023
CHƯƠNG 6. ĐỘNG HỌC VI PHÂN và JACOBI -2-
1. Một số kiến thức về động học vật rắn
Chương 6. 2. Ma trận Jacobi: jacobi tịnh tiến và jacobi quay

ĐỘNG HỌC VI PHÂN, MA TRẬN JACOBI
Differential Kinematics: velocity and jacobian Phương pháp xác định theo đạo hàm riêng
Phương pháp xác định từ công thức hình học
Cấu hình kỳ dị của robot
Nguyễn Quang Hoàng 3. Động học ngược ở mức vận tốc và gia tốc
Email: hoang.nguyenquang@hust.edu.vn
Nguyễn Quang Hoàng-Department of Applied Mechanics-SME Nguyễn Quang Hoàng-Department of Applied Mechanics-SME
1 2
-3- Véc tơ vận tốc góc của vật rắn -4-
Xét vật rắn B chuyển động đối với hệ quy chiếu F0. Vận tốc  B
 
góc của vật rắn đặc trưng cho tốc độ thay đổi hướng của vật  
rắn trong hệ quy chiếu đó.  k c j
MỘT SỐ KIẾN THỨC VỀ ĐỘNG HỌC VẬT RẮN k0
Định nghĩa: vận tốc góc của vật rắn là đại lượng vectơ thỏa 
rO 
mãn hệ thức sau F0 O
O’
i
1.Véc tơ vận tốc góc của vật rắn, công thức cộng vận tốc góc F0 
2.Vận tốc và gia tốc của điểm thuộc vật d       j0
c    c  B /F  c , c  B 
3.Công thức tính vận tốc góc từ ma trận côsin chỉ phương dt 0 i0
   
i  d
dt
i   i
Biểu diễn trong các hệ trục tọa độ khác nhau:
       
   x(0)i0  y(0) j 0  z(0)k 0   (0) j  d
dt
j  j
      
  x(1)i1   y(1) j1   z(1)k1   (1) k  d
dt
k   k
3 4
29/03/2023
Véc tơ gia tốc góc của vật rắn -5- Liên hệ vận tốc, gia tốc các điểm thuộc vật -6-
Gia tốc góc đặc trưng cho tốc độ thay đổi vận Trường hợp vật rắn quay quanh trục z cố định Liên hệ vị trí hai điểm  B

tốc góc của vật rắn theo thời gian     
P 
rP  rO  u rP(0)  rO(0)  R10 u(1)  k u j
  d   d  d   k0
      u  ez  ez 
dt dt dt Liên hệ vận tốc hai điểm
F0
rO O 
 d  d   i
  d   z vP  rP  (rO  u ) O’

      ez dt dt j0
dt     F0 : v(0)  vO(0)  
 (0)u(0), 
 vP  vO    u , P i0 R 10

(1) (1) (1) (1)

d  Liên hệ gia tốc hai điểm
F1 : vP
v O
 u ,

 d  d   
aP  v P  (vO    u )
O

u dt dt a (0)
P 
 a O(0)  S( (0) )  S( (0) )S( (0) ) u (0) 
(t )       
a P  aO    u    (  u ) a (1)
P 
 aO(1)  S( (1) )  S( (1) )S( (1) ) u (1) 
5 6
Liên hệ vận tốc, gia tốc các điểm thuộc vật -7- Công thức cộng vận tốc góc của vật rắn -8-
Đạo hàm véc tơ trong hệ quy chiếu động Công thức cộng véc tơ vận tốc góc
Lưu ý các biểu thức
của vật rắn
Xét véc tơ c và hai hqc F0 và F1 ta có:
a (0)
P
 Ra (1)
P
 a (1)
P
 R  1 a (0)
P
 RT a (0)
P B F2
F0 F
 0 d 1
d  
  z  y  c  c   F /F  c
dt dt 1 0
  S( )    z 0  x 
 x 0 
 y  B
   
  u   (1)u (1)  S( (1) )u (1)    F1
 k c j F0
k0
  (0)u (0)  S( (0) )u (0)
F0 O    
   i B /F  F /F  F /F
  (  u )   (1) (1)u (1)  S( (1) )S( (1) )u (1) O’
 0 1 0 2 1
j0  F
 F0 F0 F F
(0)
   u (0) (0)
 S( (0)
)S( (0)
)u (0)
i0 hoac B   1  1 2
7 8
29/03/2023
Công thức cộng vận tốc góc của vật rắn -9- Liên hệ véc tơ vận tốc góc và ma trận cosin chỉ hướng -10-
  
  B Ma trận cosin chỉ hướng 0
R  R  [ i1, j1, k1 ]in  

k   F / F 1 F0
 j1
k 0
Fn           k1 F1
k
 k
 i1    i1 , j 1    j1 , k1    k1 k0
  Fi / Fi 1
  i / i 1
   
i 1 i 1     O
i1
F0
F1 [ i1, j1 , k1 ]    [i1, j1, k1 ] 
O’

Fk i0 F0 j0
Công thức cộng gia tốc góc
k k Tính trong F0
     Tính trong F1
k   k   F / F   F  F /F
i 1
i i 1
i 2
i 1 / F0 i i 1 R   (0) [i1(0) , j1(0) , k 1(0) ]=  (0) R RT R   (1) [i(1) , j(1) , k (1) ]=  (1) E
   1 1 1
2  1  2/1,
k
 k
   T   (0) RRT   (0)  RR
 RR  T  RT R   (1)   (1)  RT R
  i / i 1
  i 1   i /i 1     
2  1  2/1  1  2/1
i 1 i 2
 (0)  S 1 ((0) )  (1)  S 1( (1) )
9 10
-11- Tính vận tốc góc các khâu robot, Jacobi quay -12-
• Đánh số các khâu: 0 -> n, khớp 1->n 2

• Dựng các hệ trục tọa độ gắn liền các khâu. a2 q3
• Xây dựng các ma trận biến đổi tọa độ thuần 3
q2 a3 E
nhất tương đối z0
d1
Tính vận tốc góc các khâu robot, • Tính các ma trận biến đổi tọa độ thuần nhất
tuyệt đối.
1
ma trận Jacobi quay x0

q1
• Suy ra các ma trận cosin chỉ hướng tuyệt đối

• Áp dụng công thức tính ma trận sóng ứng với vận tốc góc
(lưu ý véc tơ vận tốc góc chiếu trong hệ nào: cố định hay hệ
gắn liền vật)
11 12
29/03/2023
Vận tốc góc của khâu i trong chuỗi động -13- Vận tốc góc của khâu i trong chuỗi động -14-
Dựa trên ma trận biến đổi tọa độ thuần nhất: chứa vị trí gốc tọa độ Oi trong hệ
Vận tốc góc khâu i chiếu trong hệ cơ sở F0: Fi
cố định F0 và ma trận cosine chỉ hướng của khâu i trong hệ cố định F0:
Pi
 R i0Ri0T R i0  
Oi
(0)
Rii 1 (qi ) rO(i 1)(qi ) a2
2
q3

 i
 i(0)Ri0
i 1
A (qi )  
i
i
 3
F0
 0 1  q2 a3 E
 d1 z0
1  0 i(0) i(0)  (0) 
0
i 1
j  ,z ,y   i ,x 
T (q)  A (q j ),  (0) 
 )  i(0) 
q1 (0) (0)
i j 1 
 i
  i,z 0 i ,x  (0)
i
1
 S ( (0)
i ,y 
j 0 x0  (0)   (0) 
i,y i(0) 0  i,z 
 ,x   
i 1 R 0 (q) rO0 (q)
Ri0 (q)   R jj 1(q j ) Ti0 (q)   i i

j 0  0 1 

13 14
Fi
Tính đạo hàm ma trận cosin chỉ hướng R(q) Vận tốc góc khâu i chiếu trong hệ cơ sở F0:
Pi
Oi
q  [q1, q 2 ,..., q n ]T
 0 i(0) i(0) 
r (q ) r (q ) r (q ) r (q ) r (q ) r (q )  ,z ,y  F0
 11 12 13   11 12 13     R 0R 0T
Ri  Ri (q )  r21(q ) r22 (q ) r23 (q )  R i  r21(q ) r22 (q ) r23 (q )
(0)
0 0 0 
 i
  i(0)
,z
0 i(0),x  i i
     (0) 
r31(q ) r32 (q ) r33 (q ) r31(q ) r32 (q ) r33 (q ) i,y i(0) 0 
 ,x 
rij (q ) rij (q ) rij (q )  (0) 

rij (q )  q1  q2  ...  qn  i ,x 
 q1 q 2 q n i(0)  i(0)   S 1(
,y 
 i(0) )
n rij (q )  (0) 
  qk
qk , i, j  1,2, 3 i,z 
 
k 1
15 16
29/03/2023
Fi
Vận tốc góc khâu phụ thuộc tuyến tính vào véc tơ vận tốc khớp
Pi
q  [q1, q2 ,..., qn ]T

Oi
F0
Do đó được viết dạng tích ma trận với véc tơ như sau:
i(0)  J Ri (q )q, i  1,2,..., n

 (0) i(0) i(0) 
 i,x ,x
 ,x 
Ma trận Jacobi quay của khâu i trong hệ cơ sở F0.   
 q 1 q2 qn 
 (0) i(0) (0) 
i,y 

J Ri (q )   i,y ,y
 
i(0)  q1 q2 qn 
J Ri (q )   (0)
 i,z i(0) (0) 
i,z 
q 
 q1
,z
 
q2 qn 
 
17 18
Liên hệ Ma trận Jacobi quay của khâu i trong

Vận tốc góc khâu i chiếu trong hệ gắn liền khâu i, Fi: Fi Fi
hệ gắn liền khâu Fi và trong hê cố định F0
(i ) (i ) Pi Pi
 0    Oi Oi
i,z i ,y
 (i )  0T  0
 i(i )   i , z 0  (i )
  Ri R
  (i )  (i )
i,x

i F0
i(0)  J R,i (q )q F0
0
 i ,y 
i,x
i(0)  Ri (q )i(i )
  (i )    (i )
 i ,x   i ,x i(,ix) i(,ix)  i(i )  J R(i,)i (q )q
  i(,iy)   S 1(  q 
qn 
(i )
 i
 i(i ) )  1 q2
 (i )   (i )
 i(,iy) i(,iy) 
 i,z  J R,i (q )   i,y
(i )
 
   q1 q2 qn 
 (i ) 
Ma trận Jacobi quay của
khâu i trong hệ gắn liền  (i ) (i )
 J (q )q  i,z

 q
i(,iz)

i(,iz) 
 J R,i (q )  Ri (q )J R(i,)i (q )
i R,i
 1 q2 qn 
khâu Fi. 
19 20
29/03/2023
-21- Vận tốc điểm thuộc khâu i trong chuỗi động -22-
Xét điểm P có tọa độ điểm P trong hệ tọa độ khâu i, Fi.
u P(i )  [x P(i ), yP(i ), z P(i ) ]T  const Fi

P
Tính vận tốc các điểm, Tọa độ của điểm P trong hệ cố định F0: Oi
rP(0)(q )  rO(0)(q )  Ri0 (q )u P(i ) F0

Jacobi tịnh tiến i
rˆP(0)(q )  Ti 0 (q )uˆP(i )
Vận tốc và gia tốc, Jacobi của điểm N thuộc khâu i
Vận tốc của điểm P trong hệ cố định F0:
vP(0)(q )  rP(0)(q )  rO(0)(q )  R i0 (q )u P(i ) vO(0)  rO(0)

i
i i
 i(0)Ri0u P(i )
 rO(0)(q )  
i
21 22
Vận tốc điểm thuộc khâu i trong chuỗi động -23- Vận tốc điểm thuộc khâu i trong chuỗi động -24-
Vận tốc điểm P phụ thuộc tuyến tính vào véc tơ vận tốc khớp
Tính theo tọa độ thuần nhất
rˆP(0)(q )  Ti 0 (q )uˆP(i ) Fi q  [q1, q2 ,..., qn ]T Fi
R 0 (q ) r 0 (q ) u (i )  Oi
Pi
Oi
Pi
  i Oi  P  Do đó được viết thành

 0 1   1 
  F0 F0
R r  
0 (0) (0) 0 (0) 
vP(0)  JT ,i (q )q
Ti 0   i Oi 
   i Ri vOi 
 0 0   0 0 
   Jacobi tịnh tiến
v (0)    (0)R 0 vO(0)  u P(i ) 

vˆP(0)   P   Ti 0uˆP(i )   i i i  
 1  MAPLE: JT = jacobian(v, qdot)
1  0 0
     MAPLE: JT = jacobian(r, q)
23 24
29/03/2023
Vận tốc điểm thuộc khâu i trong chuỗi động -25- Vận tốc điểm tác động cuối E -26-
Tính Jacobi tịnh tiến Tương tự đối với điểm tác động cuối E (thuộc khâu cuối EE, n) Fi
u (i )  R 0 (q ) r 0 (q ) u (n ) 
rˆP(0)(q )  Tn0 (q )   , r (0)(q )  r 0 (q )  R 0(q )u (i ) rˆE(0)(q )  Tn0 (q )uˆE(n )   n  E 
P On
Oi
 P Oi i P
1   1 
 1   0   F0 On
vP(0)  JT ,i (q )q,
(0) (0)
r (q )  [x (q ), y (q ), z (q )](0) (0) T rE(0)(q )  rO0 (q )  Rn0 (q )u E(n )
n
P P P P
 x (0) x P(0) x P(0) 
 n   P  r (q )  [x (q ), yE(0)(q ), z E(0)(q )]T
(0) (0)
  (x (0) / q )q   q q 2 qn  E E  x (0) x E(0) x E(0) 

 j
 1  E 
 y (0) yP(0)   q qn 
P j
x (0)(q )  j 1  yP(0) q2
 P   n  JT (q )   P   1
vP(0)  yP(0) (q )    (yP(0) / q j )q j   JT ,i (q )q,  q1 q 2 qn   y (0) y E(0) y E(0) 
 (0)   j 1   z (0) z P(0)  JT (q )   E 
 zP (q )  n   P z P(0)
 vE(0)  JT (q )q,  q1 q2 qn 
      z (0)
  (z P / q j )q j 
(0)
 q1 q 2 qn 
 E z E(0) z E(0) 
 j 1    
MAPLE: JT = jacobian(rP, q) MAPLE: JT = jacobian(rE, q)  q1 q2 qn 
25 26
Ma trận Jacobi -27- Ma trận Jacobi -28-
Véc tơ chứa vận tốc điểm E và vận tốc góc khâu cuối Ví dụ : tay máy phẳng hai bậc tự do với hai khớp quay E,O2
(0)T (0)T T (0) (0) (0) (0) (0) (0) T
  [v E
,n
]  [v , v , v ,  ,  ,  ]
E ,x E ,y E ,z nx ny nz
c(q1  q 2 ) s(q1  q 2 ) 0 l1cq1  l2c(q1  q 2 )
 
s(q  q 2 ) c(q1  q 2 ) 0 l1sq1  l2s(q1  q 2 )  q2
q  [q1, q 2,..., qn ] T T (q )   1
0
 y0
rO l2
2
0 0 1 0 2
 
0 0 0 1
Kết hợp Jacobi tịnh tiến và Jacobi quay của khâu cuối EE:   l1 O1
O0 q1
vE(0)  JT (q )q,
Fi
x0
    J (q )q
n(0)  J R (q )q, Oi

On
F0
J (q )
J (q )   T 
J (q )
 R 
27 28
29/03/2023
Jacobi tịnh tiến ???

l cq  l c(q  q ) E,O2 c(q  q ) s(q  q ) 0 
O2
rE(0)  rE(0) (q ) 1 1 2 1 2
(0)   1 2 1 2

r (q )  l1sq1  l2s(q1  q2 ) R20 (q )  s(q1  q 2 ) c(q1  q 2 ) 0 
E
   q2
  q2 0 0 1 rO l2
q  [q1, q 2 ]T 
0
 y0
rO
2
l2   y0 2
l1 O1  c(q  q ) s(q  q ) 0  l1 O1
0T  1 2 1 2
 O0 q1
 x (0) (0)  O0 q1 R (q )   s(q1  q 2 ) c(q1  q 2 ) 0 
 E
x E 
2
 0 0 1 x0
 q q 2  x0  
 1
rE (q )  yE(0)
(0)
yE(0)   l sq  l s(q  q ) l s(q  q )
JT (q )  
q 2   1 1 2 1 2 2 1 2  s(q  q )(q  q ) c(q  q )(q  q ) 0 
q  q1 
 z (0) J E (q )   l1cq1  l2c(q1  q2 ) l2c(q1  q2 )   1 2 1 2 1 2 1 2

 E z E(0)   
R 20 (q )   c(q1  q 2 )(q1  q2 ) s(q1  q 2 )(q1  q2 ) 0 
  0 0  0 0 0
 q1 q 2     
29 30
 2(0)  R 20R20T  ?? O2  l sq  l s(q  q ) l s(q  q )

 1 1 2 1 2 2 1 2
 O2
J E(0) (q )   l1cq1  l2c(q1  q2 ) l2c(q1  q2 ) 
 q2 
 0 q1 2 0  0  rO l2  0 0  q2
y0 2   rO l2
    y0
 2(0)  R 20R20T  q1 2
2
0 0   2(0)  S 1( 2(0) )   0 
 0 l1 O1 0 0
0 0 q  q    l1 O1
   1 2
O0 q1 J R,2   0 0 
1 1  O0 q1
 (0) i(0)  x0  
 i, x ,x  x0
 q
 1 q2 
 (0) i(0)  0 0 
 ,y  v (0)  l sq  l s(q  q ) l s(q  q )
J R,i (q )   i,y     E ,x   1 1 2 1 2  
v (0)    l cq  l c(q  q ) l c(q  q )  q1  ,
2 2 1
 q1
 (0)
q2  J R,2  0 0    J (q )q
(0)   E ,y   1 1 2 1 2  
 i,z i,z   q2 
2 2 1
 
1 1   (0)  
 q   2,z   1 1 
q2   
 1 
31 32
29/03/2023
T
Ví dụ. Xét tay máy phẳng 3 dof như trên hình vẽ, hãy xác định ma trận Jacobi Tọa độ điểm cuối E q  [q1, q 2 , q 3 ] q13  q1  q 3
của khâu cuối.
Biết ma trận biến đổi tọa độ thuần nhất như sau: rE(0)  rE(0) (q ) y3
x3
E
cq13

sq13 0 l1sq1  q2cq1  l 3cq13 
 y3
x3 x E  l1sq1  q 2cq1  l 3cq13 q2
q3
sq cq13 0 l1cq1  q 2sq1  l 3sq13  y0

T30 (q )   13
O2
0 0 1 0 
q3
E yE  l1cq1  q 2sq1  l 3sq13 z2
 
0 0 0 1
  y0
q2
O2
zE  0 l1 q1
z2 O0
x0
q13  q1  q 3
l1 Jacobi tịnh tiến ???
q1
O0
x0
33 34
Jacobi tịnh tiến ??? rE(0)  rE(0) (q ) q  [q1, q 2 , q 3 ]T x E  l1sq1  q 2cq1  l 3cq13
y3
x3
yE  l1cq1  q 2sq1  l 3sq13 E

q3
x E  l1sq1  q2cq1  l 3cq13 zE  0 q13  q1  q 3 q2
y0
O2
yE  l1cq1  q2sq1  l 3sq13  x (0) z2
 E x E(0) x E(0)  Jacobi tịnh tiến ???
zE  0  q 
 1 q 2 qn  rE(0)
l1 q1
rE (q )  yE(0) yE(0) 

O0
(0)
yE(0) JT (q )  x0
JT (q )    q
q  q 1 q 2 qn 
 z (0)  l cq  q sq  l sq cq1 l3sq13 
 E z E(0) z E(0)   1 1 2 1 3 13

     l1sq1  q2cq1  l3cq13 sq1 l3cq13 
 q1 q 2 qn 
 0 0 0 
 
35 36
29/03/2023
q13  q1  q 3 x3
Ma trận cosin chỉ hướng khâu 3 Véc tơ vận tốc góc khâu 3 y3
E
cq sq13 0  cq sq13 0 0 q13 0  0  q3
 13   13   3(0)  R 30R30T

 q13 0

0   3(0)
 
  0   J Rq y0
q2
0
R3 (q )  sq13 cq13 0  ,  R30,T (q )  sq
 13 cq 13
0 z2
O2
0 0 0 q  q 
 0 0 1  0 0 1  
    x3  1 3
y3 l1 q1
E Jacobi quay   (0) i(0) i(0)  O0
 i,x ,x
 ,x  x0
 sq cq13 0 q3   
 13  q2 i=3  q 1 q2 qn 
 0
 R3 (q )  q13  cq13 sq13 0 , q13  q1  q3 y0
O2   (0) i(0) (0) 
i,y   0 0 0

z2 J Ri (q )   i,y ,y
   
 0 0 0  q1 q2 qn  J R,3   0 0 0
   (0) (0) 
l1 q1  i,z i(0) i,z  1 0 1 
 ,z
   
 3(0)  R 30R30T  ??
O0  q1 q2 qn 
x0  
37 38
Kết hợp lại ta được ma trận Jacobi tổng thể Trong trường hợp robot phẳng này, nếu chọn véc tơ x3
x3 y3
y3
 l1cq1  q 2sq1  l3sq13 cq1 l3sq13 
E
(0)
  [vEx (0)
, vEy , 3(0)
z
]T   J (q )q q3
E
 
 l1sq1  q 2cq1  l3cq13 sq1 l 3cq13  q3
y0
q2
JT (q )  0 0 0  q2 O2
y0 ma trận Jacobi được viết lại thành
J (q )     O2 z2
J (q )  0 0 0  z2
 R 
 0 0 0   l cq  q sq  l sq cq1 l 3sq13  l1 q1
  l1 q1 J (q )  1 1 2 1 3 13

1 0 1  J (q )   T    l1sq1  q 2cq1  l 3cq13 sq1 l 3cq13  O0
  O0 x0
x0 J R (q )  1 0 1 
(0) (0) (0) (0) (0) (0) T  
  [v , v , v ,  ,  ,  ]
Ex Ey Ez 3x 3y 3z
  J (q )q
39 40
29/03/2023
-41- -42-
• Cần tính vận tốc góc của tất cả các khâu

• Các điểm quan tâm thường gồm: khối tâm Ci của các khâu, điểm tác động
cuối, các điểm chịu tác dụng của lực,..
rCi(0)(q )  JT ,i (q ) Phương pháp hình học

rE(0)(q )  JT ,E (q ) xác định ma trận Jacobi
Ri0 (q )  
 i(0)  J R,i (q )
41 42
Phương pháp Denavit-Hartenberg -43- Phương pháp ma trận Denavit-Hartenberg -44-
zi-1 z’i-1
qi zi
Biểu diễn các tham số Denavit-Hartenberg Khớp i là khớp trượt (tịnh tiến) : di = di(t)
zi-2 qi-1
zi-1 z’i-1 Khớp i là khớp quay: qi = qi(t) khớp i ai khâu i+1
qi zi khâu i
khâu i-1
zi-2 qi-1
Biến khớp hay tọa độ suy rộng qi khớp i-1
khớp i+1
ai ai
khớp i
khâu i
khâu i+1
qi  iqi  (1  i )di , O’i Oi ai xi
khâu i-1 di
khớp i+1 x’i
1  Khớp i là khớp quay
khớp i-1 qi
ai Oi-1 xi-1
xi i   Khớp i là khớp tịnh tiến
0 
Oi ai
O’i
di
x’i Các véc tơ đơn vị của hệ trục tọa độ Fi gắn liền vào khâu thứ i
qi
Hệ trục tọa độ (Oxyz)i
Oi-1 xi-1      
gắn liền khâu i.
{x i , yi , z i } ={ii , ji , ki }
43 44
29/03/2023
Phương pháp ma trận Denavit-Hartenberg -45- Thiết lập ma trận Jacobi theo phương pháp hình học -46-
2
zi-1 z’i-1 Vận tốc góc của khâu thứ i (i = 1,2,...,n) a2 q3
qi zi
i 3
     E
i  1/0  2/1  ...  i /i1    j / j 1
zi-2 qi-1 q2 a3
ai khâu i+1 d1 z0
khớp khâu i j 1
khâu i-1 i i 1
    
i  1q1z 0  2q2z1  ...  iqi z i1    jqj z j 1
khớp i+1
khớp i-1 ai q1
xi j 1
ai x0
O’i Oi
di véc tơ đơn vị trục zi-1 Chiếu véc tơ vận tốc góc khâu i trong hệ cơ sở F0:
x’i
qi  
Oi-1 xi-1 z i 1  ez ,i 1 i(0)  1q1z 0(0)  2q2z1(0)  ...  iqi zi(0)
1
Vận tốc góc tương đối của khâu (i) đối với khâu (i-1) Chiếu véc tơ vận tốc góc khâu i trong hệ khâu Fi:
i 1     i(i )  1q1z 0(i )  2q2z1(i)  ...  iqi zi(i)1
i  i /i1  i zi 1  iqi z i1
45 46
Thiết lập ma trận Jacobi theo phương pháp hình học -47- Thiết lập ma trận Jacobi theo phương pháp hình học -48-
 Bi
    i
 x3
Ma trận Jacobi quay  
yi i  1q1z 0  2q2z1  ...  iqi zi 1    jq j z j 1 y3
 
Lưu ý rằng, hình chiếu của véc tơ đơn vị z i
zi j 1 E
q3
Chiếu véc tơ vận tốc góc khâu i trong hệ quy chiếu cơ sở F0: q2
trong hệ quy chiếu F0 zi(0) F0 O  y0
O2
xi z2
chính là cột thứ ba của ma trận quay Ri0
O’
i(0)  1q1z 0(0)  2q2z1(0)  ...  iqi zi(0)
1
l1 q1
    Chiếu véc tơ vận tốc góc khâu n trong hệ quy chiếu cơ sở F0: O0
x0
Hình chiếu của véc tơ đơn vị z i trong hqc Fi Ri0  [ii , ji , ki ] in F
   0
n(0)  1q1z 0(0)  2q2R10z1(1)  ...  nqn Rn01z n(n11)
(i ) T  [x i , yi , z i ] in F
z i
 k  [0, 0,1] 0
n(0)  1q1k  2q2R10k  ..  iqiRi01k 

(0) 0 (i )
z i
R z i i ...  nqn Rn01k  J R (q )q
47 48
29/03/2023
Ma trận Jacobi tịnh tiến

n(0)  1q1k  2q2R10k  ..  iqi Ri01k  ...  nqn Rn01k  J R (q )q n
vE(0)  JT (q )q  J q ,
T ,i i
JT (q )  [JT 1,.., JTi ,..., JTn ]
i 1
Từ đây viết được ma trận Jacobi quay như sau
x3 thành phần vận tốc qi do biến khớp i tham
J R (q )  [1k, 2R10k,.., i Ri01k,..., n Rn01k ] y3 gia vào vận tốc vE. Ở đây cần xem xét hai zi 1
E loại khớp: khớp quay và khớp tịnh tiến: qi
J R (q )  [J R1, J R 2 ,.., J Ri ,..., J Rn ] q3
q2 1. khớp quay
y0  
O2    rO s
Cột i của mt Jacobi quay z2 vE (i )  qi z i1  s i 1
E
   
z (0) 0 l1  qi z i1  (rE  rO ) rE
R k q1
J R,i  
Khớp i là khớp quay   i 1
i 1 i 1
qi zi1  i z i1
O0
x0
 0 Khớp i là khớp tịnh tiến khớp quay

49 50
z i1
Kết hợp Jacobi tịnh tiến và Jacobi quay
thành phần vận tốc qi do biến x3
khớp i tham gia vào vận tốc vE.
di y3
J (q )  [J 1, J 2 ,.., J i ,..., J n ]
2. khớp tịnh tiến: E
  J i (q ) là cột i của ma trận Jacobi J (q ) q3
rO s
  
vE (i )  qi z i 1  di z i 1
i 1
E q2
y0
 zi(0) O2
rE  1 Khớp i là khớp tịnh z2
Ji   tiến, i = 0
 0
Cột i của mt Jacobi tịnh tiến  l1 q1
q z  (r  r ),
khớp tịnh tiến z(0) (r (0)  r (0) ) O0
 khop R  1 E
vE (i )   i i 1 E Oi1 J i   i (0) Oi1 Khớp i là khớp quay, x0
qi z i 1, khop P  zi 1  Ri01k i = 1
 
viết ở dạng gộp lại
vE(0) zi1 (rE  rOi 1 )  z(0) (r (0)  r (0) )  (1   )z (0) 
(0) (0) (0) Khớp i là khớp quay
JT ,i   J i   i i 1 E Oi 1 i i 1 

qi  zi(0) Khớp i là khớp tịnh tiến  i zi(0) 
 1 1
51 52
29/03/2023
Thiết lập Jacobi quay (đối với hệ tọa độ khâu) -53- Thiết lập Jacobi quay (đối với hệ tọa độ khâu) -54-
Chiếu véc tơ vận tốc góc khâu i trong hệ cơ sở F0:

i(0)  J Ri (q )q
i(0)  1q1z 0(0)  2q2z1(0)  ...  iqi zi(0)
1
i(0)  Ri0 (q )i(i )  i(i )  [Ri0 (q )]T i(0)
i(0)  J Ri (q )q
i(i )  J Rii (q )q
Chiếu véc tơ vận tốc góc khâu i trong hệ khâu Fi:
i
i(i )  1q1z 0(i )  2q2z1(i)  ...  iqi zi(i)1 ? J Ri (q ) i(i )  [Ri0 (q )]T i(0)
 [Ri0 (q )]T J Ri (q )q
(i ) i
 i
 J (q )q
Ri
i
? J Ri (q ) i
J Ri (q )  [Ri0 (q )]T J Ri (q )
53 54
Ví dụ : tay máy phẳng hai bậc tự do với hai khớp quay 1  2  1

O2
z 0(0)  z1(0)  [0, 0, 1]T
E=O2
 q2
rO l2
 q2  0 l cq  y0 2
rO l2    1 1
 0 ,  l1sq1  ,
y0 2
rO(0) rO(0) l1 O1
l1 O1
0
  1
 
0  0  O0 q1
O0 q1
x0
x0 l cq  l c(q  q )
1 1 2 1 2 
  z(0) (r (0)  r (0) )  (1   )z (0)  rO(0)  l1sq1  l2s(q1  q2 ) ,
J i   i i1 E Oi 1 i i 1 
 2
    z(0) (r (0)  r (0) )  (1   )z (0) 
 i i 1 E i 1 
 i zi(0)
1  
0
 J i  
O i 1
i

  z (0)i i 1 
55 56
29/03/2023
z 0(0)  z1(0)  [0, 0, 1]T

S (z (0) )(r (0)  r (0) ) S (z (0) )(r (0)  r (0) )
 0
 
l cq 
 1 1
J   0
z
O2
(0)
O0 1
z
O2
(0)
O1 
   J (q )q
r(0)
  0 , r(0)
 l1sq1  ,
 0 1 
O0 O1
   
 0  0  Thực hiện các tính toán ta nhận được ma trận Jacobi
q  [q1, q2 ]T ,   [vE(0),x , vE(0),y , 2(0) ]T
l cq  l c(q  q )
1 1 2 
l sq  l s(q  q ) l s(q  q )
2 1
rO(0)  l1sq1  l2s(q1  q 2 ) ,
 1 1 2 1 2 2 1 2 
 l cq  l c(q  q ) l c(q  q ) 
2

 0

  1 1 2 1 2 2 1 2  l sq  l s(q  q ) l s(q  q )
   1 1 2 1 2 2 1 2 
0 0
 0  1 0

J (q )  

 J (q )   l1cq1  l2c(q1  q2 ) l2c(q1  q2 ) 
   0 0   
S(z 0(0) )  1 0 0    1 1 
   0 0 
 0 0 0  
 1 1 
 
57 58
Jacobi của điểm P bất kỳ thuộc khâu thứ k

J P (q )  [J 1, J 2 ,.., J i ,..., J n ]
Dùng để tính vận tốc điểm P và vận k vP
tốc góc của khâu k chứa điểm P  S (z (0) )(r (0)  r (0) )  (1   )z (0) 
J i   i i 1 
P i 1 P Oi 1 i
qk 1, qk 2 ,..., qn iz i(0)  , i  1, 2,..., k
k+1  1 
Ok
không có mặt trong vận tốc
góc khâu k, vận tốc điểm P. 0  k vP
F0 J i    , i  k  1, k  2,..., n P
0
Do đó các cột từ k+1 đến cột n  
của ma trận Jacobi này bằng 0 Ok
 v (0)  J P (q )  [J 1, J 2,.., J k , 0 ,..., 0 ]

 P   J (q )q, J P (q )  [J 1, J 2,.., J k , J k 1,..., J n ]
F0
(0)  P
 k 
59 60
29/03/2023
Jacobi hình học và Jacobi giải tích -61- Jacobi hình học và Jacobi giải tích -62-
Ma trận Jacobi được thiết lập trong các phần trên, được gọi là
Jacobi hình học (Geometric Jacobian). Ta cần phân biệt  (0)   0 cos sin q sin   
Jacobi hình học với Jacobi giải tích (analytical Jacobian) được  n(0)
,x
   
thiết lập từ cách biểu diễn tối giản hướng của khâu cuối. n ,y    0 sin  sin q cos  q 
 (0)  1 0 cos q   
 n ,z    
p (q ) F0
x   e   x  J a (q )q  B( )
  (q ) 
   B 1( )n(0)  B 1( )J R (q )q
pe (q )  rE(0)(q ) Tọa độ điểm tác động cuối. F0

 (q ) véc tơ tđsr biểu diễn hướng của bàn kẹp, Jacobi giải tích phụ thuộc vào cách biểu diễn tối giản hướng
T
của khâu cuối (tọa độ suy rộng hướng của khâu cuối)
ví dụ 3 góc Euler   [ , q ,  ]
61 62
Jacobi hình học và Jacobi giải tích -63- -64-
quan hệ giữa Jacobi hình học và Jacobi giải tích

det(B( ))  0
E
x  
0   J v (q ) 
  q
Các cấu hình kỳ dị của
1
0 B ( ) J R (q )
 
F0
robot
 J a (q )q
 J (q )  I 0 
J a (q )   1 v  1  J (q )
B ( )J R (q )  0 B ( )
   
63 64
29/03/2023
Các cấu hình kỳ dị của robot -65- Các cấu hình kỳ dị của robot -66-
Liên hệ giữa vận tốc, vận tốc góc của khâu cuối với véc tơ Việc nhận dạng các kỳ dị của robot có vai trò quan trọng vì các lý do sau:
vận tốc suy rộng (vận tốc các biến khớp). Đó là quan hệ
tuyến tính thông qua ma trận Jacobi. 1. Các kỳ dị thể hiện các cấu hình mà tại đó có một số hướng của chuyển động
có thể không thực hiện được
  J (q )q 2. Tại các điểm kỳ dị, với vận tốc giới nội của khâu cuối có thể dẫn đến vận tốc
biến khớp rất lớn
Khi nào thì ta tính được q  J 1(q )
3. Tại các điểm kỳ dị, với lực/mômen giới nội trên khâu cuối có thể dẫn đến
J (q ) vuông & det[J (q )]  0 lực/mômen khớp rất lớn
4. Các kỳ dị thông thường (nhưng không phải luôn luôn) ứng với các điểm trên
det[J (q )]  0 Robot ở vị trí (cấu hình) kỳ dị. biên của không gian làm việc
65 66
Các cấu hình kỳ dị của robot -67- Các cấu hình kỳ dị của robot -68-
Ví dụ cần tìm điểm kỳ dị của tay máy phẳng 2 bậc tự do với hai khớp quay
1. ..
2. . l cq  l c(q  q )
3. .
x  rO   1 1 2 1 2 

l1sq1  l2s(q1  q2 )
4. . 2 O2
5. Các kỳ dị ứng với các điểm bên trong vùng làm việc mà có  q2
rO l2
thể không với tới được do sự thay đổi nhỏ của tham số của y0 2
khâu như chiều dài, offset (lệch bên), v.v. l1 O1

rO(0)2
6. Ở gần các điểm kỳ dị, có thể không tồn tại nghiệm cho bài JT (q )  O0 q1
q x0
toán động học ngược. Trong những trường hợp này có thể
không có nghiệm hoặc có vô số nghiệm. x l sq  l s(q  q ) l s(q  q )
J (q )    1 1 2 1 2 2 1 2 
q l cq  l c(q  q ) l c (q  q )
 1 1 2 1 2 2 1 2 
67 68
29/03/2023
Các cấu hình kỳ dị của robot -69- Robot chuẩn và robot dư dẫn động -70-
  J (q )q q3
det[J (q )]  l1l2 sin q2  0 O2
Robot phẳng: Bàn kẹp chuyển động trong mặt phẳng có
Từ định thức này ta suy ra được các vị trí  3 bậc tự do
rO l2 q2 q2
kỳ dị của robot y0 2
dim()  3, n  dim(q )  3
sin q2  0 l1 O1
O0 q1
O0 q1 rank (J (q ))  3
x0 Robot phẳng chuẩn 3 dof
q2  0 Robot không gian: Bàn kẹp chuyển động trong không gian
q2  180
với 6 bậc tự do
dim()  3,
dim()  6, n  dim(q )  6
n  dim(q )  3
rank (J (q ))  6
69 70
Robot chuẩn và robot dư dẫn động -71- -72-
  J (q )q q4
dim()  3,
n  dim(q )  4 q3
q2
Động học ngược ở mức
q1
O0
vận tốc và gia tốc
Robot phẳng dư dẫn động
4 dof
71 72
29/03/2023
Động học ngược ở mức vận tốc và gia tốc -73- Động học ngược ở mức vận tốc và gia tốc -74-
 f f1 f1 
Nhắc lại các ký hiệu: Động học ở mức vị trí Các ma trận Jacobi  1 ... 
Véc tơ biến khớp  q1 q 2 q 6 
f (q, x)  0  f2 f2 f2 
f  ... 
q  [q1, q 2 ,..., q 6 ]T , q   6, Fq (q, x)    q1
q 
q 2 q 6  ,
... ... ... .. 
Động học ở mức vận tốc  
Véc tơ chứa vị trí và hướng khâu thao  f6 f6 f6 
...
tác (khâu n =6).
On
 f  dtd f (q, x)  0  q
 1 q 2 q 6 
 Fq (q, x)q  Fx (q, x)x  0  f1 f1 f1 
x  [x 1, x 2 ,..., x 6 ]T , x  6, F0 
 x 1 x 2
...
x 6 

 [x E , yE , z E , , ,  ]T Động học ở mức gia tốc f 

 f2 f2
...
f2 

Fx (q, x)    x 1 x 2 x 6 
x 
 f  dtd 2 f (q, x)  0
2
Sau khi giải bài toán động học thuận nhận được ... ... ... .. 
 
 f6 f6 f6 
 Fq (q, x) q  Fq (q, x)q  Fx (q, x)
x F (q, x)x  0 ...
f (q, x )  0 , f  [ f1, f2 ,..., f6 ]T , f  6 x  x
 1 x 2 x 6 
73 74
Đạo hàm ma trận theo thời gian là đạo hàm từng phần tử ma trận theo thời gian. Động học ngược ở mức vận tốc
d d
Fq (q, x)  Fq (q, x), F x (q, x)  Fx (q, x) Fq (q, x)q  Fx (q, x)x  0
dt dt Giải hệ phương trình
đại số tuyến tính
Động học ngược ở mức vị trí  q  [Fq (q, x)]1Fx (q, x)x
Biết hàm f (q, x)  0 cho x giải tìm q

t
Cần giải hệ phương
 q(t )  q(0)   q dt trình đại số phi tuyến
0
Sử dụng phương pháp hình học, phương pháp các nhóm 3, tìm q(0) từ x(0)
phương pháp lặp Newton-Raphson,…
Nghiệm không xác định tại điểm kỳ dị
Có thể gặp khó khăn khi giải hệ phương trình đại số phi tuyến. det[Fq (q, x)]  0
75 76
29/03/2023
Sau khi xác định được véc tơ vận tốc khớp, nếu tích phân ta sẽ nhận được véc tơ tọa
Động học ngược ở mức gia tốc
độ khớp, và đạo hàm sẽ nhận được gia tốc khớp.
 t
  Fq (q, x)q  Fx (q, x)
Fq (q, x)q x  Fx (q, x)x  0
q(t )  q(0)   q()d  1
q  [Fq (q, x)]1Fx (q, x)x q  .....   0

   Fq (q, x)
q   F (q, x)q  F (q, x)x  F (q, x)x 
  d  q x x
q  dt q

t
Như thế, ta có sơ đồ sau  q  q (0)   q
dt Cần giải hệ phương trình đại số
0 phi tuyến tìm q(0) từ x(0)
t
x(t ) q  q(t )  q(0)   q dt Cần giải hệ phương trình đại số
q  ..... 1 0
x (t ) s tuyến tính tìm qdot(0) từ xdot(0)
q
x(0) tìm qo
Nghiệm không xác định tại điểm kỳ dị
d q
det[Fq (q, x)]  0
dt
77 78
-79- -80-
• Ví dụ
E • Ví dụ
– Thiết lập các hệ tọa độ theo E
quy tắc DH, đưa ra bảng DH – Thiết lập các hệ tọa độ theo quy tắc DH,
l3
y0 đưa ra bảng DH a3
– Tính các ma trận DH
q2 – Tính các ma trận DH q3
– Cho quy luật chuyển động a2
các khớp, q(t) – Cho quy luật chuyển động các khớp, q(t)
z2
– Tính vị trí điểm E – Tính vị trí điểm E q2
d1 z0
– Tính vận tốc điểm E – Tính vận tốc điểm E
l1
– Tính vận tốc góc các khâu z0 q1 – Tính vận tốc góc các khâu
O0 – Tính Jacobi tịnh tiến và Jacobi quay của q1
– Tính Jacobi tịnh tiến và x0
Jacobi quay của khâu 3 khâu 3 x0
79 80

Robotics 6-1 Kinematics - Velocity - Jacobian Matrix - 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 6-1 Kinematics - Velocity - Jacobian Matrix - 4s

Uploaded by

Copyright:

Available Formats

29/03/2023

CHƯƠNG 6. ĐỘNG HỌC VI PHÂN và JACOBI -2-

1. Một số kiến thức về động học vật rắn

Chương 6. 2. Ma trận Jacobi: jacobi tịnh tiến và jacobi quay

-3- Véc tơ vận tốc góc của vật rắn -4-

• Đánh số các khâu: 0 -> n, khớp 1->n 2

ma trận Jacobi quay x0

• Suy ra các ma trận cosin chỉ hướng tuyệt đối

rij (q ) rij (q ) rij (q )  (0) 

q  [q1, q2 ,..., qn ]T

i(0)  J Ri (q )q, i  1,2,..., n

Liên hệ Ma trận Jacobi quay của khâu i trong

Xét điểm P có tọa độ điểm P trong hệ tọa độ khâu i, Fi.

u P(i )  [x P(i ), yP(i ), z P(i ) ]T  const Fi

rP(0)(q )  rO(0)(q )  Ri0 (q )u P(i ) F0

vP(0)(q )  rP(0)(q )  rO(0)(q )  R i0 (q )u P(i ) vO(0)  rO(0)

rˆP(0)(q )  Ti 0 (q )uˆP(i ) Fi q  [q1, q2 ,..., qn ]T Fi

  i Oi  P  Do đó được viết thành

v (0)    (0)R 0 vO(0)  u P(i ) 

  (x (0) / q )q   q q 2 qn  E E  x (0) x E(0) x E(0) 

Ma trận Jacobi -27- Ma trận Jacobi -28-

Ma trận Jacobi -29- Ma trận Jacobi -30-

Jacobi tịnh tiến ???

Ma trận Jacobi -31- Ma trận Jacobi -32-

 2(0)  R 20R20T  ?? O2  l sq  l s(q  q ) l s(q  q )

Ma trận Jacobi -33- Ma trận Jacobi -34-

sq cq13 0 l1cq1  q 2sq1  l 3sq13  y0

Ma trận Jacobi -35- Ma trận Jacobi -36-

yE  l1cq1  q 2sq1  l 3sq13 E

rE (q )  yE(0) yE(0) 

Ma trận Jacobi -37- Ma trận Jacobi -38-

Ma trận Jacobi -39- Ma trận Jacobi -40-

• Cần tính vận tốc góc của tất cả các khâu

rCi(0)(q )  JT ,i (q ) Phương pháp hình học

Phương pháp Denavit-Hartenberg -43- Phương pháp ma trận Denavit-Hartenberg -44-

n(0)  1q1k  2q2R10k  ..  iqiRi01k 

Ma trận Jacobi tịnh tiến

Chiếu véc tơ vận tốc góc khâu i trong hệ cơ sở F0:

Ví dụ : tay máy phẳng hai bậc tự do với hai khớp quay 1  2  1

z 0(0)  z1(0)  [0, 0, 1]T

Jacobi của điểm P bất kỳ thuộc khâu thứ k

 v (0)  J P (q )  [J 1, J 2,.., J k , 0 ,..., 0 ]

pe (q )  rE(0)(q ) Tọa độ điểm tác động cuối. F0

Jacobi hình học và Jacobi giải tích -63- -64-

quan hệ giữa Jacobi hình học và Jacobi giải tích

khâu như chiều dài, offset (lệch bên), v.v. l1 O1

Robot chuẩn và robot dư dẫn động -71- -72-

 [x E , yE , z E , , ,  ]T Động học ở mức gia tốc f 

Biết hàm f (q, x)  0 cho x giải tìm q

You might also like