Svth: L Vn Cng- T ng ha 04 K58

LI NI U

Ngy nay robot c p dng rng ri trong hu ht cc lnh vc nh sn xut,

cng nghip, cng ngh cao nh v tr, hng khng, giao thng vn ti

Cc nc pht trin xut hin xu hng to ra nhng dy chuyn v thit b t ng c

tnh linh hot cao v xu hng ny t c kh nhiu kt qu kh quan. Chnh v th
vic nghin, ng dng v pht trin cc h thng sn xut t ng linh hot ngy cng tr
nn cp thit, nht l cc loi robot. Robot cng nghip c cu trc ng hc ni tip tuy
c nghin cu v pht trin mnh song nhng tnh ton v ng dng ca n lun
lun l mt lnh vc rt phong ph, a dng v y cng l loi robot thng dng nht
hin nay. Trong qu trnh hc em nhn thy hot ng rt linh hot, c ng dng
nhiu lnh vc sn xut v trong cng nghip. Trong bi tp ln ny s tnh ton v phn
tch v cc phn ng hc, ng lc hc.Cng nh phn m phng lp trnh s dng
cc phn mm h tr.

Bi tp ln iu khin RoBot cng nghipPage 1

 Svth: L Vn Cng- T ng ha 04 K58

Cu 1:

a) Xy dng h ta cho cc thanh ni

Hnh 1: H ta cho robot

H ta cc thanh ni c xy dng theo phng php biu din Danevit-
Hartenberg(D-H), khung ta thanh ni i c xy dng theo nguyn tc sau:
+ Gc khung ta thanh ni i t trng vi chn php tuyn chung ca trc i v
i+1 v nm trn trc khp i+1
+ Trc zi t theo phng ca trc khp i+1
+ Trc xi t theo phng hng php tuyn chung ca trc i v i+1 theo hng t
trc i n i +1
b) Xc nh ma trn T biu din h ta tay Robot
Lpbng D_H
p dng cc nguyn tc bin i ta , lp c bng D-H nh sau:

k ai i i di

1 0 90 1 0
2 a2 0 2 0
3 a3 0 3 0
Bng 1: Bng D-H

Trong :
ai
+ : di php tuyn chung(i,i+1)

Bi tp ln iu khin RoBot cng nghipPage 2

 Svth: L Vn Cng- T ng ha 04 K58

i
+ : L gc gia(i,i+1)

i
+ :l gc gia hai php tuyn chung(ai,ai-1)

di
+ : L khong cch chn 2 php tuyn(ai,ai-1)

0 1 2
Xc nh ma trn A1 , A2 , A3 dng cng thc:

[ ]
c i s i c i s i s i ai c i
i1 s i c i c i c i s i ai s i
Ai =
0 s i c i di
0 0 0 1

T bng D-H xc nh c cc ma trn thnh phn tng ng:

0
- Vi A 1 : 1=90 => c
1
=0, s
1=1

[ ]
c 1 0 s 1 0
0 s 1 0 c 1 0
A1 =
0 1 0 0
0 0 0 1
1
- Vi A2 : 2=0 => c
2
=1, s
2=0

[ ]
c 2 s 2 0 a 2 c 2
1 s 2 c 2 0 a2 s 2
A2 =
0 0 1 0
0 0 0 1
2
- Vi A3 : 3=0 => c
3
=1, s
3=0

[ ]
c 3 s 3 0 a3 c 3
2 s 3 c 3 0 a3 s 3
A3 =
0 0 1 0
0 0 0 1

Khi y nhn cc ma trn trn li vi nhau ta c:

Bi tp ln iu khin RoBot cng nghipPage 3

 Svth: L Vn Cng- T ng ha 04 K58

