BTL môn ROBOTICS

Mục lục

Chƣơng 1 XÂY DỰNG CẤU TRÚC,THIẾT LẬP HỆ PHƢƠNG TRÌNH ĐỘNG HỌC
ROBOT

1.1.
Xây dựng cấu trúc robot

1.2. Thiết lập phƣơng trình động học robot

Chƣơng 2 BÀI TOÁN ĐỘNG HỌC

2.1. Bài toán động học thuận

2.2. Bài toán động học ngƣợc

Chƣơng 3 TÍNH TOÁN TĨNH HỌC

3.1. Tính lực dẫn động tại các khớp đảm bảo cân bằng tĩnh

Chƣơng 4 TÍNH TOÁN ĐỘNG LỰC HỌC

4.1 Xây dựng cấu trúc động lực học

4.2 Cơ sở lý thuyết

4.3 Xây dựng bảng tham số động học

4.4 Ma trận jacobi các khâu

4.5 Ma trận khối lƣợng của robot

4.6 Ma trận ly tâm và quán tính coriolits

4.7 Thế năng của robot

4.8 Phƣơng trình vi phân chuyển động của các khâu

Chƣơng 5 CHỌN BỘ ĐIỀU KHIỂN

Phụ lục Code maple

1
BTL môn ROBOTICS

Chƣơng 1
XÂY DỰNG CẤU TRÚC THIẾT LẬP HỆ PHƢƠNG TRÌNH ĐỘNG HỌC
ROBOT
1.1 XÂY DỰNG CẤU TRÚC ROBOT

1.1.1 Đặt hệ quy chiếu

Hình 1.1 Mô hình robot và hệ trục tọa độ

- Hệ trục tọa độ OX0Y0Z0 đặt tại khâu đế, trục OZ0 có hƣớng dọc trục khớp động 1, trục
OX0 nằm trong mặt phẳng vuông góc với OZo và có hƣớng từ trên xuống, trục OY0 xác
định theo quy tắc bàn tay phải.
- Hệ trục tọa độ OX1Y1Z1 tại khớp động 2, trục OZ1 đặt dọc trục khớp động 2, trục OX1
vuông góc với OZ0,OZ1 có hƣớng dọc theo khâu 1, trục OY1 xác định theo quy tắc bàn
tay phải.
- Hệ trục tọa độ OX2Y2Z2 đặt tại trục khớp động 3, trục OZ2 đặt dọc trục khớp động 3, trục
OX2 vuông góc với OZ1 và OZ2 hƣớng từ OZ1 sang OZ2, trục OY2 xác định theo quy tắc
bàn tay phải.
- Hệ trục tọa độ OX3Y3Z3 đặt tại khâu thao tác, trục OX3 hƣớng theo hƣớng khâu 3. OZ3
song song với trục OZ2, trục OY3 xác định theo quy tắc bàn tay phải.

2
BTL môn ROBOTICS

1.1.2 Thiết lập bộ thông số Denavit-Hartenbeg

Từ mô hình và hệ trục tọa độ ở trên ta xây dựng đƣợc bảng thông số Danavit-
Hartenbeg nhƣ sau :

Bảng 1.1: Bộ thông số Denavit-Hartenbeg

Khâu θi αi ai di

1 θ1 900 a1 d1

2 θ2 0 a2 0

3 θ3 0 a3 0

Trong đó:
θi là góc quay quanh Zi-1 đển biến Xi-1 thành Xi
αi là góc quay quanh Xi để biến Zi-1 thành Zi
Các biến khớp là θ1, θ2, θ3, đặt các biến khớp tƣơng ứng là q1,q2,q3.
Các ma trận biến đổi tọa độ thuần nhất Denavit-Hartenbeg dựa vào bộ thông số
trên :
cos(q1 ) 0 sin(q1 ) a1 cos(q1 )
 sin( q ) 0  cos(q1 ) a1 sin(q1 ) 
0
A1   1
(1.1)
 0 1 0 d1 
 
 0 0 0 1 

cos(q2 )  sin( q2 ) 0 a2 cos(q2 )

 sin(q ) cos(q ) 0 a2 sin(q2 ) 
1
A2   2 2
(1.2)
 0 0 1 0 
 
 0 0 0 1 

cos(q3 )  sin(q3 ) 0 a3 cos(q3 )

 sin(q ) cos(q ) 0 a3 sin(q3 ) 
2
A3   3 3
(1.3)
 0 0 1 0 
 
 0 0 0 1 

3
BTL môn ROBOTICS

1.2 THIẾT LẬP PHƢƠNG TRÌNH ĐỘNG HỌC ROBOT

Phƣơng trình động học robot nhận đƣợc trong dạng ma trận nhƣ sau :
0
A3 (q)  0 A3 (t ) (1.4)

