Đồ án tốt nghiệp : Tính toán động học thuận robot dư dẫn động bằng phương pháp khalil và phương pháp gradient thu gọn

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

phỏp phõn tớch cõn xng ca Khalil.
Chng 3: Da trờn phng phỏp tớnh ma trn ta nghch o trỡnh by
chng hai, chỳng ta tp trung nghiờn cu v a ra 2 phng phỏp:
phng phỏp Khalil v phng phỏp Reduced _ Gradient gii quyt bi
toỏn ng hc ngc Robot d dn ng.
Trong quỏ trỡnh thc hin ỏn ny, do kin thc v Robot cú hn, thi
gian ớt v ti liu bng ting Vit cha cú nờn ỏn ca em vn cũn nhiu
thiu xút, em kớnh mong c s ch bo, hng dn ca cỏc thy ngy
cng hon thin mỡnh hn.
Em xin chõn thnh cm n GS.TSKH Nguyn Vn Khang, Th.S
Thnh Trung ó tn tỡnh ch bo, hng dn em hon thnh bn ỏn ny.
Em cng xin chõn thnh cm n cỏc thy giỏo trong b mụn C hc ng dng
ó to iu kin giỳp em trong quỏ trỡnh lm ỏn ti b mụn.

tính toán động học thuận robot d dẫn động bằng phơng
pháp khalil và phơng pháp gradient thu gọn
2
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

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

X
i
θ
i
0
Hình 1.1
Website: Email : Tel : 0918.775.368
CHƯƠNG 1
TÍNH TOÁN ĐỘNG HỌC THUẬN RÔBỐT DƯ DẪN ĐỘNG
1.1 BỘ THÔNG SỐ DH VÀ MA TRẬN DENAVIT-HARTENBERG
1.1.1 Bộ thông số DH
Dưới đây là trình bày cách xây dựng hệ tọa độ đối với hai khâu liên tiếp i
và i+1. Hình 1.1 là trường hợp 2 khớp động liên tiếp là khớp quay. Hình 1.2
là khớp tịnh tiến.
Đối với Robot công nghiệp, Denavit-Hartenberg(1995) đã đưa ra cách chọn
các hệ trục tọa độ như sau:
• Trục
1−i
z
được chọn dọc theo hướng của trục khớp động thứ i
• Trục
1−i
x
được chọn dọc theo đường vuông góc chung của hai trục
2−i
z
và
1−i
z
, hướng đi từ trục

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

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

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

một cách duy nhất. Vì vậy ta cần có một số bổ xung như sau:
• Đối với hệ tọa độ
( )
o
Oxyz
theo quy ước trên ta mới chỉ chọn được
trục
o
z
, còn trục
o
x
chưa có trong quy ước trên. Ta có thể chọn
trục
o
x
một cách tuỳ ý.
• Đối với hệ tọa độ
( )
n
Oxyz
do không có khớp n+1 nên ta theo quy
ước trên không xác định được trục
n
z
. Trục
n
z
không xác định duy
nhất trong khi trục

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

z

một cách tuỳ ý. Tuy nhiên trong nhiều trường hợp người ta thường
chọn trục
1−i
z
dọc theo trục của khớp tịnh tiến này.
Vị trí của hệ tọa độ khâu
( )
i
Oxyz
đối với hệ tọa độ khâu
( )
1−i
Oxyz
được xác
định bởi bộ thông số DH như sau:
-
i
θ
: góc quay quanh trục
1−i
z
để trục
1−i
x
chuyển đến trục
( )
iii
xxx //

′′
.
-
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

, một là hằng số một là biến số phụ
thuộc vào khớp quay hay khớp tịnh tiến. Nếu khớp i là khớp quay thì
i
θ
là
biến còn
i
d
là hằng , nếu khớp i là tịnh tiến thì
i
d
là biến còn
i
θ
là hằng số.
1.1.2 Ma trận Denavit-Hartenberg
Trên cơ sở đã xây dựng các hệ tọa độ đối với hai khâu động liên tiếp như
đã trình bày ở trên, ta có thể thiết lập mối quan hệ giữa hai hệ tọa độ liên tiếp
theo bốn bước sau đây:
1. Quay quanh trục
1−i
z
một góc
i
θ
.
2. Tịnh tiến dọc trục
1−i
z
một khoảng

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

ii
ii
i
i
ii
ii
i
a
d
αα
ααθθ
θθ
H












−
−
=
1000
cossin0

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

i-
H
1
. Đơn giản cách viết ta sử dụng
ký hiệu
i
H
thay cho
i
i-
H
1
. Còn
i
D
được dùng với nghĩa
i
o
H
.
1.1.3 Phương trình xác định vị trí khâu thao tác (bàn kẹp) của Robot
Ma trận
i
D
là tích các ma trận
i
H
và là ma trận mô tả vị trí và hướng
của hệ tọa độ gắn liền với khâu thứ i so với hệ tọa độ cố định. Trường hợp
ni

