Chapter 4

INVERSE MANIPULATOR KINEMATICS

ĐỘNG HỌC NGHỊCH TAY MÁY

ai
Th
n
Va
4.1. Introduction - Giới thiệu

n
ye
4.2. Multiple solutions – Đa nghiệm

gu
N
by
4.3. Algebraic solution – Phương pháp đại số

d
le
pi
m
Co
4.1 Introduction
Giới thiệu

 Given the position and orientation of the

end-effector of the manipulator, calculate

ai
Th
all possible sets of joint angles that could

n
be used to attain this given position and

Va
orientation.

n
ye
gu
N
by
 Cho trước vị trí và hướng của cơ cấu chấp

d
le
hành (end-effector) của tay máy, tính toán
pi
tất cả các giá trị có thể của các góc khớp
m
Co
nối (joint angles) để tay máy có thể vươn
tới vị trí và hướng đã cho.

-2-
4.2 Multiple solutions
Đa nghiệm

 Figure shows a three-link planar arm with its

ai
end-effector at a certain position and

Th
orientation. The dashed lines indicate a

n
Va
second possible configuration in which the

n
same end-effector position and orientation are

ye
gu
achieved.

N
by
Một tay máy gồm 3 khâu liên kết với cơ cấu

d


le
chấp hành (end-effector) được xác định tại
pi
m
một vị trí và hướng cụ thể.
Co

 Các đường gạch xác định một thế khả dĩ khác

của tay máy mà cơ cấu chấp hành vẫn vươn
tới cùng vị trí và hướng.
-3-
4.2 Multiple solutions
Đa nghiệm

 The number of solutions depends upon the number of joints in the manipulator

ai
but is also a function of the link parameters (αi, ai, and di for a rotary joint

Th
manipulator) and the allowable ranges of motion of the joints.

n
Va
Tính đa nghiệm của bài toán động học ngược không chỉ phụ thuộc vào số các

n


ye
khớp nối (tức số bậc tự do) mà còn phụ thuộc vào các tham số liên kết (αi, ai, và

gu
di) và các giới hạn chuyển động của các khớp.

N
by
d
le
pi
m
Co

-4-
4.3 Algebraic solution
Phương pháp đại số

 Consider a three-link planar manipulator with its link

ai
parameters as shown in Figure.

Th
n
Va
 Cho một tay máy 3 bậc tự do với các tham số liên kết.

n
ye
gu
N
by
d
le
pi
m
Co

-5-
4.3 Algebraic solution
Phương pháp đại số

 The kinematic equations of this arm is

ai
Th
 Phương trình động học thuận của tay máy này là

n
Va
n
ye
gu
N
by
d
le
pi
m
Co

-6-
4.3 Algebraic solution
Phương pháp đại số

 The kinematic equations of this arm is

ai
Th
 Phương trình động học thuận của tay máy này là

n
Va
n
ye
gu
N
by
d
le
pi
m
Co

-7-
4.3 Algebraic solution
Phương pháp đại số

x 2  l12 c12  2l1l2 c1c12  l22 c122

ai
y 2  l12 s12  2l1l2 s1s12  l22 s122

Th
n
Va
 x  y  l1 ( s1  c1 )  2l1l2 (c1c12  s1s12 )  l2 ( s12  c12 )
2 2 2 2 2 2 2 2

n
ye
 l12  l22  2l1l2 c2

gu
N
by
Because: s 2  c 2  1

d
le
1 1
 2
pi
s12  c12  1
2
m
Co
c c  s s  c (c c  s s )  s (c s  s c )
 1 12 1 12 1 1 2 1 2 1 1 2 1 2

  c12 c2  c1s1s2  s1c1s2  s12 c2
  c2 ( s12  c12 )

  c2 -8-
4.3 Algebraic solution
Phương pháp đại số

( x 2  y 2 )  (l12  l22 )
 c2 

ai
2l1l2

Th
n
Va
 2  A tan 2s2 , c2 

n
ye
gu
N
where s2   1  c22

by
d
le
pi
m
Co

Chú ý:
Sinh viên xem lại kiến thức toán học cho hàm Atan2(x1,x2)
-9-
4.3 Algebraic solution
Phương pháp đại số

 Having found 2, we can solve 1 as follows.

ai
Th
 Từ 2, chúng ta có thể tìm 1 như sau.

n
Va
n
ye
 x  l1c1  l2 c12 c  c c  s s

gu
From  and  12 1 2 1 2

N
 y  l1s1  l2 s12 s12  c1s2  s1c2

by
d
le
 x  l1c1  l2 (c1c2  s1s2 )
pi
  m
 y  l1s1  l2 (c1s2  s1c2 )
Co

 x  c1 (l1  l2 c2 )  s1l2 s2  c1k1  s1k 2

 
 y  s1 (l1  l2 c2 )  c1l2 s2  s1k1  c1k 2
- 10 -
4.3 Algebraic solution
Phương pháp đại số

 Changing of variables

ai
Th
 Biến đổi các biến

n
Va
n
r  k 2  k 2

ye
gu

1 2

  A tan 2k 2 , k1 

N
by
d
le
pi
x m
Co

k1  rc  r  c c1  s s1
   
k 2  rs y c s s c
 r  1  1

- 11 -
4.3 Algebraic solution
Phương pháp đại số

x
 r  cos  1 

ai
Th
 

n
Va
 y  sin    

n
 r

ye
1

gu
N
by
 y x
   1  A tan 2 ,   A tan 2 y, x 

d
le
r r
pi
m
Co

 1  A tan 2 y, x   
 A tan 2 y, x   A tan 2k 2 , k1 

- 12 -
4.3 Algebraic solution
Phương pháp đại số

 Finally, from 1 and 2 we can solve for 3 as follows.

ai
Th
 Cuối cùng, từ 1 và 2, chúng ta có thể tìm 3 như sau.

n
Va
n
ye
1  2  3  A tan 2s , c   

gu
N
by
d
le
pi
m
Co

- 13 -