Industrial Robot Report

Trng i hc Bch khoa H Ni B mn C hc ng dng
Robot Simulator
Robotics Simulation Utility using 3D Picture Control
Nguyn Nho Ba CT1 K52 Tnh ton thit k robot. 1
1.Tnh s bc t do:
f = (n-k) +
i
0
f
k
i

+ f
c
+ f
p
Vi :
+ f : s bc t do ca c cu
+ f
i
: s bc t do chuyn ng cho php ca khp i
+ k : s khp ca c h
+ n : s khu ng ca c h
+ : s bc t do ca khng gian c cu thc hin chuyn ng
+ f
c
: s rng buc tha
+ f
p
: s bc t do tha
Thay s vo ta c:
f = 6(3-3) + 3 + 0 +0 = 3

Vy c cu cho c 3 bc t do.
2.Xy dng h to kho st:
3.Lp bng Denavit-Hartenbeg(DH):
Vi cch thit lp h to mi khu ca c cu, c th thnh lp c ma trn lin h gia
2 h to lin tip. H to th I c th nhn c bng cch bin i :
+ H to th i-1 dch chuyn theo trc z
i-1
mt khong d
i
+ Tip theo, quay h trc to i-1 mi quanh trc z
i-1
mt gc
i-1
chuyn trc x
i-1
n trc
x
i
.
+ Tip tc thc hin php tnh tin h trc thu c bc trn dc theo trc x
i
gc to
O
i-1
chuyn n O
i
.
+ Tip tc quay h trc to mi thu c quanh trc x
i
mt gc
i-1
a h trc to i-
1 trng h trc to i.
Bng Denavit Hartenberg (DH)
Khp th i d
i
i
a
i
i
1 d
1
1
0 90
2 0
2
a
2
= const 0
3 0
3
a
3
= const 0
4.Tnh cc ma trn truyn DH
Ma trn truyn DH c dng:
i-1
A
i
=
Theo bng DH ta c
0
A
1
=

1 1 1 1
1 1 1 1
1
cos sin cos90 sin sin90 0cos
sin cos cos90 cos sin90 0sin
0 sin90 cos90 d
0 0 0 1

]
]
]
]
]
]
t c
1
= cos
1

; s
1
=sin
1

0
A
1
=
1 1
1 1
1
0 0
0 0
0 1 0 d
0 0 0 1
C S
S C
]
]
]
]
]
]
1
A
2
=
2 2 2 2 2
2 2 2 2 2
cos sin cos 0 sin sin 0 a cos
sin cos cos 0 cos sin 0 a sin
0 sin 0 cos 0 0
0 0 0 1

]
]
]
]
]
]
=
2 2 2 2
2 2 2 2
cos sin 0 a cos
sin cos 0 a sin
0 0 1 0
0 0 0 1

]
]
]
]
]
]

t c
2
= cos
2

; s
2
=sin
2

Ta c
1
A
2
=
2 2 2 2
2 2 2 2
c s 0 a c
s c 0 a
0 0 1 0
0 0 0 1
s
]
]
]
]
]
]
2
A
3
=
3 3 3 3 3
3 3 3 3
cos sin cos 0 sin sin 0 a cos
sin cos cos 0 cos sin 0 a sin 0
0 sin 0 cos 0 0
0 0 0 1

]
]
]
]
]
]
=
3 3 3 3
3 3 3 3
cos sin 0 a cos
sin cos 0 a sin
0 0 1 0
0 0 0 1

]
]
]
]
]
]

t c
3
= cos
3

; s
3
= sin
3

