Chương IV.

Động học ngược vị trí

qn
y

Q  q1 , q2 ,..., qn 
T
…
q2 X(t)

X (t )  x(t ), y (t ), z (t ),  x (t ),  y (t ),  z (t ) 
T

q1
x

z
-Nhiệm vụ của bài toán động học ngược vị trí


X (t )  x(t ), y (t ), z (t ),  x (t ),  y (t ),  z (t ) 
T
Q  q1 , q2 ,..., qn 
T

Hoặc

 nx ox ax px 
n oy ay p y 
Tn0   y
 nz oz az pz 
 
0 0 0 1
Một số khái niệm toán trong động học đảo giải tích
Tính thông thường
py
  arctg
px

Khi px=0 Máy tính báo lỗi

Sử dụng hàm ATAN2 thay thế arctg

= ATAN2(py,px)

ATAN2(1,1)=45o
ATAN2(-1,1)=-45o
ATAN2(1,-1)=135o
ATAN2(-1,-1)=-135o
Các dạng động học đảo giải tích cơ sở

T/h 1: sin =a Nghiệm duy nhất

cos=b =ATAN2(a,b)
T/h 2: sin =a cos    1  a 2

  ATAN 2 a, 1  a 2  2 nghiệm: , 180o-

o
Lưu ý: a  1    90 Điểm cực
T/h 3: cos =b sin    1  b 2

  ATAN 2  1  b 2 , b  2 nghiệm: , -

Lưu ý:b  0    90 o ,270 0 Điểm cực

T/h 4: acos bsin=0

2 nghiệm : =Atan2(-a,b) hoặc =Atan2(a,-b)

T/h 5: acos bsin=c

2 nghiệm: 
  ATAN 2(b, a )  ATAN 2  a 2  b 2  c 2 , c 
T/h 6: sin sin =a 2 nghiệm : =Atan2(a,b) nếu sin>0
cos sin =b =Atan2(-a,-b) nếu sìn<0

Nếu cos=c 
  ATAN 2  a 2  b 2 , c 
2 nghiệm   ATAN 2(a, b)   ATAN 2(a,b)


  ATAN 2 a 2  b 2 , c  

  ATAN 2  a 2  b 2 , c 
 ad  bc
T/h 7: a cos - b sin=c sin   2 2
a sin+b cos=d a b
ac  bd
cos   2 2
Nghiệm duy nhất a b

  ATAN 2ad  bc, ac  bd 

Các phương pháp giải bài toán động học ngược

+ Phép đảo vị trí: áp dụng cho các Robot 2DOF

+ Phép đảo hướng: áp dụng cho phép quay cố định
+ Phương pháp phân ly các biến : cho các Robot có
số bậc tự do >=3
I. Phép đảo vị trí

Biết  px 
PE   
 py 

l1 ,l2

Xác định 1 

Q 
 2 

p x2  p y2  l12  l22
cos  2 
2l1l2
 p x2  p y2  l12  l22
cos  2 
2l1l2
sin  2   1  cos 2  2

 2  ATAN 2(sin  2 , cos  2 )

  2  ATAN 2(sin  2 , cos  2 )

1  ATAN 2( p y , p x )  ATAN 2(l2 s2 , l1  l2 c2 )

II. Phép đảo hướng
 nx ox ax px 
Biết n,o,a,p n oy ay p y 
T3  Rot ( z ,  z ) Rot ( y,  y ) Rot ( x,  x )   y
0
 nz oz az pz 
 
0 0 0 1
Xác định z ,y,x

Nhân 2 vế với Rot 1 ( z ,  z )

Rot 1 ( z,  z )T30  Rot 1 ( z,  z ) Rot ( z,  z ) Rot ( y,  y ) Rot ( x,  x )

Rot 1 ( z ,  z )T30  Rot ( y,  y ) Rot ( x,  x )

VT
VF
  c z s z 0 0  n x ox ax px 
  s c z 0 0 n y oy ay p y 
VT  Rot ( z ,  z )T3  
1 0 z

 0 0 1 0  n z oz az pz 
  
 0 0 0 1  0 0 0 1
 nx c z  n y s z ox c z  o y s z a x c z  a y s z 0
 n s  n c  ox s z  o y c z  a x s z  a y c z 0
 x z y z

 nz oz az 0
 
 0 0 0 1

 c y 0 s y 0 1 0 0 0
 0 1 0 0 0 c x  s x 0
VP  Rot ( y,  y ) Rot ( x,  x )  
 s y 0 c y 0 0 s y c x 0
  
 0 0 0 1  0 0 0 1
 c y s y s x s y c x 0
 0 c x  s x 0
 
 s y c y s x c y c x 0
 
 0 0 0 1
  nx c z  n y s z ox c z  o y s z a x c z  a y s z 0
 n s  n c  ox s z  o y c z  a x s z  a y c z 0