Trong đó
C1C 23  C1 S 23 S1 a3C1C 23  a2 C1C 2  a1C1 
S C  S1 S 23  C1 a3 S1C 23  a2 S1C 2  a1 S1 
0
A3 (q)  A1 . A2 . A3 
0 1 2  1 23

 S 23 C 23 0 a3 S 23  a2 S 2  d1  (1.5)
 
 0 0 0 1 

Trong đó C1,C2,S1,S2,C23 và S23 lần lƣợt là viết tắt của cos(q1), cos(q2), sin(q1), sin(q2),
cos(q2+q3), sin(q2+q3)
 c11( ,  , ) c12 ( ,  , ) c13 ( ,  , ) xe 
c ( ,  , ) c22 ( ,  , ) c23 ( ,  , ) ye 
0
A3 (t )   21
c31( ,  , ) c32 ( ,  , ) c33 ( ,  , ) ze 
 
 0 0 0 1
Trong đó cij(α,β,ɳ) là các phần tử trong ma trận Cardan

 cos( ) cos( )  cos( ) sin( ) sin(  ) 

Rcd   sin( ) sin(  ) cos( )  cos( ) sin( )  sin( ) sin(  ) sin    cos  cos( )  sin( ) cos( )

 cos( ) sin(  ) cos( )  cos( ) sin( )) cos( ) sin(  ) sin    sin   cos( ) cos( ) cos( ) 

Ta có phƣơng trình dạng ma trận nhƣ sau:

c11( ,  , ) c12 ( ,  , ) c13( ,  , ) xe  C1C23  C1S 23 S1 a3C1C23  a2C1C2  a1C1 

c ( ,  , ) c ( ,  , ) c ( ,  , ) ye  S1C23  S1S 23  C1 a3 S1C23  a2 S1C2  a1S1 
 21 22 23

c31( ,  , ) c32 ( ,  , ) c33( ,  , ) ze  S 23 C23 0 a3 S 23  a2 S 2  d1  (1.6)
   
 0 0 0 1  0 0 0 1 

4
BTL môn ROBOTICS

Chƣơng 2
BÀI TOÁN ĐỘNG HỌC

2.1 BÀI TOÁN ĐỘNG HỌC THUẬN

 Xây dựng quy luật chuyển động, vị trí khâu thao tác và ma trận chỉ hƣớng

Chọn thông số chiều dài các khâu nhƣ sau:

d1=100 mm, a1=200 mm ,a2=200 mm , a3 = 200mm

Và chọn quy luật chuyển động các khâu nhƣ sau:
 1 2  1
q1  t  4 t q1   2 t  1
 
 1 2  2
q 2  2t  t  q 2   t  2 với 0(s)≤t≤5(s) (2.1)
 3  3
 1 2  1
q 3  t  8 t q 3   4 t  1
 
Đồ thị sự biến đổi của các biến khớp:

Đồ thị q1(t)

5
BTL môn ROBOTICS

Đồ thị q2(t)

Đồ thị q3(t)

6
BTL môn ROBOTICS

Từ phƣơng trình 1.6 ta có :

 xE  a3C1C23  a2C1C2  a1C1 (2.2)


 y E  a3 S1C23  a2 S1C2  a1S1
z  a S  a S  d
 E 3 23 2 2 1

Thay các giá trị của biến vào ta có:

Hƣớng của bàn kẹp có thể đƣợc xác định từ các góc Cardan, ký hiệu tƣơng ứng là α, β, γ
quay lần lƣợt quanh các trục x-y-z.

 cos( ) cos( )  cos( ) sin( ) sin(  ) 

Rcd   sin( ) sin(  ) cos( )  cos( ) sin( )  sin( ) sin(  ) sin    cos cos( )  sin( ) cos( )


 cos( ) sin(  ) cos( )  cos( ) sin( )) cos( ) sin(  ) sin    sin  cos( ) cos( ) cos( ) 

để tính đƣợc các góc α, β, η ta so sánh ma trận chỉ hƣớng của (1.5) và ma trận chỉ

hƣớng của (1.6) giải các hệ phƣơng trình ta có :

7
BTL môn ROBOTICS


   c32  ,  ,    C1 
  arctan  c  ,  ,    arctan  S 
  31   1
   c  ,  ,   C S    3 
  arctan  21   arctan  1 23  khi    , 
  c11  ,  ,    C1C 23  2 2 

  c13  ,  ,    
   arctan  S 23 
  arctan  2   2 
  c11  ,  ,   c12  ,  ,    C1 S 23   S1C 23  
2 2



  arctan  c32  ,  ,    arctan   C1 
   c  ,  ,  
 31 
S 
 1 

   c 21  ,  ,    CS    3 
  arctan    arctan  1 23  khi    , 
   c11  ,  ,     C1C 23  2 2 
    
  c13  ,  ,    arctan  S 23 
   arctan

 2  2 
  c11  ,  ,   c12  ,  ,     C1 S 23   S1C 23  
