Optimal Navigation for a Differential Drive Disc Robot: A Game Against the Polygonal Environment

Transcription

1 Noname manuscrip No. (will be insered by he edior) Opimal Navigaion for a Differenial Drive Disc Robo: A Game Agains he Polygonal Environmen Rigobero Lopez-Padilla, Rafael Murriea-Cid, Israel Becerra, Guillermo Laguna and Seven M. LaValle he dae of receip and accepance should be insered laer Absrac This paper considers he problem of globally opimal navigaion wih respec o minimizing Euclidean disance raveled by a disc-shaped, differenial-drive robo (DDR) o reach a landmark. The robo is equipped wih a gap sensor, which indicaes deph disconinuiies and allows he robo o move oward hem. In his work we assume ha a opological represenaion of he environmen called GNT has already been buil, and ha he landmark has been encoded in he GNT. A moion sraegy is presened ha opimally navigaes he robo o any landmark in he environmen, wihou he need of using a previously known geomeric map of he environmen. To our knowledge his is he firs ime ha he shores pah for a DDR (underacuaed sysem) is found in he presence of obsacle consrains wihou knowing he complee geomeric represenaion of he environmen. The robo s planner or navigaion sraegy is modeled as a Moore Finie Sae Machine (FSM). This FSM includes a sensor- A preliminary version of porions of his work has been presened a he Tenh Inernaional Workshop on he Algorihmic Foundaions of Roboics, WAFR 2012 [25]. This work was suppored in par by NSF grans (IIS Roboics) and (Cyberphysical Sysems), DARPA SToMP gran HR , and MURI/ONR gran N Rigobero Lopez-Padilla Cenro de Innovación Aplicada en Tecnologías Compeiivas, CIATEC, León, México rlopez@ciaec.mx Rafael Murriea-Cid and Israel Becerra Cenro de Invesigación en Maemáicas (CIMAT), Guanajuao, México {murriea,israelb}@cima.mx Guillermo Laguna Iowa Sae Universiy, Ames, IA, USA glaguna@iasae.edu Seven M. LaValle Universiy of Illinois a Urbana-Champaign, Urbana, IL, USA lavalle@illinois.edu feedback moion policy. The moion policy is based on he paradigm of avoiding he sae esimaion o carry ou wo consecuive mappings, ha is, from observaion o sae and hen from sae o conrol, bu insead of ha, here is a direc mapping from observaion o conrol. Opimaliy is proved and he mehod is illusraed in simulaion. Keywords combinaorial filers opimal navigaion underacuaed sysem environmen consrains feedback moion sraegies 1 Inroducion This paper considers he problem of globally opimal navigaion wih respec o minimizing Euclidean disance raveled by a disc-shaped, differenial-drive underacuaed robo o reach a landmark in an environmen wih obsacle consrains. The robo observes he world mainly using a gap sensor, inroduced in [39], which allows i o deermine he direcions of disconinuiies in deph (disance o he boundary of he environmen) and move oward any one of hose direcions. Under his model, bu for a poin robo, a combinaorial filer called he Gap Navigaion Tree (GNT) was inroduced in [38,39], i encodes precisely he par of he shores-pah visibiliy graph ha is needed for opimal navigaion. The GNT can also be considered a opological map. A opological map can be represened as a graph, in which he verices depic paricular sensor readings and configuraions and he edges refer o he conrols beween wo differen configuraions. The GNT differs from previous approaches in ha i is a local represenaion, defined for he curren posiion of he robo, raher han a global one. The learned daa srucure corresponds exacly o he shores pah ree [12] from he robo s locaion. This enables he

2 Biangen complemen Biangen complemen Biangen Biangen Biangen complemen (a) (b) Biangen complemen g L 5 g R 4 g L 3 Fig. 2 biangens and biangen complemens (c) Fig. 1 a) The opimal pah for a poin robo, b) The opimal pah for a disc robo, c) The gap sensor (aached a he small solid disc on he robo s boundary) deecs he sequence of gaps G=[g R 1,gL 2,gL 3,gR 4,gL 5 ], in which g R 1 and gr 4 are righ (near-o-far) gaps and gl 2, gl 3, and gl 5 are lef (far-o-near) gaps. robo o navigae o any previously seen landmark by following he shores-disance pah, even hough i canno direcly measure disances. This modeling avoids building a geomeric map of he environmen. Furhermore, he robo does no need o localize iself wih respec o a global reference frame. The GNT was exended and applied o exploraion in [30]. The GNT was exended o poin cloud models in [19]. A larger family of gap sensors is described in [21]. The case of a disc robo is imporan because real robos have nonzero widh. I is herefore ineresing o sudy he case of a disc robo, which could correspond, for example, o a Roomba plaform. Unforunaely, he problem is considerably more challenging because wihou addiional sensing informaion, he robo could accidenally srike obsacles ha poke ino is swep region as i moves along a biangen. See Fig. 1 (a) and (b). In navigaion, he robo mus insead execue deours from he biangen. Sensing, characerizing, and opimally navigaing around hese obsrucions is one of he conribuions of his paper. The mehod for generaing opimal navigaion moions is modeled as a Moore Finie Sae Machine (FSM). Fig. 2 shows biangens and biangen complemens, following he presenaion of [39], a biangen is idenified wih conneced open ses I E and J E, where E is he boundary of he polygonal environmen. A pair of disjoin conneced open ses I and J idenify a biangen if a leas one poin of I is visible o one poin of J and if here is a line L ha pariions I and J ino ses I 1, I 2 and I 3, and J 1, J 2 and J 3 respecively, such ha: 1) I 1 is an open se ha does no inersec L (he same for J 1 ); 2) I 2 is a closed subse of L (he same for J 2 ); and 3) I 3 is an open se ha does no inersec g R 1 g L 2 L and ha lies on he same side of L from I 1 (he same for J 3 and J 1 ). I is imporan o keep in mind ha he robo is placed ino an environmen wih obsacles, bu i is no given he obsacle locaions or is own locaion and orienaion. The robo observes his informaion over local reference frames. The purpose is o show how opimal navigaion is surprisingly possible wihou a geomeric map. However, in his paper, we assume ha he GNT represening he environmen has been buil and ha a landmark is encoded in he GNT. Eiher he mehod for building he GNT wih a poin robo [38,39] or wih a disc robo [18] can be used. The works in [38,39] presened he original GNT, a mehod o explore he environmen and o encode a landmark in i (o come back laer o he landmark) have also been presened. Since he robo was assumed o be a poin robo, hen any visible poin in he environmen was also reachable by he robo. In [18], he robo is assumed o be a disc, hence, even if he robo can see cerain place wihin he environmen, ha place migh no be reachable for he robo. The exploraion problem addressed in [18] is more challenging han he case of a poin robo because visibiliy informaion does no provide collision free pahs in he configuraion space. The mehod proposed in [18] guaranees exploring he whole environmen or he larges possible region of i and he disc robo is able o find a landmark and encode i in he GNT or declare han an exploraion sraegy for his objecive does no exis. Noe ha differenly o [18] where he main problem is o explore he environmen and consruc he GNT encoding i, in his work he problem is o opimally navigae oward he landmark. Once he GNT has been buil, he approach proposed in his paper is able o deermine wheher or no a collision free pah exiss o reach he landmark and if a pah exiss, i proposes he moion conrols o reach he landmark. 1.1 Relaed work and main conribuions Our work is relaed o he problem of planning robo s pahs ha avoid collision wih obsacles [16, 6, 27], and paricularly wih underacuaed nonholonomic robos [20, 3, 15]. Our problem is also relaed o he problem of finding opimal

3 pahs for nonholonomic robos [2, 33, 40]. The sudy of opimal pahs for non-holonomic sysems has also been an acive research opic. Reeds and Shepp deermined he shores pahs in an environmen wihou obsacles for a car-like robo ha can move forward and backward [31]. In [33], a complee characerizaion of he shores pahs for a carlike robo is given for an environmen wihou obsacles. In [2], Balkcom and Mason deermined he ime-opimal rajecories for a Differenial Drive Robo (DDR) using Ponryagin s Maximum Principle (PMP) and geomeric analysis also for an environmen wihou obsacles. In his work, we presen a moion sraegy ha minimizes he Euclidean disance raveled by a disc-shaped, differenial-drive underracuaed robo o reach a landmark, in a simply conneced polygonal environmen. The approaches described above o find shores pahs wih nonholonomic sysems assume ha he robo moves in an environmen wihou obsacles [33, 2], in his work he robo moves in an environmen wih obsacle consrains and he robo does no have a geomeric map of he environmen as in [20,15] o avoid collision wih he obsacles, he robo discover he obsacles wih is sensors. In [1], he auhors sudy he problem of finding he shores pah for a poin robo, beween wo configuraions (posiion and orienaion) in a convex polygon. The robo pah is consrained o have curvaure a mos 1. The auhors propose an algorihm for deermining wheher a collision-free pah exiss for he poin robo beween wo given configuraions. If such a pah exiss, he algorihm reurns a shores one. In his work, we find he shores pah for a disc shaped differenial drive robo DDR (a nonholonomic under-acuaed sysem, bu ha is able o roae in place) for any simple conneced polygonal environmen (convex or no) and wihou having he exac geomeric map of he environmen. Compuing shores pahs for a poin robo, wihou nonholonomic consrains, placed ino a known polygonal region is sraighforward. The mos common approach is o compue a visibiliy graph ha includes only biangen edges, which is accomplished in O(n 2 lgn) ime by a radial sweeping algorihm [10] (an O(nlgn+m) algorihm also exiss, in which m is he number of biangens [13]). An alernaive is he coninuous Dijksra mehod, which combinaorially propagaes a wavefron hrough he region and deermines he shores pah in O(nlgn) ime. Numerous problem variaions and resuls exis. Compuing shores pahs in hree-dimensional polyhedral regions is NP-hard [7]. Allowing coss o vary over regions considerably complicaes he problem [29,32]. See [12,28] for surveys of shores pah algorihms. For recen effors on curved obsacles, see [8]. Once he robo has nonrivial dimensions, he problem can be expressed in erms of configuraion space obsacles. Soluions for finding shores pahs assuming ha he map is known, are presened in [9,24]. Given srong sensors and good odomery, sandard SLAM approaches [11, 37] could be applied o obain a geomeric map. Once a polygonal map is obained, an alernaive o find he shores pah for a disc robo is o expand he obsacles by he robo radius and reason over his exended polygonal map. Bu ha approach requires o buil firs a precise polygonal map of he environmen, which is a hard problem. One main advanage of he approach presened in his work is ha i does no require ha polygonal map. Even if he complee polygonal map is available and he obsacle expansion is done, he resuling configuraion space represenaion is no observable or measurable direcly by he robo sensors. Oher main advanage of he approach proposed in his paper is ha informaion from he workspace is obained direcly from he robo s sensors, o infer he opimal robo pahs in he configuraion space. Oher ype of environmen s represenaions are he opological maps in he form of graphs [36,39,17,4]. In hese graphs he nodes represen environmen places and he edges represen adjacency. The problem of exploring an unknown environmen for searching of one or more recognizable arges is considered in [36]. In [17], he auhors propose a surveillance graph o model surveillance asks performed by eams of robos having limied sensing capabiliies. A surveillance graph is auomaically exraced from occupancy grid maps. The work in [17] represens an effor o close a loop beween a graph-based heoreical formulaion and pracical scenarios. In [4], he auhors sudy he problem of deermining he minimal informaion required by a robo o reconsruc he visibiliy graph of an iniially unknown polygon. The auhors allow a robo o collec sensory inpu while i is locaed a a verex. Verices are no idenified globally, he verex can be disinguished only in a local sense by a relaive posiion. The GNT has been used in several oher works (e.g. [38, 39],[14], [30] and [18]). In he presened work, we sill use he GNT o esablish conneciviy in he workspace beween he robo and he landmark. However, noe ha we presen new conribuions ha are independen of he GNT (see below). Considering he problem of globally opimal navigaion for a disc robo, various objecive funcions are possible, for insance ime or energy spen by he robo. However, we choose o minimize he disance raveled by he cener of he robo because i urns ou ha such a crierion esablishes he exisence of a collision free pah for he wors scenario, ha is, he environmens wih he mos narrow passage, such ha, his passage is wider han he robo s diameer. Hence, he problem addressed in his paper can be seen as a game [23] agains he polygonal environmen. The raionale behind his resul is as following: our moion sraegy moves he cener of he robo as less as possible, in oher words he volume of he space ha he robo sweeps as i moves,

