Robotics : Bug Algorithm Simulation
Transcript of Robotics : Bug Algorithm Simulation
-
7/22/2019 Robotics : Bug Algorithm Simulation
1/13
Intond [hjddod` =Cu` Jh`nrotgi
[o:?5;5
Ju`ust 37( 3;:6
-
7/22/2019 Robotics : Bug Algorithm Simulation
2/13
Fndtbdts
Fndtbdts
I n t o n d [ h j d d o d ` = C u ` J h ` n r o t g i 6
;$: Odtrneuftond $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $6
;$3 Cu` Jh`nrotgis $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ 6;$3$: Cu` jh`nrotgi $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $
5
;$6 Intond $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ 5;$5 Cnudejry Ebtbftond $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $
2
;$2 Djvo`jtb Cnudejry $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $
-
7/22/2019 Robotics : Bug Algorithm Simulation
3/13
Intond [hjddod` = Cu` Jh`nrotgi
Intond [hjddod` = Cu`Jh`nrotgi
;$: Odtrneuftond
Od tgb jrtofhb wb wohh hnna jt oiphbibdtjtond nl cu` 3 jh`nrotgi lnrintond phjddod`
;$3 Cu` Jh`nrotgis
Ugb joi nl pjtg phjddod` jh nrotgi os tn fniphbtb j fnhhosond lrbbpjtg lrni odotojh tn `njh pnsotond$ Cu` jh`nrotgis jrb soiphbst typbnl pjtg phjddod` jh`nrotgis$
Od Cu` jh`nrotgis dn `hncjh inebh nl tgb wnrhe os jssuibe (tgb hn)fjtond jde sgjpbs nl tgb ncstjfhbs jrb udadnwd$Ndhy odlnrijtond
jfquorbe tgrnu`g sbdsod` os adnwd$Ugb Cu` jh`nrotgis jssuib hnfjh
adnwhbe`b nl bdvorndibdt jde j `hncjh `njh$
Ugb bdvorndibdt os j Uwn)eoibdsondjh sfbdb lohhbe wotg udadnwdncstjfhbs$ Ugb pbroibtbr nl jdy ncstjfhb os lodotb( jde tgjt tgb dui)cbr nl ncstjfhbs os lodotb jde fjd cb nl jrcotrjry sgjpb$
Pb fndsoebr j pnodt rncnt tgjt invbs jind` jrcotrjry ncstjfhbsod j phjdjr bdvorndibdt$ Pb jssuib tgjt tgb wnraspjfb os cnudebe$
Ugb rncnt os biphnybe wotg oebjh hnfjhozjtond ineuhb$Ugb rncnt fjd
jhwjys abbp trjfa nl pnsotond nl rncnt$ Ugus wb fjd jhwjys ibjsurbtgb eostjdfb e ' s 1 / cbtwbbd tgb snurfb jde `njh$
Ugb rncnt os biphnybe wotg tjftohb tnufg nr j rjd`b sbdsnr sbd)snr$Ugb tnufg sbdsnr fjd ebtbft fndtjft wotg tgb ncmbft $
Vjiphb wnraspjfb fndlo`urjtonds jrb sgnwd od :$Ugb rbe ent osrncnt fbdtbr jde `rbbd ent os tgb `njh$
6 | : 6
-
7/22/2019 Robotics : Bug Algorithm Simulation
4/13
Intond [hjddod` = Cu` Jh`nrotgi
'j/ Pnraspjfb 'c/ Pnraspjfb 3
Lo`urb := Pnraspjfb
;$3$: Cu` jh`nrotgi
Ugb rncnt os bquoppbe wotg j rjd`b sbdsnr$Sjd`b sbdsnr wotg j sijhhrjeous fjd cb fndsoebrbe tn cb bquovjhbdt tn j tjftohb sbdsnr$
Jt jdy `ovbd pnodt nl toib rncnt os jssuibe tn cb od ndb nl lnhhnw)od` hnw hbvbh stjtbs I n t o n d ( C n u d e j r y E b t b f t o n d ( D j v o ` j t b C n u d e j r y ( E n d b
;$6 IntondOd tgos ineb tgb rncnt invbs jhnd` j spbfolobe gbjeod` eorbftondwgofg os spbfolobe od tbris nl jd`hb wrt `hncjh fn)nreodjtb systbi$
Jt odotojhozjtond wb gjvb odlnrijtond ndhy jcnut tgb snurfb jdetgb `njh pnsotond$ Ugb sgnrtbst eostjdfb cbtwbbd twn pnodts osj strjo`gt hodb$ Ugb gbjeod` eorbftond os jhnd` tgos hodb lrni snurfbtnwjres tgb `njh$
Ugb rncnt cb`ods tn invb jhnd` tgb strjo`gt hodb tnwjres tgb `njh$
Un spbfoly eosphjfbibdt tn rncnt tn invb jhnd` tgb x jde y eorbf)tonds( tgb udot vbftnrs jhnd` tgb gbjeod` eorbftond jrb fniputbe$Hbt
x 1 y ebdntb tgb udot vbftnrs
Ugbd rncnt tjab j sijhh eosphjfbibdt od tgb gbjeod` eorbftond cy tja)od` j sijhh stbps prnpnrtondjh tn fnipndbdts nl udot vbftnr jhnd`
5 | : 6
-
7/22/2019 Robotics : Bug Algorithm Simulation
5/13
Intond [hjddod` = Cu` Jh`nrotgi
tgb x jde y eorbftonds$
x
t ! :
9 x
t
! x ' : /
y
t ! :
9 x
y
! y ' 3 /
E 9 ' x 1 y / ' 6 /
Bjfg toib pnsotond os odfrbibdtbe tgb eosphjfbibdt os rbfnrebe Hbtus fjhh tgos eosphjfbibdt E $
. . l u d f t o n d t g j t p b r l n r i i n t o n d t n w j r e s t g b ` n j h n p b r j t o n d
v n o e I n t o n d ' /
{
o l ' i $ v j h u b 9 9 i $ I n t o n d /
{
. . f n i p u t b f n i p n d b d t s n l u d o t v b f t n r j h n d ` t g b g b j e o d ` e o r b f t o n d
l h n j t e x 9 f n s ' g b j e o d ` - [ O . : 7 ; / 1
l h n j t e y 9 s o d ' g b j e o d ` - [ O . : 7 ; / 1
. . u p e j t b t g b f u r r b d t p n s o t o n d
p n s o t o n d $ x 9 p n s o t o n d $ x ! : - e x 1
p n s o t o n d $ y 9 p n s o t o n d $ y ! : - e y 1
. . s t n r b t g b f g j d ` b o d p n s t o n d
p r b v e b h t j 9 L [ n o d t 3 l ' e x ( e y / 1
}
}
;$5 Cnudejry Ebtbftond
Ugb rncnt os bquoppbe wotg j rjd`b sbdsnr$Ugb sbdsnr rjd`b os gjsj lodotb rjeous S$Ol j ncstjfhb os bdfnudtbrbe tgb rjd`b sbdsnr `ovbstgb eorbftond jde eostjdfb tn tgb ncmbft$ Hbt os jssuib tgjt eorbftondrbsnhutond nl sbdsnr os : eb`rbb$
Ugus ol j rjd`b sbdsnr ebtbfts snib ncmbft hyod` wotgod tgb rj)eous S$Ugb rncnt bdtbrs tgb cnudejry ebtbftond stjtb$
Vodfb rjd`b os hoiotbe$Ugb rncnt fjddnt bstoijtb ol sod`hb nr iuhtophbncstjfhbs jrb bdfnudtbrbe$
Ot dnw bdtbrs tgb djvo`jtb jde stjrts invod` jhnd` tgb gbjeod`eorbftond$
v n o e C n u d e j r y ' /
{
2 | : 6
-
7/22/2019 Robotics : Bug Algorithm Simulation
6/13
Intond [hjddod` = Cu` Jh`nrotgi
o l ' i $ v j h u b 9 9 i $ C n u d e j r y /
{
. . f g j d ` b t g b g b j e o d ` e o r b f t o n d c y ? ; e b `
. . t n c b s t s f j d h o d b
l n r ' o d t o 9 ; 1 o 8 s f j d $ s o z b ' / 1 o ! ! /
{
o l ' s f j d Q o Y $ o d e b x 9 9 c b s t ^ o d e b x /
{
o l ' s f j d Q o Y $ p n s o t o n d $ x * 9 ) : , , s f j d Q o Y $ p n s o t o n d $ y * 9 ) : /
{
g b j e o d ` 9 c b s t ^ o d e b x ! ? ; 1
. . f g j d ` b s t j t b t n c n u d e j r y d j v o ` j t o n d
i $ v j h u b 9 i $ D j v 1
r b t u r d 1
}
b h s b
{
c r b j a 1
}
}
}
}
;$2 Djvo`jtb Cnudejry
Dnw tgb rncnt gjs tn djvo`jtb jhnd` tgb cnudejry nl tgb ncstjfhbtohh snib prbeblodbe frotbroj os sjtoslobe$
Ugus rncnt iust fgjd`b ot gbjeod` eorbftond$Ugb eorbftond jhnd` wotgsgnrtbst eostjdfb tn tgb cnudejry rbfnrebe cy tgb rjd`b sbdsnr os
ebtbriodbe$
Ugb rncnt fgjd`bs ots gbjeod` jhnd` hodb pbrpbdeofuhjr tn eorbftond
jhnd` sgnrtbst eostjdfb tn tgb cnudejry pnodt$
Od tgos stj`b tgb rncnt stohh fjd sbdsb ncstjfhb wotg ots rjd`b jde otebtbriodbs tgb gbjeod` eorbftond jt bjfg odstjdt js eorbftond pbrpbd)eofuhjr tn eorbftond jhnd` sgnrtbst eostjdfb tn ncstjfhb$ Ot fndtodubstn invb jhnd` tgb gbjeod` eorbftond$
< | : 6
-
7/22/2019 Robotics : Bug Algorithm Simulation
7/13
Intond [hjddod` = Cu` Jh`nrotgi
o l ' s f j d $ s o z b ' / 9 9 ; /
{
. . o l r n c n t i n v b s j w j y l r n i c n u d e j r y ( t r y t n ` n t n p r b v o n u s p n s o t o n d
. . i n v b j h n d ` t g b b e ` b
p n s o t o n d $ x ) 9 p r b v e b h t j $ x 1
p n s o t o n d $ y ) 9 p r b v e b h t j $ y 1
g b j e o d ` 9 c b s t ^ o d e b x 1
}
b h s b
{
. . i n v b e o r b f t o n d p b r p b d e o f u h j r t n c b s t s f j d h o d b
g b j e o d ` 9 c b s t ^ o d e b x ! ? ; 1
}
. . f n i p u t o d ` f n i p n d b d t s n l u d o t v b f t n r j h n d ` g b j e o d `
l h n j t e x 9 f n s ' g b j e o d ` - [ O . : 7 ; / 1
l h n j t e y 9 s o d ' g b j e o d ` - [ O . : 7 ; / 1
. . f n i p u t b t g b d b w p n s o t o n d n l r n c n t
p r b v e b h t j 9 L [ n o d t 3 l ' e x ( e y / 1
p n s o t o n d $ x 9 p n s o t o n d $ x ! 2 - e x 1
p n s o t o n d $ y 9 p n s o t o n d $ y ! 2 - e y 1
Pgohb invod` jhnd` tgb gbjeod` eorbftond j pnodt os rbjfgbe wgbrbdn ncstjfhbs jrb bdfnudtbrbe$ Ugos fjd gjppbd ol rncnt gjs rbjfgbej sgjrp be`b nr ot gjs invbe jwjy lrni tgb cnudejry wgohb djvo)`jtod` tgb cnudejry eub tn odjffurjfobs od ebtbriodod` tgb sgnrtbsteostjdfb jhnd` tgb cnudejry$
Ugb dbw pnsotond nl rncnt os ebtbriodb nd tgb sjib hodbs js tgjtebsfrocbe od intond tnwjres tgb `njh sbftond$ Jt bjfg odstjdt tgbfurrbdt gbjeod` eorbftond jde prbvonus eosphjfbibdt odlnrijtond osstnrbe$
Pb tjab j stbp cjfa usod` E jde `n cjfa tn tgb hjst pnsotond$Jdecb`od tgb invb od j eorbftond pbrpbdeofuhjr tn tgb furrbdt gbjeod`
eorbftond$ Ugos os endb cy rbijodod` od tgb sjib stjtb jde fgjd`od`
tgb gbjeod` eorbftond$
Ugos wohh fjusb tgb rncnt botgbr tn invb cjfa tn cnudejry nr invbjhnd` tgb be`b$
Od tgos stjtb tgb rncnt wohh forfuidjvo`jtb tgb cnudejry nl tgb nc)stjfhb$
Un invb tnwjres tgb `njh(jt snib pnodt cjsbe nd snib prbeblodbefrotbroj rncnt wohh hbjvb tgb djvo`jtond stjtb jde bdtbr tgb intond
0 | : 6
-
7/22/2019 Robotics : Bug Algorithm Simulation
8/13
Intond [hjddod` = Cu` Jh`nrotgi
tnwjres tgb `njh stjtb$
Eollbrbdt cu` jh`nrotgis eollbr od tgb wjy tgby eblodb trjdsotondlrni cnudejry lnhhnwod` tn intond tnwjres `njh$
;$< Voiuhjtond Bdvorndibdt
J cjsof soiuhjtond bdvorndibdt os ebvbhnpbe tn tbst tgb jh`nrotgi$Ugb odput tn tgb soiuhjtond bdvorndibdt os j oij`b lohb fndtjodod`
tgb ncstjfhbs$Ugus ncstjfhbs nl jdy fniphbxoty fjd cb odfhuebe lnrtbstod`$
Ugb sbdsnr jhsn dbbes tn cb soiuhjtbe$Ugus `ovbd j hnfjtond ol j
pnodt hyod` jhnd` tgb forfhb wotg fbdtbr jt rncnt prbsbdt hnfjtondhobs wotgod` tgb ncstjfhb dbbes tn cb ebtbriodbe$Jhsn gnw ljr odsoebtgb ncstjfhb tgb pnodt hobs ebtbriodbs tgb eostjdfb nl tgb rncntlrni tgb ncstjfhb$
Voiuhjtond bdvorndibdt phnts tgb ncstjfhbs(rncnt jde `njh pnsotonds(trjmbftnry(gbjeod` eorbftond(eorbftond jhnd` wgofg ncstjfhbs jrb eb)tbftbe$ Ebcu` odlnrijtond os jhsn eosphjybe jhnd` wotg phnts$
NpbdF\ hocrjrobs jrb usbe lnr phnttod` $Un ebtbriodb ol j pnodt hobsodsoeb j pnhy`nd tgb ludftond prnvoebe cy npbdfv hocrjrobs jrb usbe$
Ugbrb jrb twn typbs nl ncstjfhbs eblodbe cnudejry jde dnrijh nc)stjfhbs$ Ugb cnudejry ncstjfhbs must os cnudejry nl soiuhjtond bdvo)rndibdt$
Ugus lnr cnudejry ncstjfhbs tgb soiuhjtond bdv ebtbriodb ol tgbpnodt nd tgb forfhb hobs nutsoeb tgb cnudejry wgohb nl ntgbr ncstjfhbs
ot fgbfas ol pnodt hobs odsoeb tgb cnudejry$
;$
-
7/22/2019 Robotics : Bug Algorithm Simulation
9/13
-
7/22/2019 Robotics : Bug Algorithm Simulation
10/13
Intond [hjddod` = Cu` Jh`nrotgi
c b s t 9 e 1
c b s t o 9 g b j e o d ` ! o 1
}
}
;$
-
7/22/2019 Robotics : Bug Algorithm Simulation
11/13
Intond [hjddod` = Cu` Jh`nrotgi
r b t u r d 1
. . b d t b r i n t o t o n d t n w j r e s ` n j h s t j t b
i $ v j h u b 9 i $ I n t o n d 1
. . o d o t o j h o z b g b j e o d `
f n i p u t b G b j e o d ` ' / 1
. . f n i p u t b r u d o t v b f t n r s j h n d ` g b j e o d `
l h n j t e x 9 f n s ' g b j e o d ` - [ O . : 7 ; / 1
l h n j t e y 9 s o d ' g b j e o d ` - [ O . : 7 ; / 1
. . u p e j t b t g b p n s o t o n d
p n s o t o n d $ x 9 p n s o t o n d $ x ! 6 - e x 1
p n s o t o n d $ y 9 p n s o t o n d $ y ! 6 - e y 1
p r b v e b h t j 9 L [ n o d t 3 l ' e x ( e y / 1
. . f h b j r t g b s t j f a
g h $ p n p ^ c j f a ' / 1
}
}
Od snib sotujtonds ot ijy gjppbd tgjt Cu`3 jh nrotgi tjabs ynuljrtgbr jwjy lrni tgb tjr`bt( nr ot ijy cb stufa od j hnnp jde rncntdbvbr rbjfgbs tgb tjr`bt
tn jvnoe sufg sotujtonds j fndstrjodt os phjfbe tgjt tgb hbjvb pnodtsgnuhe cb fhnsbr tn tgb `njh tgjd tgb got pnodt$
Hbt tgb eostjdfb cbtwbbd snurfb tn `njh cb ebdntbe cy E Hbt tgbpbroibtbr nl tgb o 0 t g ncstjfhb cb ebdntbe cy [
o
Ugb sgnrtbst eostjdfbSncnt wohh trjvbh usod` cu`3 jh`nrotgi os E $
Jdntgbr vjrojtond os tn fgbfa ol tgbrb jrb ncstjfhbs jhnd` tgb eo)rbftond tn tgb `njh$ Ugos dn ncstjfhbs jrb ebtbftbe tgbd fgjd`b tgbstjtb tn intond tnwjres tgb `njh$ Ugb lrbbgbjeod` lhj` os sbt ol
tgbrb jrb dn ncstjfhbs od tgb eorbftond nl tgb `njh$ Gnwbvbr sodfbtgb rjd`b.tjftohb sbdsnr gjvb hoiotbe rjd`b (j lrbb gbjeod` enbs dntdbfbssjrohy odeofjtb j lrbb gbjeod` tnwjres tgb `njh$
Jeeotondjh fndstrjodts sufg tgjt pntbdtojh hbjvb pnodt iust cb fhnsbtn tgb `njh tgjt tgb prbvonus got pnodt jde nd gbjeod` jd`hb jrboipnsbe tn bdsurb cnudejry lnhhnwod` os bxotbe jt prnpbr pnsotond$
l r b b g b j e o d ` 9 l j h s b 1
V f j d [ n o d t p p 1
l n r ' o d t a 9 ; 1 a 8 s f j d $ s o z b ' / 1 a ! ! /
{
V f j d [ n o d t p 9 s f j d Q a Y 1
l h n j t g : 9 p $ o d e b x ! g b j e o d ` 1
: : | : 6
-
7/22/2019 Robotics : Bug Algorithm Simulation
12/13
Intond [hjddod` = Cu` Jh`nrotgi
o l ' g : 8 9 ) : 7 ; /
g : 9 g : ! 6 < ; 1
o l ' g : 4 : 7 ; /
g : 9 g : ) 6 < ; 1
g : 9 ' o d t / g : " 6 < ; 1
o l ' g : 9 9 f u r ^ g b j e o d ` , , p $ e o s t j d f b 9 9 ) : /
{
l r b b g b j e o d ` 9 t r u b 1
p p 9 p 1
}
}
o l ' l r b b g b j e o d ` 9 9 t r u b /
{
. . f v = = w j o t A b y ' ; / 1
l h n j t e o s t j d f b : 9 L [ n o d t = = e o s t ' g h Q ; Y ( ` n j h $ p n s o t o n d / 1
l h n j t e o s t j d f b 3 9 L [ n o d t = = e o s t ' ` n j h $ p n s o t o n d ( p p $ p n s o t o n d / 1
o l ' e o s t j d f b : 4 e o s t j d f b 3 , , g b j e o d ` 8 f u r ^ g b j e o d ` ) 6 ; /
{
i $ v j h u b 9 i $ I n t o n d 1
f n i p u t b G b j e o d ` ' / 1
l h n j t e x 9 f n s ' g b j e o d ` - [ O . : 7 ; / 1
l h n j t e y 9 s o d ' g b j e o d ` - [ O . : 7 ; / 1
. . u p e j t b t g b p n s o t o n d
p n s o t o n d $ x 9 p n s o t o n d $ x ! : - e x 1
p n s o t o n d $ y 9 p n s o t o n d $ y ! : - e y 1
p r b v e b h t j 9 V f j d [ n o d t ' L [ n o d t 3 l ' e x ( e y / ( ) : ( ; / 1
. . f b r r 8 8 # = # 8 8 f u r ^ g b j e o d ` 8 8 # = # 8 8 g b j e o d ` 8 8 b d e h 1
g h $ p n p ^ c j f a ' / 1
}
}
;$
-
7/22/2019 Robotics : Bug Algorithm Simulation
13/13
Intond [hjddod` = Cu` Jh`nrotgi
d
o
$
E !
W
o
d
o
[
o
3
' 5 /
;$0 Voiuhjtond
Ugb nutput soiuhjtond bdvorndibdts fjd cb lnude jt
g t t p s = . . w w w $ y n u t u c b $ f n i . w j t f g > v 9 J 5 I E G 5 p N C 3 a , h o s t 9 [ H ` a ` j 0 O : [ r W A V B U U g N [ E [ [ B z E M p M 6 h r \ 7
;$7 Fneb
Ugb fneb lnr tgb tbstod` jde trjodod` utohoty fjd cb lnude jt g t t p s = . . ` o t g u c $ f n i . p o : ? 5 ; 5 . i : ? 5 ; 5 . t r b b . i j s t b r . r n c n t . d j v o ` j t o n d
: 6 | : 6
https://github.com/pi19404/m19404/tree/master/robot/navigationhttps://github.com/pi19404/m19404/tree/master/robot/navigationhttps://www.youtube.com/watch?%20v=A4MDH4pOB2k&list=PLgkga7I1PrXKSETThOPDPPEzDJpJ3lrV8