Bài tập lớn Robotics Đại Học Bách Khoa Hà Nội

/>TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN CƠ KHÍ
----------

BÀI TẬP LỚN
HỌC PHẦN ROBOTICS

Họ tên: Nguyễn Văn Khương
Lớp: Cơ điện tử 1
MSSV: 20142377
Mã lớp: 95966
Mã học phần: ME3168
GVDH: PGS.TS PHAN BÙI KHÔI

Hà Nội ngày… tháng… năm 2016


/>
MỤC LỤC
LỜI MỞ ĐẦU........................................................................................................ 1
CHƯƠNG 1: ĐỘNG HỌC ROBOT ................................................................... 2
1.1.

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

1.1.1. Mô hình khảo sát ................................................................................ 2
1.1.2. Đặt hệ trục tọa độ ............................................................................... 3
1.1.3. Bộ thông số Denavit-Hartenbeg ......................................................... 3
1.2.

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

1.2.1. Tìm vị trí điểm thao tác biểu diễn theo các tọa độ khớp, hướng của
khâu thao tác. ................................................................................................. 4
1.2.2. Vận tốc điểm tác động cuối E và vận tốc góc các khâu. .................... 5
1.2.3. Tính gia tốc điểm tác động cuối. Tính gia tốc góc khâu thao tác. ..... 7
1.3.

Bài toán động học ngược ......................................................................... 7

1.3.1. Tính các tọa độ khớp .......................................................................... 8
1.3.2. Vận tốc dài của các khâu tương ứng với các khớp tịnh tiến và vận
tốc góc ứng với các khớp quay. ...................................................................... 9
1.3.3. Gia tốc dài của các khâu tương ứng với các khớp tịnh tiến và gia tốc
góc ứng với các khớp quay. .......................................................................... 10
CHƯƠNG 2: TĨNH HỌC ROBOT ................................................................... 11
2.1.

Mô hình khảo sát bài toán tĩnh học ........................................................ 11

2.2.

Tính toán bài toán tĩnh học .................................................................... 11

CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG ................................ 15
3.1.

Thiết kế quỹ đạo trong không gian khớp ............................................... 15

3.2.

Thiết kế quỹ đạo trong không gian thao tác .......................................... 17

CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT ....................................................... 20
5.1.

Các tham số động lực học ...................................................................... 20

5.2.

Ma trận Jacobi tịnh tiến của các khâu .................................................... 20

5.3.

Tính ma trận jacobi quay của các khâu ................................................. 21

5.4.

Tính tensor quán tính của các khâu ....................................................... 22

5.5.

Tính ma trận khối lượng ........................................................................ 23

5.6.

Tính toán thế năng, lực thế .................................................................... 24

CHƯƠNG 5: ĐIỀU KHIỂN .............................................................................. 25

/>6.1.

Điều khiển phản hồi và điều khiển vòng kín. ........................................ 25

6.2.

Thiết kế bộ điều khiển trong không gian khớp ...................................... 26

PHỤ LỤC............................................................................................................. 27
KẾT LUẬN .......................................................................................................... 31


/>
LỜI MỞ ĐẦU
Với sự phát triển mạnh mẽ của nền công nghiệp Robot thì môn Robotics trang bị
cho sinh viên những kiến thức cơ sở về tính toán và thiết kế Robot. Đây là những kiến
thức cơ sở hết sức quan trọng. Trong bài tập lớn này để cập tới các bài toán về động
học, tĩnh học, động lực học, thiết kế quỹ đạo chuyển động và điều khiển phục vụ cho
quá trình thiết kế Robot. Mô hình robot khảo sát trong bài tập lớn này là robot 3 bậc tự
do.
Để thực hiện được bài tập lớn này, em xin cảm ơn thầy giáo PGS.TS Phan Bùi
Khôi đã giúp đỡ rất nhiều trong quá trình học tập trên lớn. Xin chân thành cảm ơn
thầy.
Hà nội, Ngày…. Tháng…. Năm ….
Sinh viên thực hiện
Nguyễn Văn Khương

1