2 2


 Tính vận tốc điểm tác động cuối E, vận tốc góc khâu thao tác
Từ phần trên ta đã xây dựng đƣợc quy luật chuyển cũng nhƣ tìm đƣợc tọa độ của
khâu thao tác cuối, các biến khớp và đạo hàm các cấp theo t đã biết :
q  [q1 , q2 , q3 ]T

q  [q1 , q 2 , q3 ] T

Vận tốc góc của khâu thao tác:

R rE 
A3= A1 A2 A3   E
1 2

1 
(2.3)
0
Vận tốc của khâu thao tác chính là đạo hàm vị trí khâu thao tác theo thời gian:

VE= r E= x E , y E , z E 
T

VEx  x E  a3 S1C 23q1  C1 S 23 (q 2  q 3 )   a2 S1C2 q1  C1 S 2 q 2   a1 S1q1


 VEy  y E  a3 C1C 23q 1  S1 S 23 (q 2  q 3 )   a2 C1C2 q1  S1 S 2 q 2   a1C1q1

VEz  z E  a3 C 23 (q 2  q 3 )   a2 C2 q 2
(2.4)
Thay (2.1) vào (2.4) ta tìm đƣợc vấn tốc của các khâu thao tác cuối.
Vận tốc góc của khâu thao tác:

 0  z y 
 
~E  R E .RET    z 0 x 
  y x 0 


8
BTL môn ROBOTICS

 S1C23q1  C1S 23q 23 q1S1S 23  q 23C1C23 C1q1   C1C 23 S1C 23 S 23 

  q1C1C23  S1S 23q 23  C1S 23q1  S1C23q 23 S1q1   C1 S 23  S1 S 23 C 23 
 C23q 23  S 23q 23 0   S1  C1 0 
 0  q1  (q 2  q3 )C1 

 q1 0  (q 2  q 3 ) S1 
q 2 C1  q 3C1 q 2 S1  q3 S1 0 

(2.5)
Suy ra vận tốc góc khâu thao tác:

E  32 13 21 T  [(q 2  q3 )S1  (q 2  q3 )C1 q1 ]T

 11 1 2
 x  (q 2  q 3 ) S1  (3  12 t ) sin(t  4 t )

 11 1
  y  (q 2  q 3 )C1  (3  t ) cos(t  t 2 ) (2.6)
 12 4
 1 2
 z  q1  1  2 t


 Ứng dụng phần mềm Matlab, Maple vẽ quỹ đạo chuyển động của khâu thao
tác cuối

Quỹ đạo điểm khâu thao tác.

Sử dụng phần mềm Maple ta vẽ đƣợc đồ thị quỹ đạo chuyển động của khâu thao
tác cuối nhƣ sau :

Chuyển động điểm cuối E theo phương X

9
BTL môn ROBOTICS

Chuyển động điểm cuối E theo phương Y

Chuyển động điểm cuối theo phương Z

2.2 BÀI TOÁN ĐỘNG HỌC NGƢỢC

Bài toán động học ngƣợc thông thƣờng cho biết trƣớc vị trí của khâu thao tác yêu
cầu tìm giá trị các biến khớp ứng với vị trí đó. Ở tiểu luận này robot 3 bậc tự do kiểu RRR
ta không cần biết hƣớng của khâu thao tác mà vẫn có thể tìm đƣợc các góc quay tƣơng
ứng.

10
BTL môn ROBOTICS

2.2.1 Xây dựng quy luật chuyển động của khâu thao tác cuối

Ta chọn quy luật chuyển động bất kì của khâu thao tác E của robot nhƣ sau:

 x E  600  60t

 y E  100t (2.7)
 z  100  30t
 E

2.2.2 Khảo sát bài toán động học ngƣợc của robot tìm quy luật chuyển động của các
khâu

Ta có phƣơng trình ma trận (xem 1.4):

0
A3 (q)  0 A3 (t )

Từ (2.2) kết hợp với (2.7) ta có hệ ba phƣơng trình vị trí :

a3C1C 23  a 2 C1C 2  a1C1  x E  600  60t


a3 S1C 23  a 2 S1C 2  a1 S1  y E  100t (2.8)
a S  a S  d  z  100t  30
 3 23 2 2 1 E

Dựa vào cấu tạo hình học của robot ta xác định đƣợc q1 nhƣ sau:
YE
tan(q1 ) 
XE
Y   100t 
 q1  arctan  E   arctan  
 XE   600  60t 

Nhân phƣơng trình 1 với C1 và phƣơng trình 2 với S1 ta đƣợc phƣơng trình:
a3C23  a2 C2  a1  (600  60t )C1  100tS1 (2.9)

Đặt:

Px  (600  60t )C1  100tS 1  a1


Py  100  30t  d1