VF   x z y z

 nz oz az 0
 
 0 0 0 1

 c y s y s x s y c x 0
 0 c x  s x 0
VT  
  s y c y s x c y c x 0
 
 0 0 0 1

 nx s z  n y c z  0  z  ATAN 2(n y , nx )
  nx c z  n y s z ox c z  o y s z a x c z  a y s z 0
 n s  n c  ox s z  o y c z  a x s z  a y c z 0
VF   x z y z

 nz oz az 0
 
 0 0 0 1

 c y s y s x s y c x 0
 0 c x  s x 0
VT  
  s y c y s x c y c x 0
 
 0 0 0 1

 z  ATAN 2(n y , nx )

 y  ATAN 2(nz , nx c z  n y s z )

c x  ox s z  o y c z
   x  ATAN 2(a x s z  a y c z ,ox s z  o y c z )
s x  a x s z  a y c z
  nx c z  n y s z ox c z  o y s z a x c z  a y s z 0
 n s  n c  ox s z  o y c z  a x s z  a y c z 0
VF   x z y z

 nz oz az 0
 
 0 0 0 1

 c y s y s x s y c x 0
 0 c x  s x 0
VT  
  s y c y s x c y c x 0
 
 0 0 0 1

 z  ATAN 2(n y , nx )

 y  ATAN 2(nz , nx c z  n y s z )

c x  ox s z  o y c z
   x  ATAN 2(a x s z  a y c z ,ox s z  o y c z )
s x  a x s z  a y c z
III. Phương pháp phân ly biến

 nx ox ax px 
n oy ay p y 
Tn0   y  A1 (1 ) A2 ( 2 )... An ( n )
 nz oz az pz 
 
0 0 0 1
Tn0 (1 ,..., n)  A1 (1 ) A2 ( 2 )... An ( n )

Tn1 ( 2 ,..., n)  A11 (1 )Tn0  A2 ( 2 )... An ( n )

Tni ( i 1 ,..., n)  Ai 1 ( i 1 )... An 1 ( n 1 ) An ( n )

Tnn 1 ( n)  An ( n )
 Đã biết Tn0  n o a p
Tìm 1
Bước 1:
VT1(1) A11 (1 )Tn0  A2 ( 2 )... An ( n ) VF1(2,3,4,5,6)

Bước 2: Tìm phần tử O hoặc hằng số trong VF1

Bước 3: Cân bằng với phần tử tương ứng trong VF1
Áp dụng các trường hợp trong động học đảo giải tích
Bước 4: Tìm 1

Tìm 2

VT2(2) A21 ( 2 )VT 1(1 )  A21 ( 2 ) A11 (1 )Tn0  A3 ( 3 )... An ( n ) VF2(3,4,5,6)

Tương tư các bước 2,3,4 Tìm 2

Quay lại từ bước 1 để xác định tiếp các biến khớp còn lại
Ví dụ : Robot Elbow T60  A1 (1 ) A2 ( 2 )... A6 ( 6 )

Tìm 1 VT1 (1 )  A11 (1 )T60  A2 ( 2 )... A6 ( 6 )  VF1 ( 2 ,...,  6 )

nx c1  n y s1 ox c1  o y s1 a x c1  a y s1 p x c1  p y s1 
 nz oz az pz 
VT 1(1 )   
 nx s1  n y c1 ox s1  o y c1 a x s1  a y c1 p x s1  p y c1 
 
 0 0 0 1 

c234c5c6  s234 s6  c234c5c6  s234c6 c234 s5 a4 c234  a3c23  a2 c2 

s c c  c s  s234c5c6  c234c6 s234 s5 a4 s234  a3 s23  a2 s2 
VT 1(1 )   234 5 6 234 6
  s5c 6 s5 s 6 c5 0 
 
 0 0 0 1 

px c1 +pys1= a4c234+a3c23+a2c2
Pz = a4s234+a3s23+a2s2
Tiếp tục tìm 2

Nhân 2 vế với A21 ( 2 )

VT2 ( 2 )  A21 ( 2 ) A11 (1 )T60  A3 ... A6 ( 6 )  VF2 ( 3 ,...,  6 ) Không
VT3 ( 3 )  A31 ( 3 ) A21 ( 2 ) A11 (1 )T60  A4 ( 4 )...A6 ( 6 )  VF3 ( 4 ,..., 6 ) có 0/C

VT4 ( 4 )  A41 ( 4 ) A31 ( 3 ) A21 ( 2 ) A11 (1 )T60  A5 ( 5 ) A6 ( 6 )  VF4 ( 5 , 6 )