/>
CHƯƠNG 1: ĐỘNG HỌC ROBOT
Trong chương 1 giải quyết các vấn đề sau:
➢ Tính số bậc tự do của robot.
➢ Vẽ các hệ trục tọa độ gắn liền với khâu theo quy tắc DH.
➢ Lập bảng DH. Tính các ma trận DH.
➢ Tìm vị trí điểm thao tác biểu diễn theo tọa độ khớp. Xác định hướng của
khâu thao tác.
➢ Tính vận tốc điểm tác động cuối E. Tính vận tốc góc các khâu.
➢ Tính gia tốc điểm tác động cuối E. Tính gia tốc góc khâu thao tác.
➢ Cho vị trí, vận tốc, gia tốc điểm tác động cuối, hướng, vận tốc góc, gia tốc
góc khâu thao tác
➢ Tính các tọa độ khớp
➢ Tính vận tốc, gia tốc dài của các khâu tương ứng với các khớp tịnh tiến
➢ Tính vận tốc góc, gia tốc góc của các khâu ứng với các khớp quay
1.1. Xây dựng cấu trúc robot
1.1.1.

Mô hình khảo sát

Hình 1: Mô hình Robot khảo sát

Bậc tự do của mô hình khảo sát:
2


/>f=6.(3-3)+3.1=3
Robot có 2 khớp quay và 1 khớp tịnh tiến
1.1.2.

Đặt hệ trục tọa độ

Hình 2: Đặt hệ trục tọa độ theo phương pháp DH
1.1.3.

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ố DanavitHartenbeg như sau:
Joint

i

di

ai

i

1

1

d1

a1

0

2

0

d2

0

0

3

3

0

a3

0

Trong đó:
-

Các thông số được biển diễn như trên hình vẽ

-

Các biến khớp tương ứng là là 1 , d2 ,3

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 là:

3


/>
cos(1 )  sin(1 )
 sin( ) cos( )
1
1
0
A1  
 0
0

0
 0
1
0
1
A2  
0

0

0
1
0
0

0 a1 cos(1 ) 
0 a1 sin(1 ) 


1
d1

0
1


;

0 0
0 0 
1 d2 

0 1

cos(3 )  sin(3 )
 sin( ) cos( )
3
3
2
A3  
 0
0

0
 0

0 a1 cos(3 ) 
0 a1 sin(3 ) 


1
0

0
1


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

Tìm vị trí điểm thao tác biểu diễn theo các tọa độ khớp, hướng của khâu

thao tác.
Ta có các ma trận biến đổi thuần nhất biểu diễn vị trí và hướng của hệ tọa độ
khâu thứ i đối với hệ tọa độ cơ sở Oo xo yo zo là:

c(1 )  s(1 )
 s( ) c( )
1
0
A1   1
 0
0

0
 0

0 a1 c(1 ) 
0 a1 s(1 ) 

1
d1 

0
1 

c(1 )  s(1 )
 s( ) c( )
1
0
0
1
A 2  A1. A 2   1
 0
0

0
 0
c(13 )  s(13 )
 s( ) c( )
13
0
0
2
A3  A 2 . A3   13
 0
0

0
 0

0 a1 c(1 ) 
0 a1 s(1 ) 
1 d1  d 2 

0
1 
0 a1 c(1 )  a 3c(13 ) 
0 a1 s(1 )  a 3s(13 ) 

1
d1  d 2

0
1


Từ các ma trận trên ta xác định được:
➢ Ma trận biểu diễn vị trí điểm thao tác biểu diễn theo các tọa độ khớp
4


/> x E  a1 c(1 )  a 3c(13 ) 
0
r E   y E    a1 s(1 )  a 3s(13 ) 
 z E  

d1  d 2

➢ Hướng của hệ tọa độ khâu thác tác so với hệ tọa độ cơ sở được xác định từ

các góc Roll-Pitch-Yaw, với các kí hiệu   Roll ,   Pitch ,   Yaw và các
phép quay tương ứng như sau:

Hình 3: Mô tả các phép quay RPY
Ma trận quay biểu diễn phép quay RPY:
R RPY

