CHƯƠNG 3

ĐỘNG HỌC VẬN TỐC ROBOT NỐI TIẾP

Động học vận tốc tìm sự liên hệ giữa vận tốc đầu cuối của robot và đạo hàm biến
khớp. Sự liên hệ này giúp giải nhiều bài toán như động học ngược, động lực học và lực
tác động…
Ví dụ 3.1: Động học vận tốc robot hai bậc tự do RP

Hình 3.1 Robot hai bậc tự do

Vị trí đầu cuối là

px  r cos()
p y  r sin()
(3.1)
Vận tốc dài đầu cuối là vector v đạo hàm của vị trí p
 dpx   dr d 
cos   r sin  
 vx   dt   dt dt    r s 
 rc
v    
   r c
v y   dp y   dr sin   r cos  d    rs
 dt   dt dt  (3.2)
đạo hàm Vx, Vy theo thời gian .....

Vận tốc góc ω là vector tỷ lệ vector đơn vị trục quay k và có giá trị là đạo hàm
góc quay k là vecto đơn vị theo trục quay

 k (3.3)
Phương trình liên hệ vận tốc đầu cuối và đạo hàm biến khớp là
c rs 
v  
     r   Jq gồm có Jv và Jomega
   s rc
  
  
0 k 
(3.4)
0 , k là vecto, ko phải số

J gọi là ma trận Jacobi của robot, phần tiếp theo trình bày cách tính J

KỸ THUẬT ROBOT- TS NGUYỄN ĐỨC THÀNH

 CHƯƠNG 3

3.1 MA TRẬN ĐỐI XỨNG GHỀNH

3.1.1 Định nghĩa Ma trận 3x3, đường chéo là zeo, đối xứng thì đổi dấu

Ma trận vuông S3x3 là ma trận đối xứng ghềnh (đxg, skew symmetry) nếu
S  ST  0
 0 3 2 
S   3 0 1   S () ma trận ddxg là biểu diễn của vecto omega dưới dạng ma
trận.
 2 1 0 
(3.5)

Ta có thể coi như S là biểu diễn dạng ma trận của vector ω=[ω1 ω2 ω3]T. Xét các
vector đơn vị
i  1 0 0 , j  0 1 0 , k  0 0 1
T T T

các ma trận S tương ứng là

0 0 0   0 0 1 0 1 0
S (i )  0 0 1 , S ( j )   0 0 0  , S (k )  1 0 0 
   
0 1 0   1 0 0  0 0 0 
(3.6)
Nếu S(u) là biểu diển ma trận đxg của vector u thì tich vector u x p biểu diển
dạng ma trận là tích ma trận đxg S(u) và vector p
 u z p y  u y pz 
 
u  p  S ( u) p   u z p x  u x p z 
 u y px  u x p y 
  (3.7)
Vector v=u x p thẳng góc hai vector u, p và có suất là |u||p|sinα, ba vecor v u p có
chiều theo qui tắc bàn tay phải.

Hình 3.2 Tích vector

3.1.2 Ma trận quay và ma trận đối xứng ghềnh

Cho a, b là các vectơ ba chiều, R là ma trận quay 3x3 góc θ , α, β là các số vô
hướng, có thể chứng minh các tính chất sau:

- S(αa+ βb) = αS(a) + βS(b) (3.8)

- R(a x b)=Ra x Rb (3.9)

68
ĐỘNG HỌC VẬN TỐC ROBOT NỐI TIẾP
- RS(a)RTb = R(a x RTb) = (Ra) x (RRTb)= (Ra) x b = S(Ra)b (3.10)
- RS(a)RT= S(Ra) (3.11)

- Đạo hàm ma trận quay:

Nếu R(θ) là ma trận quay thì

RTR=I,
dR T dRT
R R 0
d d

dR T dR
Ma trận S = R cũng là ma trận đxg, =SR
d d
Ta tìm biểu thức của S với phép quay góc θ quanh vector k, vector vận tốc góc là
 k . Ma trận quay quanh vector k=[kx ky kz]T là (2.23)
 k x2v  c k x k y v  k z s k x k z v  k y s 
 
