MC LC

Trang
2
3

4
4
4
6

7
7
10
14

21
21
21
21
25
26

32
32
34
35
35
37
39

Yu cu ca n
Li ni u
Chng I: Tng quan v Robot Motoman

1.Lchsphttrinslc
2.RobotMotoman
3.CcthngskthutcaRobotMotoman

Chng II: Cc bi ton ng hc cho Robot

1.Bitonnghcthun
2.Bitonnghcngc
3.MatrnJacobienchoRobot
Chng III: Bi ton ng lc hc Robot
1.HmLagrangevccvnvnglchcRobot
2.TnhtonccgitrnglchcchoRobotMotoman
a.ngnng
b.Thnng
c.XydngphngtrnhnglchcRobot

Chng V: Xy dng cc thut ton iu khin v m phng

1.ThitkbiukhintheophngphpPDbtrngtrng
2.ThitkquobcbachoRobot
3.MphngbakhpuRobot
a.PhngphpiukhinPDbtrngtrngtrnSimulink
b.PhngphpiukhinPDbtrngtrngckmthitkquo
c.XydngmphngbngToolboxSimMechanics

Ti liu tham kho

44

YU CU I VI N
-TmhiuccthngtinvRobotMotoman.
-TnhtonmhnhnghcRobotgmnghcthunchocckhpvng
hcngcchobakhpu
-GiibitonnglchcchobakhpuRobot
-Giibitonnghcvntc(tnhtonmatrnJacoby)
-XydnglutiukhinvthitkquochoRobot
-MphngktqutnhtoncchobakhpubngphnmmMatlab

LI NI U
Ngynaytrongsnghipcngnghipho,hinihotncvntng ho
sn xut c vai tr c bit quan trng.V vy trong nhng nm gn y cc lnh vc
thuctnghangycngcquantmnghincuvphttrin,trongkhngth
khngninnhngngdngquantrngcakthutRobottrongcngnghip.
Mc tiu ng dng k thut Robot trong cng nghip nhm nng cao nng sut dy
chuyncngngh,nngcaochtlngvkhnngcnhtranhcasnphm,ngthi
cithiniukinlaong.Scnhtranhhnghatramtvnthisllmsao
hthngtnghasnxutphictnhlinhhotnhmpngvisbinng
thngxuyncathtrnghngha.Robotcngnghiplbphncuthnhkhng
ththiutronghthngsnxuttnglinhhot.
ncta,tnhngnm1990trliy,RobotvkthutRobotcngdng
vo sn xut kh rng ri. Trong nhng Robot c ng dng vo sn xut th Robot
MotomanlmtRobotnggpphnngktronglnhvctnghacngnghip
nc ta. y l mt Robot c ng dng kh a dng trong nhiu lnh vc : hn t
ng,ccdychuynsnxutcngnghiptng,phunsn...
Nhn thy tm quan trng ca k thut Robot ni chung v ng dng ca Robot
Motoman ni ring, vi kin thc hc hi dc trong qu trnh hc tp ti b mn t
nghavshngdnnhittnhcac gioT.S Nguyn Phm Thc Anhnhm
sinhvinchngemchntinchuynnghnhlROBOT MOTOMANvi
mc ch tm hiu, xy dng, m phng m hnh iu khin robot nhm ng dng l
thuytvothct.Dcshngdntntnhcacgionhngdoqutrnhtch
lykinthcvkinhnghimcnhnchnnnkhngthtrnhkhinhngthiu
st.Chngemmongccthycgpncachngemchonthinhn.
Chngemxinchnthnhcmn!
Sinh vin thc hin
LngnhNgc
TrnThnhKin

CHNG I: TNG QUAN V ROBOT MOTOMAN

1. Lch s pht trin s lc
K t khi Robot u tin c ch to vi sn phm u tin c tn gi ngi my
cngnghiplverstrancacngtyMvonm1960ccsnphmlinquanntay
my cng nghip bt u c quan tm nghin v nh k thut Robot bt u c
nhngbcphttrinutin.
CngtheobnquyncaMccnctrnthgiichyuasnxutrobotcng
nghip,cccngtyvRobotcthnhlpnghincuvphttrin,ngdngRobot
trongtnghocngnghip.
YaskawaMotomanlmtcngtychuyncungcpccgiiphptnghasngto
chohunhttcccngnhcngnghipvngdngrobotnh:hn,lprp,sn,pha
ch, ct gt vt liu vi hn 175 m hnh robot khc bit v hn 40 gii php cng
nghgiiquytcccngvicmtcchhonchnhnhcchthngvnhnh,thitb
anton.
YaskawaMotomanlcngtyiutronglnhvcphttrinccRobotphcvcho
cccngnghgiacngvtng,RobotMotomanchnhlsnphmnghincusn
xutthnhcngcahng.

2. Robot Motoman
a. Gii thiu chung
RobotMotomanlRobottcngnhanh,linhhot,nhgnvngtincy.yl
mtloiRobothotngttvinhiucit.Ncngcungcprtnhiungdng,vi
hiusutsdngcao,mbonhngyucuvchtlng,thigianhonvnngn.
Motomancthitkcngvthng,iunydnnnlmvicthp,thigian
botrlu.Ngoirancncthitknhgn,ctaymng,hiusuthotngcao
ngayctrongnhngvtrkh.
b. Cc ng dng
RobotMotomancngdngrngrivoccdychuynsnxuttng,hin
naycclnhvcphbinnhtl:
-Ccqutrnhhnvnhitluyn.
-Cngnghgiacnglprp.
-Phunsn,vnchuynhngho.
...

c. Phn loi sn phm.

Vidngsnphmbanu,quanhiunmphttrincnhiumuMotomanc
snxutdatrncngmtmhnhnghchthngnhngccitinvkhcnhauv
ccucintvgiihnlmviccckhu.
Cc dng sn phm thuc Motoman trn th trng hin nay ph bin l cc mu
seriesMotomanHP,MotomanUP,MotomanSV.

Motoman UP

Motoman HP20

Motoman SV3

3. Thng s k thut
- S bc t do: 6
- Kiu khp: quay
M hnh Robot:

Khng gian lm vic ca Robot:

CHNG II: CC BI TON NG HC ROBOT

A. BI TON NG HC THUN ROBOT MOTOMAN
pdngphngphpDenavitHartenbergchomhnhtonhccaRobotMotoman,
tatcchtrcnhsau:

TmhnhcaRobot,taxcnhcbngthngsDHnhsau:
i
1
2
3
4
5
6

i ( )
1
2+90
3
4
5
6

i(o)
900
00
900
-900
900
00

ai(mm)
a1
a2
a3
0
0
0

di(mm)
d1
0
d3
0
d5
d6

Matrnbinithunnht :

0
0

DavobngthngsD-H,thayccgitrvomatrntac:

c1
s
A1 1
0

0 s1 a1c1
0 c1 a1s1
1 0
d1

0 0
1

c3
s
A3 3
0

0 s3
0 c3
1 0
0 0