cos .cos   sin .cos   cos .sin .sin  sin .sin   cos .sin .cos  
  sin .cos  cos .cos   sin .sin .sin   cos .sin   sin .sin .cos  
  sin 
cos .sin 
cos .cos 


Để tính được các góc , ,  thì sẽ so sánh với ma trận quay của khâu cuối:

0

c(13 )  s(13 ) 0 
R E   s(13 ) c(13 ) 0 
 0
0
1 

Giải các hệ phương trình tìm được các góc như sau:



  0


  0

  arctan  sin(13 ) 

 cos(13 ) 
1.2.2.

Vận tốc điểm tác động cuối E và vận tốc góc các khâu.

➢ Xác định vận tốc điểm tác động cuối E.
Vận tốc điểm tác động cuối E có được từ việc đạo hàm theo t ma trận định vị của
điểm tác động cuối E, nhận được ma trận biểu diễn vận tốc tuyệt đối trong hệ cơ sở:
5


/>
 a 3 (1  3 ).s(13 )  a1.1.s(1 ) 


0
v E  0 rE   a 3 (1  3 ).c(13 )  a1.1.c(1 ) 


d2


➢ Xác định vận tốc góc các khâu.
Các ma trận quay biểu diễn hướng của hệ tọa độ khâu i so với hệ tọa độ cơ

-

sở Oo xo yo zo .
0

c(1 )  s(1 ) 0 
c(1 )  s(1 ) 0 
c(13 )  s(13 ) 0 




0
0
R1   s(1 ) c(1 ) 0  R 2   s(1 ) c(1 ) 0  R 3   s(13 ) c(13 ) 0 
 0
 0
 0
0
1 
0
1 
0
1 

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

0

 1.s(1 ) 1.c(1 ) 0 
 1.s(1 ) 1.c(1 ) 0 


 0


R1   1.c(1 ) 1.s(1 ) 0  R 2   1.c(1 ) 1.s(1 ) 0 


0
0
0 
0
0
0 



 (1  3 ).s(13 ) (1  3 ).c(13 ) 0 


0
R 3   (1  3 ).c(13 ) (1  3 ).s(13 ) 0 

0
0
0 

Chuyển vị các ma trận quay:

-

 c(1 ) s(1 ) 0 
 c(1 ) s(1 ) 0 
0
R1T    s(1 ) c(1 ) 0  0 R T2    s(1 ) c(1 ) 0 
 0
 0
0
1 
0
1 
0

 c(13 ) s(13 ) 0 
R    s(13 ) c(13 ) 0 
 0
0
1 
T
3

-

Các ma trận sóng tương ứng:

 0 1 0 


1  0 R1.0 R1T  1 0 0 
0
0 0 


 0 1 0 


2  0 R 2 .0 R T2  1 0 0 
0
0 0 

6


/>
 0

3  R 3 . R  1  3
 0

0

0

1  3

T
3

0
0

0


0
0 

 Vận tốc góc của các khâu trong hệ cố định là:
 0 
0
0




2
3
   0     0     0 
1  3 
1 
1 
1

1.2.3.

Tính gia tốc điểm tác động cuối. Tính gia tốc góc khâu thao tác.

➢ Xác định gia tốc điểm tác động cuối:
Gia tốc điểm tác động cuối E có được từ việc đạo hàm theo t ma trận biểu diễn
vận tốc của điểm tác động cuối E trong hệ cơ sở, nhận được ma trận biểu diễn gia tốc
của điểm tác động cuối E trong hệ cơ sở:

 a 3 .(1  3 ).s(13 )  a 3 .(1  3 ) 2 .c(13 )  a1.1.s(1 )  a1.12 .c(1 ) 



0
a E  0 v E   a 3 .(1  3 ).c(13 )  a 3 .(1  3 ) 2 .s(13 )  a1.1.c(1 )  a1.12 .s(1 ) 


d2


➢ Xác định gia tốc góc khâu thao tác
Gia tốc góc khâu thao tác có được từ việc đạo hàm theo t ma trận biểu diễn vận
tốc góc của khâu thao tác trong hệ tọa độ cơ sở, nhận được ma trận biểu diễn gia tốc
góc khâu thao tác trong hệ tọa độ cơ sở:
 0 
 E  E  3   0 
