/>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

download by :


/>
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 tố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 tố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 tốn tĩnh học.............................................................. 11

2.2.

Tính tố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

download by :


/>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

download by :


/>
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 tố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
q 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

download by :


/>
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

download by :


/>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ố Danavit-

Hartenbeg như sau:
Joint
1
2

3
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

download by :


/>cos( )
0

A

sin( )
1


1

1
A

1

0
0

2

0
cos(
2

sin( )
3
A
3

1.2. Bài tố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à:


0

c( 1 ) s( 1) 0 a 1 c( 1)
s(

A

1

0

A

0

1

A. A

2

0

A

0
3

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

download by :


/>E a 1 c( 1 ) a3c(
0

rE

13)

yEa1 s( 1 ) a3 s( 13)
z

x
E

➢ 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:
cos .cos


R

sin .cos

RPY

sin

Để 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

R

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

0

0

arctan

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


download by :


/>
a3(
0

v

E

0

➢ 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
R s(
1
1


0

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

-

0

R

0

1

0

R

-

Chuyển vị các ma trận quay:
0
0

0

-

T


R1

R

T
3

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

0
1

0


6

download by :


/>
Vận tốc góc của các khâu trong hệ cố định là:

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ở:

0

a
E

➢ 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ở:

E

1.3. Bài toán động học ngược
Đã biết:
p

xE , yE , zE

T

p

Cần tìm:

7



download by :


/>[q1 ,q2 ,q3]T

q
q

1.3.1.

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

Ta có:
xE a1 c( 1 ) a3c(
a1 s( 1 ) a3 s( 13 )
z

0

13)

rE y E

E

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

a


1 cos 1

a
d

1 sin 1

a 3.cos( 1
a 3 .sin( 1

3)

) yE
d

3

1

Trong đó:
-

q1

-

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

1


, q2 d2 , q3

là các biến khớp, các nghiệm cần giải

3

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:
a 1 2 a 3 2 2a 1a 3[cos
a12

cos 3

1

cos( 1

a 32 2a1a 3 cos 3
x

E

2

y

2

E


a

1

2

3)

xE 2
a

sin

1

sin( 1

3 )]

xE2

yE 2

yE 2

2

3

2a1a 3

Ta có sin

3

x E (1)

1 cos 32
=> q arctan1
3

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

8


download by :


/>a
a 1 sin 1 a 3 (sin 1 cos 3 cos 1 sin
cos 1.(a 1 a 3 cos 3) a 3 sin 1 sin

3
3

) yE
xE

sin 1.(a 1
cos 1

y (a
sin E 1

a cos
3

3

) x a sin
E 3

3

q 1 arctan

Từ phương trình (3) => q 2

zE

d1

Vậy ta xác định được các tọa độ khớp như sau:

q

1

q

xE 2 yE 2 a1 2 a32

2a1 a3
q

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:

f
f
f
Áp dụng cơng thức q


a
9

download by :


/>
f
1

p

1


Jp
p

q
q
q

Ta có:
➢ Khâu 2 ứng với khớp tịnh tiến nên có vận tốc dài q
➢ 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à:

q
1.3.3.

s( )y
a1s 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

ứ
T
J

T

10

1


download by :



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

Z0
0

a1

0

X

Z1

My

Fz

2

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:
0

1

M [0,0 My ,0]T

0

[0,0 M y ,0]T

M4,3

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


g10, 0, g T , 0 g 2

0,0, g T , 0 g3 0,0, g T

1 P1

m1.0 g 1 [0, 0, m 1g]T

1 P2

m 2 .0 g 2

[0, 0, m2 g]T

1 P3

m 3 .0 g 3

[0, 0, m3 g]T

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

F


download by :


/>1


r1
0

2r12
3

r3
2

[0, 0, d 2 ], 2 rc22

2

[-a , 0, 0]
3
, 3 rc33

L

0,0,

2

-a
3

,0,0

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

F

0

M3,2

Trong đó:
0 3

r

0

0

3 3

2

r3

0

R


R

c3

3

0

c3

F
3,2

0

M

➢ 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

r

3

2


0


12

download by :


/>Trong đó:

0

r2
1

d2

0

F

M

0

➢ 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

0


Trong đó:
0 1

r

0

0

R

0

r1
c1

0

11
1

R

r

1

1 c1

0


r1