Kết hợp với phƣơng trình 3 của hệ (2.8) và phƣơng trình (2.9) ta đƣợc hệ phƣơng
trình sau:

a3C23  a2C2  Px
 (2.10)
a3 S 23  a2 S 2  Py
Bình phƣơng hai vế của hai phƣơng trình (2.10) sau đó cộng hai phƣơng trình lại
với nhau ta đƣợc phƣơng trình :

11
BTL môn ROBOTICS

a2  a3  2a2 a3 (C23C2  S 23S 2 )  Px  Py

2 2 2 2

 a2  a3  2a2 a3 (C2C3C2  S 2 S 3C2  S 2C3 S 2  S 3C2 S 2 )  Px  Py

2 2 2 2

 a2  a3  2a2 a3C3  Px  Py
2 2 2 2

Px2  Py2  a2  a3
2 2

 C3  (2.11)
2 a 2 a3

Mặt khác ta có :

S3   1  C32  
Vậy ta tính đƣợc q3:

 Px2  Py2  a 2  a3 


q3  artan  1  C32  (
2a 2a3
)

(2.12)
 
Từ hệ phƣơng trình (2.10) thay C3 và S3 vào ta có hệ phƣơng trình sau:

C2 (a3C3  a2 )  a3 S 2 S3  Px
 (2.13)
C2 a3 S3  S 2 (a3C3  a2 )  Py
Giải hệ phƣơng trình trên ta có nghiệm nhƣ sau:

 Px a2  a3C3   Py a3 S 3
C 
a3  a2  2a3 a2C3
2 2 2

 (2.14)
S  Py a2  a3C3   Px a3 S 3
 2 a3  a2  2a3 a2C3
2 2

Vậy ta tính đƣợc q2:

 Py a2  a3C3   Px a3 S3 Px a2  a3C3   Py a3 S3 

q2  artan  (2.15)
 a3  a2  2a3 a2C3 a3  a2  2a3 a2C3 
2 2 2 2

12
BTL môn ROBOTICS

Chƣơng 3

TÍNH TOÁN TĨNH HỌC

3.1 Tính lực dẫn động tại các khớp đảm bảo cân bằng tĩnh

Theo đầu bài ta có các lực tác dụng vào khâu thao tác tại điểm E gồm các vector lực FE3,
và mô men M :

FE 3  FX FY FZ 
T

M E3  M x My Mz 
T 3
r3  [a 3 0 0]T 2
r2  a 2 0 0
T

T T T
 a   a   a 
r1  a1 0 0 rC3   3 rC2   2 rC1   1
1 T 3 2 1
0 0 0 0 0 0
 2   2   2 
Pi  mi g 0 0
T

 Tính lực và momen của khâu 3 tác dụng lên khâu 2 tại khớp 3

Hệ phƣơng trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :


 F3, 2 
0 0
FE ,3  0 P3
0 0 ~0 0 ~ 0
 M 3, 2  M E ,3  r3 F3, 2  rc 3 P3

0

13
BTL môn ROBOTICS

  Fx   0   Fx 
0     F  m g 
 F3, 2   Fy   m3 g    y 3 

 
 Fz 
   0   
 Fz 

 (3.1)
  Mx   Fx   0 
0 M   0 ~ 
 m3 g   rc 3 m3 g 
 0 ~ 
3, 2   My   r3  Fy 


 
 Mz 
 
 Fz 
 
 0 


 a3 
 2 C1C 23 
a3C1C 23   a 
Trong đó : 0 r3  0 R3 r3   a3 S1 S 23  và 0 rc 3  0 R3 rc 3    3 S1C 23 
3 3
(3.2)
 2 
 a3 S 23    a3 S 
 2
23 


C1C 23  C1 S 23 S1 
S C 
 1 23  S1 S 23  C1 
Chú ý : 0 R3  0 R1 R2 R3 =  S
1 2

23 C 23 0 
 
 

Thay (3.2) vào (3.1) ta tìm đƣợc 0 M 3, 2 :




  Fx   0   Fx 
0  F   m g    F  m g 
 F3, 2   y  3   y 3 
  Fz   0   Fz 

0 ~0 0 ~ 0
 M 3, 2  M E ,3  r3 F3, 2  rc 3 P3 
0 0


  a3 a 
 0 S 23  3 S1C 23 
 M x    a3 S 23 a3 S1 S 23   Fx    0 
0 2 2
       C1C 23  m3 g 
a3 a3
   M y    a3 S 23 0  a3C1C 23   Fy  m3 g     S 23 0
  M   a S S  2 2 
 z  a3 C1C 23 0   Fz   a a   0 
 3 1 23 3
S C  3
C C 0
2 

1 23 1 23
 2 
  Fx 