1  3 

1.3. Bài toán động học ngược
Đã biết:
p   x E , y E , z E T

T

p   x E , y E , z E 

T
p   x E , y E , z E 

Cần tìm:

7


/>
q  [q1 ,q 2 ,q 3 ]T

T
q   q1 ,q 2 ,q 3 

T
q   q1 ,q 2 ,q 3 

1.3.1.

Tính các tọa độ khớp

Ta có:
0

 x E  a1 c(1 )  a 3c(13 ) 
r E   y E    a1 s(1 )  a 3s(13 ) 
 z E  

d1  d 2

Rút ra các hệ phương trình sau:

a1 cos 1  a 3 .cos(1  3 )  x E (1)

a1 sin 1  a 3 .sin(1  3 )  y E (2)

d  d  z
(3)
E
 1 2
Trong đó:
-

q1  1 , q2  d2 , q3  3 là các biến khớp, các nghiệm cần giải

-

a1 ,a 3 ,d1, x E , yE ,z E là các số liệu cho trước

Bình phương 2 vế của 2 phương trình (1) và (2) sau đó cộng 2 phương trình lại ta
được phương trình sau:

a12  a 32  2a1a 3[cos 1 cos(1  3 )  sin 1 sin(1  3 )]  x E 2  y E 2
 a12  a 32  2a1a 3 cos 3  x E 2  y E 2
 cos 3 

x E 2  y E 2  a12  a 32
2a1a 3

Ta có sin 3   1  cos 32
=>

2

2
2

2
2


x

y

a

a
E
1
3
q3  arctan   1   E


2a
a
1 3





x E 2  y E 2  a12  a 32 

2a1a 3


Biến đổi phương trình (1) (2) về dạng sau:

8


/>
a1 cos 1  a 3 (cos 1 cos 3  sin 1 sin 3 )  x E

a1 sin 1  a 3 (sin 1 cos 3  cos 1 sin 3 )  y E
cos 1.(a1  a 3 cos 3 )  a 3 sin 1 sin 3  x E

sin 1.(a1  a 3 cos 3 )  a 3 cos 1 sin 3  y E
x E (a1  a 3 cos 3 )  y E a 3 sin 3

cos 1 
a12  a 32  2a1a 3 cos 3

 
sin   y E (a1  a 3 cos 3 )  x E a 3 sin 3
1

a12  a 32  2a1a 3 cos 3

 y (a  a cos 3 )  x Ea 3 sin 3 
 q1  arctan  E 1 3

 x E (a1  a 3 cos 3 )  y Ea 3 sin 3 

Từ phương trình (3) => q 2  z E  d1
Vậy ta xác định được các tọa độ khớp như sau:



 y E (a1  a 3 cos 3 )  x E a 3 sin 3 
q1  arctan 

 x E (a1  a 3 cos 3 )  y E a 3 sin 3 


q 2  z E  d1

2


2
2
2
2



x

y

a

a
x E 2  y E 2  a12  a 32 
E

E
1
3

q 3  arctan  1  



2a1a 3
2a1a 3





1.3.2.

Vận tốc dài của các khâu tương ứng với các khớp tịnh tiến và vận tốc góc

ứng với các khớp quay.
Ta có các phương trình động học sau:

f1  a1 cos 1  a 3 .cos(1  3 )  x E  0

f 2  a1 sin 1  a 3 .sin(1  3 )  y E  0
f  d  d  z  0
1
2
E
3

Áp dụng công thức q  J q 1J p p để tính vận tốc các biến khớp:

 f1

 q1
 f
Jq    2
 q1
 f3

 q1

f1
q 2
f 2
q 2
f 3
q 2

f1 

q 3 
 a1s1  a 3s(13 ) 0 a 3s(13 ) 
f 2 


    a1c1  a 3c(13 ) 0 a 3c(13 ) 
q 3 



0
1
0
f 3 

q 3 
9


/>
 f1

 p1
 f
