Bài tập dài robot công nghiệp

I. Xây dựng phơng trình động học thuận và ngợc biểu diễn mối quan hệ giữa hệ toạ độ
tay Robot (End effector) và hệ tọa độ các khớp (Joints)
Robot hai thanh nối là robot với 2 khâu phẳng, vì vậy cơ chế và phạm vi hoạt
động của robot nằm trong mặt phẳng, bao gồm hai khớp quay và hai thanh nối nh
hình vẽ:

1. Thiết kế hệ toạ độ cho các thanh nối
a. Tham số của thanh nối và khớp
Xác định bộ thông số Denavit -Hartenberg (bảng thông số DH)
Từ hình vẽ robot ta xác định đợc các thông số sau:
+ Độ dài pháp tuyến chung của khớp 1 và khớp 2 là a
1
+ Góc chéo giữa hai trục khớp 1 và khớp 2 là
1
.
+ Đây là khớp quay cho nên khoảng cách đo dọc trục khớp động 1 từ đờng
vuông góc chung giữa trục khớp động 2 và trục khớp động 1 tới đờng vuông góc
chung giữa khớp động 1 và gốc robot là d
1
= 0. Tơng tự ta cũng dễ dàng có d
2
= 0.
+ Góc giữa hai đờng vuông góc chung là
1
Tơng tự xét khớp 2 và tay robot
+ Ta có độ dài pháp tuyến chung là a
2
+ Góc chéo giữa hai trục khớp 2 và tay robot là
1
.
+ Góc giữa hai đờng vuông góc chung là

2
Vậy biến khớp là
1
và
2
b. Thiết lập hệ toạ độ
Hệ toạ độ cố định o
0
x
0
y
0
z
0
có gốc o
0
đặt tại tâm trục khớp động 1, chiều x
0
hớng
từ o
0
đên o
1
y
0
x
0
a
1
x

1
y
1
x
2
y
2
a
2

1

2
Khớp 1
Khớp 2
0
0
0
1
0
2
Z
1
Hệ toạ độ o
1
x
1
y
1
z

1
có gốc o
1
đặt tại tâm trục khớp động 2
Hệ toạ độ o
2
x
2
y
2
z
2
có gốc o
2
đặt tại tâm trục khớp động cuối khâu 2.
Ba trục z
0
, z
1
, z
2
vuông góc với mặt phẳng tờ giấy.
2. Xây dựng phơng trình động học thuận cho robot
Phơng trình động học thuận là phơng trình biểu diễn quan hệ vị trí và hớng của
tay robot thông qua các biến khớp. Các biến khớp này là góc quay của khớp quay và
độ dịch chuyển tịnh tiến đối với khớp tịnh tiến
Căn cứ vào phơng trình động học thuận này khi biết vị trí của các khớp ta có thể
xác định đợc vị trí và hớng của tay robot.
Căn cứ vào các thông số và hệ toạ độ đã đợc thiết lập ta có bảng thông số DH
của robot nh sau:

Thanh (khâu)

i

i
a
i
D
i
1

1
*
0 l
1
0
2

2
*
0 l
2
0
*. Quan hệ giữa hai khung toạ độ o
1
x
1
y
1
z

1
và o
0
x
0
y
0
z
0
đợc xác định nh sau:
- Quay xung quanh trục z
0
một góc
1
sao cho trục x
0
trùng với phơng của trục x
1
- Tịnh tiến dọc theo trục x
1
(phơng pháp tuyến chung) một đoạn bằng a
1
Vậy phép biến đổi tổng hợp nh sau:
A
1
= Rot(z
0
,
1
) Trans (a

1
,0,0)
Trong đó: Rot(z
0
,
1
) là phép quay quanh trục z
0
một góc
1
Rot(z
0
,
1
) =














1000

0100
00
00
11
11
cossin
sincos
Trans(a
1
,0,0) là phép tịnh tiến khung toạ độ o
0
x
0
y
0
z
0
theo trục x
0
một đoạn bằng a
1
.
Trans(a
1
,0,0) =

1000
0100
0010
001
1
a
Vậy
0
A
1
đợc tính nh sau:
0
A
1
= Rot(z
0
,
1
) Trans (a
1
,0,0) =

1000
0100
00
00
11
11
cossin
sincos

1000
0100
0010
001
1
a

0
A
1
=














1000
0100
0

0
1111
1111
sinacossin
cosasincos
(1.1)
*. Quan hệ giữa hai khung toạ độ o
2
x
2
y
2
z
2
và o
1
x
1
y
1
z
1
đợc xác định nh sau:
- Quay xung quanh trục z
1
một góc
2
sao cho trục x
1
trùng với phơng của trục x

