Chương 5: Ma trận Jacoby

(Jacobian Matrix)
-Bài toán động học thuận vị trí

Q  q1 , q2 ,..., qn T  X   px , p y , pz T

 px 
 Rn0 p y 
Tn0  
 pz 
 
 

-Bài toán động học thuận tốc độ

Q  
 q1 , q 2 ,..., q n T  X  p x , p y , p z ,  x ,  y ,  z  T
Ma trận Jacoby của Robot nDOF

X
J 6n 
Q
Ma trận Jacoby của Robot nDOF

X
J 6n 
Q

 p x p x p x 
 q ... 
q2 qn
 1 
 p y p y
...
p y 
 q1 q2 qn 
 p p z p z 
 z ... 
X  q1 q2 qn 
J 6n  
Q  x  x  x 
 ... 
 q1 q2 qn 
  y  y  y 
 q ...
q2 qn 
 1 
  z  z
...
 z 
 q1 q2 qn 
Ví dụ: 1. Robot -r Q   , r 
T

Y0
 p x  rc 
 p  rs
 y
 p z  0 py H

 x  0
r
 y  0

 z  


px X0

Z0
 Dạng đầy đủ

 p x  rc Q   , r 
T
 rs c 
 p  rs  rc 
 y  s 
 p z  0
X  0 0
 J 6n   
 x  0 Q  0 0
 y  0  0 0
  
 z    1 0 

Dạng điều khiển

 rs c 
J 22 
 rc  s 
II. Ma trận Jacoby của Robot có nDOF>6


X  px , p y , pz ,  x ,  y ,  z  T

Không tường minh

Giải pháp: Sử dụng thuật toán ma trận JH

3 bước
Bước 1: Xác định ma trận Tin (i=0->n-1)

 i nx i
ox i
ax i
px 
i i i i 
 ny oy ay py 
Tn  i
i
 Ai 1 ( i 1 )...An ( n ) Theo đúng luật
 nz i
oz i
az i
pz  D-H
 
 0 0 0 1 
Bước 2: Xác định ma trận JH  H p x H
p x H
p x 
 ... 
 Hq1 H
q2 qn 
 p y p y H
p y 
 ... 
 Hq1 q2 qn 
 p z p z p z 
H H
...
 H X  q1 q2 qn 
J H 6n  H 
Q   x H
 x H
 x 
...
 q1 q2 qn 
H 
  y  y  y 
H H

...
 q1 q2 qn 
H 
  z  z  z 
H H
...
 q1 q2 qn 
Bước 2: Xác định ma trận JH
 H p x H
p x H
p x 
 ... 
 Hq1 H
q2 qn 
 p y p y H
p y 
Khi (i+1) là khớp trượt,  ... 
biến khớp ri+1  Hq1 q2 qn 
 p z p z p z 
H H
...
 H X  q1 q2 qn 
J H 6n  H 
Q   x H
 x H
 x 
Sử dụng ma trận Tni ...
 q1 q2 qn 
H 
  y  y  y 
H H
H
p x i ...
 nz  q1 q2 qn 
ri 1 H 
  z  z  z 
H H
...
H
p y  q1 q2 qn 
 i oz
ri 1
H
p z i H
 x
H
 y H  y
 az   0
ri 1 ri 1 ri 1 ri 1
Khi (i+1) là khớp quay, biến khớp i+1

H
p x i i H
 x i
 n y p x  i nx i p y  nz
 i 1  i 1

H
p y H
 y
 o y p x  ox p y
i i i i
 i oz
 i 1  i 1

H
p z i i H
 z i
 a y px i ax i p y  az
 i 1  i 1
Bước 3: Tính J

 Rn0 0 H
J  0
 J
0 Rn 