Jp   2
 p1
 f3

 p1

f1
p 2
f 2
p 2
f3
p 2

f1 

p3 

 1 0 0 
f 2  

   0 1 0 
p3 
 0 0 1
f3 

p3 

s(13 )y E  c(13 )x E




a1s3
 q1 


 q  q 2   J q 1J p p  
zE



 q 3 
 s1a1y E  s(13 )a 3 y E  c(13 )a 3x E  c1a1x E 


a1a 3s3
Ta có:

➢ Khâu 2 ứng với khớp tịnh tiến nên có vận tốc dài q 2  z E
➢ Khâu 1 và 3 ứng với khớp quay nên có vận gốc góc tương ứng lần lượt là:

q1 
1.3.3.

s(13 )yE  c(13 )x E
s1a1yE  s(13 )a 3 y E  c(13 )a 3x E  c1a1x E
; q3 
a1s3
a1a 3s3
Gia tốc dài của các khâu tương ứng với các khớp tịnh tiến và gia tốc góc

ứng với các khớp quay.
Ta có:

Jqq  J pp
 J q q  J q q  J p p  J p p
 q  J q 1  J q q  J p .p  J p .p 
Trong đó:

a11c1  a 3 (1  3 )c(13 ) 0 a 3 (1  3 )c(13 ) 
0 0 0 


J q   a11s1  a 3 (1  3 )s(13 ) 0 a 3 (1  3 )s(13 )  J p  0 0 0 





0
0
0
0 0 0 


c(13 )


a1s3

J p 1  
0

 a1c3  a 3c(13 )

a1a 3s3

s(13 )
a1s3
0
a1s3  a 3s(13 )
a1a 3s3


0

1

0



10


/>
CHƯƠNG 2: TĨNH HỌC ROBOT
2.1. Mô hình khảo sát bài toán tĩnh học
Z3
Z2

Z0

X3

a3

0
0

a1

0

Fy

My

Fz

X2

Z1
d2

X1
L2

d1
Y0
X1'

X0

Hình 4: Mô hình khảo sát tĩnh học Robot
Cho trước các thông số:
➢ Coi các thanh là thanh đồng chất, tiết diện ngang không đáng kể, khối lượng
các khâu là m1, m2, m3.
➢ Khâu 2 có chiều dài L2
Cho lực tác dụng từ robot lên môi trường F, M:
T

0

F  0, 0 Fy , 0 Fz  0 F4,3  0, 0Fy , 0 Fz 

0

M  [0,0 M y ,0]T 0 M4,3  [0,0 M y ,0]T

T

2.2. Tính toán bài toán tĩnh học
➢ Biễu diễn các lực Pi:
0

g1  0,0, g 

T

0

,

g 2  0,0, g 

0

P1  m1.0 g1  [0,0, m1g]T

0

P2  m2 .0 g 2  [0,0, m2g]T

0

P3  m3.0 g3  [0,0, m3g]T

T

0

,

g3  0,0, g 

T

➢ Biễu diễn các vector i rii1 và i rcii trong hệ tọa độ các khâu
11


/>r  [-a1 ,0, d1 ] , 1 r1   -a1 ,0,0 
c1
 2


1 1
0

r  [0,0, d 2 ] , 2 r 2  0,0, L2 
c2

2 

2 2
1

r  [-a 3 ,0,0] , 3 r 3   -a 3 ,0,0 
c3

 2


3 3
2

➢ Tính lực và momen của khâu 2 tác dụng lên khâu 3
Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở:

 0 F3,2  0 F4.3  0 P3
0
0
0 3 0
0 3 0
 M3,2  M 4,3  r2 . F3,2  rc3 . P3
Trong đó:
0
a 3s(13 ) 
 0
 a 3 c(13 ) 



0 3
r2  0 R 33r23   a 3s(13 )  0 r23   0
0
a 3 c(13 ) 
a 3s(13 ) a 3 c(13 )




0
0
 a 3


 2 c(13 ) 
 0



a 3
0 3
0
3 3
0 3


rc3  R 3 rc3 
s(13 )  rc3   0
 2




a
0


 3 s(13 )



 2