0  
 F3, 2   Fy  m3 g 
  Fz 

  1  (3.3)
  Mx  a3 ( Fy  m3 g ) S 23  a3 FzS1C 23  2 a3 m3 gS 23 
M   My  a3 FxS 23  a3 FzS1C 23 
 3, 2  
  Mz  a FzS C  a ( Fy  m g )C C  a3 m gC C 
 
3 1 23 3 3 1 23
2
3 1 23


14
BTL môn ROBOTICS

 Tính lực và momen của khâu 2 tác dụng lên khâu 1 tại khớp 2

Hệ phƣơng trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :


 F2,1 
0 0
F3, 2  0 P2
0 0 ~0 0 ~ 0
(3.4)
 M 2,1  M 3, 2  r2 F2,1  rc 2 P2

0

C1 0 S1  C 2  S2 0 C1C 2  C1 S 2 S1 
R2  R R2   S1 0  C1   S 2 0   S1C 2  S1 S 2  C1 
1
Trong đó: 0 0
1 C2
 0 1 0   0 0 1  S 2 C2 0 

 a2 
a2C1C2   2 C1C2 
 a 
r2  R2 r2   a2 S1C2  và 0 rc 2  0 R2 rc 2    2 S1C2 
0 0 2 2
(3.5)
 a2 S 2   2 
  a2 S 
 2
2 


Thay (3.5) và (3.3) vào (3.4) ta đƣợc:

  Fx 
0  F  m  m g 
F 
 2,1  y 3 2 
  Fz 

Mx  a3 Fy  m3 g S 23  a3 FzS1C 23  a3 S 23m3 g  a2 S 2 ( Fy  m2 g  m3 g )  a2 FzS1C 2  a 2 S 2 m2 g
  1 1 
  
0 
2 2

 M 2,1   My  a2 FxS 23  a3 FzC1C 23  a2 FxS 2  a2 FzC1C 2

  Mz  a3 FxS 1C 23  a3 FyC1C 23  a3 m3 gC1C 23 ( Fy  m3 g )  1 a3 m3 gC1C 23  Fx a2 S1C 2  a2 Fy  m2 g  m3 g C1C 2  1 a2 m2 gC1C 2 
  2 2 





 Tính lực và momen của khâu 1 tác dụng lên khâu 0 đế tại khớp 1

Hệ phƣơng trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :


 F1,0 
0 0
F2,1  0 P1
0 0 ~0 0 ~ 0 (3.7)

 M 1, 0  0
M 2 ,1  r1 F 1, 0  rc1 P1

Trong đó :

15
BTL môn ROBOTICS

 a1 
 2 C1 
C1 0 S1  a1C1   a 
R1   S1 0  C1  r1  R r   a1 S1  và 0 rc 2  0 R2 rc 2    1 S1 
0 0 0 1 2
1 1 (3.8)
 2 
 0 1 0   0   0 
 

Thay (3.8) và (3.6) vào (3.7) ta đƣợc hệ phƣơng trình:

  Fx   0   Fx 
0      
 F1, 0   Fy  m3  m2 g   m1 g    Fy  m3  m2  m1 g 
  Fz   0   Fz 

    m1  
 
 d1 Fy   m2  m3   g 
    2  
 0 M 0 M   d 1Fx 
 1, 0 2 ,1
 
 
0

  


  Fx 
0
 F1, 0   Fy  m3  m2  m1 g 

  Fz 

0  a3   m2 g 
 M 1, 0 x  Mx   Fya 3  m3 g 2  S 23  a3FzS1C23  a 2 S 2  2  m3 g  Fy 

   m1  
  a 2 FzS1C2  d1 Fy   m2  m3  g  (3.9)
   2  
0
 M 1, 0 y  My  a 2 FxS 23  a3FzC1C23  a 2 FxS 2  a 2 FzC1C2  d1Fx
0 a3
 M 1, 0 z  Mz  a3FxS1C23  a3FyC1C23  m3 gC1C23  Fxa 2 S1C2
 2
  mg 
   a 2 Fy  2  m3 g C1C2
  2 

16
BTL môn ROBOTICS

Chƣơng 4

TÍNH TOÁN ĐỘNG LỰC HỌC

4.1 XÂY DỰNG CẤU TRÚC ĐỘNG HỌC

Vì các khâu coi nhƣ thanh đồng chất tiết diện ngang không đáng kể nên ta có trọng tâm
mỗi khâu nằm tại trung điểm của nó.

4.2 Cơ sở lý thuyết

Động năng của robot có dạng:

1 T 1
T q M ( q ) q  qT b( q, q )
2 2

Trong đó :

b(q, q) = M (q)q , b  b1...bn 

T

Phƣơng trình Lagrange loại II:

17
BTL môn ROBOTICS

T T T
d  T   T    
      
dt  q   q   q 
T
T  T 
 qT M ( q )     M (q)q
q  q 
T
d  T 
   M (q)q  M (q)q