a 3 c3
a 3s3
d3

c5
s
A5 5
0

0 s5
0 c5
1 0
0 0

0
0
d5

s2
c
A2 2
0

c2
s2
0
0

0 a 2s 2
0 a 2c2
1
0

0
1

c 4
A s4
4
0

0 s4
0 c4
1 0
0
0

c6

A s6
6
0

s6
c6
0
0

0
0
0

0 0
0 0
1 d6

0 1

Nhnccmatrn

tacmatrnbiudinvtrvhngca

khutcngcuicaRobotnhsau:

n x c1[s 23 ( c 4c5c 6 s 4s 6 ) c 23s5s 6 ] s1 (s 4c5c 6 c 4s 6 )

n y s1[s23 (c4c5c6 s4s6 ) c23s5s6 ] c1 (s4c5c6 c4s6 )

n z c 23 (c 4c5c6 s 4s 6 ) s32c5c6

o x c1[s 23 (c 4 c5c 6 s 4 c 6 ) c 23s 5s 6 ] s1 (s 4 c5s 6 c 4s 6 )

oy c1[c23 (c4c5c6 s4s6 ) s23s5s6 ] c1 (s1c5s6 c4c6 )

o z c 23 (c 4c5s 6 s 4c 6 ) s 23s5s 6

a x c1 ( s 23c 4s5 c 23c5 ) s1s 4s5

a y s1 (c23c4s5 s23s5 ) c1s4s5

a z c 23c 4 s 5 s 23c 5

p x c1[ s 23 (c 4s5d 6 s 4d 5 ) c 23c5d 6 (s 2 c 2 )a 3s 3 a 2s 2 ) s1 (s 4s5 d 6 c 4 d 5 d 3 ) a1c1

py s1[c23 (c4s5d6 s4d5 ) s23c5d6 (c2 s2 )a 3s3 a 2s2 ) c1 (s4s5d6 c4d5 d3 ) a1s1

p z c 23 (c 4s5d 6 s 4d 5 ) s 23c5d 6 (c 2 s 2 )a 3s3 a 2s 2 d1

Giao din tnh ton ng hc thun thit k trn giao din GUIDE Matlab:

Giaodintnhtonnghcthun
9

B. BI TON NG HC NGC ROBOT MOTOMAN

Bitonvidkinbanulvtrcakhutcngcui,nhimvaraltm
gitrcaccbinkhpmbolkhichuynngngviccbinkhpth
khutcngcuicaRobotschuynngchnhxcnvtrny.
Trong bi ton ng hc thun, v tr v hng tay my c xc nh t cc bin
khp(gcquaykhpquaybit)
iukhinRobotdichuyntheoccvtrmongmuncataymytrongkhng
gian, cn xc nh cc gi tr bin khp tng ng vi v tr v hng ca tay Robot
mongmun.ylnidungcabitonnghcngc
Dovirobotmotomanc6khpquaytdonhngvicxcnhvtrtrongkhng
gianlbitoncabakhpu,cn3khpcuilkhutcngtutngnhimvk
thutnntrongnnychngemschtnhtonnghcngcvnglchcca
bakhpungdngiukhinvtrcarobot.
TacphngtrnhnghcthuncaRobotcdng:

).

).

).

).

).

(*)

Matrn bit,tclvtrvhngcakhungtatayRobotbit,cnxc
nhgitrccbinkhp.Nhnhaivcaphngtrinh(*)vimatrnnghchoca
lmatrn( ) nhncphngtrnhsau:

) .

(1)

Ttac:

0
0

0
1
0
0

0
0
0
1

10

( )
( )
( )
0

( )
( )
( )
0

( )
( )
( )
0

( )
( )

( )
1

(2)

Trong:
( )=

. +

( ) =
( )=

. +

Vix,y,zlccthnhphncavectov
Matrn

ctnhtheophngtrnh(1):

+
+
0
1

+
+

(3)

Cn bng cc thnh phn ct 4 ca hai ma trn phng trnh (2) v (3) ta nhn c
phngtrnhsau:
( )=

(4a)

( )=

(4b)

(4c)

(5a)

(5b)

( ) = 0

Tac:

= 0

Suy ra:

Kthpphngtrnh(1),(4a)v(6c)tac:
. +

=
=

+
+

11

Vitliphngtrnh(5a)v(5b)nhsau:
. +

(6a)

(6b)

Bnhphnghaiphngtrnh(6a)v(6b)vcnglitacphngtrnhsau:
(

) +(

) =(

) +(

) +2

Sdngcchmlnggic,vitgnliphngtrnhtrnnhsau:
= cos[(

]=

) [(

(7)

Donhnc:
=

) +(
2

) +(

) ]

Suy ra:

Tiptcnhnmatrnnghchoca
(

=(

) (

snhncphngtrnhsau:

) (

) (

) .

=
0

0
0

0
0

0
1

Cnbngccphnttac:

= 0

Giiphngtrnhtrn,tac:
=

2( ,

12

Taxcnh

ccphngtrnhsau:
+

=
V

v
+

+(

nntac:
+(

)+

Suyra:
=

=
Vy:

)(

+
(

) +

+
) +

).

Giao din tnh ton ng hc ngc cho 3 khp u:

Giaodintnhtonnghcngc
13

C. MA TRN JACOBIEN ROBOT MOTOMAN

1. Xc nh ma trn HJ cho 6 khp

Tac
nx6=c6;
ny6=s6;
nz6=0;

ox6=-s6;
oy6=c6;
oz6=0;

ax6=0;
ay6=0;
az6=1;

px6=0;
py6=0;
pz6=d6;

J61=-nx6*py6+ny6*px6=0
J62=-ox6*py6+oy6*px6=0
J63=-ax6*py6+ay6*px6=0
J64=nz6=0
J65=oz6=0
J66=az6=1
nx5=c5*c6;
ny5=c6*s5;
nz5=s6;

ox5=-c5*s6;
oy5=-s5*s6;
oz5=c6;

14

ax5=s5;
ay5=-c5;
az5=0;

px5=s5*d6;
py5=-c5*d6;
pz5=d5;

J51=-nx5*py5+ny5*px5=-(c5*c6)*(-c5*d6)+(c6*s5)*(s5*d6)
J52=-ox5*py5+oy5*px5=-(-c5*s6)*(-c5*d6)+(-s5*s6)*(s5*d6)
J53=-ax5*py5+ay5*px5=-(s5)*(-c5*d6)+(-c5)*(s5*d6)
J54=nz5=s6
J55=oz5=c6
J56=az5=0
nx4=c4*c5*c6-s4*s6;
ny4=c4*s6+c5*c6*s4;
nz4=-c6*s5;

ox4=-c6*s4-c4*c5*s6;
oy4=c4*c6-c5*s4*s6;
oz4=s5*s6;

ax4=c4*s5;
ay4=s4*s5;
az4=c5;

px4=c4*s5*d6-s4*d5;
py4=c4*d5+s4*s5*d6;
pz4=c5*d6;
J41=-nx4*py4+ny4*px4
=-(c4*c5*c6-s4*s6)*(c4*d5+s4*s5*d6)+(c4*s6+c5*c6*s4)*(c4*s5*d6-s4*d5)
J42=-ox4*py4+oy4*px4
=-(-c6*s4-c4*c5*s6)*(c4*d5+s4*s5*d6)+(c4*c6-c5*s4*s6)*(c4*s5*d6-s4*d5)

J43=-ax4*py4+ay4*px4
15

=-(c4*s5)*(c4*d5+s4*s5*d6)+(s4*s5)*(c4*s5*d6-s4*d5)
J44=nz4=-c6*s5
J45=oz4=s5*s6
J46=az4=c5

nx3=-c3*(s4*s6-c4*c5*c6)-c6*s3*s5;
ny3=c3*c6*s5-s3*(s4*s6-c4*c5*c6);
nz3=c4*s6+c5*c6*s4;

ox3=s3*s5*s6-c3*(c6*s4+c4*c5*s6);
oy3=-s3*(c6*s4+c4*c5*s6)-c3*s5*s6;
oz3=c4*c6-c5*s4*s6;

ax3=c5*s3+c3*c4*s5;
ay3=c4*s3*s5-c3*c5;
az3=s4*s5;

px3=c3*a3-c3*(s4*d5-c4*s5*d6)+c5*s3*d6;
py3=s3*a3-s3*(s4*d5-c4*s5*d6)-c3*c5*d6;
pz3=d3+c4*d5+s4*s5*d6;
J31=-nx3*py3+ny3*px3
=-(-c3*(s4*s6-c4*c5*c6)-c6*s3*s5)*(s3*a3-s3*(s4*d5-c4*s5*d6)-c3*c5*d6)
+(c3*c6*s5-s3*(s4*s6-c4*c5*c6))*(c3*a3-c3*(s4*d5-c4*s5*d6)+c5*s3*d6)
J32=-ox3*py3+oy3*px23
=-(s3*s5*s6-c3*(c6*s4+c4*c5*s6))*(s3*a3-s3*(s4*d5-c4*s5*d6)-c3*c5*d6)

+(-s3*(c6*s4+c4*c5*s6)-c3*s5*s6)*(c3*a3-c3*(s4*d5-c4*s5*d6)+c5*s3*d6)
J33=-ax3*py3+ay3*px3
=-(c5*s3+c3*c4*s5)*(s3*a3-s3*(s4*d5-c4*s5*d6)-c3*c5*d6)

+(c4*s3*s5-c3*c5)*(c3*a3-c3*(s4*d5-c4*s5*d6)+c5*s3*d6)
J34=nz3
=c4*s6+c5*c6*s4
16

J35=oz3
=c4*c6-c5*s4*s6

J36=az3
=s4*s5

n x2 =s 23 (-c 4 c5c6 +s 4s 6 )-c 23s 5c6 ;

n y2 =c23 (c 4 c5 c6 -s 4s 6 )+s 23s5c6 ;
n z2 =s 4 c5c6 +c4s 6 ;
o x2 =s 23 (c 4 c5s 6 +s 4c6 )-c 23s5s 6 ;
o y2 =-c 23 (c 4 c5s 6 +s 4c6 )+s 23s5s 6 ;
oz2 =-s 4c5s 6 +c 4c6 ;
a x2 =-s 23c 4s 5 +c23c5 ;
a y2 =c 23c 4s5 +s 23c5 ;
a z2 =s 4s5 ;
p x2 =-s 23 (c4s5d 6 -s 4d 5 )+c 23c5d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 ;
p y2 =c23 (c 4s5d 6 -s 4 d5 )+s 23c5d 6 -(c 2 -s 2 )a 3s3 +a 2s 2 ;
p z2 =s 4s5d 6 +c 4d 5 +d3 ;

J 21 n x2 * p y2 n y2 * p x2
s 23 (-c 4 c5 c6 +s 4s 6 )-c 23s5c6 * c 23 (c 4s 5d 6 -s 4 d 5 )+s 23c5d 6 -(c 2 -s 2 )a 3s 3 +a 2s 2
c 23 (c 4 c5c6 -s 4s 6 )+s 23s5 c6 * -s 23 (c4s5 d 6 -s 4d 5 )+c23c5d 6 -(s 2 +c 2 )a 3s 3 -a 2s 2
J 22 o x2 * p y2 o y2 * p x2
s 23 (c 4 c5s 6 +s 4 c6 )-c23s 5s 6 * c 23 (c 4s5 d 6 -s 4d 5 )+s 23c5d 6 -(c 2 -s 2 )a 3s 3 +a 2s 2
-c 23 (c 4 c5s 6 +s 4 c6 )+s 23s5s 6 * (-s 23 (c 4s5 d 6 -s 4 d 5 )+c 23c5 d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 )

J 23 a x2 * p y2 a y2 * p x2
(-s 23c 4s5 +c23c5 ) *(c23 (c 4s5d 6 -s 4d 5 )+s 23c5d 6 -(c 2 -s 2 )a 3s3 +a 2s 2 )
(c 23c 4s5 +s 23c5 )*(-s 23 (c 4s 5d 6 -s 4d 5 )+c23c5d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 )

17

J 24 n z2
s 4c5c6 +c4s6

J 25 oz2
-s4c5s6 +c4c6

J 26 a z2
s 4 s5

n x1 c1[s23 (c4c5c6 s4s6 ) c23s5s6 ] s1 (s4c5c6 c4s6 )

n y1 s1[s 23 ( c 4c5c 6 s 4s6 ) c 23s 5s 6 ] c1 (s 4 c5c6 c 4s 6 )
n z1 c23 (c4c5c6 s 4s6 ) s32c5c6

o x1 c1[s 23 (c 4c5c6 s 4c6 ) c23s5s6 ] s1 (s 4c5s6 c 4s6 )

o y1 c1[ c 23 (c 4 c 5c 6 s 4s 6 ) s 23s 5s 6 ] c1 ( s1c 5s 6 c 4 c 6 )

oz1 c23 (c4c5s6 s 4c6 ) s23s5s6

a x1 c1 ( s 23c 4s 5 c 23c5 ) s1s 4s 5

a y1 s1 (c 23 c 4 s 5 s 23s 5 ) c1s 4 s 5

a z1 c 23 c 4 s 5 s 23 c 5

p x1 c1[ s 23 (c 4s5d 6 s 4d 5 ) c 23c5d 6 (s 2 c2 )a 3s3 a 2s 2 ) s1 (s 4s 5d 6 c 4d 5 d 3 ) a1c1

p y1 s1[c 23 (c 4s 5d 6 s 4 d 5 ) s 23c5d 6 (c 2 s 2 )a 3s 3 a 2s 2 ) c1 (s 4s5d 6 c 4 d 5 d 3 ) a1s1

pz1 c23 (c4s5d6 s4d5 ) s23c5d6 (c2 s 2 )a 3s3 a 2s2 d1

J11 =-n x1*p y1 +n y1*p x1
=-(c1[s23 (-c4c5c6 +s 4s6 )-c23s5s6 ]+s1 (s 4c5c6 +c4s6 ))*(c1[-s23 (c4s5d6 -s 4d5 )+c23c5d6 -(s 2 +c2 )a 3s3 -a 2s2 )+s1 (s4s5d 6 +c4 d5 +d3 )+a1c1 )
+(s1[s23 (-c4 c5c6 +s4s6 )-c23s5s6 ]-c1 (s4c5c6 +c4s6 ))*(c1[-s 23 (c4s5d 6 -s4d5 )+c23c5d6 -(s2 +c2 )a 3s3 -a 2s2 )+s1 (s4s5d6 +c4 d 5 +d3 )+a1c1 )

J12 =-o x1*p y1 +o y1 *p x1

=-(c1[s23 (c 4c5c6 +s4 c6 )-c23s5s6 ]+s1 (-s4 c5s6 +c4s6 ))*(s1[c23 (c4s5 d6 -s4 d5 )+s23c5 d6 +(c2 -s2 )a 3s3 +a 2s2 )-c1 (s4s5d 6 +c4 d5 +d3 )+a1s1 )
+(c1[-c23 (c4c5c6 +s4s6 )+s 23s5s6 ]-c1 (-s1c5s6 +c4 c6 ))*(c1[-s23 (c4s5 d 6 -s4 d5 )+c23c5 d 6 -(s2 +c2 )a 3s3 -a 2s2 )+s1 (s 4s5d 6 +c4 d5 +d3 )+a1c1 )

J13 =-a x1*p y1 +a y1 *p x1

=-(c1 (-s 23c 4s5 +c 23c5 )+s1s 4s5 )*(s1[c 23 (c4s5d 6 -s 4d5 )+s 23c5d 6 +(c 2 -s 2 )a 3s3 +a 2s 2 )-c1 (s 4s5 d 6 +c 4 d5 +d3 )+a1s1 )
+(s1 (c 23c 4s5 +s 23s5 )-c1s 4s5 )*(c1[-s 23 (c4s5d 6 -s 4d5 )+c23c5d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 )+s1 (s 4s5d 6 +c4d 5 +d 3 )+a1c1 )

18

J14 =n z1
=c23 (c4 c5c6 -s 4s6 )+s32 c5 c6
J15 =o z1
=-c 23 (c 4 c5s6 +s 4 c6 )+s 23s5s6

J16 =a z1
=c 23c 4 s 5 +s 23c5

TacmatrnJacoby
nx
n
y
n
J z
0
0

0
nx
n
y
n
z
0
0

ox

ax

oy

ay

oz
0

az
0

0
nx

0
ox

ny

oy

nz

oz

ox

ax

oy

ay

oz
0

az
0

0
nx

0
ox

ny

oy

nz

oz

0
0
0 H
. J
ax
ay

a z
0 J11
0 J 21
0 J 31
.
a x J 41
a y J 51

a z J 61

J12

J13

J14

J15

J 22
J 32

J 23
J 33

J 24
J 34

J 25
J 35

J 42

J 43

J 44

J 45

J 52
J 62

J 53
J 63

J 54
J 64

J 55
J 65

J16
J 26
J 36

J 46
J 56

J 66

2. Ma trn Jacoby cho 3 khp u ca Robot

MatrnJacobydngiukhinivi3khpu

J11 J12
J b = J 21 J 22
J 31 J32

J13
J 23
J 33

19

Tacvtrkhutcngcuikhp3l:
p x =cos(q1 ) a1 -a 2cosq 2 -a 3sin(q 2 +q 3 ) +s1d 3

p y =sin(q1 ) a1 -a 2sinq 2 -a 3sin(q 2 +q 3 ) -cosq1d 3

p z =d1 +a 2 cosq 2 +a 3 .cos(q 2 +q 3 )

Nn
p x

J11 = q =-sinq1 a1 -a 2 cosq 2 -a 3sin(q 2 +q 3 ) +d3cosq1

1

p y
=cosq1 a1 -a 2 sinq 2 -a 3sin(q 2 +q 3 ) +d 3sinq1
J 21 =

q
1

p z
=0
J 31 =
q1

p x

J12 = q =a 2 cosq1.sinq 2 -a 3cosq1cos(q 2 +q 3 )

2

p y
=-a 2sinq1 .cosq 2 -a 3sinq1cos(q 2 +q3 )
J 22 =
q 2

p
J 31 = z =-a 2sinq 2 -a 3sin(q 2 +q 3 )
q 2

p x

J13 = q =-a 3cosq1.cos(q 2 +q3 )

3

p y
=-a 3sinq1cos(q 2 +q3 )
J 23 =
q3

p
J 33 = z =-a 3sin(q 2 +q 3 )
q3

20

CHNG III: BI TON NG LC HC ROBOT

1. Hm Lagrange v cc vn v ng lc hc:
HmLagrangenhnghaskhcbitgiangnngvthnngcahthng:
L=KP
Trong:
K:Tngngnngcahthng
P:Tngthnngcahthng

Dophngtrnhnglchccxcnhbngbiuthc:
Fi =

d L L

dt q. qi
i

Trong: lbinkhp:
= nulkhpquay
= nulkhptnhtin
biudinlchocMomen
ngnngkhpthictnhtheocngthc:
Ki =

1
1
mivi2 + Ji i2
2
2

ViJilMomenquntnhcakhpthi

2. Tnh ton cc gi tr
a. ng nng:
Thanh ni th nht
Tatrngtmthanhnithnht

.
a g1.c1

-a
.s
.
1

g1
1

a g1.s1 0 .
.

0
2
2
p c1 =
a .c .
d g1 => p c1 = g1 1 1 => V c1 =(a g1.1 )

21

ng nng thanh ni th nht:

2

.
. 2
1

K1 = m1 . a g1 .1 +J1 .1
2

Thanh ni th hai:
Tatrngtmcathanhnith2tronghta1

-ag 2 .s2

ag 2 .c2
1

pc 2
0

Tatrngtmcathanhnith2tronghta0

c1
s
0
1
p c2 =A1. p c2 1
0

0 s1
0 -c1
1 0
0 0

a1c1 -a g2 .s 2 -a g2 .c1s 2 +a l c1

a1s1 a g2 .c 2 -a g2 .s1s 2 +a1s1
=

a c d
d1 0
g2 2
1

1 1

.
.
.

-a
(-s
s

+c
s

)-a
s

1
2
1
g2
1
2
1
2
l
1

.
.
.
.

p c2 = -a g2 (c1s 2 1 +s1c 2 2 )+a l c1 1

.
-a g2s 2 2

c2

2
g2 2

2
1

.2

=(a s -2a1a g2 .s 2 +a ). 1 +a

.2

2
g2

. 2

22

ng nng thanh ni th 2:

.2
.2
.2
1
2
2
2
2
K 2 = m 2 ((a g2s 2 -2a1a g2 .s 2 +a 1 ). 1 +a g2 . 2 )+J 2 . 2
2

Thanh ni th ba:
Tatrngtmthanhni3tronghta2
a g3 .c3

a
.s
g3
3

2
p c3 =
d

g3
1

Tatrngtmthanhni2tronghtagc

-c1c 2 -c1c 2
-s s2 -c s
2 1
0
2
p c3 =A1.A 2 p c3 = 1
c2
-s 2

0
0

s1
c1
0
0

a1c1 -a1c1s 2 a g3 .c3

a1s1 -a 2s1s 2 a g3 .s3

d1 +a 2 c 2 d g3

c1 (a1 -a 2s 2 -a g3s 23 )+d g3s1

s
(a
-a
s
-a
.s
)-d
c
1
1
2
2
g3
23
g3
1

d +c a +a c
1 2 2 g3 23

.
.
.
.
X
c3 = 1 (-a1s1 +a 2 s1s 2 +a g3 .s1 .s 23 +d g3 .c1 )+ 2 (-a 2 c1c 2 -a g3 c1c 23 )-a g3c1c 23 . 3

. 0
.
.
.
.
P3 = Y c3 = 1 (a1c1 -a 2 c1s 2 -a g3 .c1.s 23 +d g3 .s1 )+ 2 (-a 2s1c2 -a g3s1c23 )-a g3s1c23 .3
.
.
.
Zc3 = 2 (-a 2s 2 -a g3 .s 23 )-a g3s 23 .3

23

V 2 c3 = 2 1 (-a1s1 +a 2s1s 2 +a g3s1s 23 +d g3c1 ) 2 + 2 2 (a 2 c1c2 +a g3c1c23 ) 2 +(a g3 .c1c 23 .3 )2

.

-21 2 (-a1s1 +a 2s1s 2 +a g3s1s 23 +d g3c1 )(a 2 c1c2 +a g3c1c23 )+2 2 3 .a g3 .c1c 23 (a 2 c1c2 +a g3c1c23 )
.

-21 3 .a g3 .c1c23 (-a1s1 +a 2s1s 2 +a g3s1s 23 +d g3c1 )+ 2 1 (a1c1 +a 2 c1s 2 -a g3c1s 23 +d g3s1 )2
.

+ 2 2 (a 2s1c2 +a g3s1c23 ) 2 +(a g3 .s1c23 .3 )2 -21 2 (a1c1 +a 2 c1s 2 -a g3c1s 23 +d g3s1 )(a 2s1c2 +a g3s1c23 )
.

+2 2 3 .a g3 .s1c23 (a 2s1c2 +a g3s1c23 )-21 3 .a g3 .s1c23 (a1c1 -a1c1s 2 -a g3c1s 23 +d g3s1 )

.
2

+ 2 (a 2s 2 +a g3s 23 ) 2 +2 2 3 .a g3c23 (a 2s 2 +a g3s 23 )+(a g3s 23 3 ) 2

.

=21 2 (d g3a g3c 23 -d g3a 2 c 2 )-2 1 3 .d g3a g3c 23 +2 2 3 .(a 2 a g3c 2 c 23 +a 2s 2 +a g3s 23 +a 2 g3c 2 23 3 ) 2
.

+ 2 1 (a12 +a 2 2s 2 2 +a 2 g3s 2 23 +d 2 g3 -2a1a 2s 2 -2a1a g3s 23 +2a 2 a g3s 23 )+ 2 2 (a 2 2 +a 2 g3 +2a2ag3c3)

.

+ 2 3 a 2 g3
K3

. .
. .
.
.
.
1
(m 3 (21 2 (d g3a g3c 23 -d g3a 2 c 2 )-21 3 .d g3a g3c 23 +2 2 3 .(a 2 a g3c 2 c 23 +a 2s 2 +a g3s 23 +a 2 g3c 2 23 3 ) 2
2

+ 2 1 (a12 +a 2 2s 2 2 +a 2 g3s 2 23 +d 2 g3 -2a1a 2s 2 -2a1a g3s 23 +2a 2 a g3s 23 )+ 2 2 (a 2 2 +a 2 g3 +2a2ag3c3)

.2

+ 2 3 a 2 g3 )+J 3 . 3 )

Ta c tng ng nng cc thanh ni

. 2
. 2
.
1 . 2
1

K=K1 +K 2 +K 3 = J1.1 +J 2 . 2 +J 3 . 3 + m1. a g1.1

2

2
.2
.2
1
2
2
2
2
+ m 2 ((a g2s 2 -2a1a g2 .s 2 +a 1 ). 1 +a g2 . 2 )
2

. .
. .
.
.
.
1
2
2
(m3 (21 2 (d g3a g3c 23 -d g3a 2c 2 )-21 3 .d g3a g3c 23 +2 2 3 .(a 2a g3c 2c 23 +a 2s 2 +a g3s 23 +a g3c 23 3 )2
2
.
2

+ 1 (a1 +a 2s 2 +a g3s
.
2

+ 3 a

2
23

+d

.
2

2
g3

-2a1a 2s 2 -2a1a g3s 23 +2a 2 a g3s 23 )+ 2 (a 2 2 +a 2g3 +2a2ag3c3)

.2

2
g3

)+J 3 . 3 )

24

b. Th nng
Th nng thanh ni th 1:

P1 =m 1 .g(d g1 +L 0 )

Th nng thanh ni th 2:

P2 =m 2 .g(d g1 +L 0 +a g2 s 2 )
Th nng thanh ni th 3:

P3=m3 .g(d g1 +L0 +a g2s2 +a g3s3 )

Ta c tng th nng cc thanh ni:

P=P1 +P2 +P3 =g.d g1 (m1 +m 2 +m3 )+gL 0 (m1 +m 2 +m 3 )+a g2 .g.s 2 (m 2 +m 3 )+a g3 .s3 .m 3g

25

c. Phng trnh ng lc hc:

PhngtrnhLagrangechun:L=KP

. 2
. 2
.
1 . 2
1

J
.
+J
.
+J
.
+
m
.
a
.
1 1
2
2
3 3
1 g1 1
2

.2
.2
. .
. .
1
1
2
2
2
2
+ m 2 ((a g2s 2 -2a1a g2 .s 2 +a 1 ). 1 +a g2 . 2 )+ m3 (21 2 (d g3a g3c23 -d g3a 2 c2 )-21 3 .d g3a g3c23
2
2
.

+2 2 3 .(a 2 a g3c2 c23 +a 2s 2 +a g3s 23 +a 2 g3c2 23 3 )2

.
2

2
1

+ 1 (a +a 2s 2 +a g3s

2
23

+d

.
2

2
g3

-2a1a 2s 2 -2a1a g3s 23 +2a 2 a g3s 23 )+ 2 (a 2 2 +a 2 g3 +2a2ag3c3)

+ 2 3 a 2g3 )- g.d g1 (m1 +m 2 +m3 )+gL0 (m1 +m 2 +m3 )+a g2 .g.s 2 (m2 +m3 )+a g3 .s3 .m3g

Momen tc ng ln khp quay th nht:

L
2
=J1. 1 +m1a g1
.1 +m2 (a 2 g2s 2 2 -2a1a g2s 2 +a 21 )+2m3 2 (d g3a g3c23 -d g3a 2 c2 )

1
-2m d a c +(a 2 +a 2s 2 +a 2 s 2 +d 2 -2a a s -2a a s +2a a s )
3 3 g3 g3 23

2 2

g3 23

g3

1 2 2

1 g3 3

2 g3 23

..
d L
2
2

=(J1 +m1a g1 )1 +m2(a g2 .2s 2 .c2 . 2 -2a1a g2 c2 2 )+

dt 1

2m3 2 (d g3 .a g3 .c 23 -d g3a 2 c 2 )+2m3 2 (-d g3 .a g3 .s 23 ( 2 + 3 )+d g3a 2s 2 2 )

2
-2m3d g3c 23 3 +2m 3d g3s 23 3 ( 2 + 3 )+2a 22s 2 c 2 2 +2a g3
.s 23 .c 23 ( 2 + 3 )

-2a1a 2 c 2 2 -2a1a g3c3 3 +2a 2 a g3c 23 ( 2 + 3 )

..

..

2
=(J1 +m1a g1
) 1 +2m3 (d g3a g3c 23 -d g3a 2 c 2 ) 2 -2m3d g3a g3c 23 3
2
2
+(2m 2 a g2
s 2 c 2 -2a1m 2 a g2 c2 +2a 22s 2 c2 +2a g3
s 23c23 -2a1a 2 c2 +2a 2 a g3c23 ) 2

.2

2
+(2a g3
s 23c23 -2a1a g3c3+ 2a 2 a g3c 23 ) 3 +(-2m3d g3 .a g3 .s 23 +2m3d g3 .a 2 .s 2 ) 2

+2m3d g3a g3s 23 (-2m3d g3a g3s 23 +2m3d g3a g3s 23 ) 2 3

L
=0
1

26

M1 =

d L L
(
)d t .
1
1

..

..

2
= 1 (J1 +m1a g1
)+ 2 (2m3d g3a g3c 23 -2m3d g3a 2 c2 )-3 .2m3d g3a g3c 23
2
2
+ 2 (2m 2 a g2
s 2 c 2 -2a1m 2 a g2 c 2 +2a 22s 2 c 2 +2a g3
s 23c23 -2a1a 2 c2 +2a 2 a g3c 23 )
.2

2
+ 3 (2a g3
s 23c 23 -2a1a g3c3+ 2a 2 a g3c23 )+ 2 (2m3d g3 .a 2 .s 2 -2m3d g3 .a g3 .s 23 )

+ 32 2m3d g3a g3s 23

Momen tc ng ln khp quay th hai:

L
+ 1 a 2 +2m (d a c -d a c )+2 (a a c c +a s +a s +a 2 c 2 )
=J
.
2
2
g2
3 1
g3 g3 23
g3 2 2
3
g3 2 2 23
2 2
g3 23
g3 23
2
2
2 2
+(a 22 +a g3
s 23 2a 2a g3c3 )
..
.
d L

( )=J 2 . 2 +2m 3 1 (d g3a g3c 23 -d g3a 2 c 2 )+2m3 1 d g3a g3s 23 +a 2s 2 +( 2 +3 )+d g3a 2s 2 2
dt 2

..

2
23 (a g3a 2 c 2 c 23 +a 2s 2 +a g3s 23 +a 2g3cg3
)-2a 2 a g3s3 3
.
.
.
.
.
.
.
.

2
+2 3 a g3a 2 (-s 2 c 23 2 -c 2s 23 ( 2 + 3 ))+a 2 c 2 2 +a g3c 23 ( 2 + 3 )-a g3
2c 23s 23 ( 2 + 3 )

..

..

..

2 2
= 2 J 2 + 1 .2.m3 (d g3a g3c 23 -d g3a 2c 2 )+ 3 2(a g3a 2 c 2 c 23 +a 2 c 2 +a g3s 23 +a g3
cg3 )
.

-3 .2a 2 a g3s3 + 1 2 (-2m3d g3a g3s 23 +2m 3d g3a 2s 2 - 1 3 2m3d g3a g3s 23
.

2
+2 3 2 (-a g3a 2s 2 c 23 -a g3a 2 c 2s 23 +a 2 c 2 +a g3c 23 -2a g3
c 23s 23 ).2

2
23 (a g3a 2 c 2s 23 -a g3c 23 +2a g3
c 23s 23 )

27

.
L
2
=(m2a g2
2s 2 c2 -a1a g2c2 )1 -2m31 2 (dg3a g3s 23 -d g3a 2s 2 )+21 3 dg3a g3s23
2
.

-23 2 a g3a 2 (s 2c23 +c2s 23 )-a 2c2 -a g3c23 +2a 2g3c23s23

.

2
+1 a 22 2s 2c2 +2a g3
c23s23 -2a1a 2c2 -2a1a g3c23 +2a 2a g3c23 +a g2 g(m2 +m3 )c2

M2 =
..

d L L
(
)dt 2 2
..

..

2 2
2 J 2 + 1 .2.m3 (d g3a g3c 23 -d g3a 2 c2 )+ 3 2(a g3a 2 c2c 23 +a 2 c2 +a g3s 23 +a g3
cg3 )
2
2
- 1 (m 2 a g2
2s 2 c 2 -a1a g2 c2 +a 22 2s 2 c2 +2a g3
c 23s 23 -2a1a 2 c 2 -2a1a g3c 23 +2a 2 a g3c 23 )
.

- 23 a 2 a g3s3 + 1 2 (-2m3d g3a g3s 23 +2m3d g3a 2s 2 +2m3d g3a g3s 23 -2m3d g3a 2s 2 )
.

2
- 1 3 (2m3d g3a g3s 23 +2d g3a g3s 23 )+23 2 (-a g3a 2s 2 c23 -a g3a 2 c2s 23 +a 2 c2 +a g3c 23 -2a g3
c 23s 23 )
.

+23 2 (a g3a 2s 2 c23 +a g3a 2 c2s 23 -a 2c2 -a g3c 23 +2a 2g3c 23s 23 )
..

..

..

2 2
= 2 J 2 + 1 .2.m3 (d g3a g3c 23 -d g3a 2 c2 )+ 3 2(a g3a 2 c2c 23 +a 2 c2 +a g3s 23 +a g3
cg3 )
2
2
- 1 (m 2 a g2
2s 2 c 2 -a1a g2 c2 +a 22 2s 2 c2 +2a g3
c 23s 23 -2a1a 2 c 2 -2a1a g3c 23 +2a 2 a g3c 23 )
.

- 23 a 2 a g3s3 - 1 3 (2m3d g3a g3s 23 +2d g3a g3s 23 )+a g 2 m 2 gc 2

28

Momen tc ng ln khp quay th ba:

L
2 2
2
=J 3 . 3 -2 1 d g3a g3c 23m3 +2 2 (a g2 a 2c 2c 23 +a 2s 2 +a g3s 23 +a g3
c 23 )m3 +2 3a g3
m3

3
..
..
.
.
..
d L
2 2
(
)=J 3 .3 -2d g3a g3m3 ( 1 c 23 - 1 s 23 ( 2 + 3 ))+2m3 2 (a g2 a 2 c 2c 23 +a 2s 2 +a g3s 23 +a g3
cg3 )

dt 3
..
.
.
.
.
.
.
.
.

2
2
+2m3 2 -a g2 a 2 (s 2 c 23 2 +c 2s 23 ( 2 + 3 ))+a 2 c 2 2 +a g3c 23 ( 2 + 3 )-a g3
2c 23s 23 ( 2 + 3 ) +2a g3
m3

..

..

..

2
2 2
= 3 (J 3 +2a g3
m3 )-1 .2.m3d g3a g3c 23 +2 2 m3 (a g2a 2 c 2 c 23 +a 2s 2 +a g3s 23 +a g3
c g3 )
.2

-2 1 2 d g3a g3s 23 m3 +2 1 3 d g3a g3s 23 m 3 + 2 (2m 3a g2 a 2s 2c 23 -2m3a g2 a 2 c 2s 23 +

.

2
2m3a 2c 2 +2m3a g3c 23 -4m3a 2g3c 23s 23 )+ 3 2 (-2m 3a g2 a 2c 2s 23 +2m3a g3c 23 +4m3a g3
c 23s 23 )

.
.
L
2
=-2m3 1 2d g3a g3s 23 +2 1 3 d g3a g3s 23 +23 2 (-a g3a 2 c 2s 23 +a g3c 23 -2a g3
c 23s 23
3
.

2
+ 1 (2a g3
c 23s 23 2a1a g3c 23 +2a 2a g3c 23 )-2 2a 2 a g3s3 +a g3gc3m3g

M3 =
..

d L L
(
)dt 3 3
..

..

2
2 2
3 (J 3 +2a g3
m3 )-1 .2.m3d g3a g3c 23 +2 2 m3 (a g2 a 2 c2c 23 +a 2s 2 +a g3s 23 +a g3
cg3 )
.2

-2 1 2d g3a g3s 23 m3 +2 1 3 d g3a g3s 23 m3 + 2 (2m3a g2 a 2s 2 c23 -2m3a g2 a 2 c2s23 +

.

2
2
2m3a 2c 2 +2m3a g3c 23 -4m3a g3
c 23s 23 )+3 2 (-2m3a g2 a 2 c2s 23 +2m3a g3c 23 +4m3a g3
c23s 23 )
.

2
-(-2m3 1 2d g3a g3s 23 +2 1 3 d g3a g3s 23 +23 2 (-a g3a 2 c2s 23 +a g3c23 -2a g3
c23s 23
.

2
+1 (2a g3
c 23s 23 2a1a g3c 23 +2a 2a g3c23 )-2 2a 2a g3s3 +a g3gc3m3g)

29

c. Phng trnh ng lc hc vit di dng ma trn trng thi:

Phngtrnhnglchcthanhni
..

T=H(Q) Q +V(Q, Q )+G(Q)

M1 H11
M = H
2 21
M 3 H 31

H12
H 22
H32

..
1
H13 V11 G1
..
H 23 2 + V21 + G 2
H33 .. V31 G 3
3

Vi:
2
H11 =J1 +m1a g1

H12 =2m 3 (d g3a g3c 23 -d g3a 2 c 2 )

H13 =-2m 3d g3a g3c 23

H 21 =2m3 (d g3a g3c 23 -d g3a 2 c 2 )

H 22 =J 2

2 2
H 23 =2(a g3a 2 c 2 c 23 +a 2s 2 +a g3s 23 +a g3c 23 )m3

H 31 =-2m3d g3a g3c 23

2 2
H 32 =2(a g3a 2 c 2 c 23 +a 2s 2 +a g3s 23 +a g3c 23 )m 3

2
H 33 =J 3 +2a g3m 3
.

2
2
V11 =(2m 2 a g2
s 2 c 2 -2a1m 2 a g2 c 2 +2a 22s 2 c 2 +2a g3
s 23c 23 -2a1a 2 c 2 +2a 2 a g3c 23 ) 2
2
g3 23 23

.2

+(2a s c -2a1a g3c3 +2a 2 a g3c 23 ) 3 +(2m3d g3a 2 c 2 -2m3d g3a g3s 23 ) 2
+2m 3d g3a g3s 23

30

2
2
V21 =-(2m 2 a g2
s 2 c 2 -a1a g2 c2 +2a 22s 2 c 2 +2a g3
s 23c 23 -2a1a 2 c 2 -2a1ag3c23+2a 2 a g3c23 ) 1
.

-2a2ag3s33 -(2m3d g3a g3s 23 +2d g3a g3s 23 ) 1 3

.

2
V31 =2d g3a g3s 23 1 2 +2(-2a g3a 2 c2s 23 +2m3a g3c 23 -4m3a g3
c 23s23 ) 2 3
.

-4d g3a g3s 23 1 2 +(a g3s 23 -a1 +a 2 ).2a g3c 23 1 -2a 2 a g3s 23 2
2
g3 23 23

.2

+2m3 (-a g2 a 2s 2 c 23 -a g2 c 2 c23 +a 2 c2 +a g3c 23 -2a c s ) 2

G1 0
G2 a g 2 m 2 gc2
G3 a g3gc3 m3g

31

CHNG IV
XY DNG CC THUT TON IU KHIN V M PHNG
1. Thit k b iu khin dng phng php PD b trng trng
Bitontralxcnhcutrcbiukhinmbohthngnnhtuyt
xungquanhimcnbng,khngphthucvokhilngthanhnivti.
LutiukhinnydatrntiuchunnnhLiapunov.

MhnhbiukhintheophngphpPDbtrngtrng

Tng quan v b iu khin PD

Phngtrnhnglchc:

+G Q
T=H Q Q+V
Q,Q

Hmiukhin:

Trong:

Tdk =Kp.E+Kd.E+G(Q)

E Qd Q

E Qd(1)

Qd:Gitrt
Mcch;

+G Q (2)

Kp.E+Kd.E+G(Q)
H Q Q+V
Q,Q

32

Tnhchtphngtrnhnglchc:

+H(Q):lhmxcnhdng,hmixng

+ V Q,Q

1
H (Q ) S (Q, Q )(3)
2

T
+ Q .S (Q, Q ).Q 0
T
T
+ a .H (Q).b a.H (Q).b (4)

Mtkhctac:
1 .Q T .H (Q).Q 1 (Q T .H (Q).Q
Q T .H (Q).Q Q T .H (Q).Q )
QT .H (Q).Q
2
2

1 d
T

=
Q .H (Q).Q (5)
2 dt
1 d
E T .Kp.E Q T .Kp.E (6)
+ E Q

2 dt

1 H (Q) S (Q, Q ) Q
T(1)(2)(3) Kp.E Kd .Q H (Q).Q
2

1 H (Q) S (Q, Q ) Q Kp.E

Kd .Q H (Q).Q
2

Nhnchaivcabiuthctrnvi Q T
1 .Q T .H (Q ).Q 1 .Q T .S (Q , Q ).Q Q T .Kp.E Q T .Kd .Q (7)
Q T .H (Q ).Q
2
2

Thay(4)(5)(6)vo(7)tac:

1 d T

Q .H (Q ).Q E T .Kp.E Q T .Kd .Q

2 dt

TbiuthctrntathychmLyapunov:
1 Q T .H (Q ).Q E T .Kp.E 0
V Q,Q
2

Q T .Kd .Q
Q,Q
V

33

chlmvicxclpccthnhphntcvgiatcbngkhngttathy
c:

Q Qd
E 0

E 0
Q Qd
VilutbtrnglcphituynvibiukhinPD,ttcccimlmviccn
bng s lm vic cn bng khng chu s nh hng ca trng lc Rbt. Mc n
nhvchtlngcaqutrnhngphthucvogitrcaKdvKp.

2. Thit k qu o bc ba cho Robot

Bitonthitkquochokhplxcnhngbiudincavtrkhp(gc
quaycakhpquayhocdichuyncakhptnhtin)theothigiankhidichuyn
tvtrbanu nvtrcuicng trongthigian ,viqlbinkhptng
qut.Quodichuyncakhpgiahaivtrsthomn4iukin:
- Vtrbanuvvtrcuicng.
- Tctivtrbanuvtivtrcuicng.
Doathcbc3sthchhpchoquocakhprobot:
q(t)=

+ t+

Cciukinuvcui:
( )=
q( )=

( )=
( )=
Trongmphngquokhpbngsimulinktassdngphngphpxydngqu
obc3tohmtngqutchoquodichuynkhp.

34

3. M phng 3 khp u robot

a. M hnh iu khin PD b trng trng trn Simulink

MhnhbiukhinPDbtrngtrng

Thngsccthanhnilnltnhsau:
m1=60(kg);
m2=30(kg);
m3=20(kg);
d1=0.275(m);
a1=0.15(m);
a2=0.6(m);
d3=0.17(m);
a3=0.12(m);

Khukhuchivtlohmclachn:
0
0
0
1800
200 0

Kp 0
1800 0 va Kd= 0 250 0
0
0
0
400
0 60

Ccgitrt:
qd1 3.4 rad ;
qd2 2.1 rad ;
qd3 2.7 rad ;
35

Kt qu chy m phng:

pnguravibiukhinPD

36

Nhn xt :
NhvicchnngthngscahaibiukhinKpvKddovyccngc
tnhcchtlngtt:thigianxclpnhanh<0.6s,quiuchnhcacbatrng
hpgnnhbngkhng.
b. M phng phng php iu khin PD b trng trng c thit k qu o bc 3
ScutrctrnSimulink:

MphngPDbtrngtrngquobc3

Khukhuchivtlohmclachn:
0
0
2000
Kp= 0
1900 0
0
0
500
0
250 0

Kd= 0 320 0
0
0 60
Xd= -0.08700.5571-1.1409

37

Kt qu m phng

KtqumphngPDcquobc3

38

c. Xy dng M phng bng Toolbox SimMechanics

Cc khi c bn:
a. Khi body 1
Mcch:Biudinmtvtthcngty:

Mt:
+Khilng cavtthvtensormmenquntnh.
+Totrngtmcavtth(CG)
+MtshtoBodytu(CSs)
b. Khi body 2
Mcch:Biudinmtvtthcngty.

Mt:Khibodybiudinmtvtthcngmthuctnhcanlty.Smiu
tcnbaogm:
+Khilngcavtthvsensormmenquntnh
+Tatrngtmcavtth
+MtshtaBodyty.
c. Khi Ground
Mcch:Biudinmtimcnhtrnnngi,nitgccahta.

39

Mt:MtkhiGroundbiudin mtimcnhtronghta tuyti.Gn

khinyvimtbncakhpngnchnschuynngcubncakhp.Nh
vythkhinytngngvingm.
Groundnmtrongthvinbodiesvnchnhlmtbodycbit.Nhungchngta
chcthnimtucunvimtkhp.Cckhigroundtnglyhtaccc
trcsongsongvihtang(gntimttrongccgroundkhctrc)vgcti
groundpoint.
d. Khi revolute
Mcch:Biudinmtkhpquaykthpvibctdotnhtin.

Mt:Khirevolutemiutmtbctdoquaynxungquanhmttrcxcnh
giahaibody.Khpquaycxcnhtheoquytcbntayphi.
e. Khi joint actuator
Mcch:Tcnglc/mmenhoctochuynngchomtkhpnguynthy.

Mt:khpgihaibodythhinbctdotngigianhngbody.KhiJoint
actuatorkchhotmtkhijointnigiahaibodyvimttrongnhngtnhiusau:
+Mtlcsuyrng:Lcdochuynngtnhtindctheokhpnguynthylngtr
hocmmenchochuynngquayquanhmtkhpnguynthyquay.
+Mtchuynng:Chuynngtnhtinchokhpnguynthylngtr,didng
vtr,vntcvgiatctheochiudi.Tnhiuvntcphilohmcatnhiuv
trvgiatclohmcuvntc.
f. Khi Joint sensor
Mcch:ochuynngvlc/mmencakhpnguynthy.

40

Mt:KhiJointsensorovtr,vntcvgiatccakhpnguynthytrongmt
khiJointsensorochuynngdc/quayquanhtrckhp(hocquanhmtimi
vikhpcu)tronghttoquychiuxcnhchokhpnguynthytronghp
thoicakhp.jointnimtbasebodyvimtfollwerquytnhhngchuynng.

c. S cu trc m phng bng SimMechanics:

S b iu khin PD b trng trng cho mt khp:

41

Kt qu m phng:
a.mhnhrobottrcvsaukhiquay.

Vtrbanu

Saukhiquayvigitrt

42

b.thquocckhp:

pngcakhp1,2,3vigitrtq1=45.q2=15,q3=-10.

Kt lun:
NhvicchnngthngscahaibiukhinKpvKddovyccngc
tnhcchtlngtt,thigianxclpnhanh,quiuchnhcacctrnghpgn
nhbngkhng.Biukhinbmtheoquot.
43

TI LIU THAM KHO

1.BigingRobotcngnghip( TS. Nguyn Phm Thc Anh )
2.RobotcngnghipNhXutBnKhoaHcVKThut2006( GSTSKH.
Nguyn Thin Phc )
3.iukhinRobotcngnghip( TS. Nguyn Mnh Tin )
4.MatlabchoksiukhinvtnghaNhxutbnKhoahcvk
thut2007( GS-TSKH. Nguyn Phng Quang )
5.TiliuthamkhotrnmngInternet.

44