2
- Tịnh tiến dọc theo trục x
2
(phơng pháp tuyến chung) một đoạn bằng a
2
Vậy phép biến đổi tổng hợp nh sau:
1
A
2
= Rot(z
1
,
2
) Trans (a
2
,0,0)
Rot(z
1
,
2
) là phép quay quanh trục z
1
một góc
2
Rot(z
1
,
2
) =

1000
0100
00
00
22
22
cossin
sincos
Trans(a
2
,0,0) là phép tịnh tiến khung toạ độ o
1
x
1
y
1
z

1
theo trục x
1
một đoạn bằng
a
2
.
Trans(a
2
,0,0) =












1000
0100
0010
001
2
a
Vậy
1

A
2
đợc tính nh sau:
1
A
2
= Rot(z
1
,
2
) Trans (a
2
,0,0) =














1000
0100
00

00
22
22
cossin
sincos












1000
0100
0010
001
2
a

1
A
2
=

1000
0100
0
0
2222
2222
sinacossin
cosasincos
(1.2)
Phơng trình động học thuận đợc xác định nh sau:
0
A
2
=
0
A
1
1

A
2
=












θθθ
θθ−θ
1000
0100
0
0
1111
1111
sinacossin
cosasincos













θθθ
θθ−θ
1000
0100
0
0
2222
2222
sinacossin
cosasincos
Nh©n hai ma trËn trªn vµ rót gän ta cã:
0
A
2
=
( ) ( ) ( )
( ) ( ) ( )













θ+θ+θθ+θθ+θ
θ+θ+θθ+θ−θ+θ
1000
0100
0
0
112122121
112122121
sinasinacossin
cosacosasincos
(1.3)
Hay:
0
A
2
=













1000
zzzz
yyyy
xxxx
paon
paon
paon
=
( ) ( ) ( )
( ) ( ) ( )












θ+θ+θθ+θθ+θ
θ+θ+θθ+θ−θ+θ
1000
0100

0
0
112122121
112122121
sinasinacossin
cosacosasincos

(1.4)
Nh vËy hÖ ph¬ng tr×nh ®éng häc thuËn cña robot nh sau:
n
x
= cos (θ
1
+ θ
1
)
n
y
= sin (θ
1
+ θ
1
)
n
z
= 0
o
x
= - sin (θ
1

+ θ
1
)
o
y
= cos (θ
1
+ θ
1
)
o
z
= 0
a
x
= 0
a
y
= 0
a
z
= 1
p
x
= a
1
cosθ
1
+ a
2

cos (θ
1
+ θ
1
)
p
y
= a
1
sinθ
1
+ a
2
sin (θ
1
+ θ
1
)
p
z
= 0
3. X©y dùng ph¬ng tr×nh ®éng häc ngîc cho robot
Phơng trình động học ngợc robot nhằm xác định các giá trị biến khớp từ các
vị trí và hớng của tay robot mong muốn. Phơng trình động học ngợc thờng khó giải
và không có lời giải tổng quát cho mọi robot.
Ta thấy rằng ma trận
0
A
2
đã biết, tức là vị trí và hớng của khung toạ độ tay robot,

cần xác định giá trị các biến khớp
1
và
2
:
0
A
2
=
0
A
1
1
A
2
=












1000
zzzz

yyyy
xxxx
paon
paon
paon
(1.5)
Nhân hai vế của phơng trình trên với ma trận nghịch đảo của ma trận
0
A
1
là (
0
A
1
)
-1
ta có phơng trình sau:
(
0
A
1
)
-1

0
A
2
=
1
A

2
Ta có (
0
A
1
)
-1
=














1000
0100
00
0
11
111
cossin
asincos

(1.6)
Vậy (
0
A
1
)
-1

0
A
2
=














1000
0100
00
0

11
111
cossin
asincos












1000
zzzz
yyyy
xxxx
paon
paon
paon
(
0
A
1
)
-1

0
A
2
=
1
A
2
tơng đơng với:












++++
++++
1000
11111111
111111111
zzzz
yxyxyxyx
yxyxyxyx
Paon
cosPsinPcosasinacososinocosnsinn

asinPcosPsinacosasinocososinncosn
=














1000
0100
0
0
2222
2222
sinacossin
cosasincos
(1.7)
Cân bằng hai vế của hai ma trận tại cột thứ 4 ta đợc hệ phơng trình sau:

=+
=+
2211
22111
sinacosPsinP
cosaasinPcosP
yx
yx
(1.8)
Với các thông số P
x
, P
y
và a
1
, a
2
đã biết giải hệ phơng trình này ta đợc các biến
khớp
1
và
2
nh sau:
Ta có:

=+
+=+
2211
12211
sinacosPsinP
acosasinPcosP
yx
yx
(1.9)
Bình phơng ai vế 1.9, cộng vế theo vế ta đợc:
2
2221
2
1
22
2 acosaaaPP
yx
++=+
Vậy