Ta c
2
A
3
=
3 3 3 3
3 3 3 3
c s 0 a c
s c 0 a
0 0 1 0
0 0 0 1
s
]
]
]
]
]
]
Vy ta c
2
T
3
=
2
A
3
=
3 3 3 3
3 3 3 3
c s 0 a c
s c 0 a
0 0 1 0
0 0 0 1
s
]
]
]
]
]
]
1
T
3
=
1
A
2
.
2
T
3
=
2 2 2 2
2 2 2 2
c s 0 a c
s c 0 a
0 0 1 0
0 0 0 1
s
]
]
]
]
]
]
.
3 3 3 3
3 3 3 3
c s 0 a c
s c 0 a
0 0 1 0
0 0 0 1
s
]
]
]
]
]
]
=
2 3 2 3 3 2 2 3 3 2 3 3 2 3 2 2
2 3 3 2 2 3 2 3 3 2 3 3 2 3 2 2
c c - s s - s c - s c 0 a c c - a s + a c
s c + s c - s s + c c 0 a s c + a c + a
0 0 1 0
0 0 0 1
s
s s
]
]
]
]
]
]

0
T
3
= A
1
.
1
T
3
=
=
1 1
1 1
1
0 0
0 0
0 1 0 d
0 0 0 1
C S
S C
]
]
]
]
]
]
.
2 3 2 3 3 2 2 3 3 2 3 3 2 3 2 2
2 3 3 2 2 3 2 3 3 2 3 3 2 3 2 2
c c - s s - s c - s c 0 a c c - a s + a c
s c + s c - s s + c c 0 a s c + a c + a
0 0 1 0
0 0 0 1
s
s s
]
]
]
]
]
]

=
1 2 3 1 2 3 1 3 2 1 2 3 1 1 3 2 3 1 3 2 3 1 2 2
1 2 3 1 2 3 1 3 2 1 2 3 1 1 3 2 3 1 3 2 3 1 2 2
2 3 3 2 2 3 2 3 3 2 3 3 2 3 2 2 1
c c c - c s s - c s c - c s c c a c c - c a s + c a c
s c c - s s s - s s c - s s c s a c - s a s + s a
0 a s c + a c + a d
0 0 0 1
s s
c c s c
s c s c s s c c s s
]
]
]
] + + +
]
]
5.Thit lp h phng trnh ng hc robot
V tr ca khu tc ng cui vi khu c nh bi ma trn bin i thun nht 4x4 sau:
0
T
3
=
vi u = ; v = ; w = ; p =
H phng trnh ng hc ca robot l:
u
x
=
1 2 3 1 2 3
c c c - c s s ;
u
y
=
1 2 3 1 2 3
s c c - s s s ;
u
z
=
2 3 3 2
s c s c + ;
v
x
=
1 3 2 1 2 3
- c s c - c s c ;
v
y
=
1 3 2 1 2 3
- s s c - s s c ;
v
z
=
2 3 2 3
s s c c + ;
w
x
= s
1
;
w
y
= -c
1
;
w
z
= 0 ;
p
x
=
1 3 2 3 1 3 2 3 1 2 2
c a c c - c a s + c a c s ;
p
y
=
1 3 2 3 1 3 2 3 1 2 2
s a c - s a s + s a c s c ;
p
z
=
3 2 3 3 2 3 2 2 1
a s c + a c + a d s s +
6.Bi ton ng hc thun
a.Gn quy lut chuyn ng cho cc khu ca robot
+ q
1
=
1
= t
+ q
2
=
2
= 2t
+ q
3
=
3
= 3t
hay ta c ma trn:
q = 2t
3
t
t
]
]
]
]
]
b.Phng php xc nh v tr ca im tc ng cui v hng ca khu thao tc theo quy
lut cho
Thay cc gi tr q
1
, q
2
, q
3
vo h phng trnh ng hc ca robot ta c:
+V tr ca im tc ng cui c xc nh bng to im tc ng cui E theo
phng x,y,z ca h to gc tng ng l p
x
, p
y
, p
z
, th hin trong ma trn :
p = p(q) =
Vi p =
1 3 2 3 1 3 2 3 1 2 2
1 3 2 3 1 3 2 3 1 2 2
3 2 3 3 2 3 2 2 1
c a c c - c a s + c a c
s a c - s a s + s a
a s c + a c + a d
s
c s c
s s
]
]
]
] +
]
=
3 1 2 3 3 1 2 3 2 1 2
3 1 2 3 3 1 2 3 2 1 2
3 2 3 3 2 3 2 2 1
cos cosq cosq - a cos sin q sinq + a cos cosq
a sinq cos cosq - a sinq sinq sinq + a sinq cos
a sin cos cos sin sin
a q q q
q q
q q a q q a q d
]
]
]
] + + +
]
ta c p(t) =
3 3 2
3 3 2
3 3 2 1
cos .cos2t.cos3 - a cos .sin 2t.sin3 + a cos .cos2t
a sin .cos 2 .cos3 - a sin .sin2t.sin3 + a sin .cos 2
a .sin 2 .cos3 .cos 2 .sin3 sin 2
a t t t t t
t t t t t t t
t t a t t a t d
]
]
]
] + + +
]
ti mi thi im t im cui E 1 v tr xc nh
+Hng ca khu tc ng: c xc nh bng to ca cc vct n v u, v, w
ca h to gn vi khu tc ng cui trong h to gc, th hin trong ma trn cosin ch
hng:
C = C(q) =
=
1 2 3 1 2 3 1 3 2 1 2 3 1
1 2 3 1 2 3 1 3 2 1 2 3 1
2 3 3 2 2 3 2 3
c c c - c s s - c s c - c s c
s c c - s s s - s s c - s s c
0
s
c
s c s c s s c c
]
]
]
] + +
]

