Điều khiển Robot

Đề bài
Cho một cơ cấu Robot 2 thanh nối đợc truyền động bởi động cơ một chiều.
Động cơ một chiều đợc cấp điện từ một bộ khuếch đại điện áp.
1. 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ệ toạ độ các khớp (Joints).
2. Xây dựng quan hệ giữa tốc độ của các khớp và tốc độ của tay Robot.
3. Viết hàm MATLAB thực hiện các phơng trình khi tay Robot di chuyển
từ vị trí [0,4: 0,0 m] đến [0,0: 0,4 m] theo một đờng thẳng. Đồ thị tốc độ
đặt trớc của tay Robot dọc theo quỹ đạo cho ở hình 2.
4. Thiết kế bộ điều khiển bù trọng lực cho Robot.
5. Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1
thay đổi nhảy cấp từ 0 đến 1 rad.

Hình 1. Cơ cấu động học robot hai thanh nối.
1
2
l
2g
l
1
l
1g
l
2

1

2
x
2
y

y
x
z
1
m
2
m
1
J
2
J
Hình 2. Đồ thị tốc độ.
Bảng thông số của Robot:
STT Đại lợng Giá trị
1 Chiều dài thanh nối1 (l
1
) 0,4m
2 Chiều dài thanh nối1 (l
2
) 0,3m
3 Khối lợng thanh nối 1 (m
1
) 10Kg
4 Khối lợng thanh nối 2 (m
2
) 5,0Kg
5 Momen quán tính khớp 1 quay quanh tâm khối (J
1
) 0,528 Kgm
2

6 Momen quán tính khớp 2 quay quanh tâm khối (J
2
) 0,19 Kgm
2
7 Khoảng cách từ khớp 1 đến tâm khối 1 (l
g1
) 0,25 m
8 Khoảng cách từ khớp 2 đến tâm khối 2 (l
g2
) 0,15 m
9 Hằng số momen của động cơ khớp 1, 2 (K
M
) 0,5 Nm/A
10 Điện trở phần ứng (r
1
,r
2
)
3
11 Momen lớn nhất của động cơ khớp 1 (M
1max
) 1,2 Nm
12 Momen lớn nhất của động cơ khớp 2 (M
2max
) 0,7 Nm
13
Tốc độ lớn nhất của động cơ khớp 1, 2 (
max
)
90 rad/s

14 Momen quán tính của động cơ (J
Đ
) 0,0004 Kgm
2
15 Khối lợng tải lớn nhất (m
t
) 5 Kg
16 Tỉ số truyền cho cả hai khớp (i) 12
Bài giải
2
0 0.25 0.75 1.0 t(s)
V (m/s)
Câu 1. 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ệ toạ
độ các khớp (Joints)


a. Xây dựng phơng trình động học thuận
Bài toán động học thuận là bài toán đi tìm các thông số về vị trí và hớng của
tay robot so với khung cơ sở (gốc).
Thực hiện phép biến đổi đồng nhất dựa trên hệ tọa độ thanh nối (Denavit -
Hartenberg) mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các
khớp (Joints) đợc xác định nh sau:
2
1
1
0
2
0
n

0
AATT ==
(1)
Với:
n
0
T
: biểu diễn hệ toạ độ thanh thứ n so với hệ toạ độ gốc.
E
n
T
: biểu diễn hệ toạ độ tay (điểm kẹp) so với thanh n.












=
1000
paon
paon
paon
T

zzzz
yyyy
xxxx
E
0
3
2
l
2g
l
1
l
1g
l
2

1

2
x
2
y
y
x
z
1
m
2
m
1

J
2
J
i
i
A
1
: đợc định nghĩa là ma trận chuyển đổi đồng nhất, biểu diễn mối
quan hệ vị trí của một điểm trong khung i và vị trí của điểm đó trong
khung thứ i-1.
Từ mô hình cơ cấu động học của robot ta xây dựng đợc bảng Denavit -
Hartenberg nh sau:
Thanh a
i

i

i
d
i
1
2
l
1
l
2
/2
/2

1

2
0
0
Theo đó ta xác định đợc các ma trận biến đổi A của robot nh sau:













=
1000
dcs0
sascccs
casscsc
A
iii
iiiiiii
iiiiiii
i
-1i

=
1000
0100
sl0cs
cl0sc
A
1111
1111
1
0

=
1000
0100
sl0cs
cl0sc
A
2222
2222
2
1
( )

sins;cosc ==
Thay vào (1) ta đợc:
4
( ) ( ) ( )
( ) ( ) ( )

