Mobile robot group Ph vn Ngc SIC PFIEV K47

M T CU TO V TNH TON NG HC
CHO MOBILE ROBOT
I - M t cu to ca mobile robot
- Mobile Robot l mt xe c 4 bnh , hai bnh trt pha trc v hai bnh pht
ng pha sau ( nh hnh v di y )

Bnh pht ng

Mobile
Robot

Bnh trt

- Pha sau robot , c gn hai ng c bc . Mi trc ng c bc c gn bi

mt bnh rng bn knh r1 . truyn ng , ta dng phng php truyn ng bng dy
ai . Dy ai c mc gia bnh trn trc ng c vi mt bnh rng khc trn trc bnh
xe pht ng . Gi bn knh ca bnh rng ny l r2 .Bn knh ca bnh xe robot l R .
- Thng s ca ng c : gc bc 1,8 vi chui xung 4 bc v 0,9 vi
chui xung 8 bc

ng c

r1
Dy ai
R

r2
II Tnh ton ng hc

Mobile robot group Ph vn Ngc SIC PFIEV K47

- Vi cc k hiu quy c , ta c t s truyn gia bnh rng trn trc ng c vi

r1
bnh rng trn trc bnh xe l : k =
. iu ny c ngha l : khi ng c quay c mt
r2
gc th bnh xe robot quay c mt gc = k .
1. Trng hp robot chuyn ng thng
a) Tnh khong thi gian gi chm cho ng c

- Gi vn tc ca robot l v , vn tc ca mi ng c l v 0 => v t l thun vi vn

tc ca ng c hay t l nghch vi thi gian tr hay thi gian gi chm trong chui xung
lm vic ca ng c .
- Gi khong thi gian gi chm ny l t 0 . chnh l khong thi gian gi chm
gia hai ln chuyn mch cho hai cun dy ng cnh nhau ca ng c .
1,8

=
Suy ra , vn tc ca gc ng c tnh theo t0 l : 0 =
( rad/s )
180t0 100t0

- Vn tc gc ca bnh xe robot l : = k0 = k
100to

=> vn tc di ca robot cng chnh l vn tc di ca bnh xe robot : v=R =R k

100to
- T suy ra : robot i vi vn tc v th khong thi gian gi chm cho ng c
Rk
khi ng c quay vi chui xung 4 bc l : t0 =
(s)
100v
b) Tnh s xung cp cho ng c

Mobile robot group Ph vn Ngc SIC PFIEV K47

- ng c c iu khin bi vi iu khin . Vi iu khin s to xung ti cc cng

song song v cung cp trc tip cho mnh iu khin ng c .
- Gi s ta mun robot i c mt qung ng l x (cm) => Bnh xe robot ( bnh
x
180x
xe pht ng ) cn phi quay c mt gc l : = ( rad ) hay =
( )
R
R
- Mt khc , gc bc ca ng c l 1,8 => s xung m vi iu khin cn truyn
100x
cho mch iu khin ng c l : n= =
Trong k hiu [ ] ch phn
1,8 R
nguyn ca mt s thc .
- Sai s ca cch tnh ny l 1 xung , vi sai s ny qui ra sai s qung ng i ca
robot vi cc s liu thc vo khong 0,2 cm .
2. Trng hp robot quay mt gc
- quay mt gc , robot c th quay theo cc kiu sau y :
+ Quay sang tri , phi v tm quay l im tip xc gia bnh xe v mt t
+ Quay sang tri , phi v tm quay trung im ca trc bnh xe
- Ta s ln lt i tnh cc thng s cn thit robot thc hin c cc kiu quay nh
trn
a ) Tm quay nm ti im tip xc gia bnh xe v mt t

2d

2d
- Gi di trc bnh xe robot l 2d => bn knh quay ca robot l 2d

Mobile robot group Ph vn Ngc SIC PFIEV K47

- Gi gc quay l ( ) v gi s robot quay theo chiu tin sang pha phi . Do

tm quay l im tip xc gia bnh xe v mt t nn bnh bn phi s dng v bnh bn
tri quay theo chiu tin .
2d d
- Suy ra : bnh tri cn i c mt qung ng l
=
180
90
200

10d
- Ta c : s xung cn truyn cho ng c tri l : nt=
=
180 R 9 R
v
- Gi vn tc gc khi quay l q => q =
vi v l vn tc di ca robot khi quay .
2d
- Mt khc ta c lin h gia vn tc gc ca robot khi quay vi thi gian gi chm t 0 nh
sau :
R k
Rk
q =
T suy ra thi gian gi chm l t0 =
2d 100t0
100v
- Trng hp robot quay sang bn tri th lm tng t
b ) Tm quay nm ti trung im ca trc 2 bnh xe

d
- Gi s robot quay sang phi mt gc => bnh tri quay tin mt gc v bnh
phi quay li cng mt gc . Tnh ton hon ton tng t ta c :
5d
+S xung cn truyn cho ng c : nt=np=
9R
4

Mobile robot group Ph vn Ngc SIC PFIEV K47

Rk
100v
3. Robot chuyn ng theo mt ng cong bt k
- Ta s xt hai trng hp : ng cong l trn v khng trn (gp khc )
a) Nu ng cong l trn :
- Ti mi im ca ng cong lun tn ti mt vc t tip tuyn vi ng cong .
+ Thi gian gi chm : t0 =

vt
v
vp

D
O
- Gi vn tc ca robot l v , vn tc bnh tri l v t , vn tc bnh phi l vp => Tm
quay ca robot l O c xc nh nh trn hnh v .
- Gi khong cch t O n bnh phi l D . Ta c cc h thc sau :
vp
D
=
v D+d
vt D + 2d
=
v
D+d
T suy ra vn tc cc bnh ln lt l :
( D + 2 d )v
vt =
D+d
Dv
vp =
D+d
- Kt hp vi cc kt qu trn ta c thi gian gi chm cho mi ng c s l :