4 is as small as possible, hence he proposed moion sraegy requires he smalles free space region o move he robo. The main conribuions of his work are heoreical and have no been presened in our relaed previous works [38, 39,18]; we consider ha hey are he following: 1. The original GNT guaranees ha a poin robo will ravel he shores Euclidian disance pah o a landmark. In his work, we exend he work o find he shores pah for a disc robo, beween any iniial robo configuraion and he landmark. The GNT is used for esablishing conneciviy beween a poin robo and he landmark, bu noe ha a disc robo could srike obsacles ha poke ino is swep region as i moves along a biangen. In navigaion, he disc robo mus execue deours from he biangen (see Figure 1). In his work, we presen a Finie Sae Machine ha commands he robo o execue hose deours direcly maping observaions ino conrols and yielding he shores pah for a disc shaped robo. 2. We consider a differenial drive robo (DDR) nonholonomic under acuaed sysem. In he original GNT, nonholonomic consrains over he roboic plaform have no been considered. 3. We have shown, ha he crierion ha we propose o opimize, ha is minimizing he Euclidean disance raveled by he cener of he robo, gives as a consequence he exisence of a geomeric soluion for he navigaion problem of finding a pah owards he landmark. Hence if he proposed moion sraegy does no find a collision free pah o he landmark hen such pah does no exis. The conen of he paper is organized as follows: Secion 2 formally presens he problem saemen, and he robo s model including moion and sensing capabiliies. Secion 3 presens an observaion vecor ha is used o decide he robo acion. Secion 4 describes he FSM ha is used for generaing opimal robo navigaion. Secion 5 presens a feedback moion policy ha maps observaions o robo s commands. Secion 6 argues he opimaliy of he moion sraegy. Secion 7 presens an implemenaion in simulaion, and Secion 8 concludes he paper. 2 Problem saemen The robo is a differenial drive (underacuaed) sysem having a defined forward heading. The robo has he shape of a disc wih radius r moving in a planar and polygonal environmen which could be any compac se E R 2 for which he inerior of E is simply conneced. The boundary E of E is he image of a piecewise-analyic closed curve. However, i is assumed ha he collision-free subse of he robo s configuraion space is simply conneced or i migh have several conneced componens. C-space obsacle projeced in he plane corresponds o ha of a ranslaing disc, ha is, he exended boundary of E which is due o he robo radius 1. Le Λ be a saic landmark locaed in he environmen having he shape of a disc wih he same radius as he robo. Le assume ha Λ is pained on he ground, hence i does no have volume and i does no produce disance disconinuiies (gaps). This assumpion is made since he robo has as is goal o park on he landmark. In his work we assume ha a GNT represening he environmen has already been buil, and ha he landmark has been encoded in he GNT. One objecive of his paper is o deermine wheher or no a collision free pah oward he landmark Λ exiss, if a pah exiss hen a second objecive is o deermine commands o ravel he shores pah oward he landmark in finie ime. 2.1 The GNT The GNT is an efficien daa srucure ha dynamically changes according o some criical evens unil he whole environmen has been discovered. The GNT can be consruced incremenally as he robo moves along a pah τ. Iniially, he GNT consiss of a roo node ha is conneced o one leaf node for every gap in G(τ(0)). Each ime a which a change in G(τ()) occurs corresponds o a criical even. This requires updaing he GNT. There are four differen kinds of criical evens: A new gap g appears: A node g is added as a child of he roo, while preserving he cyclic ordering from he gap sensor (see Fig. 3 (a)). For a descripion of he gap sensor see Secion 2.2. A gap g disappears: The node g, which mus be a leaf, is removed (see Fig. 3 (b)). Gaps g 1 and g 2 merge ino g: Nodes g 1 and g 2 become children of a new node, g, which is added as a child of he roo and preserving he ordering of gaps (see Fig. 3 (c)). Gap g 1 splis ino g 2 and g 3 : If g 1 is a leaf node, hen g 2 and g 3 become new nodes;oherwise, hey already exis as children of g 1. Boh g 2 and g 3 are conneced o he roo, preserving he ordering of gaps and removing g 1 (see Fig. 3 (d)). If any leaf verex has he poenial o spli, hen he GNT is incomplee because i could expand, some gaps spli and oher gaps simply disappear. The gaps ha disappear are called primiive (heir corresponding nodes in he GNT are also called primiive). If all he leaf nodes of he GNT are primiive, hen he GNT is said o be complee. Indeed, he following Lemma in [39] guaranees he erminaion of he GNT s consrucion. 1 Noe ha his is he configuraion space for a ranslaing disc raher han for a rigid body because of roaional symmery.

5 robo gap robo gap gap (a) g Fig. 4 A landmark encoded in a node of he GNT. (b) g Theorem 1 The exended GNT encodes a pah o any objec or landmark in he environmen from he curren posiion of he robo. The proof is presened in [39]. 2.2 Sensing model (c) (d) g 1 g 2 g g 1 g 2 g g 1 g 2 g 1 g 2 Fig. 3 Criical evens: (a) Gap appears, (b) Gap disappears, (c) Gap merge, (d) Gap splis. Lemma 1 The procedure of ieraively chasing non-primiive leaves erminaes wih a resuling complee GNT. For he proof please refer o [39]. The GNT can be exended o encode landmarks in i. If a landmark disappears behind a gap g, hen i is added o he GNT in he node corresponding o g (see Fig. 4). The robo can reurn o any previously visible landmark by raveling o he node g unil he landmark is visible. The nex heorem saes ha once compleely consruced, he exended GNT can be used for navigaion from he curren posiion of he robo o any landmark in he environmen. The robo has an omnidirecional sensor, which is used o sense he environmen. The omnidirecional sensor is also able o deec and rack disconinuiies in deph informaion (gaps). Hence, over he omnidirecional sensor, i is possible o build a gap deecor, furher referred as he gap sensor. The robo uses a side sensor ha is a laser poiner use o measure disance in a paricular direcion. The robo is also equipped wih a conac sensor ha is able o ell he robo when i is in conac wih he environmen in paricular poins over he robo boundary. Finally he robo has a landmark deecor sensor ha ells he robo ha i has reached he landmark. The sensor model is minimalis in he sense ha he navigaion sraegy described in Secion 4 makes use of informaion provided by hese four sensors and if any componen is aken away hen he robo will fail o solve is ask. On he oher hand, i is ineresing o noice ha navigaion sraegy only needs he informaion provided by hose sensors, so any componen more powerful is unnecessary. Furhermore, he sensing componens are sandard in he lieraure and can be implemened using exising echnology. For insance using laser range finders [39], laser poiners [26], acile bumpers [5] and for deecing he landmark a color [5] or line deecors [35]. Below we describe in more deail he sensing requiremens. 1) Gap sensor: The omnidirecional sensor [21, 39] is able o deec and rack wo ypes of disconinuiies in deph informaion (considering a counerclockwise direcion along E): disconinuiies from far o near and disconinuiies from near o far (see Fig. 1(c)). Le G=[g 1,...,g k ] denoe he circular sequence of gaps observed by he sensor. Using his noaion, represens he disconinuiy ype, in which = R refers o a righ gap where he hidden environmen s porion