=
1 2 3 1 2 3 1 3 2 1 2 3 1
1 2 3 1 2 3 1 2 3 1 2 3 1
2 3 3 2 1 3 2 3
cos .cosq .cosq - cos .sinq .sin q - cos sin q cos q - cos sinq cosq sin
sinq cos cosq - sinq sin q sinq - sinq cosq sin q - sinq sinq cosq cos
sinq cos sin cos -sinq sinq cos cos 0
q q q q q
q q
q q q q q
]
]
]
+ +
]
]
Thay q
1
=

t, q
2
= 2t, q
3
= 3t ta c:
C = C(t) =
cos .cos2t.cos3 - cos .sin2t.sin3 - cos sin3 cos2t- cos sin2 cos3 sin
sin cos 2 cos3 - sin sin 2 sin3t - sint.cos2 .sin3 - sin .sin2t.cos3 cos
sin2t cos3 sin3 cos 2 -sin .sin3 cos 2 .cos3 0
t t t t t t t t t t
t t t t t t t t t t
t t t t t t t
]
]
]
] + +
]
c. ng dng matlab tnh ton v v qu o chuyn ng ca im tc ng cui E
Dng lp trnh Matlab ta c vi d
1
= a
2
= a
3
= 10 (cm)
>> t=linspace(0,3*pi,300);
>> x=10.*(cos(t).*cos(2*t)).*(cos(3*t))-10.*cos(t).*(sin(2*t)).*(sin(3*t))
+10.*cos(t).*(cos(2*t));
>> y=10.*(sin(t)).*(cos(2*t)).*(cos(3*t))-10.*(sin(t)).*(sin(2*t)).*(sin(3*t))
+10.*(sin(t)).*(cos(2*t));
>> z=10.*(sin(2*t)).*(cos(3*t))+10.*(cos(2*t)).*(sin(3*t))+10.*(sin(2*t))+10;
>> plot3(x,y,z);
Ta c qu o im cui nh hnh v
Qu o im E
7. tnh ton tnh hc.
Cho lc tc dng vo khu thao tc ti im E gm vector lc F, v momen M (F=[F
x
, F
y
,
F
z
]
T
; M=[M
x
, M
y
, M
z
]
T
). Tnh lc v momen dn ng ti cc khp m bo cho robot cn
bng tnh. (b qua ma st v khi lng cc khu).
Ta xc nh ma trn Jacobien ca robot.
S dng:
[ ]
1
2
3
4
5
6
x
y
z
x
y
z
dq d
dq d
dq d
matran Jacobien
dq
dq
dq
] ]
] ]
] ]
] ]

] ]
] ]
] ]
] ]
] ]
] ]
xc nh ma trn Jacobien.
1 3 2 3 1 3 2 3 1 2 2
1 3 2 3 1 3 2 3 1 2 2
3 2 3 3 2 3 2 2 1
c a c c - c a s + c a c
s a c - s a s + s a
a s c + a c + a d
1 1
x
y
z
p s
p c s c
p s s
] ]
] ]
] ]
] ] +
] ]
] ]
=> cc thnh phn ca mt hng ca ma trn Jacobien.
11 1 3 2 3 1 3 2 3 1 2 2
1
12 1 3 2 3 1 3 2 3 1 2 2
2
13 1 3 2 3 1 3 2 3
3
a c c + s a s - s a c
a c - c a - c a
a s - c a
x
x
x
p
J s s
p
J c s c s s
p
J c s s c