0 1 2
T3 A 1 ( 1 ) . A 2 ( 2 ) . A 3 ( 3 .
=

[ ][ ]
c 1 0 s 1 0 c 2 s 2 0 a 2 c 2
0 1 s 1 0 c 1 0 s 2 c 2 0 a2 s 2
Vi A1 . A2 = .
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
c 1 c 2 c 1 s 2 s 1 a2 c 1 c 2
s 1 s 2 s 1 c 2 c 1 a 2 s 1 c 2
= s 2 c 2 0 a2 s 2
0 0 0 1

0 1 2
A1 . A2 . A3 =

c1c 2 c1s 2 s1 a2c1c 2 c3 s3 0 a3c 3

s s s1s 2 c1 a2 s1c 2 s3 c 3 0 a3 s 3
1 2 .
s 2 c 2 0 a2 s 2 0 0 1 0

0 0 0 1 0 0 0 1

c1 (c2 c3 s 2 s3 ) c1 (c 2 s3 s 2 c3 ) s1 a3c1 (c 2 c3 s 2 s3 ) a2c1c 2

s (c c s s ) s (c s s c ) c a3 s1 (c 2c 3 s 2 s 3 ) a2 s1c 2
T3 1 2 3 2 3 1 2 3 2 3 1

s 2 c3 c 2 s 3 s 2 s 3 c 2 c 3 0 a3 s 2c 3 a3c 2 s 3 a2 s 2

0 0 0 1

Vy ma trn T biu din h ta tay RoBot c dng :

c1 (c 2 c3 s2 s3 ) c1 (c 2 s3 s 2 c3 ) s1 a3c1 (c 2 c 3 s 2 s3 ) a2c1c 2
s (c c s s ) s (c s s c ) c a3 s1 (c 2c 3 s 2 s 3 ) a2 s1c 2
T3 1 2 3 2 3 1 2 3 2 3 1

s 2 c3 c 2 s3 s 2 s 3 c 2 c 3 0 a3 s 2c 3 a3c 2 s 3 a2 s 2

0 0 0 1

T3
c. ngha ca ma trn .

Bi tp ln iu khin RoBot cng nghipPage 4

 Svth: L Vn Cng- T ng ha 04 K58

T3
- l ma trn biu din quan h gia tay Robot so vi thn Robot. Hay T 3 l
ma trn biu din hng v v tr ca tay Robot so vi khung ta Robot thng
qua cc bin khp .
- Do khi bit cc bin khp, ma trn T cho php xc nh v tr v nh hng tay
Robot.
1
d. Xc nh v tr ca tay Robot trong h ta gc khi =30 ; 2 =30 ;
3
=30 .

t = 1 = 2 = 3 =30 .

1 3
S = 2; c = 2 .

[ ]
3
3 ) 3 . 1 3 1 3 1 1 1 3 3 3 1 3
(
2 2 4
. .
4 2 4 2 2
.a
2 3 4 2 (
2 4
+ a2
4 )
1 3 1 1 3 1 3 3 3 1 3 1 1 3
T3
=
.
2 4 8
. .
4 2 4 2 2 2 (
. a3 . +a2 .
2 4 8 2 2 )
1 3 3 1
. + .
2 2 2 2
1 3
+
4 4
0 a3 ( 12 23 23 12 )+a 12 2

0 0 0 1

[ ][ ]
3 3 1
a3
3 +a 3 3 3 1 3+6
2
4 4 2 4 4 4 4 2 40
1 3 3 1 3 1 3 3 2 3+1
a3 +a2
4 4 2 4 4 = 4 4 2 40
3 1
0 a2
1 3 1
0
1
2 2 2 2 2 10
0 0 0 1 0 0 0 1

Cu 2.

Bi tp ln iu khin RoBot cng nghipPage 5

 Svth: L Vn Cng- T ng ha 04 K58

Hnh 2: Robot r

a. Xc nh gc ca robot cui hnh trnh chuyn ng.

r r
- V khp tnh tin chuyn ng vi tc r =0,1(m/s) t 1 n max =1,2m
Thi gian chuyn ng:
r maxr 1 1,20,6
t= r = 0,1 =6(s)

Gc ca Robot cui hnh trnh chuyn ng l:

= .t= .6=
36 6 (rad)

b. Xc nh m men ca khp quay v lc tng hp khp tnh tin cui hnh

trnh chuyn ng ca Robot.
- Xy dng phng trnh ng lc hc ca Robot r .

+ Theo phng php Lagrange ta c :

L
d L

qi
Mi(Fi)= - qi
dt

Bi tp ln iu khin RoBot cng nghipPage 6

 Svth: L Vn Cng- T ng ha 04 K58

Vi Mi(Fi): Momen(lc) ca khp i( quay hoc tnh tin)

L=K-P: lagrangecah Robot

Vi K l tng ng nng v P l tng th nng.

- Xt Robot -r.

r1=const =>rmax=2r1vrmin=r1

Ta c: L=K-P vi K=K1+K2;P=P1+P2
1 1
12
2
+ K1= 2 m1 V 1 + 2 J 1

1 2
B qua momen qun tnh ta c K1= 2 m1 V 1

x 1=r 1 sin .
V 12 = x 1 + y 1 vi {
x 1=r 1 cos
y 1=r 1 sin => { y1 =r 1 cos .

1
V 12 = r 02 2 2
=> K1= 2 m1 r 1 .

12
+ Tng t: K2= 2 .m2. V 2

V 22 = x 12 + y 12 vi { x 2=rcos
y 2=rsin

x 2= r cosrsin.
{ y 2=r sin +rcos .

1
V 22 = r 2 + r 2 . 2 => K2= r 2 + r 2 . 2 )
2 .m2(

2 21 1 2
Vy K=K1+K2= 2 m1 r 1 . 2 2
+ 2 .m2( r + r . ).

Bi tp ln iu khin RoBot cng nghipPage 7

 Svth: L Vn Cng- T ng ha 04 K58

P=P1+P2=m1gr1sin + m2grsin

1 2 2 1
L=K-P= 2 m1 r 1 . r 2 + r 2 . 2 )-m1gr1sin - m2grsin
+ 2 .m2(

- Tm M: Khp quay (qi= )

L
=-m1gr1cos - m2grcos
L 2 2
=m1 r 1 + m2 r

L
d 2 2

=m1 r 1 +m2 r +2m2r. r
dt
2 2
M =m1 r 1 +m2 r +2m2r. r +m1gr1cos +m2grcos
2 2
= (m1 r 1 +m2 r ) + 2m2r. r +(m1r1+m2r)gcos
- Tm Fi khp tnh tin: (qi=r)
L 2
r =m2 r . -m2gsin

L
r =m2. r

L
d
r =m2 r
dt

F =m r -m r . 2 +m gsin
2 2 2

H phng trnh ng lc hc ca robot -r

{M =( m1 r 12 +m2 r 2) +2 m2 r . r +(m
1 r 1+m 2 r ) gcos
2
F=m2 . r . + m2 . gsin

V cc khp u chuyn ng vi vn tc khng i nn cc gi tr o hm bc 2 u

bng 0 nn:

Bi tp ln iu khin RoBot cng nghipPage 8

 Svth: L Vn Cng- T ng ha 04 K58

{M =2m2 r . r +(m 1 r 1 +m 2 r ) gcos

F=m 2 r m2 . r . 2+ m2 . gsin

Theo bi ta c:

m1=m2=2kg;r1=0,6m; r =0,1(m/s);rmax=1,2(m); = 36 (rad/s)

Ti v tr cui hnh trnh chuyn ng ta c = 6 ; r=1,2 (m)

2 2
M= (m1 r 1 + m2 r ) + 2m2r. r +( m1r1+m2r)gcos

=2.2.1,2.0,1. 36 +(2.0,6+2.1,2).9,8=30,595(N.m)

2
F = m r -m r . +m2gsin = -2.1,2.( 36 +2.9,8.sin 6 =9,78(N)
2 2
2

c. Thit k b iu khin b trng lng

Mc ch: Tm b iu khin sao cho kh nh hng mi n chnh xc tnh.

C s: Da trn l thuyt n nh liapunov => B iu khin

Do chu nh hng ca khi lng m n chnh xc tnh:

M =H(q). q +C. q +G( q )

Cn iu khin sao cho khi t n v tr cn t th khng ph thuc vo khi

lng G na
Xy dng lut iu khin:

Chn V( q , q ) thc dng

1 T T 1
V= 2 . q .H( q ). q + 2 . . K p . q (Kp>0)

Bi tp ln iu khin RoBot cng nghipPage 9

 Svth: L Vn Cng- T ng ha 04 K58

1 1 T 1 T
V = q
T
q
q
q
H q
q
+ 2 . q . H
2 . .H( ). + 2 . . ( ). (

1 1
q ). q + T . K p . q T
2 . + 2 . . K p . q

Do tnh i xng ca Kd v H:
1
V = . qT
.

H ( q ). q + qT . H ( q ). q + T . K p . q
2

T T
M do =- q (do q *=hng)

1 T
V = . q .

H ( q ). q + qT . H ( q ). q - q T . K p . q
2

Thay H. q = M C . q G

1 T T T
V =
H ( q ). q

+ q .( M C . q G - q . K p . q +
2 . q .

q T . K D . q - q T . K D . q

Vi KD>0 v thuc tnh H ( q )-2C=0

V = qT ( M T
-G-Kp. q +Kd. q )- q . K D . q

Chn Kd l ma trn i xng dng

T
- q . K D . q <0 vi mi q 0

V <0 => M -G-Kp. q +Kd. q =0

M k=Kp. q - Kd. q +G

q
Kim chng =0 khng ph thuc vo G

Bi tp ln iu khin RoBot cng nghipPage 10

 Svth: L Vn Cng- T ng ha 04 K58

Cn bng M = M k

^
H(q). q +C. q +G= Kp. q - Kd. q + G

^
G : l ma trn tnh ton t tham s

G : l ma trn thc ca Robot

Khi xc lp th q =0; q =0 => Kp. q =0 => q =0

Khng ph thuc vo G
Vy ta c phng trnh ng lc hc tng qut nh sau:

M =H( q ). q +V( q , q )+G( q )
S cu trc ca h thng iu khin Robot vi b iu khin PD dng nh
sau:

Hnh 3: S cu trc ca h thng iu khin Robot vi b iu khin PD

Phng trnh lut iu khin c dng
M dk = K pK D p +G(q )

Trong: =q q : Sai s v tr ca khp

q :Tc ca khp robot
q :Bin khp

K P =diag ( K p 1 , K p 2 )
:Ma trn ng cho h s khuch i

Bi tp ln iu khin RoBot cng nghipPage 11

 Svth: L Vn Cng- T ng ha 04 K58

K D =diag (K d 1 , K d 2) : Ma trn ng cho h s o hm

M dk
: Momen iu khin, G(q): Thnh phn momen trng lc

600 0
Ta chn K P = 0 600 [ ] v KD= [ 40 0
0 40 ]
S dng phng trnh ng lc hc Robot -r 2 khp dng tng qut, nhn c
cc ma trn ca h phng trnh ng lc Robot nh sau:

H=
[
H 11 H 12
H 21 H 22
=
][
m1 r 21 +m2 r 2+ J 1+ J 2 0
0 m2 ] V=
[ ][
V1
=
2m2 r r
V 2 m2 r ( )2 ] ;

G=
[ ][
G1
G2
m r + m r gcos
=( 1 1 2 )
m2 g sin ]
B iu khin PD b trng lng vi robot -r:
( m1 r 1+ m2 r ) gcos
M dk=k p k d +
Khp quay:

M dkr=k pr r k dr r m2 g sin
Khp tnh tin:

Vi = v r =r r

cho n gin ta xy dng qu o dt cho hai khp l qu o bc 3

Bi ton thit k qu o cho khp l xc nh ng biu din khp ca v tr
q0
(gc quay hay di chuyn ca khp tnh tin) theo thi gian khi i t v tr ban u
tc q
n v tr cui qc trong thi gian , vi l khp tng qut. Qu o di chuyn ca khp
gia 2 v tr s tha mn 4 iu kin: v tr ban u v v tr cui cng; tc ti v tr ban
u v ti v tr cui cng. Do a thc bc 3 s thch hp cho qu o ca khp robot:
q(t ) a0 a1t a2t 2 a2t 3

Cc iu kin u v cui l:
q (t 0 ) q0

q (t c ) qc

Bi tp ln iu khin RoBot cng nghipPage 12

 Svth: L Vn Cng- T ng ha 04 K58

q t 0 q 0
( =
q tc q c
( =
Trong thc t, ta thng gp trng hp tc ti v tr ban u v v tr cui bng
khng :
q 0=0 ; q c =0 Ta c:
2
q (t)=a1+2a1tc+3a3. t
Kt hp vi iu kin u v cui ta c phng trnh sau:
q (t 0 ) a0 q0
q (t c ) a0 a1tc a2t22 a3tc3 qc

q (t0)=a1=0
2
q (tc)=a1+2a2tc+3a3. t c =0
Gii h phng trnh ta c cc h s ca a thc qu o.
d. M phng h thng
Xy dng cc m file
File Dyna.m
function out = Dyna(in)
%q(1) goc theta1
%q(2) Gia tri r
%dq(1) toc do quay khop 1
%dq(2) toc do tinh tien khop 2
%cac thong so robot
m1 = 2;
m2 = 2;
L = 0.6;
g = 9.81;

J1 = 1/12*m1*L^2;
U(1) = in(1);
U(2) = in(2);
q(1) = in(3);
q(2) = in(4);
J2=1/12*m2*(q(2)^2);
% ma tran quan tinh
h(1,1) = m1*(L^2) + m2*(q(2)^2) + J1 + J2 ;
h(1,2) = 0 ;
h(2,1) = 0 ;
h(2,2) = m2;
% gia toc khop
out=h\[U(1);U(2)];
end

Bi tp ln iu khin RoBot cng nghipPage 13

 Svth: L Vn Cng- T ng ha 04 K58

File V.m: Biu th thnh phn lc hng tm v lc nht

function out = V(in)
%q(1) goc theta1
%q(2) Gia tri r
%dq(1) toc do quay khop 1
%dq(2) toc do tinh tien khop 2
%cac thong so robot
m1 = 2;
m2 = 2;
L = 0.6;
g = 9.81;

q(1) = in(1);
q(2) = in(2);
dq(1) = in(3);
dq(2) = in(4);
% Detailed explanation goes here
out(1,1) = 2*m2*q(2)*dq(2)*dq(1);
out(2,1) = - m2*q(2)*dq(1)^2 ;
end

File G.m: Biu th momen trng lc

function out = G(in)

%q(1) goc theta1
%q(2) Gia tri r
%dq(1) toc do quay khop 1
%dq(2) toc do tinh tien khop 2
%cac thong so robot
m1 = 2;
m2 = 2;
L = 0.6;
g = 9.81;

q(1) = in(1);
q(2) = in(2);
% trong luc
out(1,1) = (m1*L + m2*q(2))*g*cos(q(1));
out(2,1) = m2*g*sin(q(1)) ;

end
File Quydaobac3.m: dung to qu o bc 3 robot

function out = Quydaobac3(in)

tf = 6;
t = in(1);
q1f = in(2);
q2f = in(3);
a01=0;
a11=0;
a21=3*(q1f-a01)/(tf^2)-2*0/tf-0/tf;
a31=-2*(q1f-a01)/(tf^3)+(0+0)/(tf^2);
a02=0.6;

Bi tp ln iu khin RoBot cng nghipPage 14

 Svth: L Vn Cng- T ng ha 04 K58

a12=0;
a22=3*(q2f-a02)/(tf^2)-2*0/tf-0/tf;
a32=-2*(q2f-a02)/(tf^3)+(0+0)/(tf^2);
%Quy dao bac 3 cua cac khop
out(1,1) = a01+a11*t+a21*(t^2)+a31*(t^3);
out(2,1) = a02+a12*t+a22*(t^2)+a32*(t^3);

end

S m phng

Hnh 4 : S cu trc m phng ca Robot bng phm mm Matlab simulink

Kt qu m phng

Bi tp ln iu khin RoBot cng nghipPage 15

 Svth: L Vn Cng- T ng ha 04 K58

Hnh: Qu o t ca 2 khp t ban u

Hnh 2.2. th qu o khp th nht so vi qu o t

Bi tp ln iu khin RoBot cng nghipPage 16

 Svth: L Vn Cng- T ng ha 04 K58

Hnh 2.3: th qu o khp th hai so vi qu o t

Hnh 2.4. th gia tc 2 khp

Bi tp ln iu khin RoBot cng nghipPage 17

 Svth: L Vn Cng- T ng ha 04 K58

Hnh 2.5. th vn tc 2 khp

Nhn xt
- ng th t bm st ng th thc t, cho thy thng s ca b iu
khin l ph hp.
- Gia tc ca 2 khp tin v khng v vn tc ng yu cu
- th qu o khp ph hp vi qu o cn thit k.

Bi tp ln iu khin RoBot cng nghipPage 18

 Svth: L Vn Cng- T ng ha 04 K58

KT LUN
Qua m t gn mt k lm bi tp ln cng vi s gip nhit tnh v s quan tm
thy gio hng dn TS. Nguyn Mnh Tin gip em hon thnh tt bi tp ny.

Qua t lm n ny gip em hiu thm nhiu kin thc v nguyn l hot ng ca

Robot trong thc t v c th bit cch tnh ton v thit k cc b iu khin cho
Robot.

Tuy nhin, vi thi gian khng nhiu cng nh hn ch v kin thc, bi bo co v bi

tp ln ca em cn nhiu hn ch . Rt mong qu thy c v cc bn gp bi bo co
c hon chnh hn.

Qu trnh lm bi tp ln gip em thu c nhiu kin thc b ch v kin thc thc

t v Robot cng nghip. iu ny s l hnh trang qu bu gip ch cho em sau ny khi
em i lm.

Em xin chn thnh cm n!

Bi tp ln iu khin RoBot cng nghipPage 19

 Svth: L Vn Cng- T ng ha 04 K58

Ti Liu Tham Kho

1. iu Khin RoBot Cng Nghip-Ts. Nguyn Mnh Tin_ Nh xut bn khoa hc

k thut.

Bi tp ln iu khin RoBot cng nghipPage 20