6 is o he righ (a disconinuiy from near o far), and = L refers o a lef gap where he hidden environmen s porion is o he lef (a disconinuiy from far o near). For example, he gap sensor in Fig. 1(c) deecs gaps of differen ypes: G=[g R 1,gL 2,gL 3,gR 4,gL 5 ]. We place he gap sensor on he robo boundary and define moion primiives ha during navigaion send he robo on collision-free rajecories ha possibly conac he obsacles (moving along he boundary of he free subse of he configuraion space, ha is semi-free rajecories, is necessary for opimal pahs). These moion primiives, described in deail in Secion 2.4, allow he robo o roae iself so ha i is aligned wih a desired gap, o move forward while chasing a gap, and o follow E while he sensor is aligned o a gap. I is assumed ha he gap sensor can be moved o wo differen fixed posiions on he robo s boundary: he exremal lef and righ sides wih respec o he forward wheel direcion. One way o implemen his is wih a urre ha allows he robo o move he gap sensor from is righ side o is lef side and vice-versa. Fig. 6(c) shows he sensor aligned o a righ gap in which he gap sensor is on he righ side of he robo. To align he sensor o a lef gap, he robo moves he gap sensor o he lef side of he robo. The omnidirecional sensor is able o measure disance and angles o he verices ha generae gaps. Le d u be he disance beween he omnidirecional sensor and he verex u i ha originaed he gap g i (in Fig. 5(d) g i = g R 0 ). 2) Side sensors: To deec obsacles ha obsruc he robo, our mehod needs o measure disances beween he exremal lef and righ side robo s poins along he direcion of he robo heading (forward) and he obsacles. Le hose paricular robo poins be lef side poin lp and righ side poin rp. The paricular forward direcion angen o he robo s boundary a rp is called r. The paricular forward direcion angen o he robo boundary a lp is called l (see Fig. 5(a)). Noe ha based on disance he disconinuiies can be deeced. Le d R be he disance beween rp and he obsacles a he paricular direcion r, and d L be he disance beween lp and he obsacles a he paricular direcion l (see Fig. 5(b)). If he paricular direcion, eiher r or l, is poining o a reflex verex 2 (a gap is aligned wih his direcion), hen a disconinuiy in he sensor reading a his direcion occurs. Le d R denoe he disance from rp o he closer poin along he disconinuiy direcion in he boundary of environmen E. Similarly, d L denoes he disance from lp. See Fig. 5(c). Sensor errors are no considered in his work. However, noe ha our moion sraegies will require only comparisons of disances o deermine which is larger, raher han needing precise disance measuremens. π. 2 A reflex verex is a polygon verex of an inernal angle greaer han Laser poiner lp rp l direcion Robo heading, forward direcion Omnidirecional r direcion sensor (a) Laser poiner lp d L rp d R Omnidirecional sensor (c) Laser poiner Laser poiner lp rp d L d R Omnidirecional sensor (b) Laser poiner lp d u rp Omnidirecional sensor (d) Omnidirecional sensor Paricular omnidirecional sensor reading (e) g R 0 Fig. 5 Side Sensors: (a) Poins rp and l p, and direcions r and l, (b) d L and d R, (c) d L, (d) d u, (e) Omnidirecional sensor readings for conac deecion. 3) Conac sensors: Our approach requires deecing wheher he robo is conacing E a rp or lp. 4) Landmark reached sensor: Our approach also requires a sensor ha deecs ha he landmark has been reached. Disance measuremens beween he obsacles and rp and lp in direcions r and l (forward), and he informaion of wheher he robo is ouching E a rp or lp, can be obained wih differen sensor configuraions. For example, i is possible o use wo laser poiners and wo conac sensors, each of hem locaed a rp and lp. However, o use a smaller number of sensors and faciliae he insrumenaion of he roboic sysem, i is possible o emulae boh he conac sensors and one of he laser poiners, using he omnidirecional sensor. The omnidirecional sensor reading in he paricular forward robo heading direcion emulaes he laser poiner reading. The sensor readings a direcions perpendicular o he robo heading are used in his case. If he robo is ouching E a he poin a which he omnidirecional sensor is locaed, hen he sensor reading is zero. If robo is ouching E a he poin diamerically opposed o he omnidirecional sensor, hen he sensor reading will correspond o he robo diameer (see Fig. 5(e)). Thus, one opion is o have he robo equipped wih an omnidirecional sensor and a laser poiner; hey will be locaed a lp and rp. Recall ha a urre can be used o swap he locaions of he laser poiner and he omnidirecional sensor o avoid unnecessary robo roaions in place. The urre moves on he boundary of he robo. The gaps or landmarks are always chased wih he omnidirecional sensor; he laser poiner is used o help o deec obsacles (see below). Also o deec ha he landmark has been reached several differen hardware implemenaion migh be used, for insance using he omnidirecional sensor.

7 An observaion vecor is obained wih he robo s sensors, his vecor is used o decide he robo acion. In Secion 3 his observaion vecor is described in deail. g R 0 g R 0 (a) (b) 2.3 Landmark encoding In [18], he landmark is said o be recognized if Λ is visible a leas parially from he locaion of he omnidirecional sensor. In [18], if he landmark is oally visible (fully conained in he visibiliy polygon V(q)) from he omnidirecional sensor locaion hen he landmark is encoded in he GNT as a node child of he roo. If he landmark is oally or parially occluded from he omnidirecional sensor locaion hen he landmark is encoded as a node child of he node represening a gap in he GNT [39]. Le rp Λ be an exremal poin on he landmark such ha whenever r is aligned o rp Λ he body of he landmark is o lef of paricular direcion r. There is an analogous definiion for poin lp Λ. If a reflex verex occludes poin rp Λ hen he Λ is encoded wih he gap generaed by he verex. Similarly, if a reflex verex occludes poin lp Λ hen i is also encoded wih he gap generaed by he verex. This is he encoding used in [18], hence he landmark Λ can be encoded a mos wih wo gaps. The navigaion sraegy proposed in his paper is able o find he pah ha minimizes he Euclidian disance o reach he landmark. 2.4 Moion model The robo navigaes using a sequence of moion primiives ha are generaed by an auomaon for which sae ransiions are induced by sensor feedback alone. To navigae, a gap (or equivalenly he verex ha generaes i) or a landmark is given o he robo as goal. There are five moion primiives (see Fig. 6). Le he angular velociy of he righ and lef wheels be w r and w l, respecively, wih w r,w l { 1, 0, 1}. Thus, he moion primiives are generaed by he following conrols: Clockwise roaion in place: w r = 1,w l = 1. Counerclockwise roaion in place: w r = 1,w l = 1. Clockwise roaion w.r.. o poin rp: w r = 0,w l = 1. Counerclockwise roaion w.r.. o poin lp: w r = 1,w l = 0 Forward sraigh line moion: w r = 1,w l = 1. In his work, we propose he simple conrols described above, which produce he hree needed moion primiives (sraigh line moion, roaion in place and roaion w.r.. poin rp or lp) ha yield he shores pah in erms of he Euclidian disance. The roaion primiives are used o align r or l o a specific gap (or landmark). Once r or l is aligned (c) g R 0 (e) g R 0 g0 R Fig. 6 The moion primiives: (a) Clockwise roaion in place, (b) Counerclockwise roaion in place, (c) Sraigh line moion, (d) Clockwise roaion w.r.. o poin rp, (e) Counerclockwise roaion w.r.. o poin l p. o a gap, he robo moves in a sraigh line o chase he gap. If he pah o he chosen gap is blocked, hen he robo execues a deour by choosing a new verex as a subgoal. More deails are given in Secion 4. I would be ineresing o consider some suiable properies of he robo conrols. Since he opimizaion crierion is he Euclidian disance and no ime, hen o preserve opimaliy is no mandaory o ravel a sauraed maximal speed. This gives flexibiliy in how o execue he moion primiives and concaenae hem. We leave for fuure work o include suiable properies over he robo conrols. For insance, o include smooh ransiions beween conrols or o propose conrol laws able o deal wih noise. 3 Observaion vecor The GNT encodes he shores pah o any place in he environmen for a poin robo, i also encodes he robo s goal. However, for a disc robo his informaion is no enough for opimal navigaion. The observaion vecor provides he informaion ha he robo needs o make decisions abou he acions yielding opimal moion. Firs, some useful definiion are presened. Definiion 1 The gap ha encodes he pah oward he landmark in he GNT is called a goal gap g goal. I is encoded in a node child of he roo in he GNT. Definiion 2 A goal verex u goal is he verex ha generaes he goal gap. A goal verex is visible o he omnidirecional sensor. (d)

8 Definiion 3 A sub-goal verex is he nex verex o be visied in he opimal pah for a disc robo, when a deour o he goal verex is required. Definiion 4 A verex ha generaes a righ gap, is called a righ verex. Definiion 5 A verex ha generaes a lef gap, is called a lef verex. We will also refer o a candidae verex, which is a verex ha migh become a sub-goal verex. The precise condiions ha define a candidae verex are given in definiion 6. The observaion vecor yn i has 14 binary observaions, in he lis below a descripion of each one is presened. Some of hem are more complex and hey are described wih more deail. 1. FI: he landmark Λ is reached (1) or no (0). 2. LV: he landmark is oally visible from he omnidirecional sensor locaion (1) or no (0). 3. FR: he robo is aligned by he firs ime o a sub-goal verex (1) oherwise (0). 4. RP: he robo is ouching E wih poin rp (1) or no (0). 5. LP: he robo is ouching E wih poin lp (1) or no (0). 6. VR: he verices generaing righ gaps have been locaed over a local reference frame (1) or no (0). 7. VL: he verices generaing lef gaps have been locaed over a local reference frame (1) or no (0). 8. AL: he robo is aligned o a given verex or o a landmark (1) or no (0). 9. BL: here is a blockage oward a given verex or a landmark (1) oherwise (0). 10. UN: he goal gap has merged (1) or no (0). 11. GT: he ype of goal gap, righ (1) or lef (0). 12. CT: he ype of he candidae verex, righ (1) or lef (0). 13. O1: sensor locaion (wo bis are needed o esablish he sensor locaion). 14. O2: sensor locaion. VL and VR: To find a candidae verex, i is necessary o locae he verices in local reference frames. There are four ypes of local reference frames. Two of hem are jus he symmeric cases of he oher wo. We describe he wo basic local reference frames in Appendices A and B. VR = 1 when he locaions of righ verices has been compued, oherwise VR=0. VL = 1 when he locaions of lef verices has been compued, oherwise VL=0. Robo alignmen AL: The robo migh be aligned o a given verex or o a landmark. The robo aligns direcion r o a righ verex. Symmerically, he robo aligns direcion l o a lef verex. To chase a landmark, he robo aligns l wih lp Λ or r wih rp Λ. Blockage BL: This bi is (1) if here is a blockage oward a given verex or a landmark and (0) oherwise. A given verex or landmark is blocked if he robo canno ravel in sraigh line oward i. A formal definiion for blockage condiions is given in Secion 4. There are four ways o deec a blockage: 1. The firs one akes places when he robo is aligned o a verex. To deec he blockage, disances d L, d R, d L and d R are used. If direcion r is aligned o a righ verex and d R > d L hen a sraigh line robo pah oward his verex is blocked. Likewise, if direcion l is aligned o a lef verex and d L > d R hen a sraigh line robo pah oward ha verex is blocked. 2. The second way o deec a blockage is used when he robo is no aligned o a given verex. I migh happen ha he robo canno align eiher r or l o he goal verex, because his moion will produce an unnecessary robo ranslaion, and he global opimaliy would be los. In Fig. 7, if he robo roaes counerclockwise wih respec o rp hen he cener of he robo will move backward and opimaliy would be los. Whenever he robo is ouching a verex wih poin rp o align r wih he goal verex u goal or landmark, he maximal angle measured in clockwise sense from r o he goal verex can never be larger han π, hus, if his angle is larger han π a blockage is deeced, as a robo collision necessarily occurs (robo ouches E wih a poin differen o rp). g L 4 u 4 Fig. 7 Blockage ype 2 lp rp u3 l r g L 2 u goal g R goal There is a symmeric blockage deecion whenever he robo is ouching a verex wih poin lp. 3. The hird way o deec a blockage requires several measuremens, his deecion ype is equivalen o fulfill he blockage condiion defined in Secion For he fourh way o deec a blockage, he robo is aligned o a landmark and he landmark is oally visible from he omnidirecional sensor locaion. The disance from he omnidirecional sensor o he landmark canno be sensed. Consequenly, he pah o reach he landmark is declared as blocked, since a urre moion mus be execued o esablish wheher or no he landmark can be reached wihou visiing firs a verex. Thus, a Landmark