Rk , ()   k x k y v  k z s k y2v  c k y k z v  k x s 
 
 k x k z v  k y s k y k z v  k x s k 2
v  c  
 z 
v  1  cos (3.12)

 k x2 s  s k x k y s  k z c k x k z s  k y c 
dRk ,  
  k x k y s  k z c k y2 s  s k y k z s  k xc 
d  
 k x k z s  k y c k y k z s  k xc k 2
s  s 
 z 

 k x2 s  s k x k y s  k z c k x k z s  k y c 
dRk ,  
RkT,   k x k y s  k z c k y2 s  s k y k z s  k xc  *
d  
 k x k z s  k y c k y k z s  k x c k z s  s 
2

 k x2v  c k x k y v  k z s k x k z v  k y s 
 
 k x k y v  k z s k y v  c
2
k y k z v  k x s 
 
 k x k z v  k y s k y k z v  k x s k z v  c 
2


 0 k z ky 
dRk ,  
RkT,   kz 0 k x   S (k ) (3.13)
d
 k y kx 0 


Vậy, nếu Rk,θ là ma trận quay góc θ quanh vector đơn vị k thì

69
 CHƯƠNG 3

dRk ,
 S (k ) Rk , (3.14)
d

Nếu lấy đạo hàm theo thời gian t

dRk , d
 S (k ) Rk ,  S ( k ) Rk ,  S (ω) Rk , (3.15)
dt dt

Ví dụ 3.2: Phép quay quanh trục z góc α có ma trận quay là

c  s0
R   s c 0  với α thay đổi theo thời gian, lấy đạo hàm theo thời gian
 0 0 1 
  s  c 0 
R    c  s 0 
 0 0 0 
  s  c 0  c s 0
 T    c  s 0   s c 0
Ma trận đxg là S  RR
  
 0 0 0   0 0 1 
 0  0 
S   0 0 
 0 0 0 

Vậy vector vận tốc góc là   0 0   là vector trục z và có suất 

T

Ví dụ 3.3: Cho hai hệ trục Ooxoyozo và O1x1y1z1 liên hệ với nhau bởi ma trận
 R01 (t ) O01 (t ) 
H (t )   
 0 1 
điểm P có toạ độ trong hệ 1 là p1 và vận tốc trong hệ 1 là v1  p1 , tìm vận tốc góc và
vận tốc dài của P trong hệ 0
Vận tốc góc của điểm P là vận tốc góc của hệ trục 1 so với hệ trục 0, giống như
Ví dụ 3.2 ta tính S  R 1 R1T từ đó tính 1 .
0 0 0
Vận tốc dài tính được bằng cách đạo hàm vector vị trí
p0  R01 p1  O01
p 0  R 01 p1  R01 p1  O 01
v0  SR01 p1  R01v1  O 01  10 xR01 p1  R01v1  O 01

70
ĐỘNG HỌC VẬN TỐC ROBOT NỐI TIẾP
3.2 VẬN TỐC GÓC ĐẦU CUỐI

Cho robot n bậc tự do, hướng đầu công tác biểu thị bởi các tích ma trận quay

R0n  R01 R12 ...Rnn1

dR0n dR01 2 dR 2 dR n
 R1 ...Rnn1  R01 1 R23 ...Rnn1  ...  R01 R12 ...Rnn21 n1
dt dt dt dt
n 1 2 n
dR0 dR0 n dR dR
 R1  R01 1 R2n  ...  R0n1 n1
dt dt dt dt

Gọi ii1 vận tốc góc khâu i so với khâu i-1, 0n là vận tốc góc đầu công tác so với hệ
trục gốc,

dR0n
 S (0n ) R0n
dt
dRii1
 S (ii -1 ) Rii1
dt

S (0n ) R0n  S (10 ) R01 R1n  R01 S (12 ) R12 R2n  ...  R0n1S (n-1
n
) Rnn1
S (0n ) R0n  S (10 ) R0n  S (R0112 ) R0n  ...  S (R0n1nn1 ) R0n
 {S (10 )  S (R0112 )  ...  S (R0n1nn1 )}R0n
 S (10  R0112  ...  R0n1nn1 ) R0n