Rk ( D + d )
100( D + 2d )v
Rk ( D + d )
tp =
100 Dv
tt =

b) Nu ng cong l ng gp khc :

Mobile robot group Ph vn Ngc SIC PFIEV K47

- Ta s chia ng gp khc ra thnh cc on thng xc nh .

Ak+2
A3
A1
Ak+1
A0

A2

Ak

- Gi tp cc im ct ng gp khc cho thnh cc on thng xc nh l A 0A1A2

An . Nh vy , vic cho robot i theo ng gp khc ny thc cht l cho robot ln lt i
theo cc on thng AiAi+1 ( vi i = 0..n-1 ) . Vn cn gii quyt uuuuur
y l xc nh
khong cch AiAi+1 v gc m robot cn quay tc l gc gia vct Ai Ai +1 v vc t
uuuuuuur
Ai +1 Ai + 2
uuuuur
- Ta c di vct Ai Ai +1 = ( xi +1 xi ) 2 + ( yi +1 yi ) 2 Vi (xi,yi) v (xi+1,yi+1) ln lt l to
ca Ai , Ai+1 .
- Gc quay ca robot : = arccos(

xi xi +1 + yi yi +1

)
x + yi2 xi2+1 + yi2+1
- By gi ta cn xc nh chiu quay ca robot . Chng hn khi i n A 1 th robot cn quay
sang phi nhng khi n A2 th robot li cn quay sang tri . Vic xc nh gc quay , chiu
quay ca robot s n gin hn bng cch dng h to ng
- Ta gi s rng robot ang v tr A0(0,0) l gc ca to tnh ban u ,v phng robot
trng vi phng ca 0x
- Chng ta gi s bng quy np nh sau.khi rbt v tr k v ang c phong l Auuuuuu
k-1Ak v
r
robot cn n v tr Ak+1 mun th robot cn quay mt gc k bng gc gia vct Ak 1 Ak
uuuuuur
v Ak Ak +1 .
- By gi chng ta thay i h trc to mi gi l h trc to Ok c tm chnh l im
Ak+1 phng 0x trng vi phong ca AkAk+1 nh vy s chuyn ng ca robot s tng
t nh trng hp bt u chuyn ng ca robot khi to tnh
- Nh vy s chuyn ng ca robot ti im Ak c chia lm cc phn sau
+ Quay mt gc bng k theo mt chiu nht nh
+ Di chuyn mt on
rk= x k ^ 2 + y k ^ 2 .
+ Chuyn h trc to mi
- Lc ny cc im ban u trong to mi c to l
xk:=xicos+yisin-rk
yk:=yicos-xisin
2
i

Mobile robot group Ph vn Ngc SIC PFIEV K47

- Nh vy vic quan trng by gi l tnh gc k

-uuuuuu
Sau rmi ln thay i to nh th th Ak(0,0) do ta ch cn xc nh gc ca vector
Ak Ak +1 v 0x
- Gc quay c tnh bng hm sau (vit theo C++)
float deg(float a,float b)
{float _deg;
if(a==0)return ((b>0)?90:-90);
else { _deg=180*atan(b/a)/M_PI
if(a>0) return _deg;
else
{
if(b<0)return _deg-180;
if(b==0)return 180;
if(b>0)return _deg+180
}
}
}
- Bng hm ny ta s xc nh c gc k t to Ak+1
tnh chiu quay ta dng hm sau
int sign(float a,float b)
{if(deg(a,b)>0) return -1;//quay trai
If(deg(a,b)==0)return 0;//di thang
If(deg(a,b)<0)return 1;//quay phai
}
- Nh vy , vic i theo ng gp khc ca robot thc cht l bi ton i theo ng thng
v quay mt gc xc nh nh xt trn .
c ) V d trng hp ng trn
- Gi s ta c mt ng trn tm O bn knh D nh hnh v :

Mobile robot group Ph vn Ngc SIC PFIEV K47

- Ta xt cch i th nht : Gi vn tc robot l v , vn tc bnh tri l v t , vn tc bnh phi

l vp . Theo cng thc thit lp trn ta c :
( D + d )v
D
( D d )v
vp=
D
Suy ra thi gian gi chm cho mi ng c :
RkD
tt=
100( D + d )v
RkD
tp=
100( D d )v

Gi s robot i c mt gc
=> s xung cn truyn cho mi ng c l :
5( D + d )
nt=
9R
5( D d )
np=
9R
Ly s liu thc t :
R=4,5 cm
d=15 cm
D=1m
K=0,38
V=50 cm/s
=90
vt=

=>

tt=0,9 ms
tp=1,26 ms

S xung cn truyn :
nt=1278 (xung )
np=945 (xung )
- Xt cch i th hai : Chia ng trn thnh mt a gic u ni tip ng trn vi s
cnh a gic l rt ln . Gi s ta chia u ng trn thnh m im .

Mobile robot group Ph vn Ngc SIC PFIEV K47

- di ca cnh a gic u : a=2Dsin(

2
)
m

- Gc quay ca robot sau khi i ht qung ng a : =

2
m

- S xung cn thit i ht qung ng a :

nt=np=

200 D sin(

- S xung cn cp quay mt gc :

2
)
m

10d
9 Rm
- Suy ra s xung tng cng cn cp cho mi ng c i ht 90 l :
mn
m
n= 1 + ( 1)n2
4
4
3600 D + 10 d
=
( khi m-> )
36 R
Thay s vo ta c : n=2225 (xung )
nt=np=