9 blockage is deeced using visibiliy. If he landmark is oally visible from he omnidirecional sensor locaed a boh poins rp and lp hen he pah oward a landmark is no blocked. Case 2-R and Case II-RP (see subsecion 4.2) presen he procedures for alignmen wih a landmark. Sensor Locaion O1 and O2: We use wo bis o esablish he omnidirecional sensor locaion. O1=0 and O2=0 esablishes ha he omnidirecional sensor is a l p, O1=0 and O2=1 esablishes ha he omnidirecional sensor moves from lp o rp, O1=1 and O2=0 esablishes ha he omnidirecional sensor is a rp, and O1=1 and O2=1 esablishes ha he omnidirecional sensor moves from rp o lp. 4 Finie Sae Machine for Opimal Navigaion The robo navigaes oward a landmark by moving oward gaps based on he GNT. The GNT in [39] was designed for a poin robo, for a disc robo, he robo mus execue deours from he biangen lines beween verices. A he end, he navigaion consiss in visiing goal verices. However, he sraigh line pah o a goal verex migh be blocked, in such a case a sub-goal verex is found. The verex ha generaes he goal gap, which is encoded in he GNT, is always visied. For his reason, we call he modified pah a deour. A deour sars whenever a goal-verex is blocked and i ends when he sub-goal verex is reached. In his secion a Finie Sae Machine (FSM) for opimal navigaion is provided. A main objecive of his FSM is o find he sub-goal verex. Once he sub-goal verex is found, he robo roaes o ge aligned wih ha verex and hen i moves in sraigh line o reach i. There are wo ypes of roaion: roaion in place or roaion wih respec o a poin (rp or lp). To find a sub-goal verex, he robo deermines candidae verices. The selecion of a sub-goal verex from a se of candidae verices is slighly differen for a roaion in place and for a roaion wih respec o poin rp or lp which is furher described in he nex subsecions. The definiion of candidae verices is given below. This definiion is recursive and makes use of he local frames F, search domains, and θ angles defined in Appendix A (roaions in place) and Appendix B (roaions wih respec o an exremal poin), which are used o verify raversabiliy oward a verex u. The definiion slighly changes depending on wheher F is over rp or lp, and if he verex u, which is used o build F, is righ or lef verex, producing wo differen scenarios. Scenario 1 corresponds o F cenered on rp (or lp) and u being righ (or lef respecively), which appears in boh roaions in place and roaions wih respec o an exremal poin, and scenario 2 corresponds o F cenered on rp (or lp) and u being lef (or righ respecively), which only appears in roaions wih respec o an exremal poin. To adequae he correc definiion for each scenario, jus make he proper subsiuions in he definiion according o Table 1. Table 1 Labels for Definiion 6 Scenario 1 Scenario 2 q p = rp u is righ u is lef (or q p = lp) (or u lef respecively) (or u righ respecively) Ξ means larger smaller ϒ means larges smalles Used local Appendix A or Appendix B frame F buil according o Appendix B Condiions B (blockage condiions): Consider a local frame F properly buil (see Table 1) over q p (rp or lp). An opposie ype verex o (lef if u is righ, or righ if u is lef) blocks u in F, if (a) o is wihin he search domain of u wih respec o F (b) o has a θ o angle (θ L if o is lef, or θ R if o is righ) ha is Ξ han he θ u angle relaed o u (θ R if o is lef, or θ L if o is righ), boh measured according o F, and (c) o has a disance do (dl if o is lef, or d R if o is righ) smaller han disance du (dr if o is lef, or d L if o is righ) relaed o u. Definiion 6 For a given goal verex u goal, a candidae verex c is a reflex verex such ha: (a) c is an opposie ype verex o u goal, where c fulfills he blockage condiions for u goal, and c has he ϒ θ angle properly generaing he local frame F (see Table 1), or (b) c is an opposie ype verex o anoher candidae verex c, where c fulfills he blockage condiions for c, and c has he ϒ θ angle properly generaing he local frame F (see Table 1). The complee FSM is presened in subsecion 4.1. Laer, we describe he procedures o find a sub-goal verex and o align he robo o ha verex. The procedures described in subsecions 4.2 and 4.4 are par of he FSM presened in subsecion Complee FSM for Opimal Navigaion Fig. 8 shows he FSM M for navigaion. For modulariy and simpliciy M is organized in 3 procedures and 5 saes. Noice ha he procedures hemselves are compued by a se of saes. The procedures are ALIGN, RPALING and LPALIGN. These procedures have as a main objecive o find a sub-goal verex. The cases in procedures ALIGN, RPALIGN are described in deail in Appendix C, he procedure LPALIGN is jus he symmeric case of RPALIGN. The Finie Sae Machine modeling allows one o organize he several cases and o consider symmery on hem.

10 ALIGN RCHASE DETOURR DETOURL LCHASE yn 61 yn 61 yn 60 yn 60 yn 62 yn 62 Fig. 8 Complee FSM for Opimal Navigaion RPALIGN Λ-REACHED LPALIGN Once ha he FSM is designed, i can be used o navigae in any simple conneced environmen by direcly mapping observaions ino conrols. The robo roaes o ge aligned wih a sub-goal verex. In ALIGN, he robo roaes in place eiher in clockwise or counerclockwise sense. In RPALIGN he robo roaes in clockwise sense wih respec o poin rp and in LPALIGN he robo roaes in counerclockwise sense wih respec o poin lp. The saes in he machine M are RCHASE, LCHASE, DETOURL, DETOURR and Λ -REACHED. RCHASE makes he robo o move in sraigh line o a righ goal verex or o he landmark (when he robo moves oward he landmark direcion r is aligned wih poin rp Λ ). The omnidirecional sensor is placed a rp. LCHASE makes he robo o move in sraigh line o a lef goal verex or o he landmark (when he robo moves oward he landmark direcion l is aligned wih poin lp Λ ). The omnidirecional sensor is placed a lp. DETOURR makes he robo o move in sraigh line o a righ sub-goal verex. The omnidirecional sensor is placed a rp. DETOURL makes he robo o move in sraigh line o a lef sub-goal verex. The omnidirecional sensor is placed a lp. The sae Λ -REACHED indicaes ha he robo has reached he landmark and he navigaion ask is over. The observaion vecors for he saes in machine M are shown in Table 2 (refer o Fig. 8). For simplifying he presenaion of Fig. 8, he observaions yielding ransiions beween procedures and saes are no labeled, because here migh several ones ha generae he ransiion. However, inside he procedures a he level of saes all he ransiions are labeled wih he corresponding observaion. 4.2 Roaion in Place: ALIGN ALIGN is a procedure ha is organized in cases. Each case corresponds o a se of saes in he FSM. In procedure ALIGN a roaion in place is execued o (1) align he robo o a no blocked goal verex, (2) align i o a landmark, or (3) align i o a sub-goal verex; an alignmen o a sub-goal verex migh require one or more alignmens o candidae verices. Fig. 9 shows he design of he par of he FSM dealing wih he hree cases menioned above, which in urn each of hem is insaniaed in a lef case and a righ case. Thus, his par of he FSM is organized in hree righ cases and hree lef cases. Noe ha in he machine he cases are relaed, ha means ha while he procedure o find he sub-goal verex is carried ou he curren case migh change. The sae START1 is he iniial sae of he whole FSM, i decides wheher he robo mus ge align wih a lef goal verex, a righ goal verex, o lp Λ or o rp Λ. The observaion vecors ha make his sae o ransi o oher sae are shown in Table 3. In all he furher ables 1 denoes rue, 0 false and x any value. In Appendix C hree of he six cases are described in deail. The hree cases correspond o an alignmen o a righ goal verex, o rp Λ, and o a lef candidae verex. The oher hree cases are jus symmeric. 4.3 Example of he execuion of ALIGN Fig. 10 shows an example of he execuion of Case 1-R, Case 3-L (lef candidae verex) and Case 3-R (righ candidae verex) in ALIGN o find a sub-goal verex. These cases occur sequenially one afer he oher. Fig. 10(a) shows he alignmen of r o he goal verex corresponding o he execuion Case 1-R. Firs, he goal verex is a righ verex, hence he FSM ransis o sae RV1. Second, he FSM ransis o sae RCCW1 o align r o he righ goal verex. Third, once he robo is aligned, he FSM ransis o T1B1. This sae decides wheher or no he righ goal verex is blocked. Since his goal verex is blocked he FSM ransis o sae TCCW2, he firs sae in Case 3-L. Fig. 10(b) shows he alignmen of l o he lef candidae verex corresponding o he execuion of Case 3-L (lef candidae verex). Firs, in sae TCCW2 he robo moves he omnidirecional sensor o poin lp. Once he omnidirecional sensor is a lp he robo locaes he lef verices (in he reference frame F defined in Appendix A) and selecs as he curren candidae verex. Second, since he l is no aligned o he lef candidae verex he FSM ransis o RCW1. In his sae he robo roaes o align l o candidae verex (see Fig. 10(b)). Third, once he robo is aligned, he FSM ransis o T1B2. This sae decides wheher or