dt  q 

Sử dụng định lý đạo hàm riêng theo vector tích của hai ma trận ta có:

T 1  1  qT b 
 (q b)  
T
(b  I n )  qT  (4.1)
q 2 q 2  q q 

Trong đó:

 b1 I n 
 . 
qT
(b  I n )  e1T ...enT     b eT I ...b eT I 
q  .   1 1 n n n n
 
bn I n 

 b1...bn   bT   M (q)q   qT M (q)

T
(4.2)

Mặt khác:

b 
M q q   qT  M q  q  I n   M q  q 

q T  q T
q q  q q 

 qT  0  M (q) I n   qT M (q) (4.3)

Thay (2) và (3) vào (1) ta đƣợc:

T T
T  T  d  T 
 qT M ( q )     M (q)q ,    M (q)q  M (q)q (4.4)
q  q  dt  q 

Tính toán tƣơng tự ta có:

T 1  1  qT b  1  T b 
 (qT b)   (b  I n )  qT   0 q 
q 2 q 2  q q  2  q 
1  1  M (q) q 
 qT  M (q )q   qT  (q  I n )  M (q ) 
2 q 2  q q 
1 T  M (q)  1 M (q )
 q  ( q  I n )  0   qT (q  I n )
2  q  2 q
(4.5)

Thay (4) và (5) vào phƣơng trình lagrange II ta đƣợc:

18
BTL môn ROBOTICS

T T
1  M (q)    
M (q )q  M (q )q   qT (q  I n )     
2 q   q 
M (q)
M (q)  ( I n  q)
q

 M (q) 1  M (q )  
T

Đặt : v ( q, q )   ( I n  q)   ( q  I n )   q  C ( q, q ) q
 q 2  q  

T
1  M (q) 
Ta có: v ( q, q )  M ( q ) q   (q  I n )  q (4.6)
2  q 
Theo định lý đạo hàm toàn phần và đạo hàm riêng ta có:
M (q)
M (q)  ( I n  q)
q

Thay vào (6) ta đƣợc:

 M (q) 1  M (q )  
T

v ( q, q )   ( I n  q)   ( q  I n )   q  C ( q, q ) q
 q 2  q  

T
M (q) 1  M (q ) 
C ( q, q )  ( I n  q)   (q  I n ) 
q 2  q 

Ma trận ly tâm và coriolis có dạng:

T
M (q) 1  M ( q) 
C ( q, q )  ( I n  q)   (q  I n ) 
q 2  q 

Khi đó phƣơng trình vi phân chuyển đông của các khâu :

T
  
M ( q ) q  C ( q, q ) q    
 q 
4.3 Xây dựng bảng tham số động lực học

Bảng 4.1 Bảng mô tả vị trí trọng tâm khối lượng và mô men quán tính khối của từng khâu

Khâu Vị trí trọng tâm Khối Ma trận mômen quán tính

xC yC zC lƣợng I xx I yy I zz I xy I yz I zx
1 a 0 0 m1 I1 x I1 y I1z 0 0 0
 1
2
2 a 0 0 m2 I2x I2y I2z 0 0 0
 2
2
3 a 0 0 m3 I 3x I3y I 3z 0 0 0
 3
2

19
BTL môn ROBOTICS

4.3 Ma trận Jacobi của các khâu

Tạo độ trọng tâm của khâu i trong hệ tọa độ 0 tính nhƣ sau :
0
rci  0 ri  0Ri .i rci
Với 0
rci là tọa độ trọng tâm khâu i trong hệ tọa độ i
i
rci là tọa độ trọng tâm khâu i trong hệ tọa độ i
0
Ri là ma trận quay biến đổi hệ 0 thành hệ i
0
ri là tọa độ của gốc tọa độ i trong hệ tọa độ 0
Ta có các ma trận tọa độ trọng tâm của các khâu nhƣ sau :

a 
 a1   1 cos(q1 )
a1 . cos(q1 ) C1 0 S1   
2  a
2

0
rC1   a1. sin(q1 )    S1   
0  C1  0   1 sin(q1)  (4.7)
  2 
 d1   0 1 0   0   d1 
   


 a2   a2 
a2 C1C2  a1C1  C1C2  C1 S 2 S1     C1C2  a1C1 
2 2
0
rC 2   a2 S1C2  a1 S1    S1C2  S1 S 2  C1   0    l2 S1C 2  (4.8)
   
 a2 S 2  d1   S 2 C2 0   0   a2 S  d 
   2 2 1

a 
 a3   3 C1C23  q2C1C2  a1C1 
a2C1C2  a3C1C23  a1C1  C1C23  C1S 23 S1    2
2  
  a2 S1C2  a3 S1C23  a1S1    S1C23 
 C1   0    a2 S1C2  3 S1C23  a1 S1 