=
, 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

(1.2)
Trong đó
x
p
,
y
p
,
z
p
là tọa độ vị trí của khâu thao tác.
1.1.4 Giải quyết bài toán động học thuận của Robot
Giải quyết bài toán động học thuận của Robot thực chất là chúng ta cho
trước cấu hình của Robot và quy luật chuyển động của các khâu, từ đó chúng
ta xác định quy luật chuyển động của bàn kẹp (điểm tác động cuối): vị trí bàn
6
Website: Email : Tel : 0918.775.368
kẹp, vận tốc bàn kẹp, gia tốc chuyển động bàn kẹp, vận tốc góc, gia tốc góc
v.v…
1.2 TÍNH TOÁN ĐỘNG HỌC THUẬN CỦA MỘT SỐ ROBOT
1.2.1 Bài toán động học thuận của Robot 3 khâu 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
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

HHHD =
n












+++++++++
+++++++−++
=
1000
0100
1sin)sin()321sin(0)cos()sin(
1cos)cos()321cos(0)sin()cos(
12123321321
12123321321
qaqqaqqqaqqqqqq
qaqqaqqqaqqqqqq
Từ đó ta có được vi trí bàn kẹp trong hệ tọa độ cố định:
112123213
cos)cos()cos( qaqqaqqqap
x
+++++=

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

Website: Email : Tel : 0918.775.368
112123213
sin)sin()sin( qaqqaqqqap
y
+++++=
0=
z
p
c. Dữ liệu đầu vào của Robot 3 khâu phẳng
Để tính toán cụ thể bài toán động học thuận ta phải cho trước quy luật
chuyển động của các biến khớp theo thời gian và cho giá trị cụ thể của các
khoảng cách
i
a
.
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
3
1
π
=
;
tq
2
2
π
=
;
tq

π
=
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

11
sin(15
ttt
p
y
πππ
++=
0=
z
p
- Vận tốc:
π
π
π
π
π
π
)
3
sin(5)
6
5
sin(
2
25
)
6
11
sin(
2

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(

2
25
)
6
11
cos(
2
55 ttt
v
y
++=
0=
z
v
- Gia tốc:
222
)
3
cos(
3
5
)
6
5
cos(
12
125
)
6
11

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

605
π
π
π
π
π
π
ttt
a
y
−−−=
0=
z
a
Đồ thị vận tốc bàn kẹp v
y
(t)
Đồ thị vận tốc bàn kẹp v
x
(t)
Quỹ đạo bàn kẹp
9
Website: Email : Tel : 0918.775.368
1.2.2 Bài toán động học thuận của Robot 4 khâu 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
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
:













+++
+++−
=
=
++++++++++++
++++++++++++
1000
0100
0
0

1121232134321443214321
1121232134321443214321
4
3
3
2
2
1
1

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

q
1
a
2
x
2
a
3
a
2
q
3
q
4
q
3
y
3
x
4
a
Hình 1.4
10
Website: Email : Tel : 0918.775.368
11212321343214
cos)cos()cos()cos( qaqqaqqqaqqqqap
x
+++++++++=
11212321343214
sin)sin()sin()sin( qaqqaqqqaqqqqap

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
π
=

Và các khoảng cách:
15,15,15,15
4321
==== aaaa
Thay vào chương trình tính toán ta được kết quả:
- Vị trí:






+






+






+




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

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

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:
ππππππππ






−






−







−






−= tttv
x
3
1
sin5
6
7
sin
2
35
3
5
sin25
3
2
sin10
ππππππππ




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

7
cos
2
35
3
5
cos25
3
2
cos10
0=
z
v
- Gia tốc:
2222
3
1
cos
3
5
6
7
cos
12
245
3
5
cos
3
125

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




−= tttta
x
2222
3
1
sin
3
5
6
7
sin
12
245
3
5
sin
3
125
3
2
sin
3
20
ππππππππ




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

12
Website: Email : Tel : 0918.775.368
13
Website: Email : Tel : 0918.775.368
1.2.3 Bài toán động học thuận của Robot 5 khâu phẳng
a. Hình vẽ và bảng thông số động học Denavit- Hartanberg
Bảng thông số:
Trục θ
i
d
i
a
i
α
i
1
2
3
4
5
q1
q2
q3
q4
q5
0
0
0
0
0

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
:












++++
++++−
=

=
++++++++++++++++++
++++++++++++++++++
1000
0100
0
0

112123213432145432155432154321
112123213432145432155432154321
5
4
4
3
3
2
2
1
1
0
SaSaSaSaSaCS
CaCaCaCaCaSC
n
HHHHHD
5
y
0
y
1
x

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

5
q
5
x
5
a
Hnh 1.5
14
Đồ thị tọa độ bàn kẹp
Đồ thị tọa độ bàn kẹp
Website: Email : Tel : 0918.775.368
Ở đây ta sử dụng kí hiệu:
)sin(),cos(sin,cos
, jijijijiiiii
qqSqqCqSqC +=+===
++
Từ đó ta có được công thức tính vị trí bàn kẹp trong hệ tọa độ cố định:
11212
321343214543215
cos)cos(
)cos()cos()cos(
qaqqa
qqqaqqqqaqqqqqap
x
+++
+++++++++++=
11212
321343214543215
sin)sin(
)sin()sin()sin(

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
,

4
.
Và các khoảng cách
i
a
:
10,10,10,10,10
44321
===== aaaaa
.
Thay vào chương trình tính toán ta được kết quả như sau:
-Vị trí:






+






+





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

cos10
12
1
cos10






+






+






+






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

Website: Email : Tel : 0918.775.368
- Vận tốc:
ππππππππππ






−






−






−







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

1
sin
6
5
ππππππππππ






+






+






+





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

115
12
1
sin
6
5
0=
z
v
- Gia tốc:
.
12
5
cos
72
125
4
1
cos
8
5
12
11
cos
72
605
12
23
cos
72

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

−






−






−=
tt
ttta
x
.
12
5
sin
72
125
4
1
sin
8
5
12

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


−






−






−






−=
tt
ttta
y
.0=
z
a
Đồ 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
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

a3
Pi/2
Pi/2
0
b. Vị trí bàn kẹp:
Từ biểu thức (1.2) ta có thể xác định ma trận chuyển
n
D
:
3
2
2
1
1
0
HHHD =
n












+−

+
=
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

z
3
a
3
q
1
q
3
y
Hình 1.6
18
Website: Email : Tel : 0918.775.368
233
sin qqap
x
+=
0=
y
p
133
cos qqap
z
+=
c. Dữ liệu đầu vào cho Robot PPR phẳng
Chọn quy luật chuyển động của các biến khớp theo thời gian như sau:
tqtqtq
π
===
321
;4;3

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

Đồ thị tọa độ bàn kẹp
Đồ thị tọa độ bàn kẹp
Đồ thị vận tốc
Đồ thị vận tốc
19
Website: Email : Tel : 0918.775.368
- Gia tốc:
2
)sin(20
ππ
ta
x
−=
0=
y
a
2
)cos(20
ππ
ta
z
−=
Đồ thị gia tốc
Đồ thị gia tốc
Quỹ đạo bàn kẹp
20
Website: Email : Tel : 0918.775.368
1.2.5 Bài toán động học thuận của Robot RRRP1
a. Hình vẽ và bảng thông số động học Denavit-Hartenberg
Bảng thông số:

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













−−
++−
++
=
=
+++++++
+++++++
1000
100
0
0

4
112123213321321
112123213321321
4
3
3

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

x
3
x
2
x
1
a
2
a
2
q
1
q
3
q
4
q
3
a
Hình 1.7
21
Website: Email : Tel : 0918.775.368
Vị trí bàn kẹp trong hệ tọa độ cố định là:
112123213
cos)cos()cos( qaqqaqqqap
x
+++++=
112123213
sin)sin()sin( qaqqaqqqap
x

+++++=
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
=
.

Và các khoảng cách
i
a
:
20
1
=a
,
20
2
=a
,
20
3
=a
Thay vào phương trình ta tính được kết quả như sau:
- Vị trí:
)
4
cos(20)
12
7
cos(20)
4
5
cos(20
ttt
p
x
πππ

++=
)
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
sin(5)
12
7
sin(
3
35
)
4
5
sin(25
ttt
v
x
−−−=
π
π
π
π
π
π
)
4
cos(5)
12
7
cos(
3
35

)
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

π
π
π
π
π
π
ttt
a
x
−−−=
Đồ thị vận tốc bàn kẹp
Đồ thị vận tốc bàn kẹp
Đồ thị vận tốc bàn kẹp
23
Website: Email : Tel : 0918.775.368
222
)
4
sin(
4
5
)
12
7
sin(
36
254
)
4
5

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












+
++−−+
++−−−+
=
=
++
+−−−++−−++−−++
+−++−−++−−++−−
1000
00
2
1
2
1
2
1
2
1
2

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

2
2
1
1
0
dSaqCC
SaSaCqCqCCCSS
CaCaSqSqSSSCC
n
HHHHD
0
x
1
x
0
y
0
z
2
q
2
z
3
q
1
d
32
xx
≡
2

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

d1
0
0
q4
0
a2
0
0
Pi/2
0
-Pi/2
0
25