11 Table 2 Observaion vecors for he saes in machine M, Fig. 8 FI LV FR RP LP VR VL AL BL UN GT CT O1 O2 yn x x x x x x x x x yn 61 x x x x 0 x x x x x x yn 62 x x x x 0 x x x x x x Table 3 Observaion vecors for sae START1 FI LV FR RP LP VR VL AL BL UN GT CT O1 O2 yn 0 x 0 x x x x x x x x 0 x x x yn 1 x 0 x x x x x x x x 1 x x x yn 2 x 1 x x x x x x x x 0 x 0 x yn 3 x 1 x x x x x x x x 0 x 1 x RCHASE DETOURL CASE 1-R yn 8 yn 9 yn 11 CASE 3-L yn 19 yn 9 yn 11 yn 4 yn 7 yn6 TCW1 RV1 RCCW1 yn 10 T1B1 yn 12 TCCW2 yn 20 RCW1 yn 10 T1B2 yn 5 yn 1 yn 3 CASE 2-R RL1 yn 1 yn 1 yn 13 yn 16 yn 14 yn 15 yn 17 RCCW2 TCCW1 yn 18 LCHASE yn 12 START1 yn 2 yn 0 LL1 yn 13 yn 14 yn 15 yn16 RCW3 TCW2 RCHASE yn 25 yn 0 yn 0 yn 25 yn 12 CASE 2-L yn 6 yn 7 TCCW3 yn 21 yn 22 LV1 yn 23 RCW2 yn 10 T1B3 yn 9 yn 11 yn 12 TCW3 yn 27 RCCW3 T1B4 yn 10 yn 26 yn 9 yn 11 CASE 1-L CASE 3-R LCHASE DETOURR Fig. 9 Procedure ALIGN no he lef candidae verex is blocked. Given ha he lef candidae verex is blocked, he FSM ransis o sae TCW3, he firs sae in Case 3-R (righ candidae verex). Fig. 10(c) shows he alignmen of r o he righ candidae verex corresponding o he execuion of Case 3-R (righ candidae verex). In his case, while in TCW3, he FSM selecs as a candidae verex hen he FSM ransis o RCCW3 and laer o T1B4. Sae T1B4 decides ha he righ candidae verex is no blocked. Therefore, he righ candidae verex becomes he righ sub-goal verex. 4.4 Roaion wih respec o poin rp: RPALIGN In his subsecion, we presen a procedure ha makes he robo roae w.r.. a poin. Again, he main objecive is o find he sub-goal verex. RPALIGN is also organized in cases. Each case corresponds o a se of saes in he FSM. A roaion w.r.. poin rp is execued o (I) align he robo o a righ goal verex, (II) align i o a landmark, (III) align i o a lef goal verex or (IV) align i o a sub-goal verex. In Appendix C, we presen in deail he cases of he procedure RPALIGN ha makes he robo o roae wih respec o poin rp. There is an equivalen procedure called LPALIGN which is symmeric o RPALIGN and makes he robo o roae wih respec o poin lp. In a roaion in place, he cener of he robo does no ranslae, and he robo can align r o any righ verex or l o any lef verex wihou he risk of loosing opimaliy in erms of he disance raveled by he cener of he robo. In conras, in a roaion wih respec o poin rp he cener of he robo ranslaes. Consequenly, he robo canno align l o each lef verex, or r o each righ verex exhausively one by one o check wheher ha verex is blocked or no. Tha is because he cener of he robo migh ranslae unnecessarily and opimaliy migh be los. To deal wih his problem Al-

12 4.5 Algorihm for finding a sub-goal verex d L d R d R d L g 1 (a) Case 1-R g 2 g 2 g R 0 u1 u g g (b) Case 3-L (lef candidae verex) d L d R g 1 g 2 g R 0 (c) Case 3-R (righ candidae verex) Fig. 10 An example of he execuion by he robo of Case 1-R, Case 3-L (lef candidae verex) and Case 3-R (righ candidae verex) in ALIGN gorihm 1 is used. Thus, in Case IV-RP, a sub-goal verex is found wih Algorihm 1. This algorihm guaranees ha he robo is able o roae o ge aligned wih he sub-goal verex wihou losing pah opimaliy. I migh happen ha he gap generaed by a sub-goal verex merges wih oher gap, while he robo roaes o ge aligned wih a sub-goal verex. If his occurs hen he subgoal verex is re-calculaed by execuing again Algorihm 1. There is anoher issue, here are verices ha from he omnidirecional sensor locaion do no generae a gap when Algorihm 1 is invoked. Such a verex migh block he pah oward he goal verex or he landmark. We call his oher issue a hidden verex (see cases III-RP and IV-RP in Appendix C and Lemma 5 for more deails abou a hidden verex). In Lemma 5 is shown ha a hidden verex always will evenually generae a gap. Once a hidden verex is deeced Algorihm 1 is invoked again and he sub-goal verex is recalculaed. Fig. 11 shows he design of he par of he FSM dealing wih he four cases menioned above. Again, he cases are relaed, while he procedure o find he sub-goal verex is carried ou he curren case migh change. The symmeric cases correspond o LPALIGN. This algorihm is only used in RPALIGN Case IV-RP ( presened in Appendix C) o find a sub-goal verex (or in he symmeric lef Case IV-LP in LPALING). We call u p o a lef sub-goal verex and u n o a righ sub-goal verex. To find verices u p or u n all verices are locaed in he local reference frame F presened in Appendix B. Using he verices locaions, he disances and angles of alignmens d R, d L, θ R and θ L are compued. Algorihm 1 uses wo orders. The firs order is esablished w.r.. disances. This order includes disances d R for he righ verices and disances d L for he lef verices. The order is defined from smaller o larger disances. The second order is an angular order also from smaller o larger; verices are ordered by angle including boh θ R and θ L, angle θ R is used o consider righ verices and θ L is used o consider lef verices. The verices ha Algorihm 1 mus consider o find a sub-goal verex are locaed in he search domain presened in Appendix B. Algorihm 1 Find a sub-goal verex in RPALIGN or LPAL- ING 1) The algorihm sars from he goal verex. The goal verex is declared a candidae verex. 2) If he candidae verex is a lef verex, hen go o Sep 6. 3) Deec lef verices ha block he pah oward he righ candidae verex u R c. To block he pah oward u R c, lef verices mus have an angle θ L larger han he angle θ R relaed o u R c, and a disance d L smaller han disance d R relaed o u R c. 4) If no verex blocks he pah oward he candidae verex u R c, hen go o Sep 9. 5) Selecion of a lef candidae verex u L c. The lef verex wih larges θ L, he las in he angular order is seleced as u L c. 6) Deec righ verices ha block he pah oward he lef candidae verex u L c. To block he pah oward u L c, righ verices mus have an angle θ R smaller han he angle θ L relaed o u L c, and a disance d R smaller han disance d L relaed o u L c. 7) If no verex blocks he pah oward he candidae verex u L c, hen go o Sep 9. 8) Selecion of a righ candidae verex u R c. The righ verex wih smalles angle θ R, he firs in he angular order is seleced as a new candidae verex u R c. Go o Sep 3. 9) The righ verex u R c is seleced as sub-goal verex u n or he lef verex u L c is seleced as sub-goal verex u p. Table 4 shows an example of he execuion of Algorihm 1 and he selecion of a u p verex, his example is shown in Fig. 12. Each row in he boom par of Table 4 is an ieraion of Algorihm 1. In Table 4, indicaes he curren candidae verex o which he blockage condiions are verified, indicaes he verices whose disance o each of hem is smaller han he disance o he candidae verex, indicaes he verex seleced as he nex candidae a each ieraion, indicaes ha he disance o his verex is smaller han he disance o he candidae verex, + indicaes ha he disance