a
0
rC 3  S1S 23
   2 
 a2 S 2  a3 S 23  d1   S 23 C23 0   0   a3 
   S 23  a2 S 2  d1
 2 

Từ (4.7) (4.8) và (4.9) ta có ma trận Jacobi tịnh tiến của các khâu :

 a1 
 2 S1 0 0
r  a 
JT1  C1   1 C1 0 0 (4.10)
q  2 
 0 0 0
 

 1 1 
 2 a2 S1C2  S1a1 
2
a2C1 S 2 0
r 1 1 
JT 2  C1   a2C1C2  C1a1  a2 S1 S 2 0 (4.11)
q  2 2 
 0
1
a2 C 2 0
 2 

20
BTL môn ROBOTICS

 1 1 1 
 a2 S1C2  2 a3 S1C23  C1a1  a2C1S 2 
2
a3C1S 23  a3C1 S 23 
2
r  1 1 1 
JT 3  C1   a2C1C2  a3C1C23  C1a1  a2 S1S 2  a3 S1 S 23  a3 S1 S 23  (4.12)
q 
 2 2 2 
 0
1
a2C2  a3C23
1
a3C23 
 2 2 
Cũng từ các ma trận Denanvit- Hartenberg ta xác định đƣợc các ma trận cosin chỉ hƣởng
của các khâu (xem 1.1, 1.5, 3.5) :
Ma trận cosin chỉ hƣớng của khâu 1 :

C1 0 S1 
0
R1   S1 0  C1 
 0 1 0 

Ma trận cosin chỉ hƣớng của khâu 2 :

C1C 2  C1 S 2 S1 
0
R2   S1C 2  S1 S 2  C1 
 S 2 C2 0 

Ma trận cosin chỉ hƣớng của khâu 3:

C1C 23  C1 S 23 S1 
0
R3   S1C23  S1 S 23  C1 
 S 23 C 23 0 

Toán tử sóng của vector vận tốc góc của khâu 1:

 
 .0  q 1 0
~1`  R1T R1  q 1 0 0
 
0 0 0
 

Suy ra vận tốc góc và ma trận Jacobian của khâu 1 :

~1`  0 0 q1 T  J R1q

0 0 0 
1 
J R1   0 0 0 (4.13)
q
1 0 0

Toán tử sóng của vector vận tốc góc của khâu 2:

 0  q1  q 2C1 
2  R2 R2   q1
~ ` T 
0  q 2 S1 
q 2C1 q 2 S1 0 

21
BTL môn ROBOTICS

Suy ra vận tốc góc và ma trận Jacobian của khâu 2 :

~2`  q 2 S1  q2C1 q1 T  J R 2 q

0 S1 0
 2 
J R2   0  C1 0 (4.14)
q 
1 0 0

Toán tử sóng của vector vận tốc góc của khâu 3:

 0  q1  (q 2  q3 )C1 

~ T  
3  R3 R3  
`
q1 0  (q 2  q 3 ) S1 
q 2C1  q3C1 q 2 S1  q3 S1 0 

Suy ra vận tốc góc và ma trận Jacobian của khâu 3 :

~3`  S1 (q 2  q3 )  (q 2  q3 )C1 q1 T  J R3q

0 S1 S1 
3 
J R3   0  C1  C1  (4.15)
q
1 0 0 

4.5 Ma trận khối lƣợng suy rộng của robot

 m11 m12 m13 

3
 
M   (mi JTiT JTi  J RiT Ai I i AiT J Ri ) =  m21 m22 m23 
i 1 m m33 
 31 m32

Trong đó:

m12=m13=m31=m21=0

22
BTL môn ROBOTICS

4.6 Ma trận ly tâm và coriolis :

T  c11 c12 c13 

M (q) 1  M (q)   
C ( q, q )  ( I n * q)   (q * I n )  =  c21 c22 c23 
q 2  q  c
 31 c32 c33 
 c1 
 
 C (q, q ).q   c2 
c 
 3

Sử dụng phần mềm Maple nhân ra ta đƣợc :

Trong đó : diffq i  qi

23
BTL môn ROBOTICS

4.7 Thế năng của robot

  m1 gd1  m2 g (d1  l2 S2 )  m3 g (d1  l2 S2  l3S23 )

Từ đó suy ra :

 g1 
  
g   g2 
q  
 g3 

Trong đó :

4.8 Phuơng trình vi phân chuyển động của các khâu

Thế các biểu thức vào phƣơng trình Lagrange loại hai :

M (q)q  C (q, q)q  g (q)  

Ta nhận đƣợc hệ phƣơng trình vi phân chuyển động của robot ba khâu trong không gian :
Khâu 1 :

24
BTL môn ROBOTICS

Khâu 2 :

Khâu 3 :

Trong đó

d 2qi  qi
diffq i  q

25
BTL môn ROBOTICS

Chƣơng 5