Suy ra

0n  10  R0112  ...  R0n1nn1 (3.16)

Vận tốc góc đầu công tác so với hệ trục gốc là tổng vận tốc góc các khâu so với
hệ tọa độ gốc. Nếu khớp tịnh tiến thì vận tốc góc của khớp bằng không, do đó vận tốc
góc đầu công tác so với hệ trục gốc là tổng vận tốc góc các khớp quay so với hệ tọa
độ gốc. Đặt ηi=1 nếu khớp i là khớp quay, ηi=0 nếu khớp i là khớp trượt, qi=θi là biến
khớp, zii  0 0 1 là vector đơn vị trục quay của khớp thứ i trong hệ trục i, tích
T

z0i = R0i zii tính zii trong hệ tọa độ gốc Ox0y0z0 , là cột thứ ba của R0i

 0n x 
 n  n 1 n
0 y   1q1z0  2 q 2 R0 z1  3q3 R0 z2  ...  n q n R0 zn
1 1 2 2

 n 
 0 z 

71
 CHƯƠNG 3

 q1   q1 
 0n x   q   q 
 n   2   2 
0 y   [1z0 2 z0 3 z0 ...n z0 ]  :   J   : 
1 2 n-1

 n     
 q   q 
 0 z  n 1 n 1
 q n   q n 
(3.17)
Jω là ma trận Jacobi vận tốc góc 3 hàng n cột, J i  z0i 1 nếu khớp i là khớp quay,
J i  0 nếu khớp i là khớp tịnh tiến,

3.3 VẬN TỐC DÀI ĐẦU CÔNG TÁC

Vận tốc dài đầu công tác là đạo hàm vectơ vị trí theo thời gian

px = f1(q1 ,q2 ,q3 ,...,qn )

p y = f 2 (q1 ,q2 ,q3 ,...,qn )
pz = f 3 (q1 ,q2 ,q3 ,...,qn )
 dpx   df1 df1 df 
 ... 
1

 vx = dt   dq1 dq2 dqn   q1   q1  (3.18)

     q 
 dp y   df 2 df 2 df 2   q 2 
v y = = ...  = Jv  2 
dt   dq1 dq2 dqn   .   .
     
v = dpz   df df3 df3   q n   q n 
dt   dq 
3
 z ...
 1 dq2 dqn 

Jv là ma trận Jacobi vận tốc dài 3 hàng n cột.

Thay vì tính đạo hàm ta dùng ma trận đồng nhất để tính vận tốc dài

Hình 3.3 Tính vận tốc dài

a/ Trường hợp khớp i tịnh tiến: đầu công tác cũng sẽ tịnh tiến theo

72
ĐỘNG HỌC VẬN TỐC ROBOT NỐI TIẾP
 R0n p0n  i 1 i  R0i 1 p0i 1   Rii1 pii1   Rin pin 
H 0n    H 0 H i 1H i  
n
  
0 1  0 1  0 1  0 1
 R0n R0i pin  R0i 1 pii1  p0i 1 
 
0 1 

Tọa độ đầu công tác : p0n  R0i pin  R0i 1 pii1  p0i 1
Nếu chỉ có khớp i di chuyển pin , p0i 1 không đổi, các ma trận quay cũng không
đổi, và pii1  [ai ci ai si di ] trong đó di thay đổi

0 
dp0n i
i 1 dpi 1
 R0  di R0 0   di z0i 1
i 1
dqi ddi
1 

z0i1 là hình chiếu trục zi-1 trên hệ tọa độ gốc, là hướng tịnh tiến của khớp i
J vi  di z0i 1 (3.19)
b/ Trường hợp khớp i quay quanh trục zi-1:

dp0n d i n d d
 ( R0 pi  R0i 1 pii1 )  ( R0i pin )  R0i 1 ( pii1 )