13 CASE III-RP yn 42 DETOURL yn 41 RPCW3 yn 40 yn 21 yn 44 yn 46 TCCW6 yn 45 TCW4 yn 48 RPCW4 yn 47 LCHASE yn37,39 yn 43 yn 50 CASE IV-RP yn 21 yn 45 yn 54 yn 56 yn 57 yn 55 TCCW7 TCW5 RPCW5 LV2 yn 36 TCCW5 yn 38 D yn 49 T2B1 CASE II-RP yn 34 yn 28 yn 9 yn 33 yn 50 yn 53 yn 51 A1 yn 51 TCW6 yn 59 RPCW6 yn 53 yn 10 yn 44 START2 yn 29 yn 30 RL2 yn 31 RPCW2 yn 14 TCCW4 yn 35 LCHASE yn 32 CASE I-RP yn 12 TCCW9 yn 52 yn 53 yn 58 yn 9 TCCW8 yn 12 T1B6 yn 9 yn 12 yn 11 RV2 yn 11 RPCW1 yn 10 T1B5 yn 11 RCHASE DETOURR Fig. 11 Procedure RPALIGN Table 4 Example of orders for selecing a u p verex (refer o Fig. 12). Angular order Disance order Index Index Direcion r r l r l r l Direcion r l l r l r r Type R R L R L R L Type R L L R L R R Verex u goal u 5 u 3 u 4 u 6 Verex u 6 u 5 u 4 u 3 u goal Table 5 Orders for selecing a u n verex (refer o Fig. 13). Angular order Disance order Index Index Direcion r l r l Direcion l r l r Type R L R L Type L R L R Verex u goal u 3 u 4 Verex u 3 u 4 u goal o his verex is larger han he disance o he candidae verex, indicaes ha for a lef verex, he verex ha mus be seleced as he nex candidae is he las in he angular order, and indicaes ha for a righ verex, he verex ha mus be seleced as he nex candidae is he firs in he angular order. The algorihm deermines ha u 4 is a u p verex. Table 5 shows anoher example of he execuion of Algorihm 1 and he selecion of a u n verex, is a u n. The corresponding example is shown in Fig Example of he execuion of RPALIGN Fig. 14 shows an example of he execuion by he robo of Case I-RP (righ goal verex) and Case IV-RP (lef goal verex) in RPALIGN. These cases occur sequenially one afer he oher. Fig. 14(a) shows he execuion of Case I-RP. Firs, he goal verex is a righ verex, hence he FSM ransis o sae RV2. Second, he FSM ransis o sae RPCW1 o align r o he righ goal verex. Third, once he robo is aligned (see Fig. 14(b)), he FSM ransis o T1B5. This sae decides wheher or no he righ goal verex is blocked (deec-

14 u5 u r u u u 3 u1 goal g 2 g Search region for u p Search region for u n Fig. 12 A u p verex, u 4 in his example. d L (a) g 2 u 3 u 4 d R g 0 r (b) u goal Search region for u p Search region for u n Fig. 13 A u n verex, in his example. (c) ion of ype 1). Since his goal verex is blocked he FSM ransis o sae TCCW9, he firs sae in Case IV-RP. In sae TCCW9, firs, he robo locaes righ verices in he local reference frame F presened in Appendix B, second he robo moves he omnidirecional sensor o poin lp. Third, once he omnidirecional sensor is a l p, lef verices are locaed in he reference frame presened in Appendix B. The FSM ransis o sae A1. In his sae he lef sub-goal verex is seleced using Algorihm 1. Once he lef subgoal verex is seleced he FSM ransis o sae RPCW5. In his sae he robo roaes w.r. poin rp o ry o align l o (see Fig. 14(b)). During he roaion he gap generaed by verex merges wih he gap generaed by verex u 3. Refer o Fig. 14(c), he omnidirecional sensor crosses he biangen line beween verex and u 3. When he merge beween he gaps occurs he FSM ransis o sae TCW5. In sae TCW5 he robo sops, and he urre places he omnidirecional sensor a rp. The righ verices are locaed in he reference frame F defined in Appendix B. Once he righ verices are locaed in he local reference frame, he FSM ransis o sae TCCW7. In sae TCCW7 he omnidirecional sensor is placed a lp, and lef verices are locaed in he reference frame. The FSM ransis o sae A1. In his sae he u 3 is seleced as sub-goal verex using Algorihm 1. Once he lef sub-goal verex is seleced he FSM ransis o sae RPCW5. In his sae he robo roaes w.r. poin rp o align l o u 3 (see Fig. 14(d)). Since u 3 is no blocked no re-invocaion of Algorihm 1 is required and u 3 remains as he sub-goal verex. u 3 g 3 (d) Fig. 14 Case I-PR (righ goal verex ) and Case IV-RP (lef sub-goal verices and u 3 ) in RPALIGN 4.7 Considering all possibiliies for he FSM The graphs shown in Figs. 15(a) and 15(b) have as objecive o show ha he design of he FSM is exhausive, all possibiliies are considered. For he case of roaion in place (see Fig 15(a)) he possibiliies are as follows. (G) The robo s objecive is o ge aligned wih a gap, or, (Λ) he robo s objecive is o ge aligned wih he landmark. Firs, consider he possibiliies for node (G). Indeed, here is only one possibiliy as he alignmen wih he gap is always possible by execuing a roaion in place, herefore, (R) he robo roaes in place, and (A) i ges aligned wih he gap. Once he robo is aligned wih he gap, here are only wo possibiliies. (B1) There exiss a blockage (deecion ype 1) or no (NB1). If (B1) happens here is only one possibiliy, namely, (C) he robo finds a candidae verex. Since he candidae verex generaes a gap he only possibiliy is o reurn o node (G) for he

15 robo o ge aligned wih his gap. If (NB1) here is no blockage hen (SL) he robo moves in sraigh line. A sraigh line moion can only yield wo possibiliies: (S) here is an opimal pah oward he gap (he robo ouches he verex wih poin rp or lp), or, (NS) he robo collides (i ouches E wih a poin differen o rp or lp and hence here is no soluion pah). Now, consider ha (Λ) he robo s objecive is o ge aligned wih he landmark. There is only one possibiliy, (R) he robo roaes in place, and (A) ges aligned wih he landmark. Once he robo is aligned o he landmark, (B4) he pah oward he landmark is always assumed as blocked (blockage deecion of ype 4). From (B4) here are wo possibiliies: (S) here is an opimal pah oward he landmark, or, he pah oward he landmark is blocked by a verex, since his verex generaes a gap, (G) he robo mus ge aligned wih a gap. This analysis liss all possibiliies for roaion in place. UG NS NB2, NB3 NB1 SL R A B1 US B1 C G G R A S NB1 SL NS (a) Roaion in place SG B2, B3 B2 Λ Λ R A B4 S A B4 S NB2 R NS Algorihm 1. Once he sub-goal verex has been found, (R) he robo roaes. If (NB2,NB3) occurs he only possibiliy is (R). If (R) happens here are four possibiliies: (NS) he robo collides (he robo ouches E wih a poin differen o rp or lp), (UG) he gap generaed by he goal verex merges wih oher gap, (US) he gap generaed by he sub-goal verex merges wih oher gap, or (A) he robo is able o achieve he alignmen wih he verex generaing he gap. If (UG) hen here is a new goal verex and i is needed o verify a blockage for his verex; he process sars again in (G). If (US) occurs hen he sub-goal mus be re-calculaed using Algorihm 1 in (SG). If (A), once he alignmen has been done, here are wo possibiliies: (NB1) he pah oward he verex is no blocked, or, (B1) i is blocked (blockage deecion of ype 1). If (B1) hen a sub-goal verex is calculaed using Algorihm 1 in (SG). If (NB1), no blockage, hen (SL) he robo moves in sraigh line. A sraigh line moion can only yield wo possibiliies: (S) here is a an opimal pah oward he gap (he robo ouches he verex wih poin rp or lp), or, (NS) he robo collides (i ouches E wih a poin differen o rp or lp). Finally, consider ha (Λ) he robo s objecive is o ge aligned wih he landmark. There is only one possibiliy, (B2) he pah oward he landmark is blocked (blockage deecion of ype 2), or, (NB2) he pah is no blocked. If (B2) hen he pah oward he landmark is blocked by a verex, since his verex generaes a gap, (G) he robo mus ge aligned wih such gap. If (NB2) hen (R) robo roaes. Once he robo roaes here are wo possibiliies: (NS) robo collides, here is no soluion, or, (A) he robo is able o achieve he alignmen wih he landmark. If (A) hen, once he robo is aligned o he landmark, (B4) he pah oward he landmark is always assumed as blocked (blockage deecion of ype 4). From (B4) here are wo possibiliies: (S) here is an opimal pah oward he landmark, or, he pah oward he landmark is blocked by a verex, since his verex generaes a gap, (G) he robo mus ge aligned wih a gap. This analysis liss all possibiliies for roaion wih respec o poin rp or lp. S NS (b) Roaion w.r.. poin rp or l p Fig. 15 Graphs ha presen all he possibiliies in he FSM Le us consider a roaion w.r. poin rp or lp. (G) The robo s objecive is o ge aligned wih a gap, or, (Λ) he robo s objecive is o ge aligned wih he landmark. Firs, consider he possibiliies for node (G). There are wo possibiliies: (NB2,NB3) he pah oward he verex generaing he gap is no blocked, or, (B2,B3) i is blocked (blockage deecion of ype 2 or ype 3). If (B2,B3) occurs here is only one possibiliy, (SG) a sub-goal verex is found using 5 The feedback moion sraegy for navigaion In his secion wo moion policies are synehized from he FSM, which maps observaions o acions are presened. In he firs one, he moion of a urre ha changes he omnidirecional sensor posiion is included, and in he second one i is implici. The moion policy is based on he paradigm of avoiding he sae esimaion o carry ou wo consecuive mappings: y x u, ha is from observaion y o sae x and hen o conrol u, bu insead of ha here is a direc mapping y u. The observaion vecor yn for navigaion including he urre s moion has 14 binary sensor observaions (see Secion 3 for he descripion of each binary obser-

16 vaion). I is ineresing o noe ha he observaion vecor ym for he feedback moion sraegy wihou urre s moion is a subse of yn, ha is ym yn. This means ha some of he observaion elemens of yn are no relevan for he feedback moion sraegy ha makes implici he urre s moion and hey can be any value, consequenly hey are no included. 5.1 Feedback moion sraegy including urre s moion In his feedback moion sraegy he urre s moion is included. Whenever he robo ranslaes he urre is saic and whenever he urre moves he robo is moionless. Depending on he observaion, one of he five differen moion primiives will be execued for he robo and wo for he urre: (1) sraigh line moion; (2) clockwise roaion in place; (3) counerclockwise roaion in place; (4) clockwise roaion wih respec o poin rp and (5) counerclockwise roaion wih respec o poin lp. In he nex wo moion primiives he robo is moionless and he urre moves: (6) he robo moves he urre on is boundary in clockwise sense and (7) he robo moves he urre on is boundary in counerclockwise sense. The feedback moion sraegy can be esablished by: γ : {0,1} 14 { 1,0,1} 3. The feedback moion sraegy is given by γ(yn i )=(w r,w l,v ), where w r and w l are he angular velociies of he righ and lef wheels, and v is he velociy displacemen of he urre on he robo boundary. The se of all possible observaion vecors can be grouped by leing x denoe any value. This can be done using sandard boolean algebra and reducion echniques, which migh be implemened using sofware. Doing so we obain: yn 63 =(0,x,x,x,x,x,1,1,0,0,x,x,x,0), yn 64 =(0,x,x,x,x,x,0,1,0,0,x,x,x,0), yn 65 =(0,x,0,x,x,0,0,0,x,0,0,0,0,0), yn 66 =(0,x,0,x,x,1,1,0,0,0,x,0,0,0), yn 67 =(0,0,0,x,x,0,0,0,0,0,1,0,1,0), yn 68 =(0,x,0,x,x,0,0,0,1,0,x,0,1,0), yn 69 =(0,x,0,x,x,1,1,0,0,0,x,1,1,0), yn 70 =(0,x,1,1,0,1,1,0,0,0,x,0,0,0), yn 71 =(0,x,1,1,0,1,1,0,0,0,x,1,1,0), yn 72 =(0,0,1,1,0,x,0,0,0,0,1,0,1,0), yn 73 =(0,1,1,1,0,1,0,0,0,0,0,0,1,0), yn 74 =(0,0,1,1,0,x,0,0,0,1,1,0,0,0), yn 75 =(0,0,1,1,0,0,0,0,0,0,0,0,0,0), yn 76 =(0,x,1,0,1,1,1,0,0,0,x,1,1,0), yn 77 =(0,x,1,0,1,1,1,0,0,0,x,0,0,0), yn 78 =(0,x,1,0,1,0,x,0,0,0,0,0,0,0), yn 79 =(0,0,1,0,1,0,x,0,0,1,0,0,1,0), yn 80 =(0,0,1,0,1,0,0,0,0,0,1,0,1,0), yn 81 =(x,x,x,x,x,x,x,x,x,x,x,x,0,1), yn 82 =(0,x,x,x,x,x,x,1,1,0,x,x,0,0), yn 83 =(0,0,0,x,x,0,0,0,0,0,1,0,0,0), yn 84 =(0,0,1,1,0,0,0,0,0,1,0,0,0,0), yn 85 =(0,x,1,1,0,1,1,0,0,0,x,1,0,0), yn 86 =(0,x,1,1,0,1,1,0,0,1,x,0,0,0), yn 87 =(0,0,1,0,1,0,1,0,0,x,1,0,0,0), yn 88 =(0,x,1,0,1,0,1,0,0,1,x,0,0,0), yn 89 =(0,x,1,0,1,0,x,0,1,0,0,0,0,0), yn 90 =(x,x,x,x,x,x,x,x,x,x,x,x,1,1), yn 91 =(0,x,x,x,x,x,x,1,1,0,x,x,1,0), yn 92 =(0,0,x,x,x,x,0,0,0,0,0,0,1,0), yn 93 =(0,x,1,1,0,1,0,0,0,1,x,0,1,0), yn 94 =(0,x,1,1,0,x,0,0,1,0,x,0,1,0), yn 95 =(0,0,1,0,1,x,x,0,0,1,1,x,1,0), yn 96 =(0,x,1,0,1,1,1,0,0,0,x,0,1,0), yn 97 =(0,x,1,0,1,1,1,0,0,1,0,1,1,0). The sraegy γ can be encoded as 1 γ(yn 63 yn 64 )=(1,1,0); 2 γ(yn 65 yn 66 )=( 1,1,0); 3 γ(yn 67 yn 68 yn 69 )=(1, 1,0); 4 γ(yn 70 yn 71 yn 72 yn 73 yn 74 yn 75 )=(0,1,0); 5 γ(yn 76 yn 77 yn 78 yn 79 yn 80 )=(1,0,0); 6 γ(yn 81 yn 82 yn 83 yn 84 yn 85 yn 86 yn 87 yn 88 yn 89 )=(0,0,1); 7 γ(yn 90 yn 91 yn 92 yn 93 yn 94 yn 95 yn 96 yn 97 )=(0,0, 1). in which means or. 5.2 Feedback moion sraegy wih urre moion made implici Only six binary sensor observaions affec he conrol of he wheels moors. The used observaion vecor is ym i = (FR, RP, LP, AL, BL, O1). Refer o Secion 3 for he meaning of each elemen of he observaion vecor. Depending on he observaion, one of he five differen moion primiives will be execued: (1) sraigh line moion; (2) clockwise roaion in place; (3) counerclockwise roaion in place; (4) clockwise roaion wih respec o poin rp and (5) counerclockwise roaion wih respec o poin l p. Recall ha