0
0
a 3
c(13 )
2

a 3

s(13 ) 
2

a3
c(13 ) 

2


0



0

 0   0  
0
 


0  
0
Fy
 F3,2   Fy    0   


 0 Fz   m3g   0 Fz  m3g 

1



0
s(

)a
(m
g

F
)

s(13 )a 3m3g
13
3
3
z
 



2



 0 M   0 M  c( )a (m g 0 F )  1 c( )a m g 
13
3
3
z
13
3 3

 3,2  y
2


0

c(13 )a 3 Fy






➢ Tính lực và momen của khâu 1 tác dụng lên khâu 2
Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở:

 0 F2,1  0 F3.2  0 P2

0
0
0 2 0
0 2 0
 M 2,1  M3,2  r1 . F2,1  rc2 . P2
12


/>Trong đó:
 0 
 0


0 2
r  R r   0   r1   d 2
 d 2 
 0

0 2
1

0

2 2
2 1

0
0 
0 

d2
0
0




 0
 0 


 0 2  L 2
0 2
0
2 2
rc2  R 2 rc2   0   rc2 
 2
 L2 
 0



 2 


L2
2
0
0


0

0

0 



0


0


0
Fy
 F2,1  

0

  Fz  m3g  m 2g 

1



0
0
s(


)a
(m
g

F
)

s(

)a
m
g

d
F
13
3
3
z
13
3
3
2
y
 


2




 0 M   0 M  c( )a (m g 0 F )  1 c( )a m g 
13
3
3
z
13
3 3

 2,1  y
2


0

c(13 )a 3 Fy






➢ Tính lực và momen của khâu 0 tác dụng lên khâu 1
Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở:

 0 F1,0  0 F2.1  0 P1
0
0
0 10

0 1 0
 M1,0  M 2,1  r0 . F1,0  rc1. P1
Trong đó:
d1
a1s1 
 a1c1 
 0
0 1
r0  0 R11r01   a1s1  0 r01   d1
0
a1c1 
 d1 
a1s1 a1c1
0 
 1


 2 a1c1 
 0



1
0 1
0
1 1
0 1


rc1  R1 rc1 

a s  rc1   0
 2 1 1

 0 
1


 a1s1


 2

0
0
1
 a1c1
2

1

 a1s1 
2

1
a1c1 

2

0 


13


/>
0


0


0
Fy
 F1,0  


 0 Fz  m3 g  m 2 g  m1g 

1
1



0
0
0
0
 
s(13 )a 3 (m3 g  Fz )  2 s(13 )a 3 m3g  d 2 Fy  d1 Fy  a1s1 (m 1g  m 2 g  m3g  Fz )  2 a1s1m1g 




1
1
0
0
0
0 M  
M y  c(13 )a 3 (m3 g  Fz )  c(13 )a 3 m3g  a1c1 (m 1g  m 2 g  m 3g  Fz )  a 1c1m1g 

 1,0 
2
2


0
0

c(13 )a 3 Fy  a1c1 Fy







14


/>

CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG
3.1. Thiết kế quỹ đạo trong không gian khớp
Cho 2 điểm A, B bất kì trong không gian làm việc, biết tọa độ điểm thao tác

(x E , yE ,z E ) và hướng của khâu thao tác. Thiết kế quỹ đạo chuyển động bất kì từ A đến
B, chỉ quan tâm đến các điều kiện về vị trí, vận tốc gia tốc tại điểm đầu và điểm cuối.
Chọn quỹ đạo thiết kế là đa thức bậc 3 theo thời gian có dạng:

qi (t)  a i  bi t  ci t 2  di t 3 với i=1..3 ứng với 3 biến khớp tương ứng với 1 ,d 2 , 3
Ta được hệ phương trình quỹ đạo chuyển động:

q1 (t)  a1  b1t  c1t 2  d1t 3

2
3
q 2 (t)  a 2  b 2 t  c 2 t  d 2 t

2
3
q 3 (t)  a 3  b3t  c3t  d 3t
Yêu cầu thời gian robot đi từ A đến B trong t(s) và vận tốc tại 2 điểm A, B bằng 0.
Từ đó ta có:

 q i (0)  q i (A )  a i