'
21 1 3 2 3 1 3 2 3 1 2 2
1
22 1 3 2 3 1 3 2 3 1 3 2
2
23 1 3 2 3 1 3 2 3
3
a c c - c a s +c a c
a c - s a - s a
a s - s a
y
y
y
p
J c s
p
J s s c s s
p
J s c s c

'

31
1
32 3 2 3 3 2 3 2 2
2
33 3 2 3 3 2 3
3
0
a c - a a
a s + a
z
z
z
p
J
p
J c s s c
p
J s c c

+
'
=> ta thu c ma trn Jacobien 6x6.

Xc nh ma trn Jacobien
3 T
J
so vi khung ta tay T
3
.
Cc khp ca robot u l khp quay : d
i
=0. Nn ta c.
3 3
3 3
3 3
1
2
3
. .
. .
w . w .
T T
i xi xi yi yi xi
T T
i yi xi yi yi xi
T T
i zi xi yi yi xi
J d u p u p
J d v p v p
J d p p
+
'
Cc thnh phn u, v, w, p xc nh t cc ma trn bin i thun nht.

3
w
0 0 0 1
i i i i i
u v p
T
]
]
]
Qua ta xc nh c cc thnh phn ca ma trn Jacobien
3 T
J
so vi khung ta tay
robot T
3
.
Lc v m men ca cc khp.
3
3
.
T
T T
M J F
Sau khi xc nh c ma trn Jacobien
Nu bit lc tc dng vo khu thao tc E => ta s xc nh c m men cn thit ca cc
khp da vo phng trnh trn.
Nu bit m men tc ng vo khu thao tc E tac cng xc nh c lc cn thit ca cc
khp.
8. Tnh ton ng lc hc.
Yu cu : tnh m men ng lng ca khu 2 i vi khi tm ca n.
Khu 2 quay vi vn tc gc.
1 2

| `
+

. ,
Di vi khu 2, ta khi tm c xc nh theo phng trnh :
( )
( )
2 1 1 2 1 2
2 1 1 2 1 2
os os
sin sin
g
g
x l c l c
y l l

+ +
+ +
Hai thnh phn tc ca khi tm trn 2 trc l :
( )
( )
2 1 1 2
1 1 2 1 2
1 1 2
1 1 2 1 2 2
sin sin
os os
g
g
x l l
y l c l c

| `
+ +