17 he angular velociies of he differenial-drive wheels yield one of hese moion primiives. Hence, he feedback moion sraegy can be esablished by: γ : {0,1} 6 { 1,0,1} 2 o obain γ(ym i )=(w r,w l ). The se of all 64 possible observaion vecors can be grouped by leing x denoe any value o obain: ym 1 =(x,x,x,1,0,x), ym 2 =(0,x,x,0,x,0), ym 3 = (0,x,x,0,x,1), ym 4 =(1,0,1,0,x,x), ym 5 =(1,1,0,0,x,x). The sraegy γ can be encoded as (1)γ(ym 1 )=(1,1); (4)γ(ym 2 )=( 1,1); (2)γ(ym 3 )=(1, 1); (5)γ(ym 4 )=(0,1); (3)γ(ym 5 )=(1,0). 6 Proof of opimal navigaion In his secion, we prove ha he cener of he disc nonholonomic robo ravels he shores disance o reach he landmark. Our mehodology is no based on numerical opimizaion echniques bu on a geomerical and opological reasoning. To our knowledge his is he firs ime ha he shores pah for a DDR ( sysem) is found in he presence of obsacles (environmen consrains) wihou knowing he complee geomeric represenaion of he environmen. Noe ha our resul is no an approximaion bu he exac soluion. The robo pah is found in he coninuous. However, recall ha he robo follows only hree moions primiives (sraigh line moion, roaion in place and roaion wih respec o poin rp or l p), he finie sae machine riggers one of hese moion primiives according o criical changes in he sensor readings. Hence, we hink ha he approach is relaed o hybrid conrol in which coninuous and discree modeling is combined. In his secion, we also sress he fac ha minimizing he Euclidean disance raveled by he cener of he robo, gives as a consequence he exisence of a geomeric soluion for he navigaion problem of reaching a landmark. Hence if he proposed moion sraegy does no find a collision free pah o he landmark hen such pah does no exis. The mehodology presened here migh be used o solve oher relaed problems, e.g., finding he shores pah for oher nonholonomic underacuaed sysems. Subsecion 6.1 considers he case when he GNT encoded pahs are non-blocked and Subsecion 6.2 considers he case of blocked pahs. 6.1 Non-blocked GNT-encoded pahs The shores pah o Λ is encoded as a sequence of gaps in he GNT. Le U =(u n,u n 1,..., ) be he sequence of conneced inervals u i E ha he robo conacs when he gap sensor (fixed o he robo boundary) moves from is iniial posiion o is final posiion in Λ. In his secion we esablish ha he robo execues an Euclidean, disance-opimal pah in he absence of blockages, namely, no deours beween inervals u i+1 and u i are done. Le H = (g n,g n 1,...,g 0 ) denoe he corresponding sequence of gaps ha are chased, in which g i H is he gap ha is being chased on he pah o u i or while raversing u i. Now consider he problem in erms of he configuraion space of he robo. The obsacle region in he configuraion space is obained by growing he environmen obsacles by he robo s radius. Le C denoe he projecion of he obsacle region ino he plane, hereby ignoring roaion. Le V = (v n,v n 1,...,v 0 ) be he sequence of inervals v i C obained by ransforming he inerval sequence U from E o C, elemen by elemen. The following lemma uses he definiion of a generalized biangen from [39]. Lemma 2 Chasing he sequence H of gaps produces he shores pah if and only if: 1) here is a sraigh collisionfree pah from he cener of he robo o v n, 2) here is a (generalized) biangen line beween v i+1 and v i, 3) here is a sraigh collision-free pah from v 0 o he landmark cener, and 4) C is conneced. Proof: We firs proof he lef o righ direcion of he if and only if saemen. The gap sensor is locaed over poins rp or lp. Chasing he sequence H of gaps makes he robo o ouch he inervals u i E wih poins rp or lp, as a consequence, he robo s cener visis inervals v i C. If chasing he gaps in H produces he shores pah and he robo s cener visis each inerval v i V, hen a soluion global pah mus exis (C is conneced), and here mus exis local sraigh collision free pahs connecing he iniial locaion of he robo s cener wih he sequence V, connecing each pair v i+1 and v i in V, and he sequence V wih Λ, which in conjuncion are he four condiions in he Lemma saemen. To proof he oher direcion of he saemen, we proceed by conradicion. Le s suppose ha he hree firs condiions are fulfilled and ha here exiss a soluion pah, hence, he fourh condiion is also fulfilled. Now, le s suppose ha he shores pah is no he one depiced by he hree firs condiions. If such shores pah exiss, hen here mus be a blockage in he sraigh pahs of eiher he iniial posiion of he robo s cener and V, beween inervals v i+1 and v i in V, or beween V and Λ, herefore, a leas one of he hree condiions is no fulfilled, which is a conradicion. Furhermore, as he pah depiced by he hree condiions visis he inervals v i V, hen i is raversable chasing he gaps in H. The resul follows. 6.2 Blocked GNT-encoded pahs We now consider he cases for which eiher of he firs hree condiions of Lemma 2 is violaed, meaning ha he robo