=












++++
++++
=
1000
paon
paon
paon

1000
0100

slsl0cs
clcl0sc
T
zzzz
yyyy
x
ã
xxx
212112121
212112121
E
0
(2)
Với :
( ) ( )
( ) ( )
2121
2121
sins
cosc
+=+
+=+
P là điểm thuộc hệ toạ độ gắn với tay robot, có vị trí đợc xác định bằng vectơ
cột thứ t của
E
0
T
.
Từ (2) ta đợc phơng trình động học thuận biểu diễn mối quan hệ giữa hệ tọa
độ tay Robot và hệ tọa độ các khớp:

=
++=
++=
0z
)sin(lsinly
)cos(lcoslx
T
21211T
21211T
(3)
b. Xây dựng phơng trình động học ngợc
Bài toán động học ngợc là tìm vị trí các khớp khi biết vị trí của tay robot
(End-effector).
Ma trận biểu diễn vị trí tay robot:

=
1000
paon
paon
paon
T
zzzz
yyyy
x
ã
xxx
E
0
Theo (1) ta có:
2
1
1
0
E
0
AAT =
Nhân hai vế của (1) với
1
1

A
ta có:
5
( )

2
1
E
0
1
1
0
ATA =ì

(4)
tơng đơng với:













=

1000
0100
sl0cs
cl0sc
1000
0fff
0fff
0fff
2222
2222
333231
232221
131211


(5)
trong đó:
( )

ì












=

=ì

1000
paon
paon
paon
1000
0100
00cs-
l-0sc
1000
ffff
ffff
ffff
TA
zzzz
yyyy
x
ã
xxx
11
111
34333231
24232221
14131211
E
0

1
1
0












++++
++++
=
1000
paon
cpspcasacosocnsn
lspcpsacasocosncn
zzzz
1y1x1y1x1y1x1y1x
1y1xy1xy1x1y1x


Cân bằng vế trái và phải của (5), ta có:

=
=
2224
2214
slf
clf




=+
+=+

221y1x
2211y1x
slcpsp
cllspcp
(6)
Với





=
=
=+
y
x

2
2
2
2
py
px
1sc

Bình phơng hai vế của hai phơng trình trong (6) và cộng vế ta có:
6
21
2
2
2
1
22
2
l2l
)l(l)y(x
c
++
=
[ ]
)l(l)y(x
)l(l)y(x2)lly(x
tg
2
2
2
1

22
4
2
4
1
22222
2
2
1
22
2
++
++++++
=
(7)
Đặt :
[ ]





++=
++++++=
)l(l)y(xk
)l(l)y(x2)lly(xk
2
2
2
1

22
2
4
2
4
1
22222
2
2
1
22
1

ta có:
)k,atan2(k
212
=
(8)
Thay (7) vào (6) ta đợc:
1
2
2
2
1
22
11
2l
llxy
ysxc
++

=+
Giải ra ta có:

)k,atan2(kx)atan2(y,
311
=
(9)
trong đó:
)l(l)y(xk
2
2
2
1
22
3
++=
.
Vậy phơng trình động học ngợc của hệ đã cho đợc biểu diễn dới dạng



=
=
),(2tana
),(2tana)x,y(2tana
212
311


(10)

Với :
[ ]







++=
++=
++++++=
)l(l)y(xk
)l(l)y(xk
)l(l)y(x2)lly(xk
2
2
2
1
22
3
2
2
2
1
22
2
4
2
4

1
22222
2
2
1
22
1
7
Câu 2: Xây dựng quan hệ giữa tốc độ các khớp và tốc độ của tay
Robot
Đạo hàm hai vế phơng trình động học thuận theo thời gian ta có:





+++=
++=
)cos()l(cosly
)sin()l(sinlx
21221111
21221111




hay

+++
++
=






2
1
21221211
21221211


)cos(l)cos(lcosl
)sin(l)sin(lsinl
y
x

(11)
Đặt:






+++
++
=
)cos(l)cos(lcosl
)sin(l)sin(lsinl
J
21221211
21221211

(J: ma trận Jacobien)
Phơng trình biểu diễn quan hệ giữa tốc độ các khớp và tốc độ của tay Robot
đợc biểu diễn dới dạng:






=

y
x
J


1
2
1




(12)
Với:
(J)det
)(sinlsinl)(coslcosl
)(sinl)(cosl
J
2121121211
212212
1

++
++
=

( ) ( ) ( ) ( ) ( )
1121211212
coslsinlsinlcoslJdet

+++=
8