CHỌN BỘ ĐIỀU KHIỂN

Nhiêm vụ của bài toán điều khiển là tìm ra quy luật của lực/ mô men do các động
cơ điện tạo ra tác dụng lên các khâu để đảm bảo robot chạy đúng theo quy luật q d(t) cho
trƣớc, nhằm thực hiện một số nhiệm vụ nào đó. Trên cơ sở chuyển động mong muốn qd(t)
đƣợc định nghĩa trƣớc và chuyển động hiện tại của robot đƣợc đo bởi các cảm biến đặt tại
khớp, bộ điều khiển có nhiệm vụ đƣa ra các lực/mômen cần thiết. các lực/mômen này tác
động làm cho robot thực hiện chuyển động mong muốn một cách ổn định và chính xác. Sơ
đồ khối của bộ điều khiển cho robot có dạng nhƣ hình 5.1

Bộ điều q(t)
q d (t) khiển PD
Robot

_

Cảm biến

Hình 5.1 Sơ đồ điều khiển robot

Để có đƣợc luật điều khiển đáp ứng các yêu cầu vừa nêu, thông thƣờng luật điều
khiển dựa trên động lực học ngƣợc đƣợc sử dụng. Với luật điều khiển này lực/mô men của
các bộ phận dẫn động đƣợc tính nhƣ sau:

  M (q ).q  C (q .q ).q  g (q ) (5.1)

Giả thiết thành phần mômen trọng lực G(q) đƣợc bù hoàn toàn, sơ đồ hệ thống điều
khiển phản hồi với cấu trúc điều khiển PD có dạng đơn giản nhƣ sau:

26
BTL môn ROBOTICS

q
Kp
 q
RB
q
Kp
q

Tín hiệu đặt vị trí qd đƣợc so sánh với vị trí thực của khớp q, sai lệch đƣợc đặt vào
khâu khuếch đại với hệ số Kp. Tín hiệu ra của khâu tỉ lệ đƣợc cộng đại số với tín hiệu tỉ lệ
với tốc độ của khớp và đặt cơ cấu chấp hành của robot:

 dk  K p  K D (5.2)

Hay viết với khớp thứ i ta có:

 dki  K pi i  Kdi i

(5.3)

Trong đó :

  qd  q sai số vị trí khớp robot.

  qd  q sai số tốc độ khớp robot.

K p  diag ( K p1 , K p 2 ,..., K pn ) là ma trận đƣờng chéo các hệ số khuếch đại

của từng khớp hợp riêng.

Kd  diag ( Kd1 , Kd 2 ,..., Kdn ) là ma trận đƣờng chéo các hệ số đạo hàm của
từng khớp hợp riêng.

Hệ thống điều kiển với cấu trúc bộ điều khiển (3…) ổn định tuyệt đối toàn cục. thật
vậy, chọn hàm lyapunov có dạng :

1
VL  ( T .K p .  q T Hq ) (5.4)
2

1 T
Hàm VL biểu thị tổng năng lƣợng hệ thống robot: thành phần  K p tỉ lệ năng
2
1 T
lƣợng đầu vào và thành phần (q .H .q ) là động năng của robot. KP và H là các ma trận
2
hệ số dƣơng; nên hàm VL > 0 với q  qd .

Tính đạo hàm cấp 1 hàm VL ta đƣợc:

27
BTL môn ROBOTICS

1 1 1 1 1
VL   T K p   T K p  q T Hq  q T Hq  q T Hq (5.5)
2 2 2 2 2

Do tính đối xứng của các thành phần  T K p , q T Hq nên (4.5) đƣợc rút gọn lại nhƣ sau:

1
VL   T K p  q T Hq  q T Hq (5.6)
2

Thay phƣơng trình dạng tổng quát vào phƣơng trình trên với giả thiết không có thành phần
mômen trọng lực G(p) ta có :

1
VL   T K p  q T Hq  q T [  C (q , q )q ] (5.7)
2

Sử dụng thuộc tính của phƣơng trình động lực học và áp dụng luật điều khiển (4.1),
Phƣơng trình (4.7) đƣợc biến đổi nhhuw sau:

1 1
VL  q T K p  q T H (q )q  q T C (q , q )q  q T K p  q T [ H (q )  C (q , q )]q (5.8)
2 2

1 1
Do ma trận H (q )  C (q , q ) là ma trận đối xứng ngƣợc nên H (q )  C (q , q ) = 0 với mọi
2 2
q nên từ (4.7) ta nhận đƣợc :

VL  q T K p q  0

Bất đẳng thức trên cho thấy rằng hệ thông ổn định tuyệt đối. mức độ dƣơng của VL phụ
thuộc vào Kp và mức độ âm của VL phụ thuộc vào KD. Do đó tăng tốc độ hội tụ bằng cách
tăng giá trị ma trận KD.

28