18 would become blocked when applying he GNT in he usual way. For hese cases, various forms of deours are required. The GNT-encoded pah is based on he biangen lines beween inervals in E. However, in C, some biangen lines disappear. Biangen lines in he workspace ha remain in C are displaced by a disance r or are roaed by some fixed angle. The GNT-encoded pah canno be execued by he robo when here is a blockage while chasing g i H (or Λ). If his happens i means ha: 1) he robo is in a zone in which i canno deec he crossing of a biangen line in C, ha means ha because of he widh of he disc robo, here is a biangen line beween he sensor locaion and he verex u n, bu here is no a biangen line beween he cener of he robo and v n, 2) here is no biangen line beween v i+1 and v i in C, 3) here is no sraigh line pah o chase Λ when he robo sees Λ, or 4) C is disconneced. These are he condiions of Lemma 2. In Theorem 2, we sae ha our navigaion sraegy is able o deal wih he firs hree cases presened above. Therefore, i is always possible o deec an opimal collision free pah if one exiss. The disconnecion of C, he case in which here is no pah o Λ will produce a robo collision when commanded by he FSM M. In Theorem 3, we prove ha if our moion sraegy does no find a collision free pah o reach he landmark, hen here is no pah o reach i. If he robo deecs a blocked pah, hen i performs a deour o avoid he obsacles ha block he GNT-encoded pah. We canno re-plan he enire pah o Λ because he pah depends on he gap g i H (or Λ) ha is in he gap sensor field of view. For his reason he deour o avoid obsacles is done when he robo deecs a blocked pah while chasing g i or Λ. In he remaining of his secion, wihou loss of generaliy, when a roaion w.r.. a poin is referred i is a roaion w.r.. rp. To prove some lemmas and heorems below, we inroduce he following conceps. Le us consider a given candidae verex u c. If u c is blocked by any oher verex hen he candidae verex u j ha deforms he mos he sraigh line robo pah oward u c is said o lie on he boundary of he resricions. For a u c = u R c, he u j ha lies on he boundary of he resricion corresponds o he u L j ha blocks he pah oward u R c, and has he larges θ L angle, he las in he angular order, see Fig. 16. Symmerically, for a u c = u L c, he u j ha lies on he boundary of he resricion corresponds o he u R j ha blocks he pah oward u L c, and has he smalles θ R angle, he firs in he angular order. Lemma 3 Algorihm 1 finds a sub-goal verex among he verices generaing gaps a he robo configuraion when he algorihm is invoked. The candidaes verices, including he sub-goal verex, lie on he boundary of he resricions of semi-collision free moion imposed by he verices blocking he pah oward he goal verex. Fig. 16 verex lies on he boundary of he resricion Proof: By Consrucion Algorihm 1 deecs lef verices ha block he pah oward a righ candidae verex or righ verices ha block he pah oward a lef candidae verex. For a blocked righ candidae verex, Algorihm 1 selecs as a new candidae verex he lef verex wih larges θ L, he las in he angular order, which lies on he bordary of he resricion. For a blocked lef candidae verex, Algorihm 1 selecs as a new candidae verex he righ verex wih smalles θ L, he firs in he angular order, which lies on he bordary of he resricion. This procedure repeas unil he candidae verex is no blocked selecing his las one as he sub-goal verex. Hence, he resul follows. A pah oward a landmark migh be blocked by a hidden verex. Wheher or no a verex is hidden depends on he relaive posiions beween he omnidirecional sensor and he verex. A hidden verex is imporan because if i blocks he robo i mus be considered o find he opimal pah, which is done using Algorihm 1, and hence if here was a hidden verex when Algorihm 1 was invoked hen his algorihm mus be invoked again when he verex generaes a gap. Exhausively, here are four possible cases analyzing if (EG) he hidden verex evenually generaes a gap or (NG) i never generaes a gap, and if (S) here exiss a collision free pah oward he landmark or no (NS), while he robo moves commanded by he FSM. Below we show, hrough he four cases, ha eiher, if here is a soluion (an opimal pah) hen he FSM finds i, or if here is no soluion hen he robo collides. The case 1 is he following: 1) in he pah ha he robo follows he hidden verex evenually generaes a gap (EG) and 2) here is no an opimal pah (NS). See Fig. 17(a) and elemen (EG,NS) in Table 6. In his case he FSM reurns a sub-goal verex bu robo collides since here is no soluion pah. The case 2 is: 1) he hidden verex evenually generaes a gap (EG) while he robo moves in he opimal pah and 2) here is an opimal pah (S). See Figs. 17(b) and 17(c), and elemen (EG, S) in Table 6. Lemma 4 proves ha if here exiss a soluion pah hen each hidden verex always evenually generaes a gap while he robo is commanded by he FSM.

19 Table 6 4 cases relaing he exisence of he opimal pah and a hidden verex NS S EG Yes Yes NG Yes No The case 3 is: 1) he hidden verex never generaes a gap while he robo moves commanded by he FSM (NG) and 2) here is no an opimal pah (NS). See Fig. 17(d) and elemen (NG, NS) in Table 6. In his case he FSM will never ake ino accoun he hidden verex bu he no soluion will be deeced because he robo collides. The case 4 is: 1) he hidden verex never generaes a gap while he robo moves commanded by he FSM (NG) and 2) here is an opimal pah (S). Elemen (NG, S) in Table 6. Noe ha given ha here exiss a soluion, his case is he complemen of case 2 and by Lemma 4 case 2 always occurs, hence case 4 does no exis. Omni direcional sensor (O) l Omni-direcional sensor Omni-direcional sensor rajecory a c Omni-direcional sensor Omni-direcional sensor rajecory a c r (a) case 1 u 3 (b) case 2: biangen complemen u 3 (c) case 2: same conneced inerval over E Omni direcional sensor (O) u 3 u1 g L 1 (d) case 3 Fig. 17 Cases relaing he exisence of he opimal pah and a hidden verex g L 1 g R 0 l h l h g L 1 g L 1 Lemma 4 Le us assume ha a sub-goal verex u s is obained by Algorihm 1. If here is a collision free pah o he landmark Λ, hen a hidden verex u h ha blocks o u s mus generae a gap while he robo roaes wih respec o rp o align l o a lef sub-goal verex u s. Proof: Refer o Figs. 17(b) and 17(c). The robo roaes w.r.. poin rp o align l o a lef u s. While he robo roaes he omnidirecional sensor is moving in an arc of circle pah a c. Consider a line l h ha conains u s and u h. For blocking he pah oward u s by u h, he line l h mus inersec a c, oherwise he robo is always able o align l o u s. Furhermore he line l h mus be crossed by he omnidirecional sensor in order o align l o he u s verex, as l is angen o he robo boundary, l h conains u s and l h is a secan line o he circle a c. For his robo roaion, wo cases exis according o he gap evens riggered by crossing l h. The firs one occurs when verex u h generaes a gap and his gap merges wih he gap generaed by he u s, see Fig. 17(b). Since wo gaps merged here mus exis a biangen complemen beween u h and u s (as defined in [39]) over l h. Originally he verex u h does no generae a gap, bu due o he omnidirecional sensor moion he biangen complemen is crossed (he gaps generaed by u s and u h merge), hence a gap originaed by verex u h mus appear. The second case occurs when he gap generaed by u s changes of verex generaing i, and he gap is generaed by verex u h, see Fig. 17(c). In his second case he segmen joining verices u s and u h lies over line l h. Since he line segmen joining u h and u s is he same conneced inerval over E, hen he gap generaed by u s mus change he verex generaing i. The gap mus be generaed by verex u h a he momen ha line l h is crossed by he gap sensor while he robo is roaing. Hence he verex u h mus evenually generae a gap. The resul follows. There is an equivalen lemma for he case when he robo roaes w.r.. poin lp o align r o a righ u s. Lemma 5 Consider a procedure ζ, which invokes Algorihm 1 a each ime ha a hidden verex is deeced. Procedure ζ erminaes and finds he sub-goal verex. The pah oward he sub-goal verex is opimal in he sense of Euclidean disance raveled by he cener of he robo. Proof: By Lemma 3 Algorihm 1 delivers a sub-goal verex. By Lemma 4 a hidden verex is always deeced. If he sub-goal verex is no blocked by a hidden verex hen he sub-goal verex is no re-calculaed. Oherwise, Algorihm 1 is invoked again. Therefore, procedure ζ finds he sub-goal verex u s and as here is a finie number of verices he procedure ζ erminaes. Furhermore, by Lemma 3 a sub-goal verex u s lies on he boundary of he resricion and i is no blocked hen he pah oward he sub-goal verex u s is opimal. Now, we presen he heorem ha ensure globally opimal navigaion when using M.

20 Theorem 2 The pah ha he robo cener follows when commanded by he auomaon M, using he informaion encoded in he GNT and making deours when he sraigh line pah o chase g i H is blocked or when he sraigh line pah o chase Λ is blocked, is globally opimal in he sense of Euclidean disance. Proof: Le assume ha here is a collision free pah oward he landmark, ha is, C is conneced. The GNT-encoded pah is he shores pah for a poin in he workspace and i is in he same homoopy class ha he shores pah in C because E and C are simply conneced. The sequence of conneced inervals V in C ha he robo raverses is only changed when eiher of he firs 3 condiions of Lemma 2 are no saisfied. However, even if any of he firs 3 condiions of Lemma 2 are no saisfied hen he original sequence of inervals in V does no change he order, as he homoopy class of he shores pah in C remains he same, bu new sub-goals verices are added corresponding o new inervals of C. These sub-goals produce deours beween original consecuive inervals of v i and v i 1, and hey are locally opimal (as proved in Lemma 5) as all of hem belong o he boundary of he resricion (as proved in Lemma 3). Hence, he resuling global pah is opimal. Theorem 3 is one of our main resuls. In shor i indicaes ha if our moion sraegy does no find a soluion, hen here is no soluion. This resul also increases he ineres of he proposed opimizaion crierion, i.e. o move he cener of he circular robo as less as possible, since from a poin of view of he geomeric exisence of a soluion, i esablishes a soluion for he wors scenario -he mos resriced polygonal environmen, ha is, he environmen wih he mos narrow passage, such ha, his passage is wider han he robo s diameer. So ha, he problem addressed in his paper can be considered a game [23] agains he polygonal environmen. Theorem 3 If robo is commanded by he auomaon M yielding he opimal pah in he sense of Euclidean disance and he robo ouches E wih a poin differen o lp or rp (robo collides) hen here does no exis a pah o reach he landmark. Proof: By Lemma 5 sub-goal verices are found. The resricion of collision free pah is imposed by all verices blocking he sraigh line pah oward he goal verex. The sub-goal verices are locaed on he boundary of he resricion of semi-collision free moion and mus be ouched by poin rp or lp. Hence, if robo collides (ha is he robo ouches E wih a poin differen o lp or rp) hen here is no soluion. 7 Implemenaion The whole mehod has been implemened and simulaions resuls are included. All our simulaion experimens were (a) (c) (e) Fig. 18 A simulaion of opimal gap navigaion for a disc robo. (e) shows he pah in he projeced configuraion space ha he robo raverses o go o he landmark. run on a PC wih a quad-core processor, equipped wih 4 GB of RAM, running Linux, and were programmed in C++ using he compuaional geomery library LEDA. Our sofware implemenaion exacly emulaes he FSM. The planning ime for obaining each one of he five simulaion experimen presened in his secion, is always less han a second. These planning imes do no include he execuion ime of he navigaion iself. We have implemened he mehod wih several objecives: (1) o illusrae he execuion of he FSM, (2) o graphically show he pah in C, and (3) o display he evoluion of he GNT as he navigaion ask is execued. Fig. 18 shows a simulaion of he opimal gap navigaion for a disc robo. In his firs example, he verices ha he robo visis in is shores pah oward he landmark are he same ha he ones visied by a poin robo. Figs. 18(a) and 18(c) show he robo a differen imes while following (b) (d)