2
3
 q i (t)  q i (B)  a i  bi t  ci t  d i t
 q (0)  q (A)  b  0
i

i
 i
 q (t)  q (B)  b  2c t  3d t 2  0
i
i
i
i
 i
a i  q i (A)
b  0
 i

 ci  3[q i (B)  q i (A)]
t2


2[q i (B)  q i (A)]
d i  
t3

Cho các thông số của robot: a1  0.2,a 3  0.2,d1  0.2
Giả sử Robot cần dịch chuyển từ điểm A(-0.2932, 0.2250, 0.3) tới điểm B(0.3, 0.1732, 0.35) trong thời gian t=5(s). Giải động học ngược ta được
2
5


q1 (A)  3
q1 (B)  3



q 2 (A)  0.1 và q 2 (B)  0.15




q 3 (A) 
q 3 (B) 
4
3



Từ đó ta tính được các hệ số của phương trình quỹ đạo cho khâu 1:
15


/>
a 1 2.0944
b  0
 1
=> q1 (t)  2.0944  0t  0.3770t 2  0.0503t 3

c1  0.377
d1  0.0503
Tương tự ta tính được

q1 (t)  2.0944  0t  0.3770t 2  0.0503t 3

1 3


2
t
q 2 (t)  0.1  0t  0.006t 
1250

q 3 (t)  0.7854  0t  0.0314t 2  0.0042t 3
Sử dụng matlab để vẽ đồ thị:

Hình 5: Đồ thị góc khớp 1

Hình 6: Đồ thị khớp 2
16


/>
Hình 7: Đồ thị khớp 3
3.2. Thiết kế quỹ đạo trong không gian thao tác
Thiết kế quỹ đạo cho điểm tác động cuối đi từ điểm A tới điểm B nhận AB làm
đường kính. Dùng phương trình được tròn trong không gian là mặt phẳng giữa 2 điểm

A(x 0 , y0 ,0),B(x e , ye ,0) lấy AB làm đường kính.
Phương trình đường tròn (x  x i )2  (y  yi )2  R 2
Với
x0  xe

xi  2

y0  ye

 yi 

2

1

R

(x e  x 0 ) 2  (y e  y 0 ) 2

2


Phương trình viết dưới dạng tham số như sau:

 x  x i  R cos((t))

 y  yi  R sin((t))
z  0

Để thỏa mãn điều kiện về vận tốc thì tính toán hàm (t) là hàm bậc 3 theo thời
gian:
(t)  a 0  a1t  a 2 t 2  a 3t 3

17


/>
s(0)  A(x 0 , y 0 ,0)
 v(0)  v  0

0

Thỏa mãn các điều kiện: 
s(t e )  B(x e , y e ,0)
 v(t e )  v e  0
Từ điều kiện trên tìm được các hệ số a 0 ,a1 ,a 2 ,a 3 như sau:


 x0  xi 
a 0  arccos  R 



a 1  0

3k

a

2

t e2

2k

a 3  t 3
e


 x  xi 
 xe  xi 
k  arccos  0

  arccos 

 R 
 R 

Giả sử cần tạo quỹ đạo chuyển động từ điểm A(0.7, 0) đến B(-0.7, 0) trong thời
gian te=5(s).Ta tính được các giá trị sau:

a 0  1.5708

a 1  0

a 2  2.3562
a 3  0.7854
 x  0  0.7sin (t)

=> (t)  1.5708  0.t  2.3563t 2  0.7854t 3   y  0  0.7 cos (t)
z  0


Sử dụng Matlab vẽ được đồ thị:

18


/>
Hình 8: Đồ thị x(t) và vx(t)

Hình 9: Đồ thị y(t) và vy(t)

19


/>
CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT
4.1. Các tham số động lực học
Coi các thanh đồng chất, tiết diện ngang không đáng kể, trọng tâm của các khâu
đặt tại trung điểm của nó.
Các tham số động học:
Vị trí trọng tâm

Khối

xC

yC

zC