c5c6  c5c6 s5 0
 s c  s c  c 0
VF4 ( 5 ,  6 )  A5 ( 5 ) A6 ( 6 )   5 6 5 6 5 
 s6 c6 0 0
 
 0 0 0 1 

  a 3 s234  c234 (a x c1  a y s1 ) 
  a y c1  a x s1 
VT4 ( 4 )  
  a 3 c234  s234 (a x c1  a y s1 ) 
 
   
a 3 c234  s234 (a x c1  a y s1 )  0   234  ATAN 2(a 3 , a x c1  a y s1 )
VT2 ( 2 )  A21 ( 2 ) A11 (1 )T60  A3 ... A6 ( 6 )  VF2 ( 3 ,...,  6 ) Không
VT3 ( 3 )  A31 ( 3 ) A21 ( 2 ) A11 (1 )T60  A4 ( 4 )...A6 ( 6 )  VF3 ( 4 ,..., 6 ) có 0/C

VT4 ( 4 )  A41 ( 4 ) A31 ( 3 ) A21 ( 2 ) A11 (1 )T60  A5 ( 5 ) A6 ( 6 )  VF4 ( 5 , 6 )

c5c6  c5c6 s5 0
 s c  s c  c 0
VF4 ( 5 ,  6 )  A5 ( 5 ) A6 ( 6 )   5 6 5 6 5 
 s6 c6 0 0
 
 0 0 0 1 

  a 3 s234  c234 (a x c1  a y s1 ) 
  a y c1  a x s1 
VT4 ( 4 )  
  a 3 c234  s234 (a x c1  a y s1 ) 
 
   

 5  ATAN 2[a 3 s234  c234 (a x c1  a y s1 ), a x s1  a y c1 ]

p1= px s1 –pyc1-a4c234=a3c23+a2c2
p2= pz -a4s234 =a3s23+a2s2

Do 1, 234 đã xác định

p1= px c1 +pys1-a4c234=a3c23+a2c2
p2= pz -a4s234 =a3s23+a2s2

Do 1, 234 đã xác định p1, p2 xác định

p2

a3
3

a2

2
p1
Sử dụng phép đảo vị trí, tìm 3
Sử dụng phép đảo vị trí, tìm 3

p12  p22  a22  a32

cos  3 
2a2 a3
sin  3   1  cos 2  3
 3  ATAN 2(sin  3 , cos  3 )
 3  ATAN 2(sin  3 , cos  3 )

Tìm 2 Có 2 cách
C1 Áp dụng cho Planar đảo vị trí

 2  ATAN 2( p2 , p1 )  ATAN 2( a3 s3 , a2  a3c3 )

C2 p1=(a2+a3c3)s2+a3s3c2
p2 =(a2+a3c3)c2-a3s3s2
C2

p1=(a2+a3c3)s2+a3s3c2 (a3c3  a2 ) p2  a3 s3 p1
s2 
p2 =(a2+a3c3)c2-a3s3s2 ( a3c3  a2 ) 2  a32 s32
( a3c3  a2 ) p1  a3 s3 p2
c2 
( a3c3  a2 ) 2  a32 s32
 2  ATAN 2(sin  2 , cos  2 )
 4   234   2   3
 Tìm 6

VT5 ( 5 )  A51 ( 5 ) A41 ( 4 ) A31 ( 3 ) A21 ( 2 ) A11 (1 )T60  A6 ( 6 )

VT5 ( 5 )  A51 ( 5 ) A41 ( 4 ) A31 ( 3 ) A21 ( 2 ) A11 (1 )T60  A6 ( 6 )

c6  s6 0 0
s c 0 0 
A6 ( 6 )   6 6 
0 0 1 0
 
 0 0 0 1 
 
 c5 c234 (ox c1  o y s1 )  oz s234  s5 (ox s1  o y c1 )  
 oz c234  s234 (ox c1  o y s1 )  

   
 
   
 VT5 ( 5 )  A51 ( 5 ) A41 ( 4 ) A31 ( 3 ) A21 ( 2 ) A11 (1 )T60  A6 ( 6 )

c6  s6 0 0
s c 0 0 
A6 ( 6 )   6 6 
0 0 1 0
 
 0 0 0 1 
 
 c5 c234 (ox c1  o y s1 )  oz s234  s5 (ox s1  o y c1 )  
 oz c234  s234 (ox c1  o y s1 )  

   
 
   

 6  ATAN 2 c5 c234 (ox c1  o y s1 )  oz s234  s5 (ox s1  o y c1 ), oz c234  s234 (ox c1  o y s1 )