. ,
| `
+ + +

. ,
ng lng ca khi tm khu 2 l :
.
.
x
y
k m x
k m y
=>
2 2
x y
k k k +
M men ng lng :
. L k r
Vi r l vector ta ca khi tm khu 2.
9. Thit k v m phng robot.
S dng CAD thit k m hnh 2D :
S dng labview thit k v m phng robot.
LABVIEW (Laboratory Virtual Instrumention Engineering Workbench).
Hng National Instruments cung cp cng c thit k v m phng robot.
Robotics Simulation Utility using 3D Picture Control
VI ny s dng iu khin v m phng robot hoc mt i tng 3D bt k.
VI s dng cy iu khin (tree control) , cc quan h trong s hnh cy l quan h cha-con
gia cc i tng iu khin.
Giao din ca s block diagram : dng G-code vit code cho h thng trn giao din front
panel.
Trn block diagram, s dng cc gi lp tn hiu iu khin thng qua cc VI v cc SubVI
ca chng trnh thay th cho cc thit b iu khin tht. (VI=Virtual Instruments).
Sau khi chy chng trnh, giao din trn ca s front panel c cc nt iu khin v cc nt
nhp gi (numeric controls) tr thng s cnh tay robot.
Cc thng s iu khin s c hin th thng qua numeric indicators.
V tr v trng thi ca robot thay i trong qu trnh hot ng s c hin th thng qua
numeric indicator. (trn front panel ).
Trnh t thit k v m phng robot.
Chy chng trnh (VI). cn c y SubVI v symbols h tr i km, v trong block
diagram windown c s dng cc khi thit b o SubVI h tr vic iu khin m phng
robot.
Trn front panel windown, click Run (hnh mi tn play).
Thit lp khu 0 (gi c nh).
Trong giao din ca s robot tree c cc table robot configuration , type (kiu hnh dng
khu robot), Joint type (kiu khp), Geometry Definition (kch thc ca khu), Pos WRT
Parent (X,Y,Z) (v tr ca khu trn trc ta gn vi khu/khp trc ), Rot WRT
(X,Y,Z,Angle) (quay trc ta mt gc Angle), Color (mu sc khu).
To gi c nh (khu Base). Thit k trong table edit/add/delete item.
Ta s to khu base v add khu ny vo World Space. Mc name (nhp tn khu = base),
color (mu sc), type (chn box), Geometry Definition (kch thc cc chiu, Length X,Y,Z),
(nhp cc thng s nh sau).
Kt qu thu c trn ca s Camera :
Thit lp khu 1.
Khu 1 l khu c nh, ta add khu 1 vo World Space. Name (K1), type (cylinder-tr), color
(red), Geometry Definition (height=chiu cao khi tr, Radius=bn knh tr, detail=chi tit).
Pos WRT Parent (X,Y,Z), Rot WRT (X,Y,Z,Angle) nhp nh hnh v.
Ta thu c kt qu nh hnh sau :
Thit lp khp 1. (Joint1)
Name (Joint1), type (Joint), Joint type (Rot_Z =>khp quay quanh trc Z).
Thit lp khu 2 (K2)
Thit k trong table edit/add/delete item. Nh hnh v, v kt qu thu c nh sau :
Trong khu 2 ta add thm mt khi cylinder m phng cho khp (ch : khp (Joint) v
khu hon ton khc nhau, ta add thm thnh phn ny vo khu 2 ch cho minh ha cho
khp Joint2 add vo khu 2)
Hon ton tng t cho Joint2, Joint3 v khu 2, khu 3. Ta thu c kt qu nh hnh sau.
Ta thit lp trng thi cc khu thng hng theo cc trc d dng thit k (nhp v
tr, gc xoay cc khu v thit lp v tr cc khp).
Cc khu lin kt vi nhau nh cc khp. Nh vy ta thit k xong phn to m hnh cho
robot.
Chy m phng nh vic thay i cc thng s trn bng iu khin (thay i gc quay ca
cc khp). Trn bng iu khin s mc nh c 7 thng s khp iu khin. Robot ang m
phng c 3 bc t do (DOF=3), v vy ta ch iu khin 3 thng s khp u tin.
Thay i s khp iu khin bng cch tng hoc gim s khp (s dng nt bm button
numeric control). Dng chng trnh (bm Stop).
Khi ta thay i thng s gc quay ca cc khp => cc khu s quay.
Mt vi hnh nh kt qu thu c.

Industrial Robot Report

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Industrial Robot Report

Uploaded by

Copyright:

Available Formats

Trng i hc Bch khoa H Ni B mn C hc ng dng

Nguyn Nho Ba CT1 K52 Tnh ton thit k robot. 11

=> ta thu c ma trn Jacobien 6x6.

Cc thnh phn u, v, w, p xc nh t cc ma trn bin i thun nht.

You might also like