+
=
21

2
2
2
1
22
2
2 aa
aaPP
cosar
yx
(1.10)
Bình phơng hai vế và cộng vế theo vế của 1.8 ta đợc:
( )
2
2111
222
1
2 asinPcosPaPPa
yxyx
=+++
1
2
2
2
1
22
11
2a
aaPP
sinPcosP

yx
yx
++
=+
22
1
2
2
2
1
22
1
22
1
22
2
yx
yx
yx
y
yx
x
PPa
aaPP
sin
PP
P
cos
PP
P

+
++
=
+
+
+










+











+
++

=
2222
1
2
2
2
1
22
1
2
yx
x
yx
yx
PP
P
arcsin
PPa
aaPP
arcsin
(1.11)
Vậy:

+
=










+

+
++
=
21
2
2
2
1
22
2
2222
1
2
2
2
1
22
1
2
2
aa
aaPP
cosar
PP
P
arcsin
PPa

aaPP
arcsin
yx
yx
x
yx
yx
(1.12)
II. Xây dựng quan hệ giữa tốc độ của các khớp và tốc độ của tay robot
Để xây dựng quan hệ giữa tốc độ của khớp và tốc độ của tay robot ta cần phải
thiết lập phơng trình động lực học cho robot.
Gọi v là tốc độ chuyển động tịnh tiến của tâm vật thể, là tốc độ góc, m là khối
lợng vật thể, J
t
là mômen quán tính của vật thể.
Sử dụng phơng pháp Lagrange để thiết lập phơng trình động lực học cho robot. Giả
thiết khớp 1 sinh ra mômen M
1
tác dụng giữa bệ và thanh nối 1, khớp 2 sinh ra mômen
M
2
tác dụng giữa thanh nối 1 và 2. Trọng lực có hớng theo trục y.
1. Tính tốc độ của một điểm bất kỳ trên robot
Nh trong phần I ta có phép biến đổi toạ độ từ thanh nối 2 đến hệ toạ độ gốc
là
0
A
2

0

A
2
=
0
A
1
1
A
2
(2.1)
Trong đó: A
1
là phép biến đổi toạ độ từ thanh nối 1 đến hệ toạ độ gốc, là hàm
của biến
1

0
A
1
=

1000
0100
0
0
1111
1111
sinacossin
cosasincos

1
A
2
là phép biến đổi toạ độ từ thanh nối 2 đến hệ toạ độ thanh nối 1, là
hàm của biến
2

1
A
2
=

1000
0100
0
0
2222
2222
sinacossin
cosasincos
Và
0
A
2
=
( ) ( ) ( )
( ) ( ) ( )

++++
++++
1000
0100
0
0
112122121
112122121
sinasinacossin
cosacosasincos
Giả thiết biểu diễn một điểm bất kỳ nào đó trên thanh nối i nh sau

[ ]
T
iii
i
,z,y,xr 1=
(2.2)
Vậy vị trí của điểm đó trong hệ toạ độ gốc o
0
x
0
y
0
z

0
là:
rAr
i
i

00
=
=
( ) ( ) ( )
( ) ( ) ( )












++++
++++
1000
0100
0
0
112122121

112122121
sinasinacossin
cosacosasincos












1
i
i
i
z
y
x
(2.3)
Vì thanh nối là vật thể cứng, nên quan hệ
r
i
trong thanh nối i là hằng số, hay:
0=
dt
rd