lượng

I xx

I yy

I zz

I xy

I yz

I zx

1

a1
2

0

0

m1

0

m1a12
12

m1a12
12

0

0

0

2

0

0

L2
2

m2

m2 L2 2
12

m2 L2 2
12

0

0

0

0

3

a 3
2

0

0

m3

0

m 3a 3 2
12

m 3a 3 2
12

0

0

0

Khâu

Ma trận momen quán tính

4.2. Ma trận Jacobi tịnh tiến của các khâu
Khâu 1:
Ta có:

c(1 )  s(1 )
 s( ) c( )
1
0

A1   1
 0
0

0
 0


0 a1 c(1 ) 
1


0 a1 s(1 )  1
; A c1  0
1
d1 
0


0
1 
0


c(1 )  s(1 )

0
=> A c1   s(1 ) c(1 )
 0
0


 0
0

0
0
1
0

0 0
1 0
0 1
0 0

a 1 
2 

0 
0 

1 

a 1

 a1

c1 
c1 

2

2



a1  0
a
0
1
1
s1  rc1  A1. rc1   s1 

2

2



d1 
 d1 

1 


20


/> 1


a

s

0
0
 1
1
1
 2

  2 a1s1



1
0 J T1   a1c1 0 0  ; 0 J TT1   0
 2

 0
 0

0
0







1


a1c1 0 
2

0
0
0
0



Khâu 2:
Tính toán tương tự khâu 1 ta có:





a1c1


0
rc2  0 A 2 . 2 rc2  
a1s1


1 
d

d


L2 
 1
2
2 

 J T2
0

 a1s1 0 0 
 a1s1 a1c1 0 


0 T
  a1c1 0 0  ; J T2   0
0
1 
 0
 0
1 0
0
0 

Khâu 3:
1


a1c1  2 a 3c(13 ) 



1
0
0
3

rc3  A3 . rc3  a1s1  a 3s(13 ) 


2


d1  d 2





0

J T3

0

JT3

1
1




a
s


a
s(

)
0

a
s(

)
1
1
3
13
3
13


2
2


1
1

 a1c1  a 3c(13 ) 0

a 3c(13 ) 


2
2


0
1
0




1
1


 a1s1  2 a 3s(13 ) a1c1  2 a 3c(13 ) 0 



0
0
1


1
1
a 3c(13 )

0
  a 3s(13 )
2
2



4.3. Tính ma trận jacobi quay của các khâu
Khâu 1:
21


/>

c(1 )  s(1 )

0
A c1   s(1 ) c(1 )

 0
0

 0
0

0
0
1
0

a1

c1 
2
c(1 ) s(1 ) 0 

a 1  0
s1  R c1   s(1 ) c(1 ) 0 



2



0
0
1

d1 
1 

 c(1 ) s(1 ) 0
 s(1 ) c(1 ) 0


0
 R   s(1 ) c(1 ) 0 ; R c1   c(1 ) s(1 ) 0 1
 0
 0

0
1 
0
0
0

T
c1

   R
r
1

0

T 0
c1

 0 1 0 
0


r
R  1 0 0  => 1   0 
 
0

1 
0 0


T
c1

0 0 0 
0 0 1 


T
 J R1  0 0 0  ; J R1  0 0 0 
1 0 0 
0 0 0 

Khâu 2:
Tính toán tương tự khâu 1 ta có:

 0 1 0 
0


T 0 T
r2  0 R c2
R c2  1 0 0   1r   0 
0
1 
0 0 

 J R 2

0 0 0 
0 0 1 

 0 0 0  ; J TR 2  0 0 0 
1 0 0 
0 0 0 

Khâu 3:

  R
r
3

0

 J R 3

T 0
c3

 0

R  1  3
 0

T
c3

1  3 0 
 0 

r
0

0   3   0 
1  3 
0
0 

0 0 0 
0 0 1 
 0 0 0  ; J TR 3  0 0 0 
1 0 1 
0 0 1 

4.4. Tính tensor quán tính của các khâu
Coi các khâu của robot là các thanh đồng chất tiết diện đều và bề ngang không
22