Website: Email : Tel : 0918.775.368
LỜI NÓI ĐẦU
Trong sự nghiệp công nghiệp hoá, hiện đại hoá đất nước, vấn đề tự động
hoá sản xuất có một vai trò đặc biệt quan trọng. Nhu cầu nâng cao năng suất
và chất lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện
tự động hóa sản xuất. Xu hướng đó tạo ra những dây chuyền về thiết bị tự
động có tính linh hoạt cao. Vì vậy nhu cầu ứng dụng Robot để tạo ra hệ thống
sản xuất linh hoạt ngày càng tăng nhanh.
Khi xét về vấn đề Robot chúng ta đặt ra các bài toán: động học Robot,
động lực học Robot và điều khiển Robot. Bài toán động học Robot là cơ sở,
đầu vào cho các bài toán động lực học Robot và điều khiển Robot. Bài toán
động học Robot gồm có động học thuận và động học ngược. Với những
Robot có số bậc tự do bằng kích thước không gian làm việc thì tọa độ các
khâu hoàn toàn xác định và là duy nhất. Trường hợp Robot có số bậc tự do
lớn hơn kích thước không gian làm việc người ta gọi là Robot dư dẫn động,
loại Robot này có tính mềm dẻo và linh hoạt cao đang được ứng dụng rộng
rãi.
Robot dư dẫn động được tạo ra nhằm thoả mãn các yêu cầu như: tránh
cấu hình kỳ dị, tránh chướng ngại vật, tạo ra sự khéo léo cho Robot, giới hạn
mômen khớp, cực tiểu động học v.v….
Để thấy được các ưu điểm của Robot dư dẫn động, đồ án tốt nghiệp này
tập trung nghiên cứu bài toán động học thuận và động học ngược của Robot
dư dẫn động. Đồ án gồm các chương sau:
Chương 1: Dựa trên lý thuyết cơ bản về ma trận Denavit- Hartenberg để
giải bài toán động học thuận: xác định vị trí bàn kẹp, vận tốc bàn kẹp, gia tốc
bàn kẹp v.v….Quan trọng nhất của phần này là xác định được vị trí bàn kẹp là
đầu vào để giải quyết bài toán động học ngược được trình bày ở phần sau.
1
Website: Email : Tel : 0918.775.368
Chng 2: a ra mt s phng phỏp tỡm nghch o ma trn ch nht
l c s gii quyt bi toỏn ng hc ngc Robot. c bit l phng

1−i
Z
1
0
−i
i
a
i
X
i
Y
1−
′
i
Z
i
0
i
α
i
0
′
i
d
1−i
Y
'
i
X
1−i

2−i
z
sang trục
1−i
z
. Nếu trục
1−i
z
cắt
trục
2−i
z
thì hướng của trục
1−i
x
được chọn tuỳ ý.
3
Khớp
1
0
−i
Khớp
Khâu i-1
Khớp
Khâu i
Khâu i+1
2−i
Z
i
Z

X
i
θ
i
0
Hình 1.2
Website: Email : Tel : 0918.775.368
• Gốc tọa độ
1−i
O
được chọn tại giao điểm của trục
1−i
x
và trục
1−i
z
.
• Trục
1−i
y
được chọn sao cho hệ
( )
1−i
Oxyz
là hệ quy chiếu thuận.
Với cách chọn trên, đôi khi các hệ tọa độ khâu
( )
1−i
Oxyz
không được xác định

n
x
lại được xác định theo pháp tuyến của trục
1−n
z
. Trong trường hợp này nếu là khớp quay ta nên chọn trục
n
z

song song với trục
1−n
z
. Ngoài ra ta có thể chọn tuỳ ý sao cho hợp
lý.
4
Website: Email : Tel : 0918.775.368
• Khi hai trục
1−i
z
và
2−i
z
song song với nhau thì giữa hai trục này
có nhiều đường pháp tuyến chung, ta có thể chọn trục
1−i
x
hướng
theo pháp tuyến chung nào cũng được.
• Khi khớp thứ i là tịnh tiến, về nguyên tắc ta có thể chọn trục
1−i

′′
.
-
i
d
: khoảng cách đo dọc trục khớp động thứ i từ đường vuông góc chung
giữa hai trục khớp động i+1 và i tới đường vuông góc chung giữa khớp
động i và trục khớp động i-1.
-
i
a
: độ dài đường vuông góc chung giữa hai trục khớp động i+1 và i.
-
i
α
: Góc chéo giữa hai trục khớp động i+1 và i.
Trong bốn tham số trên, các tham số
i
a
và
i
α
luôn luôn là các hằng số,
độ lớn của chúng phụ thuộc vào hình dáng và sự kết nối các khâu thứ i-1 và
khâu thứ i. Hai tham số còn lại
i
θ
và
i
d