i
(2.4)
Lấy đạo hàm phơng trình (2.3) và chú ý đến (2.4) ta có phơng trình nh sau:
r
dt
rd
Ar)A(
dt
d
dt
rd
i
i
i
i
T
i
2
1i
i
i
0
i
00
0
0

A

r











=+==

=


Với khái niệm vết của ma trận (Trace), bình phơng tốc độ của điểm đó đợc tính
nh sau:

=








==

= =


2
1
2
1
i
j
i
0
i
i
i
0
00002

A

A
r r r r
i
T
j
ji
TT
i
rrTraceTracev
(2.5)
Sau khi biến đổi ta đợc:

ji
2
1
2
1
i
0
ii
i
0
2

A

A

= =

=

j k
j
T
T
i
t
rrTracev
(2.6)
2. Động năng của robot đợc tính nh sau
Động năng của một điểm bất kỳ trong thanh nối i
dmrrTracedmvdK
j k
k
T
j

==

= =

kj
2
1
2
1
i
0
T
ii
i
0
2
i2

A

A
2

1

2
1

Do đó động năng của thanh nối i đợc tính bằng tích phân theo toàn bộ thanh nối
i
K
i
=

=

= =
ithanh
j k
k
T
T
j
dmrrTracedK

kj
2
1
2
1
i
0
ithanh
ii
i
0
2

A

A
2
1
Ký hiệu J
i
=
dmrr
T

ithanh
ii

là ma trận quán tính giả

=




i 2
i
i
i

i
i
2
2
i
i
i
i
i
2
2
i
i
i
i2
2
i

thanhthanh
z
thanh
y
ithanh
x
z
thanh
i
z
thanh
i
zy
thanh
i
zx
thanh
i
y
thanh
i
yz
thanh
ii
y
ithanh

i
yx
thanh
i
thanh
xxz
thanh
i
xy
ithanh
ii
x
ithanh
i
i
dmdmrdmrdmr
dmrdmrdmrrdmrr
dmrdmrrdmrdmrr
dmrdmrrdmrrdmr
J
Có thể viết lại ma trận J
i
nh sau:

+
+
++
=
i
i
i
i
i
i
i
i
i
izziyyixx
iyzixz
i
iiyz

izziyyixx
ixy
i
iixzixy
izziyyixx
i
mzmymxm
zm
III
II
ymI
III
I
xmII
III
J
2
2
2
Trong đó
[ ]
T
z
i
y
i
x
ii
,r,r,rr 1=
là bán kính vectơ biểu diễn trọng tâm của khâu i

trong hệ toạ độ i.
Các phần tử của J
i
đợc tính nh sau:
( )

+=
ithanh
z
i
y
i
ixx
dmrrI

22
( )


=
+=
ithanh
x
i
i
i
ithanh
y
i
x

i
ixy
dmrxm
dmrrI


Các thành phần còn lại đợc tính tơng tự
Tổng động năng của robot đợc tính nh sau:

= =

=

=













=
2
1

2
1
2
1
i
0
i
0
1
AA
2
1
i j
k
k
j
k
T
i
j
n
i
i
JTraceKK
(2.7)
Hay

= =

=

=
2
1
2
1
2
1
2
1
i j
k
k
j
jk
hTraceK
trong đó thành phần ma trận h
jk
đợc tính theo:

=

=
n
i
k
T
i
i
j
i
jk
A
J
A
Traceh
1
00
Do
0
0
=

j
i
A
(j > i) nên có thể viết h
jk
nh sau:
( )

=












=
n
k,jmaxi
k
T
i
i
j

i
jk
A
J
A
Traceh
00
Do h
jk
= h
kj
nên ma trận vuông H() là ma trận đối xứng, đồng thời động năng là
một đại lợng dơng với
0

, nên các thành phần của H() là các số dơng.
3. Thế năng của robot đợc tính nh sau
Thế năng của thanh nối thứ i đợc xác định theo biểu thức:
r
i
i
0T
ii
A g m - P =
Với g = [ g
x
g
y
g
z

1]
T
là

véctơ gia tốc trọng trờng
m
i
là khối lợng thanh nối thứ i
r
i
là véctơ vị trí thanh nối i trong hệ toạ độ của thanh nối thứ i
Tổng thế năng của hệ thống là:
P =

=
2
1
0
i
i
i
T
i
rAgm
Ta thấy
rm
i
i
là cột thứ t của ma trận J
i

nên ta có tổng thế năng nh sau:
[ ]

=
=
2
1
0
1000
i
T
ii
T
JAgP
(2.8)
4. Mô hình động lực học robot
Hàm Lagrange của robot là:
L =
( )

=

n
i
ii
PK
1
Thay lần lợt các biểu thức vào và biến đổi ta có:
rAgm
A

J
A
TraceL
i
i
n
i
T
i
i
k
.
i
j
i
k
j
.
k
i
i
j
i

== = =
+

=
1
0
2
1 1 1
00
2
1
(2.9)
Phơng trình Lagrange có dạng nh sau:
Mi
.
i
.
i
F
LL
dt
d
=

F
Mi
- động lực, hình thành trong khớp động thứ i khi thực hiện chuyển động.
Ta có:
( )
.

H
KL
=


=


( ) ( )
.

.

.
HH
L
dt
d
=












( )
( )

=

P
H
L
.
T
.
2
1
Do đó phơng trình động lực học có dạng:
( ) ( ) ( )
( )
M
.
.
T
.
.
F
P
HHH =

+











+
2
1
Ta ký hiệu:
( ) ( )


=

=









K
qHHH,V

.
T

.
.
2
1
gọi là thành phần
mômen nhớt và hớng tâm.
( )
( )


=
P
G

gọi là thành phần mômen trọng lực.
Khi đó phơng trình động lực học nh sau:
( ) ( )
MG,VH

=+








+
Mômen của khớp i đợc xác định nh sau:
i
n
j
k
.
n
k
j
.
ijk

j
n
j

iji
GVqHM ++=

= == 1 11
Trong đó:












=

=
j
T
p
p
j
p
)j,imax(p
ij
A
J

A
Traceh
00
2
(i, j = 1,2)
( )

= = =













=







2

1 1
0
0
2
j
j
k
.
m
.
k
j
k,j,imaxp
p
p
kj
p
.
i
A
J
A
Trace,V
(i, j, k =1,2)
r
A
gmg
n
p
j

p
T
pi
p
1
0


=


=
Hay viết dới dạng ma trận
( )
+






+= G,V)(HM

Trong đó:
( )

=
2221
1211
hh
hh
H



















=
111
11
2

11
1
00
2
1
0000
0000
2
1
00
3
1
mlm
lmlm
J
;

=
222
22
2
21
2
00
2
1
0000
0000
2
1
00
3
1
mlm
lmlm
J

+












=
1
2
0
2
1
2
0
1
1
0
1

1
1
0
11
T
T
A
J
A
Trace
A
J
A
Traceh

2
2
22
2
22
2
11
3
4
3
1
++= coslmlmlm

=












==
1
2
0
2
2
2

0
2
2
0
2
1
2
0
2112
TT
A
J
A
Trace
A
J
A
Tracehh

2
22
2
12
2
1
3
1
lmlm +=

=
2
2
0
2
2
2
0
22
T
A
J
A
Traceh
2
22
3
1
lm=

=







2
1
v
v
,V
.
22
1
2
0
2
22
2
0
2
21
1

2
0
2
12
2
0
2
11
1
1
0
1
11
1
0
2
1

A
J
A
Trace
A
J
A
Trace
A
J
A
TraceV

+












+

=

21
2
122
2
2
2
2221
2
1

lsinmlsinmV =
2
1
2
222
22
2
2
0
2

22
2
0
2
21
2
2
0
2
12
2
0
2
2
2
1

lsinm
A
J
A
Trace
A
J
A
TraceV =

+












=
Ma trận
( )

=
2
1
g
g
V
g
1
=
[ ] [ ]

1
0
0
21
000
1
0
0
21
000m-
1
2
1
1
1
/
T
gm

/
A
g
( )
212211211
2
1
2
1
++






+= cosglmcosglmglm
g
2
=
[ ]

1
0
0
21
000
2
2
/
T
gm
( )
2122
+= cosglm
Vậy phơng trình động lực học của robot là:
( )
+






+= G,V)(HM

+




















+
+++
=

lmlmlm
lmlmcoslmlmlm
M
M
2
1
2
22
2
22
2
12
2
22
2
122
2
22
2
22
2
11
2
1
3
1
2

1
3
1
2
1
3
1
3
4
3
1
( )
( )








+
++






+

+














+
2122
212211211
2
1
2
222
21
2
122
2
2
2
222
2

1
2
1
2
1
2
1
cosglm
cosglmcosglmglm
lsinm
lsinmlsinm
.

Với các mômen khớp cho trớc ta giải hệ phơng trình vi phân bậc hai này thì thu
đợc các biến khớp là góc
1

và
2

, vận tốc góc
.
1

,
.
2

, gia tốc góc

1

,

2

5. Quan hệ giữa tốc độ của khớp và tốc độ của tay robot
Biểu diễn tay robot trên thanh nối 2 nh sau:
[ ]
T
tay
,,,r 1000
2
=

Vậy vị trí của điểm đó trong hệ toạ độ gốc o
0
x
0
y
0
z
0
là:
taytay
rr
2
2
00
A=

=
( ) ( ) ( )
( ) ( ) ( )












++++
++++
1000
0100
0
0
112122121
112122121
sinasinacossin
cosacosasincos

1
0
0
0


( )
( )












++
++
=

1
0
11212
11212
sinasina
cosacosa
Bình phơng tốc độ của tay robot là:









==

T
taytay
T
tay
tay
Tracev r r r r
0
tay
0002
=

= =

2
1
2
1
2
j
2
0
2
i
2
0

i
T
j
tay
j
tay
i
r
A
r
A
Trace
( )( ) ( )( )
2
21211
2

21211
+++++= cosacosasinasina
Khai triển vận tốc của tay robot theo hai thành phần trục x và trục y và ta có thể
viết rằng:
222
vuv
tay
+=

Trong đó:
v = a
1
1
sin

+ a
2
)sin(
21

+
u = a
1
1
cos

+ a
2
)cos(
21

+
Đạo hàm theo thời gian ta đợc:
v

=a
1
1
cos

1


+ a
2
)cos(
21

+
(
1


+
2


)
u

= -a
1
1
sin

1


- a
2
)sin(
21

+
(
1


+
2


)
Nh vậy
u

và
v

chính là các véc tơ vận tốc của tay rô bốt chiếu theo các

phơng x và y.
y
v
=
v

= [a
1
1
cos

+ a
2
)cos(
21

+
]
1


+a
2
)cos(
21

+
2

x
v
=
u

= -[a
1
1
sin

+ a
2
)sin(
21

+
]
1


- a
2
)sin(
21

+
2


Từ hệ phơng trình trên ta có quan hệ giữa tốc độ các khớp và tốc độ của tay rô bốt

nh sau:
1


=
21
21

+
sina
)sin(
y
v
+
21
21

+
sina
)cos(
x
v
2


=
221
21211

++

sinaa
)sin(asina
y
v

221
21211

++

sinaa
)cos(acosa
x
v
III. Viết hàm MATLAB thực hiện các phơng trình ở phần 1 và 2, vẽ đờng biểu diễn vị
trí và tốc độ của khớp khi tay robot di chuyển từ vị trí [0.4; 0.0 m] đến [0; 0.4m] theo
một quỹ đạo đờng thẳng. Đồ thị tốc độ đặt trớc của tay robot dọc theo quỹ đạo cho trớc
Khi robốt di chuyển từ vị trí (0.4; 0.0m) đến (0.0; 0.4m) theo một đờng thẳng
khi đó: vận tốc của tay robot, quỹ đạo chuyển động, p
x
, p
y
, p
z
là các đại lợng đã biết;
thời gian chuyển động t = 1s. Cần tính tốc độ của các khớp
1
và
2

theo t.
1. Chơng trình tính toán động học ngợc
-Hàm tính góc
1

:
function y = teta_1(px,py)
l1 = 0.4; % chieu dai cua thanh noi 1 (m)
l2 = 0.3; % chieu dai cua thanh noi 2 (m)
% goc cua khop mot (rad)
y = asin((l1^2+px^2+py^2-l2^2)/(2*l1))- asin(px/sqrt(px^2 + py^2));
-Hàm tính góc
2

:
function y=teta_2(px,py)
l1=.4; % chieu dai cua thanh noi 1 (m)
l2=.3; % chieu dai cua thanh noi 2 (m)
% goc cua khop hai (rad)
y = acos((px^2+py^2-l1^2-l2^2)/(2*l1*l2));
2. Chơng trình tính toán vận tốc độ của các khớp theo tốc
độ tay rô bốt
-Hàm tính tốc độ góc của thanh 1:
function y = dhone(vy,vx,t1,t2)
l1=.4;
l2=.3;
% vy van toc cua tay robot theo phuong y
% vx van toc cua tay robot theo phuong x
% t1 goc quay cua khop 1
% t2 goc quay cua khop 2

y = sin(t1+t2)*vy/(l1*sin(t2))+cos(t1+t2)*vx/(l1*sin(t2));
-Hàm tính tốc độ góc của thanh 2:
function y = dhtwo(vy,vx,t1,t2)
l1=.4;
l2=.3;
y = -(l1*sin(t1)+l2*sin(t1+t2))*vy/(l1*l2*sin(t2)) -
(l1*cos(t1)+l2*cos(t1+t2))*vx/(l1*l2*sin(t2));
3 Xây dựng mối quan hệ giữa v
x
,v
y
,p
x
và p
y
theo thời gian t

Gọi v
1
là vận tốc của tay rô bốt trên đoạn [0 0.25], v
2
là vận tốc của tay rô
bốt trên đoạn [0.25 0.75], v
3
là vận tốc của tay rô bốt trên đoạn [0.75 1].
Ta có:
v
1
=4v
o

t.
v
2
=v
o
.
v
3
=-4v
o
(t-1) với v
0
là vận tốc ban đầu.
Gọi s
1
là quãng đờng mà tay rô bốt dich chuyển đợc trong khoảng thời gian [0
0.25], s
2
là quãng đờng mà tay rô bốt dich chuyển đợc trong khoảng thời gian
[0.25 0.75], i s
3
là quãng đờng mà tay rô bốt dich chuyển đợc trong khoảng thời
gian [.75 1]
Ta có:
s
1
= v
o
/8.
s

2
= v
o
/2.
s
3
= v
o
/8.
Vậy s
1
+ s
2
+ s
3
=3v
o
/4
Khi tay rô bốt di chuyển từ điểm (0.4 0) đến (0 0.4) quãng đợc đi đợc là: 0.4
2
. Suy ra: v
o
=1.6
2
/3. Thay vào các công thức trên ta đợc:
v
1
=6.4
2
t/3.

v
2
=1.6
2
/3
v
3
=-6.4
2
(t-1)/3 .
Chiếu véc tơ vận tốc lên hai trục x và y ta đợc:
v
1x
=-6.4t/3.
v
1y
=6.4t/3.
v
2x
=-1.6/3.
v
2y
=1.6/3.
v
3x
=6.4(t-1)/3 .
P
x
P
y

0
0.4m
0.4m
V
tay
(m/s)
t(s)
1
0 0.25
0.75
v
3y
=-6.4(t-1)/3 .
Vị trí của tay rô bốt khi di chuyển tơng ứng là:
p
1x
=0.4-3.2t
2
/3.
p
1y
=3.2t
2
/3.
p
2x
=(1.4-1.6t)/3.
p
2y
=(1.6t-0.2)/3.

p
3x
=6.4(t
2
/2-t+1/2)/3.
p
3y
=-6.4(t
2
/2-t+5/16)/3.
Trên cơ sở các phơng trình này,chơng trình tính và vẽ đồ thị vị trí, tốc độ các
khớp đợc viết nh sau:
% Vi tri va toc do cac khop trong khoang thoi gian tu 0 den 0.25
t=0:.01:.24;
vx=-6.4*t/3; % toc do cua tay ro bot theo truc x
vy=6.4*t/3; % toc do cua tay ro bot theo truc y
px=0.4-3.2*t.^2/3; % vi tri cua tay ro bot theo truc x
py=3.2*t.^2/3; % vi tri cua tay ro bot theo truc y
t1=tone(px,py); % vi tri cua khop 1
t2=ttwo(px,py); % vi tri cua khop 2
dh1=dhone(vy,vx,t1,t2); % toc do goc cua khop 1
dh2=dhtwo(vy,vx,t1,t2); % toc do goc cua khop 2
tm=t;
o1=t1;
o2=t2;
d1=dh1;
d2=dh2;
% Vi tri va toc do cac khop trong khoang thoi gian tu 0.25 den 0.75
t=0.25:.01:.74;
vx=-1.6/3;

vy=1.6/3;
px=1.4/3-1.6*t/3;
py=1.6*t/3-0.2/3;
t1=tone(px,py);
t2=ttwo(px,py);
dh1=dhone(vy,vx,t1,t2);
dh2=dhtwo(vy,vx,t1,t2);
th=t;
o3=t1;
o4=t2;
d3=dh1;
d4=dh2;
% Vi tri va toc do cac khop trong khoang thoi gian tu 0.75 den 1
t=.75:.01:1;
vx=6.4*(t-1)/3;
vy=-6.4*(t-1)/3;
px=6.4*(t.^2/2- t + 1/2)/3;
py=-6.4*(t.^2/2- t + 5/16)/3;
t1=tone(px,py);
t2=ttwo(px,py);
dh1=dhone(vy,vx,t1,t2);
dh2=dhtwo(vy,vx,t1,t2);
o5=t1;
o6=t2;
d5=dh1;
d6=dh2;
tb=t;
% Vi tri va toc do cac khop trong khoang thoi gian tu 0 den 1
t=[tm th tb];
om=[o1 o3 o5];

oh=[o2 o4 o6];
dm=[d1 d3 d5];
dh=[d2 d4 d6];
plot(t,om,'b') % Vi tri cua khop 1.
pause
plot(t,dm,'c') % Toc do cua khop 1.
pause
plot(t,oh,'r') % Vi tri cua khop 2
pause
plot(t,dh,'k') % Toc do cua khop 2
- Ch¹y ch¬ng tr×nh ta cã kÕt qu¶ nh sau:
§å thÞ vÞ trÝ gãc quay cña khíp 1:
§å thÞ vËn tèc gãc quay cña khíp 1
§å thÞ vÞ trÝ gãc quay cña khíp 2:
§å thÞ vËn tèc gãc quay cña khíp 2:
IV. ThiÕt kÕ bé ®iÒu khiÓn ®éng häc ngîc
Tõ ph¬ng tr×nh ®éng lùc häc cña robot:
( )
θ+






θθ+θθ= G,V)(HM

+









θ
θ










+
+θ++
=








lmlmlm

lmlmcoslmlmlm
M
M
2
1
2
22
2
22
2
12
2
22
2
122
2
22
2
22
2
11
2
1
3
1
2
1
3
1
2

1
3
1
3
4
3
1
( )
( )








θ+θ
θ+θ+θ






+
+













θθ
θθθ−θθ−
+
2122
212211211
2
1
2
222
21
2
122
2
2
2
222
2
1
2
1
2

1
2
1
cosglm
cosglmcosglmglm
lsinm
lsinmlsinm
.

Sai lệch vị trí:








==
22
11
d
d
dt
e

Sai lệch tốc độ:

==
22
11
.
d
.
.
d
.
.
dt

e
Sai lệch gia tốc:

==
22
11

d


d


dt

e
Vậy:

d

e=
( )
MGVHe
d

++=
1
đặt
NGV =+
Đặt
( )
MNHU

d

+=
1
Vậy
Ue

=
Từ phơng trình trên ta có đợc mômen của khớp cần điều khiển:
NUHM
d

dk
+








=
1
Sơ đồ cấu trúc của hệ thống điều khiển nh sau:
Tiếp theo ta đi đến thiết kế bộ điều khiển
U
Bộ điều khiển
U
có thể điều khiển PD, PID, bộ điều khiển phản hồi trạng thái.

Thiết kế bộ điều khiển PD.
Phơng trình mạch vòng ngoài nh sau:

pd
eeKeKU ==
Với






=
2
1
e
e
e
,








=
2
1

.
.
.
e
e
e
, do vậy:
.
),(N




d


H Robot
Bộ ĐK phụ
Bộ điều khiển vòng trong
(khử phi tuyến)
Bộ điều khiển vòng ngoài (đảm
bảo độ ổn định, chất lợng quá
trình quá độ, chế độ tĩnh)






=

2
1
0
0
p
p
p
K
K
K
;






=
2
1
0
0
d
d
d
K
K
K
Phơng trình cho 2 khớp nh sau:
0

0
2
2222
1
111
1
=++
=++
.
pd

.
pd

eKeKe
eKeKe
Tính toán các hệ số K
d1
, K
d2
, K
p1
, K
p2
bằng phơng pháp dựa vào các khâu động
học điển hình:
+ Thiết kế bộ điều khiển cho khớp 1:
Theo phơng pháp đặt nghiệm ta mong muốn:
( )( )
033

11
2
==++ ssKsKs
pd
K
p1
= 9 và K
d1
= -6
Ta có mô men điều khiển đợc tính nh sau:
M
đk
= H(
d


-
u
) + N
với
u
= 6*
1

-9*
1


+ Thiết kế bộ điều khiển cho khớp 2:
Tơng tự ta có: K

d2
= -6, K
p2
= 9.
V. Mô phỏng hệ thống với bộ điều khiển ở câu IV của khớp 1 thay đổi nhảy cấp từ 0
đến 1 rad/s
1. Thiết kế bộ điều khiển tốc độ cho động cơ khớp 1
Ta có:
-Vì ta cần mô phỏng hệ thống với góc quay
1

thay đổi nhảy cấp, cho nên ta có
thể cho góc
2

bằng hằng số.
Chọn nh sau:
2

=60
o
, suy ra
2


và
2


đều bằng không.

Thay các giá trị vào ta đợc:
H
11
= 3.0905 Kgm
2
H
22
= 0.3025 Kgm
2
- Hệ số cản của động cơ và phụ tải coi bằng không.
- Mô men quan tính tổng hiệu dụng của hai động cơ là:
+ Động cơ khớp 1:
J
qd1
=J
đ
+ i
2
H
11
= .0004 + 3.0905/12
2
=0.0219 Kgm
2
+ Động cơ khớp 2:
J
qd2
=J
đ
+ i

2
H
22
= .0004 + 0.3025/12
2
=0.0025 Kgm
2
Trong đó i là tỉ số truyền: i = tốc độ của động cơ / tốc độ của khớp = 12.
- Ta có sơ đồ hệ thống điều khiển tốc độ quay nh sau:
- Với các thông số:
K
b
= K
a
= 0.5 .
R
a
= 3 .
- Sử dụng bộ điều khiển PI: Các thông số đợc xác định theo phơng pháp gán cực.
- Hàm truyền đạt của bộ điều khiển có dạng:
W
c
= K
P
+ K
I
s
- Hàm truyền đạt của đối tợng là:
W
ĐT

= K/s(s+K*K
b
)
Trong đó: K=K
M
/(r*J
qd1
) =0.5/(3*0.0219) = 7.6104
Hàm truyền đạt của hệ thống sẽ là:
W
k
= K*(K
I
+ sK
P
)/(s
3
+ s
2
*K
0
+ sK
P
+ K
I
)
Với K
0
= K*K
b

= 3.8052.
Chọn các điểm cực là:
-s
1,2
= -1; s
3
=-1.8052 , ta có các tham số của bộ điều khiển là:
K
I
= K
0
2 = 1.8052
K
P
= 2K
0
3 = 4.6104
- Sau khi chạy chơng trình ta đợc đồ thị hàm quá độ của hệ thống nh sau:


2 Mô phỏng hệ thống
-Sơ đồ điều khiển nh sau:
- Cho tín hiệu đặt góc quay của khớp 1 là tín hiệu step.
- Cho tín hiệu đặt tốc độ góc quay của khớp 1 là bằng không.
- g
1
đợc xác định nh sau:
g
1
=

111
cos

g
glm
+
gm
2
[
)cos(cos
21211

++
g
ll
]
=10*9.7*0.2*cos(
1

)+5*9.7*(.4*cos(
1

)+.15*cos(
1

+pi/3))
- K
P
và K
D

đợc xác định nh ở trên.
- Sau khi chạy chơng trình ta đợc đồ thị góc quay của khớp nh sau: