tính toán thiết kế robot

Trng i hc bch khoa H Ni B mn c hc
ng dng
Semester project
ROBOT CNG NGHIP
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
SVTH : Nguyn Quang Nam
GVHD : PGS.TS. Phan Bi Khi 1
ng dng
+ : 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:
ng dng
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)
ng dng
Khp th i d
i
i
a
i
i
1 d1 1
0
90
2 0
2
a
2
= const 0
3 0 3 a3 = const
90
4.Tnh cc ma trn truyn DH
Ma trn truyn DH c dng:
1
1 1
cos sin cos sin sin a cos
sin cos cos cos sin a sin
0 sin cos d
0 0 0 1
i i i i i i
i i i i i i i
i
i i i
A

]
]
]
]
]
Theo bng DH ta c
1 1
1 0
1
1 1
cos0 sin 0cos 0 sin 0sin 0 a cos0 1 0 0 a
sin 0 cos0cos0 cos0sin0 a sin 0 0 1 0 0
0 sin 0 cos0 d 0 0 1 d
0 0 0 1 0 0 0 1
A
] ]
] ]
] ]

] ]
] ]
] ]

ng dng
2 2 2 2 2 2 2 2 2
2 2 2 2 2 2 2 2 2 1
2
cos sin cos 0 sin sin 0 a cos cos sin 0 a cos
sin cos cos 0 cos sin 0 a sin sin cos 0 a sin
0 sin 0 cos 0 0 0 0 1 0
0 0 0 1 0 0 0 1
A

] ]
] ]
] ]

] ]
] ]
] ]

t
2 2 2 2
cos ; sin c s
Ta c
2 2 2 2
2 2 2 2 1
2
c s 0 a c
s c 0 a
0 0 1 0
0 0 0 1
s
A
]
]
]
]
]
]
3 3 3 3 3 3
3 3 3 3 3 3 2
3
cos sin cos sin sin0 a cos
sin cos cos cos sin0 a sin
0 sin 0 cos 0 0
0 0 0 1
A

]
]
]
]
]

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
3 3 3 3
os ; sin c c s
ng dng
Ta c
3 3 3 3
3 3 3 3 3
4
c s 0 a c
s c 0 a
0 0 1 0
0 0 0 1
s
A
]
]
]
]
]
]
Vy ta c
2
T
3
= 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
= 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
=
ng dng
=
1
1
1 0 0 a
0 1 0 0
0 0 1 d
0 0 0 1
]
]
]
]
]
]
.
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
]
]
]
]
]
]

=
2 3 2 3 3 2 2 3 3 2 3 3 2 3 2 2 1
2 3 3 2 2 3 2 3 3 2 3 3 2 3 2 2
1
c c - s s - s c - s c 0 a c c - a s + a c a
s c + s c - s s + c c 0 a s c + a c + a
0 0 1 d
0 0 0 1
s
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
=
2 3 2 3
c c - s s ;
u
y
=
2 3 3 2
s c + s c ;
u
z
= ;
ng dng
v
x
=
3 2 2 3
- s c - s c ;
v
y
=
2 3 2 3
- s s + c c ;
v
z
= 0 ;
w
x
= 0 ;
w
y
= 0 ;
w
z
= 1 ;
p
x
=
3 2 3 3 2 3 2 2 1
a c c - a s + a c a s +
;
p
y
=
3 2 3 3 2 3 2 2
a s c + a c + a s s
;
p
z
=
1
d
6.Bi ton ng hc thun
a.Gn quy lut chuyn ng cho cc khu ca robot
+ q
1
= d
1
= sint
+ q
2
=
2 = 2t
+ q
3
=
3 = 3t
hay ta c ma trn:
q =
sin
2t
3
t
t
]
]
]
]
]
ng dng
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 =
3 2 3 3 2 3 2 2 1
3 2 3 3 2 3 2 2
1
a c c - a s + a c a
a s c + a c + a
d
s
s s
+ ]
]
]
]
]
=
3 2 3 3 2 3 2 2 1
3 2 3 3 2 3 2 2
1
a cosq cosq - a sin q sinq + a cosq a
a sinq cosq + a cosq sinq + a sinq
q
+ ]
]
]
]
]
ta c p(t) =
3 3 2 1
3 3 2
a cos2tcos3t- a sin 2tsin3t+ a cos2t a
a sin2tcos3t+ a cos2tsin3t+ a sin2t
sint
+ ]
]
]
]
]
ti mi thi im t im cui E 1 v tr xc nh
+Hng ca khu tc : 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:
ng dng
C = C(q) =
=
2 3 2 3 3 2 2 3
2 3 3 2 2 3 2 3
c c - s s - s c - s c 0
s c + s c - s s + c c 0
0 0 1
]
]
]
]
]

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

sint, q
2
= 2t, q
3
= 3t ta c:
C = C(t) =
cos2tcos3t- sin2t sin3t - sin3tcos2t- sin2tcos3t 0
sin2tcos3t+ sin3tcos2t - sin2t sin3t+ cos2tcos3t 0
0 0 1
]
]
]
]
]
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 a
1
= a
2
= a
3
= 10 (cm)
>>t=linspace(0,3*pi,300);
>> x=10.*(cos(2*t)).*(cos(3*t))-10.*(sin(2*t)).*(sin(3*t))+10.*(cos(2*t))+10;
>> y=10.*(sin(2*t)).*(cos(3*t))+10.*(cos(2*t)).*(sin(3*t))+10.*(sin(2*t));
>> z=sin(t);
>> plot3(x,y,z)
ng dng
Ta c qu o im cui nh hnh v
Qu o im E
7. Bi ton ng hc ngc
Bi ton ngc l bi ton c ngha rt quan trng trong
thc t. Khi bit quy lut chuyn ng ca khu thao tc v ta
phi tm cc gi tr ca bin khp. Vic xc nh cc gi tr ca
bin khp cho php ta iu khin robot theo ng qu o
cho.
-5
0
5
-5
0
5
4
4.5
5
5.5
6
ng dng
Trong bi ton ny, ta bit trc 3 tham s l x, y, z v da
vo 3 phng trnh xc nh v tr, ta xc nh c quy lut
ca d
1
,
2,3
.
Cho qu o khu thao tc E chuyn ng
5sin( )
5cos( )
5
x t
y t
z
'
Nhim v ca chng ta by gi l tm d
1
,
2,3
ng dng
Ta c
0
T
3
=
2 3 2 3 3 2 2 3 3 2 3 3 2 3 2 2 1
2 3 3 2 2 3 2 3 3 2 3 3 2 3 2 2
1
c c - s s - s c - s c 0 a c c - a s + a c a
s c + s c - s s + c c 0 a s c + a c + a
0 0 1 d
0 0 0 1
s
s s
+
]
]
]
]
]
]
=
w
w
w
0 0 0 1
Ex Ex Ex Ex
Ey Ey Ey Ey
Ez Ez Ez Ez
u v p
u v p
u v p
]
]
]
]
]
]
(*)
T (*) ta c
p
Ex
= a
3
c
2
c
3
- a
3
s
2
s
3
+ a
2
c
2
+a
1
= a
3
c
23
+ a
2
c
2
+a
1
= 5 sin(t) (1)
p
Ey
= a
3
s
2
c
3
+ a
3
c
2
s
3
+ a
2
s
2
= a
3
s
23
+ a
2
s
2
= 5 cos(t) (2)
p
Ez
= d
1
= 5
vi c
23
= cos(
2+3
)
s
23
= sin(
2+3
)
T (1) v (2) ta c
(p
Ex
- a
1
)
2
= (a
3
c
23
+ a
2
c
2
)
2
= a
3
2
. c
23
2
+ 2a
2
a
3
c
2
c
23
+ (a
2
c
2
)
2
(1)
p
Ey
2
= (a
3
s
23
)
2
+ 2a
3
s
23
a
2
s
2
+ (a
2
s
2
)
2
(2)
t (1) v (2) ta suy ra
(p
Ex
- a
1
)
2
+ p
Ey
2
= a
3
2
+ a
2
2
+ 2a
2
a
3
c
3
ng dng
Hay c
3
=
( )
2
2 2 2
Ex 1 Ey 3 2
2 3
p a p a a
2a a
+ +
Vy
3 = arcosc3 = arcos
( )
2
2 2 2
Ex 1 Ey 3 2
2 3
p a p a a
2a a
]
+ +
]
]
]
Thay vo (2) ta c
s
2
=
2 2 2 2 2 2 2
Ey 2 Ey 2 3 Ey 3 3 3 2
2 2 2 2
3 3 3 2
p a ( a ) (sin p )(a os sin a )
(a os sin a )
p c
c

t

2 = arcsin s2 = arcsin
2 2 2 2 2 2 2
Ey 2 Ey 2 3 Ey 3 3 3 2
2 2 2 2
3 3 3 2
p a ( a ) (sin p )(a os sin a )
(a os sin a )
p c
c

]
t
]
]
]
chn a
1
= a
2
= a
3
= 10 ta c
t=linspace(0,3*pi,300);
>> p1 = 5*sin(t);
>> p2 = 5*cos(t);
>> p3 = 5;
ng dng
>> x = acos((((p1)-10).^2+(p2).^2-10.^2+ 10.^2)/(2.*10.*10));
>>y=asin(((p2)*10-sqrt(((p2*10).^2)-((sin(x)).^2-(p2).^2).*((10*cos(x)).^2-
(sin(x)).^2)-10.^2))/((10*cos(x)).^2-(sin(x)).^2-10.^2));
>>z = p3;
ng dng
>>plot3(x,y,z)

ng dng

th ca cc khp khi bit khu thao tc E
8. Khng gian lm vic ca robot
Khng gian lm vic ca cnh tay robots l khng gian c gii hn bi mt tr
nh hnh v c chiu cao bng vi gii hn di chuyn ca khu 1 v c ng knh
bng tng chiu di ca khu 2 v khu 3. V vng lm vic ca robot b gii hn
mt phn do b cn bi khu 1 nn gc quay
2
s khng quay c ht 360
o
ng dng
9. Trnh by gii thut v chng trnh tnh ton ng lc hc ca robot


tính toán thiết kế robot

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

tính toán thiết kế robot

Uploaded by

Copyright:

Available Formats

Trng i hc bch khoa H Ni B mn c hc

You might also like