i
d
.
3. Tịnh tiến dọc trục
1−i
x
một đoạn
i
a
.
5
Website: Email : Tel : 0918.775.368
4. Quay quanh trục
i
x
một góc
i
α
.
Ma trận của phép biến đổi thuần nhất ký hiệu là H
i
, là tích của bốn phép biến
đổi cơ bản và có dạng như sau:




















−
=
1000
0cossin0
0sincos0
0001
1000
0100
0010
001
1000
100
0010
0001
1000
0100
00cossin
00sincos

sinsincoscoscossin
cossinsincossincos
iii
iiiiiii
iiiiiii
i
d
a
a
αα
θαθαθθ
θαθαθθ
H
(1.1)
Ma trận
i
H
gọi là ma trận Denavit-Hartenberg. Ma trận Denavit-Hartenberg
i
H
là ma trận chuyển tọa độ của hệ quy chiếu
( )
1−i
Oxyz
đối với hệ quy chiếu
( )
i
Oxyz
. Chính xác hơn ta phải ký hiệu là
i

=
, với n là chỉ số chỉ hệ tọa gắn liền với bàn kẹp của Robot, từ đó ta có:












===
−
1000

21
110
zzzz
yyyy
xxxx
nn
n
pasn
pasn
pasn
HHHHHHD
n 21

i
1
2
3
q1
q2
q3
0
0
0
a
1
a
2
a
3
0
0
0
b. Phương trình xác định vị trí bàn kẹp của Robot
Áp dụng công thức (1.2) ta xác định được ma trận chuyển
n
D
:
3
2
2
1
1
0

3
y
0
y
1
x
1
y
2
y
0
x
3
x
1
q
1
a
2
x
2
a
3
a
2
q
3
q
Hình 1.3
7

π
=
3
Và các khoảng cách:
15,15,15
321
=== aaa
, thay vào chương trình tính toán:
- Vị trí:
)
3
cos(15)
6
5
cos(15)
6
11
cos(15
ttt
p
x
πππ
++=
)
3
sin(15)
6
5
sin(15)
6