dt dt dt dt
d
  i S ( z0i 1 ) R0i pin  R0i 1 ( pii1 )
dti
 ai ci   ai si   0  i 0   ai ci  0
d i d   
 a c         0  ) pi
( pi 1 )  a s    0 0  a s  S (
d i         i 1
i i i i i i i i
dt
 di   0   0 
0 0   di   i 

 S ( z  ) pi
i 1 i i 1

d i
R0i 1 ( pi 1 )  R0i 1S ( zi 1 i ) pii1  R0i 1S ( zi 1 i )( R0i 1 )T R0i 1 pii1
dt
 S ( R0i 1zi 1 i ) R0i 1 pii1   i S ( z0i 1 ) R0i 1 pii1
dp0n 
 i S ( z0i 1 )( R0i pin  R0i 1 pii1 )
dt

mà R0i pin  R0i 1 pii1  p0n  p0i 1

dp0n 
Nên  θi S ( z0i 1 ) ( p0n  p0i1 )= θ i ( z0i1 x( p0n  p0i1 ))
dt

73
 CHƯƠNG 3

Vậy J vi  z0i 1 x ( p0n  p0i 1 ) (3.20)

p0n là tọa độ đầu công tác trong hệ tọa độ gốc
p0i1 là tọa độ gốc hệ trục i-1 trong hệ tọa độ gốc
p0n  p0i1 là vectơ nối gốc hệ trục i-1 với đầu công tác, biểu thị trong hệ tọa độ gốc

3.4 MA TRẬN JACOBI

3.4.1 Ma trận Jacobi hình học

J 
J   v    J1 J 2 ... J n 
J  (3.21)
Ma trận Jacobi có số cột là số bậc tự do, số hàng là 6
Nếu khớp i là khớp quay
 z0i 1  ( p0n  p0i 1 )   S ( z0i 1 )( p0n  p0i 1 ) 
Ji    
 z0i 1   z0i 1  (3.22)
Nếu khớp i là tịnh tiến
 z0i 1 
Ji   
 0  (3.23)
p là vị trí gốc tọa độ i-1 so với hệ trục gốc, là cột thứ tư của ma trận H
i1
0
i1
0

z0i1 là vectơ trục zi-1 so với hệ trục gốc, là cột thứ ba của ma trận quay R0i1
Sự liên hệ của đạo hàm biến khớp và vận tốc dài, vận tốc góc đầu công tác được
biểu thị bởi công thức
v 
   Jq
  (3.24)
Nếu chọn một hệ tọa độ w khác so với hệ tọa độ gốc 0 ta tìm ma trận quay R0,w
biểu thị hướng của hệ 0 so với hệ w, ta có các biểu thức sau để tìm Jacobi của robot
trong hệ w:
 p e,w   R0,w 0   p e,0 
ψ    0 R0,w  ψ e,0 
 e,w  
 R0,w 0 
Jw  
R0,w 
J
 0 (3.25)

Ví dụ 3.4: Robot RR

74
ĐỘNG HỌC VẬN TỐC ROBOT NỐI TIẾP

Hình 3.4 Tính ma trận Jacobi robot RR

c1  s1 0 a1c1  c2  s2 0 a2c2 

s c1 0 a1s1  2  s2
 c2 0 a2 s2 
H0   1
1
, H1  ,
0 0 1 0  0 0 1 0 
   
0 0 0 1  0 0 0 1 
c12  s12 0 a1c1  a2c12 
s c12 0 a1s1  a2 s12 
H 0   12
2
0 0 1 0 
 
0 0 0 1 
 z xp2 z10 x(p02  p10 ) 
J  0 0 
 z0 z10 
z10 tương ứng cột thứ ba của H 01 , p10 , p02 lần lượt tương ứng cột thứ tư của H 01 và H 02
Các trục quay thẳng góc mặt phẳng robot z0 = z10 = [0 0 1]T, p10 =[a1c1 a1s1 0]T,
p02 =[a1c1+ a2c12 a1s1+ a2s12 0]T. Áp dụng công thức (3.7)

  p02y   a1s1  a2 s12 

 
z0 xp02   p02x    a1c1  a2 c12 
 0   0 
 

 ( p02y  p01 y )   a2 s12 

 
z1x(p02  p10 )   p02x  p01 x    a2 c12 
 0   0 
 

75
 CHƯƠNG 3

 a1s1  a2 s12 a2 s12 

 ac a c a2c12 
 1 1 2 12
 0 0 
Suy ra J   (3.26)
 0 0 
 0 0 
 
 1 1 
Ví dụ 3.5: xét robot phẳng RRR Ví dụ 2.9, ma trận Jacobi là
 z x( p3  p0 ) z1x( p3  p1 ) z2 x( p3  p2 ) 
J  0 
 z0 z1 z2 
ta tính được
p0  0 0 0 , p1   a1c1 a1s1 0 ,
T T

p2   a1c1  a2c12 0 ,
T
a1s1  a2 s12
p3   a1c1  a2c12  a3c123 a1s1  a2 s12  a3s123 0 ,
T

z0  z1  z2  0 0 1
T

Suy ra
 (a1s1  a2 s12  a3s132 ) (a2 s12  a3s132 ) a3s132 
 ac a c a c a2c12  a3c123 a3c123 
 1 1 2 12 3 123
 0 0 0 
J   (3.27)
 0 0 0 
 0 0 0 
 
 1 1 1 

Loại bỏ các hàng bằng 0 ta được ma trận 3x3

 (a1s1  a2 s12  a3s132 ) (a2 s12  a3s132 ) a3s132 
J   a1c1  a2c12  a3c123 a2c12  a3c123 a3c123 
 1 1 1 
Ví dụ 3.6: Xét Robot RPR Ví dụ 2.13 ta viết chương trình tính ma trận Jacobi
bằng hai cách: tính Jv dùng hàm jacobian(p3,[theta1;d2;theta3]) và dùng công thức

 z  p3 z1 z2  ( p03  p02 ) 
J  0 0 
 z0 0 z2 

clc
syms a alp d th d1 d2 th1 th3 pi a3;
hi=[cos(th) -cos(alp)*sin(th) sin(alp)*sin(th) a*cos(th);
sin(th) cos(alp)*cos(th) -sin(alp)*cos(th) a*sin(th);
0 sin(alp) cos(alp) d;

76
ĐỘNG HỌC VẬN TỐC ROBOT NỐI TIẾP
0 0 0 1];
a=0; alp=pi/2; d=0; th=th1;
h1=subs(hi);
h1=expand(h1)
a=0; alp=-pi/2;th=0 ; d=d2;
h2=subs(hi);
h2=expand(h2)
a=a3; alp=0;d=0;th=th3;
h3=subs(hi);
h3=expand(h3)
h02=h1*h2
%forward kinematics
h03=h1*h2*h3;
z0=[0 0 1];
z1=h1(1:3,3)
z2=h02(1:3,3)
p3=h03(1:3,4)
%Tính Jacobi
Jac=jacobian(p3,[th1;d2;th3])
p2=h02(1:3,4)
%z0xp3
j1=[-p3(2) p3(1) 0]
%z2x(p3-p2)
p32=p3-p2
j3=[-z2(3)*p32(2)+z2(2)*p32(3) z2(3)*p32(1)-z2(1)*p32(3) -
z2(2)*p32(1)+z2(1)*p32(2)]
jacobcv=[j1 z0; z1' 0 0 0;j3 z2']

Từ kết quả chương trình ta suy ra

 d 2c1  a3s13 s1 a3s13 

d s  a c c1 a3c13 
 21 3 13
 0 0 0 
J   (3.28)
 0 0 0 
 0 0 0 
 
 1 0 1 

Xét trong mặt phẳng

 d 2c1  a3s13 s1 a3s13 
J   d 2 s1  a3c13 c1 a3c13 
 1 0 1 

3.4.2 Ma Trận Jacobi Giải Tích

Mục 3.4.1 cho thấy thành phần Jv trong ma trận Jacobi có thể tính bằng cách lấy
đạo hàm biểu thức của vị trí đầu cuối, suy ra nếu ta biểu diễn hướng của đầu cuối theo
ba góc Euler α=[φ θ ψ]T và tìm biểu thức của α theo biến khớp thì ta có thể tìm Jacobi
của thành phần vận tốc góc tương tự.
77
 CHƯƠNG 3

Xét biểu thức

 p (q ) 
X  
 α (q ) 

vector X6x1 gọi là tư thế (pose) của robot, đạo hàm riêng theo biến khớp q các biểu
thức của p(q) tọa độ đầu cuối và α(q) góc Euler, ta được ma trận gọi là Jacobi giải tích
 p 
X     J a (q)q
 α  (3.29)
Ma trận Jacobi giải tích khác với ma trận Jacobi mục 3.4.1 (gọi là ma trận Jacobi
hình học vì α khác  (vector vận tốc góc).
Xét phép quay ba góc Euler quanh trục hiện tại ZYZ Rz,ψ Ry,θ Rz,φ, ta công nhận
không chứng minh biểu thức quan hệ giữa α và 
c s  s   c s  s 0    
 
   s s  c     s s c 0      B( )
 c      c 0 1  
 
 (3.30)
từ đó suy ra

 v   v  I 0  v  I 0 
J (q)q             J a q (3.31)
  B(α)α   0 B(α)  α   0 B(α) 

Vậy quan hệ giữa J và Ja là

I 0 
J (q )    J a (q )
 0 B ( α )  (3.32)
I 0 
J a (q )   1  J (q), det B(α)  0
 0 B ( α )  (3.33)
Nếu xét phép quay Roll Pitch Yaw quanh hệ trục cố định góc φ quanh x0, θ
quanh y0 và ψ quanh z0 thì
c c  s 0 
B ( )   s c c 0 
  s 0 1 
(3.34)
Với robot RRR ta có
 (a1s1  a2 s12  a3s132 ) (a2 s12  a3s132 ) a3s132 

J a   a1c1  a2c12  a3c123 a1c1  a2c12  a3c123 a3c123  (3.35)
 1 1 1 

78
ĐỘNG HỌC VẬN TỐC ROBOT NỐI TIẾP
3.5 ĐỘNG HỌC VẬN TỐC NGƯỢC
Ma trận Jacobi có 6 hàng n cột, nếu n=6 và det(J)≠0 ta có thể tính động học vận
tốc ngược theo biểu thức
 v (t ) 
q (t )  J 1 (q(t ))  
(t )  (3.36)
Điều này có nghĩa cho trước vận tốc đầu cuối và các biến khớp, ta tìm được đạo
hàm biến khớp tương ứng.
Điều kiện khả đảo của J không phải lúc nào cũng được thỏa mãn, xét ví dụ 3.2,
J có hai hàng độc lập tuyến tính, nghĩa là rank(J)=2, vậy ta chỉ tìm được [ p x p y ] theo
 1  2  còn vận tốc góc không độc lập. Từ (3.26)

 p x   a1s1  a2 s12 a2 s12    1 

 p     ,
 y   a1c1  a2c12 a2c12   2 

tính ma trận ngược của J:

 a2c12 (a1c1  a2c12 ) 

a s (a1s1  a2 s12 ) 
J   2 12
1
a1a2 s2

 a2c12 (a1c1  a2c12 ) 

  1   a2 s12 (a1s1  a2 s12 )   p ex (t ) 
    p (t )  (3.37)
 2  a1a2 s2  ey 

Khi s2=sin(θ2)=0 thì det(J)=0, ta không tính nghịch đảo được, lúc này robot rơi
vào vị trí bất thường (singular) , θ2=0 hai cánh tay thẳng hàng hay θ2=±180o, hai cánh
tay chập vào nhau. Ở điểm bất thường đạo hàm biến khớp tăng vô hạn, vậy ta phải
tránh không cho robot rơi vào điểm bất thường.
Xét Ví dụ 3.6, robot ba bậc tự do, từ (3.28) J có ba hàng độc lập tuyến tính là 1,2
và 6, vậy ta có thể viết

 p x   d 2c1  a3s13 s1 a3s13    1 

    
 p y    d 2 s1  a3c13 c1 a3c13   d2 
    1 0 1    3 
 z

det(J) = -d2 ≠ 0 , vậy ma trận J luôn luôn khả đảo.

Hạng của ma trận J là số hàng r của ma trận độc lập tuyến tính nghĩa là bậc của
định thức lấy từ J khác 0. Khi robot có số bậc tự do n lớn hơn hạng r của J thì gọi là
robot dư (redundant robot) ta có thể dùng ma trận giả đảo (pseudo inverse) J+ thông
79
 CHƯƠNG 3

qua ma trận (JJT)-1, ở đây Jrxn là ma trận có r hàng rút ra từ ma trận hình học Jacobi,
JJT là ma trận vuông rxr, từ các biểu thức sau
( JJ T )( JJ T ) 1  I ,
J [J T ( JJ T )1 ]  I (3.38)
Đặt J   J T (JJ T )1 ta thấy JJ = I, ma trận J n hàng r cột gọi là ma trận giả đảo
+ +

bên phải của J. Gọi η vector r x 1 lấy từ các thành phần của [ v ω]T ( theo các hàng
độc lập tuyến tính của J) ta tính được
qnx1  J nxr rx1
(3.39)
Cho biểu thức quỹ đạo định trước của đầu cuối robot ta dễ dàng tính η(t) từ đó
suy ra q (t ) và tích phân để được q(t), chính là giá trị đặt cho bộ điều khiển vị trí các
khâu của robot
t
q(t )  q(0)   q ()d 
0 (3.40)
q(tk 1)  q(tk )  q (tk )t (3.41)
Vậy ta đã giải bài toán động học ngược vị trí thông qua động học ngược vận tốc.
Ta cũng có thể tìm biểu thức của gia tốc, lấy đạo hàm (3.24) theo thời gian
 v  
    Jq  Jq
 

 v  
   J 1  v   J 1JJ
 1  v 
q  J 1    J 1Jq    
     (3.42)

3.6 VÒNG KÍN ĐỘNG HỌC NGƯỢC VỊ TRÍ

Công thức (3.39) thay tích phân bằng tổng do đó sẽ gây ra sai số, vì vậy ta sẽ
dùng sai số vị trí trong không gian để hiệu chỉnh giá trị của biến khớp. Đặt
e = xd - xe
lấy đạo hàm ta được
e  x d  x e  x d  J a (q)q
Đặt q  J a (q)1 ( x d  Ke) (3.43)
ta suy ra
e  Ke  0
Vậy sai số vị trí tiến về 0 khi chọn K là ma trận chép vuông có hệ số lớn, sơ đồ
tính q trình bày ở Hình 3.5

80
ĐỘNG HỌC VẬN TỐC ROBOT NỐI TIẾP

Hình 3.5 Tính động học ngược vị trí thông qua động học ngược vận tốc

Phương pháp trên đòi hỏi phải tính ma trận Jacobi đảo, một phương pháp khác
dùng ma trận Jacobi chuyển vị, ta đặt hàm Liapunov
1
V  eT Ke > 0
2
Lấy đạo hàm theo thời gian
V  eT Ke = eT Kx  eT Kx
d e

 e Kx d  e KJ a q
T T

Nếu x d  0 ta chọn q  J aT Ke suy ra

V  eT KJ a J aT Ke < 0
Vậy sai số e tiến về 0

Hình 3.6 Tính động học ngược vị trí thông qua động học ngược vận tốc
Nếu xét gia tốc biến khớp ta sẽ có sơ đồ vòng kín động học ngược vị trí bậc hai
sử dụng luật điều khiển bậc hai
a x  J q )
q  J 1 ( e a

81
 CHƯƠNG 3

Hình 3.7 Vòng kín bậc hai động học ngược vị tri

3.7 LỰC VÀ MOMENT

Lực và moment tác động lên đầu công tác sẽ truyền xuống các biến khớp, ngược
lại moment và lực do các động cơ khớp cũng sẽ tác động lực và moment lên môi
trường đầu công tác tiếp xúc. Nếu khớp tịnh tiến ta có lực tác động, nếu khớp quay ta
có moment.
Trong nhiều trường hợp ngoài việc điều khiển đầu công tác theo một quĩ đạo, ta
còn phải điều khiển lực tác động lên môi trường làm việc của đầu công tác, ví dụ như
lắp ráp, gia công.

Hình 3.8 Lực và moment tác động lên môi trường

Gọi F lực tác động lên đầu công tác và τ moment tác động lên các khớp, δX
khoảng dịch chuyển nhỏ của đầu công tác do lực F, δq khoảng dịch chuyển tương ứng
của biến khớp, ta có biểu thức:
δX=J(q) δq
Công ảo sinh ra là δw=F δX- τ δq= (FT J(q)- τT )δq
T T

Khi hệ thống cân bằng ta có:

82
ĐỘNG HỌC VẬN TỐC ROBOT NỐI TIẾP
τ  J T q F
(3.44)
Trong không gian F là vectơ 6x1 gồm ba thành phần lực và ba thành phần
moment.
Ví dụ 3.7: Xét robot RR, lực mà đầu công tác cần tác động là F=[1 1]T, tìm
moment các động cơ khớp phải tạo ra
Từ (3.26)

 a s  a s a1c1  a2c12 
J T   1 1 2 12
 a2 s12 a2c12 

 a s  a s  a c  a c 
τ  J T  q  F   1 1 2 12 1 1 2 12 
 a2 s12  a2c12 

Hình 3.9 Moment các khớp

BÀI TẬP
BT1 Hệ trục 1 liên hệ với hệ trục 0 bởi ma trận đồng nhất
0 1 0 1 
1 0 0 1
H 01   
0 0 1 0 
 
0 0 0 1 
Một điểm trong hệ toạ độ 1 có vận tốc v11  3 0 2 , tìm vận tốc điểm này trong hệ
T

toạ độ 0.
BT2 Cho ba hệ toạ độ 0, 1, 2 với các trục quay 10  1 1 0 , 12  1 0 1
T T
.
Tính 02 khi ma trận hướng là
1 0 0 
R12  0 0 1
0 1 0 

BT3 Viết chương trình Matlab tính Jacobi robot phẳng RRR

83
 CHƯƠNG 3

Đáp số:
 a1s1  a2 s12  a3s123 a2 s12  a3 s123 a3 s123 
 ac a c a c a2c12  a3c123 a3c123 
 1 1 2 12 3 123
 0 0 0 
J  
 0 0 0 
 0 0 0 
 
 1 1 1 
BT4 Viết chương trình Matlab tính Jacobi robot khuỷu RRR
Đáp số:
  s1 (a2c2  a3c23 ) c1 (a2 s2  a3s23 ) a3c1s23 
 c (a c  a c )  s (a s  a s ) a s s 
 1 2 2 3 23 1 2 2 3 23 3 1 23 
 0 a2c2  a3c23 a3c23 
J  
 0 s1 s1 
 0 c1 c1 
 
 1 1 1 
BT5 Tính J của robot RRP có bảng DH sau
a α d θ
0 90 0 θ1
0 90 0 θ2
0 0 d3 0

BT6 Cho robot RRR có chiều dài cánh tay 0.25 m, vị trí ban đầu q = [π -π/2 -π/2]T,
tương ứng xe = 0 m, ye = 0.5 m, ϕe = 0 rad di chuyển theo vòng tròn bán kính 0.25m,
tâm (0.25 0.5) m trong thời gian 4s với phương trình
 
 d
x  0.25  0.25cos t 
 y   0.5  0.25sin t 

 d  
 d   t 
 sin 
 24 
Lập trình simulink giải bài toán động học ngược vi trí vòng kín, vẽ q(t) và sai số vị trí,
sai số hướng.

84