55 ttt
v
x
−−−=
Đồ thị tọa độ bàn kẹp p
x
(t)
Đồ thị bàn kẹp p
y
(t)
8
Đồ thị gia tốc bàn kẹp a
y
(t)
Đồ thị gia tốc bàn kẹp a
x
(t)
Website: Email : Tel : 0918.775.368
π
π
π
π
π
π
)
3
cos(5)
6
5
cos(

cos(
12
605
π
π
π
π
π
π
ttt
a
x
−−−=
222
)
3
sin(
3
5
)
6
5
sin(
12
125
)
6
11
sin(
12

i
α
i
1
2
3
4
q1
q2
q3
q4
0
0
0
0
a1
a2
a3
a4
0
0
0
0
b. Phương trình xác định vị trí bàn kẹp của Robot
Áp dụng công thức (1.2) ta xác định được ma trận chuyển
n
D
:



0
SaSaSaSaCS
CaCaCaCaSC
n
HHHHD
Ở đây do kết quả ma trận
n
D
dài nên ta sử dụng kí hiệu:
)sin(),cos(sin,cos
, jijijijiiiii
qqSqqCqSqC +=+===
++
.
Từ đó vị trí bàn kẹp trong hệ tọa độ cố định:
0
y
1
x
1
y
2
y
0
x
4
y
4
x
1

y
+++++++++=
0=
z
p
c. Dữ liệu đầu vào cho Robot 4 khâu phẳng
Chọn quy luật chuyển động các biến khớp theo thời gian:
tq
3
1
π
=
,
tq
3
2
π
=
,
tq
2
3
π
=
,
tq
2
4
π
=




= ttttp
x
ππππ
3
1
cos15
6
7
cos15
3
5
cos15
3
2
cos15






+






0=
z
p
Đồ thị tọa độ bàn kẹp
Đồ thị tọa độ bàn kẹp
11
Đồ thị vận tốc bàn kẹp
Đồ thị vận tốc bàn kẹp
Đồ thị gia tốc bàn kẹp
Đồ thị gia tốc bàn kẹp
Website: Email : Tel : 0918.775.368
- Vận tốc:
ππππππππ






−






−







+






+






+






= ttttv
y
3
1
cos5
6

3
2
cos
3
20
ππππππππ






−






−






−







−






−






−






−= tttta
y
0=
z
a
Quỹ đạo bàn kẹp

a1
a2
a3
a4
a5
0
0
0
0
0
b. Phương trình xác định vị trí bàn kẹp của Robot:
Từ biểu thức (1.2) ta xác định được ma trận
n
D
:












++++
++++−
=

1
y
2
y
0
x
4
y
4
x
1
q
1
a
2
x
2
a
3
a
2
q
3
q
4
q
3
x
4
a

qaqqa
qqqaqqqqaqqqqqap
y
+++
+++++++++++=
0=
z
p
c. Dữ liệu đầu vào cho Robot 5 khâu phẳng
Chọn quy luật chuyển động của các biến khớp như sau theo thời gian như sau:
tq
t
q
t
q
t
q
t
q
π
ππππ
===−==
54321
,
2
,
3
,
6
,



+






+






= tttttp
x
πππππ
12
5
cos10
4
1
cos10
12
23
cos10
12
11


+






= tttttp
y
πππππ
12
5
sin10
4
1
sin10
12
23
sin10
12
11
sin10
12
1
sin10
0=
z
p
15

−






−= tttttv
x
12
5
sin
6
25
4
1
sin
2
5
12
11
sin
6
55
12
23
sin
6
115
12



+






= tttttv
y
12
5
sin
6
25
4
1
sin
2
5
12
11
sin
6
55
12
23
sin
6

2645
12
1
cos
72
5
22
222
ππππ
ππππππ






−






−







11
sin
72
605
12
23
sin
72
2645
12
1
sin
72
5
22
222
ππππ
ππππππ






−






Đồ thị vận tốc bàn kẹp
Đồ thị gia tốc bàn kẹp
Đồ thị gia tốc bàn kẹp
16
Website: Email : Tel : 0918.775.368
Quỹ đạo bàn kẹp
17
Website: Email : Tel : 0918.775.368
1.2.4 Bài toán động học thuận của Robot PPR phẳng
a. Hình vẽ và bảng thông số động học Denavit-Hartenberg
Trục
θ
i
d
i
a
i
α
i
1
2
3
Pi/2
Pi/2
q3
q1
q2
0
0
0

+
=
1000
cos0sincos
0100
sin0cossin
13333
23333
qqaqq
qqaqq
Từ đó:
0
x
3
x
2
x
1
x
0
y
2
q
2
y
1
y
2
z
1

Và khoảng cách:
20
3
=a
.
Thay vào chương trình ta được:
- Vị trí:
ttp
x
4)sin(20 +=
π
0=
x
p
ttp
z
3)cos(20 +=
π
- Vận tốc:
4)cos(20 +=
ππ
tv
x
0=
y
v
3)sin(20 +−=
ππ
tv
z

Trục
θ
i
d
i
a
i
α
i
1
2
3
4
q1
q2
q3
0
0
0
0
q4
a1
a2
a3
0
0
0
Pi
0
b. Phương trình xác định vị trí bàn kẹp của Robot

2
2
1
1
0
q
SaSaSaCS
CaCaCaSC
n
HHHHD
Ở đây ta sử dụng kí hiệu:
)sin(),cos(sin,cos
, jijijijiiiii
qqSqqCqSqC +=+===
++
0
z
4
z
2
z
1
z
3
z
0
x
1
x
4

+++++=
4
qp
z
−=
c. Dữ liệu đầu vào cho rôbôt RRRP1
Ta chọn quy luật chuyển động của các biến khớp theo thời gian t như sau:
tq
4
1
π
=
,
tq
3
2
π
=
,
tq
3
2
1
π
=
,
tq 3
1
=
.

++=
)
4
sin(20)
12
7
sin(20)
4
5
sin(20
ttt
p
y
πππ
++=
tp
z
2−=
Đồ thị tọa độ bàn kẹp
Đồ thị tọa độ bàn kẹp
Đồ thị tọa độ bàn kẹp
22
Website: Email : Tel : 0918.775.368
- Vận tốc:
π
π
π
π
π
π

)
4
5
cos(25
ttt
v
y
+++=
2−=
z
v
-Gia tốc:
222
)
4
cos(
4
5
)
12
7
cos(
36
254
)
4
5
cos(
4
125

sin(
4
125
π
π
π
π
π
π
ttt
a
y
−−−=
0=
z
a
Đồ thị gia tốc bàn kẹp
Đồ thị gia tốc bàn kẹp
Quỹ đạo bàn kẹp
24
Website: Email : Tel : 0918.775.368
1.2.6 Bài toán động học thuận Robot RRRP2
a. Hình vẽ và bảng thông số động học Denavit-Hartenberg
Bảng thông số:
b. Phương trình xác định vị trí bàn kẹp






1
2
1
2
1
2
1
2
1
2
1
2
1
2
1
2
1
2
1
2
1
2
1

12243232
212212321432143213211321321
212212321432143213211321321
4
3
3

y
4
q
3
z
4
x
1
y
2
z
2
a
Hình 1.8
Trục θ
i
d
i
a
i
α
i
1
2
3
4
q1
q2
q3
0