From indoor GIS maps to path planning for autonomous wheelchairs

Transcription

1 From indoor GIS maps to path planning for autonomous wheelhairs Jérôme Guzzi and Gianni A. Di Caro Abstrat This work fouses on how to ompute trajetories for an autonomous wheelhair based on indoor GIS maps, in partiular on IndoorGML maps, whih set the standard in this ontext. Good wheelhair trajetories are safe and omfortable for the user and the people sharing the spae with him, turn gently, are high legible, and smooth (at least G 2 ontinuos). We derive a navigation graph from a given IndoorGML map. We define and solve an optimization problem to find the desired path: given a suession of ells to traverse, the path orresponds to the best omposite Bézier trajetory for the wheelhair. We disuss a related multi-objetive path planning problem. Experimental results and an implementation on real robots show the planner performane. I. INTRODUCTION With the ageing of the population and the inreasing need for assistive devies to support elderly s quality of life, robots are going to leave the researh labs and begin to populate nursing homes, hospitals, and even houses. In partiular, intelligent roboti wheelhairs will represent an important support for people with limited mobility to help maintaining their autonomy. Semi-autonomous wheelhairs will assist the driver when traversing diffiult areas, suh as rowded and luttered spaes. Fully autonomous wheelhairs will serve people unable to drive and ould be ordered from a remote loation, like a taxi. In this senario, a set of fundamental questions arise. Whih trajetories should a roboti wheelhair follow to be effiient but also safe and omfortable for the user? Whih spatial knowledge is neessary to aomplish the task? How to gather it? Contrary to most indoor mobile robotis senarios, we do not assume that the wheelhair uses an oupation map, built with loal sensors. Instead, we onsider that the wheelhair path planner relies on stati floor maps, whih may ontain semanti information that failitates the planning tasks. In this paper, we address the above questions starting from the observation that indoor GIS maps ontain all the (stati) spatial information relevant to path planning and trajetory optimization in indoor spaes. In this respet, we adopt the new standard IndoorGML [1] to represent indoor maps, whih we desribe in Setion III. Next we disuss how to derive the navigation graph for a wheelhair from the map. In Setion IV, we desribe a planner that ompute the best trajetories for a wheelhair, taking into aount the Authors are with the Dalle Molle Institute for Artifiial Intelligene (IDSIA), USI-SUPSI, Manno-Lugano, Switzerland. {jerome,gianni}@idsia.h This work was partially supported by the AAL Programme (ALMA projet ). user omfort and the harateristis of indoor buildings. At this aim, omposite Bézier urves are used. We define the related optimization problem and propose an algorithm to solve it approximately. Finally, in Setion V, we address the multi-objetive optimization problem that arises from the need to plan trajetories that are both omfortable for the user and short. A robot implementation (Setion VI) and experimental results (Setion VII) serves to evaluate, ompare, and validate the different planners. The paper s sope has three main limitations. First, we use, as a ost funtion for the optimization problem, a geometri feature that we expet to affet the user s omfort but that has not been validated yet through user studies. Nevertheless, the proposed system does not depend on the atual ost funtion being optimized and an be adapted to take into aount additional geometri features. Seond, for simpliity, we selet the shortest route on the navigation graph and we fous on the best trajetory that traverse those spaes. In fat, humans pik routes by taking into onsideration additional riteria, like when we prefer a longer but less rowded orridor. Lastly, the safety and omfort of a user depends also on the ergonomis and on the navigation behavior of the wheelhair, whih should avoid dynami obstales that were not part of the map, like people, and modulate speed gently [2]. We expet to address these aspets within the multidisiplinary researh projet Ageing without losing mobility and autonomy [3] on tehnologies (ambient monitoring, user interfae, assisted navigation, and ontrol of (semi-) autonomous wheelhairs) to support the autonomous mobility and orientation of the elder, and, more in general, of the person with redued mobility. The paper main ontribution is the formulation and solution of the trajetory optimization problem, and the presentation of a omplete pipeline, based on IndoorGML, from data representation and gathering to roboti path planning. II. RELATED WORK Robots an extrat semanti and topologial information from oupany grid maps built with their loal sensors [4], [5], and roboti wheelhairs an take advantage of this information for path planning [6]. Path planning for wheelhairs on oupation grid maps uses ost funtions that take into aount the user s omfort, like lane preferene and high visibility along the route [7]. Geographi information system (GIS) maps are ommonly used by robots to navigate outdoors [8]. Reently, IndoorGML, a new standard for indoor maps that ontains semanti, geometrial and topologial information, has been

2 approved [1]. IndoorGML maps partition the spae into ells. In the ontext of (topologial) path planning, ells are preferably onvex. Convex ell deomposition of polygons is widely studied in robotis [9] as well in omputational geometry [10] to ahieve that goal. Planning for smooth trajetories has been an important topi of researh in robotis [11]. Robots ontroller have an easier task when following a smooth trajetory, whih should ideally be G 3 ontinuos [12]. Splines and omposite Bézier urves are families of smooth urves that researhers often look at for optimal smooth trajetories, for whih the bend energy [13] i.e the integrated squared urvature of a trajetory has provided a natural objetive to minimize. Curvature onstraints an be added to ensure the feasibility of the trajetory by non-holonomi robots [14]. Reent researh has investigated the use of Bézier omposite urves for planning trajetories along orridors [15], and aross doors [16]. In this paper, we disuss how to plan an optimal trajetory that may traverse a whole building. The wheelhair loal navigation ontrol may inrease legibility by following pedestrian navigation rules [17], and favor omfort by reduing urvature [7] and modulating aelerations [2]. We fous instead on the ontribution given by path planning. Interestingly, drawing Bézier urves on a sreen may also provide a human friendly interfae for the remote ontrol of a roboti wheelhair [18]. III. INDOOR SPATIAL MAPS The Open Geospatial Consortium has reently approved IndoorGML [1], a new standard that extends the Geographi Markup Language (GML) to desribe indoor spaes and indoor navigation of heterogeneous agents [19]. The vision of the future is that an inreasing number of buildings floor plans will be stored in CAD formats like IFC, whih ontains rih geometrial and semanti information. Simple automati tools will onvert IFC data to IndoorGML maps. Users will visualize maps and get navigation instrutions when entering inside a building, using IndoorGML to exhange spatial information. An IndoorGML map is a multi-layered graph of ells. Eah layer onsists of a graph, where nodes are ells and edges are boundaries between ells. An edge may represent adjaeny between ells or traversability aross the ommon boundary. Additionally, IndoorGML maps ontain interlayer edges to onnet ells belonging to different layers. In this paper, we assume that an IndoorGML floor map with at least one geometri layer is available. This layer ontains all geometri information about the indoor spaes tagged by type: navigable ell type, like doors, stairs, rooms, or orridors, and non navigable ell type, like walls. Cells are geometrially desribed as polygons (possibly with holes) and ell boundaries as polylines. Doors, like all other spaes, are also desribed by ells 1. 1 In IndoorGML terms, this is named thik door model. A seond layer, the navigation layer, is automatially derived from the geometri layer as following. We start by removing all ells that are non navigable by a wheelhair, like stairs. Next, we ompute the non navigable area as the union of all walls, inflated by the wheelhair size plus a safety margin. For eah navigable ell, we subtrat the inflated non navigable area. We subdivide the resulting ells into onvex ells (see Setion III-A). If all navigable ells are onvex, the trajetories are guaranteed to be feasible (see Setion IV). Finally, for eah pair of adjaent ells, an edge orresponding to the ommon boundary is added to the navigation graph. The resulting navigation layer represents the onfiguration spae of the wheelhair as a topologial navigation graph of ells, where routes are speified as a list of boundaries to ross. A. Cell deomposition for planning The navigation layer should favor effiient path planning. In partiular, to minimize omputational time, it should ontain a small number of ells. Moreover, trajetories are generally very onstrained when rossing narrow passes (like along a orridor). Therefore we prefer to ut ells along short diagonals suh that narrow passes are expliitly represented in the graph. To keep the graph size small, the onvexity onstraint an be relaxed a little. We aept ells that are quasi-onvex, that is ells for whih the differene between them and their onvex hull is very small. In any ase, by adding a proper safety margin when inflating walls, trajetories will remain feasible even if passing through suh quasi-onvex ells. The main idea of our ell deomposition algorithm is to sequentially subdivide non onvex ells into quasi-onvex ells by utting them along the shortest diagonal that removes one of the non-onvex verties. An example of resulting navigation layer is illustrated in Fig 4. IV. TRAJECTORY PLANNING In this setion, we assume that a topologial path on the navigation graph is given that ontains a list (b 1,..., b n ) of boundaries to pass while moving between the onvex ells ( 0,..., n ). Given start (s) and end (e) poses loated in the first and last ells, we want to ompute the best trajetory, for a wheelhair, that is ontained inside the ells and rosses the boundaries in the right order. A. Cost as a variation of the path bend energy Trajetories with small urvature κ are benefiial for multiple reasons. The wheelhair an keep aelerations small, inreasing in this way the omfort for a user sitting on it. For the same linear speed, the roboti wheelhair an redue the angular speed, inreasing performane (less slipping, less risk to lose loalization). The paths may also look more natural, preditable and legible, whih are important features when autonomous wheelhairs share the spae with people. Therefore we searh for a trajetory γ that minimizes the ost C(γ), expressed as a variant of the path bend energy

3 that takes into aount the variation of the urvature to redue urvature hanges: ( ( ) ) 2 κ C(γ) = κ(s) 2 + ds, (1) s γ where γ is parametrized by length s. Contrary to the original formulation of C [16], the integral is not defined with respet to a speifi parametrization, but as an intrinsi geometrial property of γ. B. Bézier omposite urve We searh for the optimal γ in the spae of Nth order omposite Bézier urves [20], meaning that γ is omposed by suessive Bézier urves of order N, one for eah ell, smoothly joined at the ell boundaries. For eah ell i, the urve segment [0, 1] i has the form t N j=0 P i j b j,n (t), (2) where b j,n are Nth order Berstein polynomials ( ) n b j,n (t) = t j (1 t) n j [0, 1], for t [0, 1] (3) j and P i j i are N ontrol points that ompletely define the urve. Bézier urves have limited degrees of freedom, allowing for effiient optimization, and are well known for their use to approximate urves of any shape. They have several additional useful properties. For instane, Bézier urves are ontained into the onvex hull of their ontrol points. Thus, if the ontrol points are ontained in a onvex ell, the orresponding Bézier urve will also be ontained. Moreover, they are effiient to ompute by a sequene of linear ompositions using the de Casteljau s algorithm. The ontrol points adjaent to the segments joins define the smoothness degree of a omposite Bézier urve. More preisely, the nth order derivative at a join depends only on the adjaent n ontrol points: γ (0) = P 0 γ (1) = PN γ (0) = N (P1 P0 ) γ (1) = N ( PN PN 1 ) κ γ (0) = N 1 (P2 P1 ) (P1 P0 ) N P1 P 0 3 κ γ (1) = N 1 ( ) ( P N PN 1 P N 1 PN 2) N P N PN 1 3. G 0 geometri ontinuity (i.e. C 0 ontinuity for urves parametrized by length), requires that suessive segments share the last, resp. first, ontrol point. G 1 ontinuity needs that the three ontrol points around the join to be ollinear. Furthermore, a trajetory, in order to be followed by a differential driven robot like a typial wheelhair should be G 2 : the urvature (and onsequently the angular (4) speed) must be ontinuous. This puts a onstraint on the 5 adjaent ontrol points around a join, whih generally requires N 6 to be satisfied. Note that G 3 ontinuity may also be neessary for a robot navigation ontroller to follow a urve [12]. However, in most ases the robot has to ope with loalization errors and obstales avoidane. Therefore G 2 or even G 1 ontinuity may be smooth enough for a trajetory that will be loally adjusted anyway. In order to keep the number of degrees of freedom small, instead of searhing in the spae of 6th order Bézier urves, we first searh for the optimal 4th order (ubi) Bézier omposite urves. Then we transform the urve to a G 2 one (see Setion IV-D). Parametrization: We parametrize the 4n + 2 degrees of freedom of the 3n + 2 ontrol points defining γ as following. The urve passes through ontrol points loated at start P 0 n 0, end P3, and on eah boundary (P i 0 = P i 1 3 b i, i {1,..., n}). We linearly parametrize by s i [0, 1] the position of P i 0 = p 0(s i ; b i ) along b i, where s = 0, 1 lie on the border. For the points P 0 1 = q 1 (u i 1; s) and P n 2 = q 2 (u i 2; e) we use a polar parametrization with respet to s, respetively e, where u 0 1, u n 2 [0, 1] are radial distanes divided by the ell length (the largest edge of the ell bounding box). Similarly, the points P i 1,2, whih ontrol the derivatives at the ell boundaries, are polar parametrized with respet to P i 0 with axis along b i, i.e. P i 1 = p 1(α i, u i 1, s i ; b i ), P i 2 = p 2(α i+1, u i 2, s i+1 ; b i ). Figure 1 (left) illustrates the notation. C. Optimal trajetory The parameters defining the optimal γ = γ(u, s, α) with respet to C are obtained through the solution of the following non-linear onstrained optimization problem: Minimize C(γ(u, s, α)) Subjet to u 0 1, u i 1, u2 i 1, u n 2, s i, α i [0, 1] p 1 (α i, u i 1, s i ; b i ) i p 2 (α i, u2 i 1, s i ; b i ) i 1 q 1 (u 0 1; s) 0 q 2 (u n 2 ; e) n, i {1,..., n}, whih we solve numerially using onstrained optimization by linear approximations (COBYLA solver) [21]. Sine the solver works better for differentiable onstraints, we reformulate all onstraints of form P i j i as: P i j (5) i d(p i j, i) 0, (6) where d(p, ) is the distane between P and the border of ell, negated if P lie outside of the ell. Beause of non linearity, the ost funtion may present several loal minima. The COBYLA solver onverges towards one of them, depending on the initial guess we provide. Therefore, a good initial estimate of the optimal parameters is needed. Figure 2) illustrates the onvergene of two trajetories towards the loal minima.

4 u 1 1 s = P 0 3 b 1 P 2 0 = 3 1/3 1/3 1/3 apple 1 apple 2 s u 0 1 e Fig. 1: Towards the generation of the optimal Bézier omposite urve aording to model 5. Left: start (s) and end (e) poses, and a topologial path that traverses ells ( 0, 1, 2 ) and rosses borders (b 1, b 2 ), are given. The ontrol points P0,1,2,3 define the urve. They have four degrees of freedom at boundaries (α 1, s 1, u 1 1, u 0 2 at b 1 ) and one DOF at start and end (u 0 1 at s). Center: The trajetory is initialized after omputing the shortest path (dashed). Right: the G 2 trajetory is obtained by elevating the optimized 4th order trajetory (blak ontrol points) to 6th order (gray ontrol points). The 6th order ontrol points 2 and 3 are moved slightly along their onneting line to make the urvature ontinuos at b 1 and b 2. Initialization: The initial guess is given by the following heuristis (see Figure 1 (enter)), whih gives satisfatory results even if the parameters are not further optimized. First, we find the shortest path (ignoring headings) from s to e that rosses (b 1,..., b n ), whih is a trajetory γ 0 made of line segments, one for eah ell. The parameters s are hosen in order to plae the points P i 0 at the intersetion of γ 0 with eah boundary. The angles α are initialized as a midway between the angles of the two inident segments of γ 0. u 1,2 are set by plaing the ontrol points P2 i and P3 i at one third of the distane between P0 i and P 3 i (or on the ell borders if they would lie outside of the ell i ). D. G 2 trajetory 4th order omposite Bézier urves are not generally G 2 beause of disontinuities in the urvature at the joins. The order of the segments should be inreased at least to 6 to adjust the urvature at one endpoint without modifying the derivative and the urvature at the other end. We apply the following proedure to transform the optimal 4th order urve to a 6th order G 2 urve. The shape and ost C(γ) will not be hange signifiantly. The degree of eah segment is elevated to 6th order [20], (i.e., we find 6 ontrol points that define the same Bézier urve). Then, for eah join, we ompute the urvatures κ i (0) and κ i 1 (1) on the two sides (see Equation IV-B), whih should be equal for the urve to be G 2. If they are different, we set the target urvature at boundary b i to { κ i κi (0)κ = i 1 (1), if κ i (0)κ i 1 (1) 0 (7) 0, else and move the ontrol point P i i 2 along the line passing by P2 and P i 3 as neessary to ahieve urvature κ i (see Figure 1 (right)). Similarly, on the other side of the boundary, P i 1 3 is moved along the line passing by P i 1 3 and P i 1 2. For some trajetories, whih are unommon on indoor building maps, this proess may not be possible beause the resulting ontrol point would lie outside of the ell. In those ases, we have to rely on other smoothing tehniques. E. Optimal trajetories for indoor buildings a) Doors and narrow passes: Maps of indoor buildings generally have more struture than just being olletion of ells. For instane, they ontain orridors and rooms separated by doors. We use these harateristis to add additional onstraints to the ontrol points of trajetories in indoor buildings. In partiular, we want to ensure that the wheelhair passes perpendiularly in the middle when rossing doors. That is, α i = π/2 and s i = 1/2 for all i where i or i 1 are doors. This way of proeeding brings two advantages. First, legibility and preditability of the wheelhair trajetory near doors inrease, where good negotiation of shared rossing with humans is ritial. Seond, the optimization problem is split into smaller problems, sine the path (b 1,..., b n ) an be subdivided into m smaller hains ( (b1,..., b n1 ), (b n1,..., b n2 ),..., (b nm 1,..., b nm ) ), (8) for whih the trajetory is perpendiularly onstrained into the middle point at b ni, i = 1,..., m. The optimization of one hain is independent from the other hains. We further impose the same onstraints on ell boundaries that are very narrow, losing a little bit of freedom but reduing omputational omplexity. If all boundaries in a hain are onstrained, like a sequene of doors, optimization is trivial and results in a straight line. b) Trajetories graph: Contrary to start and end pose, whih are only known at query time, all onstrained boundaries an be identified at initialization. Moreover, the optimal trajetory between them an be pre-omputed. In fat, they form the edge of a graph G opt with the nodes onsisting of all doors and narrow passages. Based on these insights, the performane of path omputations at query time an be greatly speed up proeeding as following. Instead of optimizing the trajetory for the full topologial path, the topologial path is first subdivided into hains as desribed above. Then, the optimization is ran for the first and last hains (i.e., from s to the first door and from the last door to e), while the optimized intermediate segments from G opt are retrieved from a pre-omputed ahe. In this

5 way, omputational osts beome almost independent of the number of boundaries rossed. V. MULTI-OBJECTIVE PATH PLANNING In general, there may be multiple topologial paths onneting two ells in the navigation layer. In this ase, looking for the minimal urvature ost C(γ) while negleting other geometri features may be too limited. As a matter of fat, humans prefer short and less urved trajetories. Thus, we formulate a multi-objetive problem that looks for trajetories of minimal length L(γ) and ost C(γ). A dominated solution of a multi-objetive problem is a solution for whih there is at least another solution that is better with respet to every objetive. No smart strategist would hoose a dominated solution. Therefore, one general way to takle multi-objetive problem is to ompute the set of non-dominated solutions, the Pareto front [22]. Computing the Pareto front of optimal trajetories between poses s and e is done as follows. For simpliity, let us assume that different trajetories have different osts. We will use Yen s single soure k-shortest path algorithm [23] to ompute the next best simple path on G opt between s and e with respet to a given (positive) edge ost. First, we add to G opt all optimal trajetories from s and to e that originate from a node of G opt and do not ross any other node. Then, Yen s algorithm is applied, alternating between osts L and C. At eah step, if the returned solution is already part of the Pareto front, the iteration stops. Else, unless dominated, the solution is added to the Pareto front. The algorithm terminates beause the number of simple paths is limited but does not need to hek them all. In fat, it terminates as soon as it reahes a solution that is know to be better then the suessive solutions with respet to both objetives. If an approximation of the Pareto front is suffiient, we keep the query time limited by stopping the algorithm after a fixed number of iterations. In the ase different trajetories have the same ost, the algorithm is slightly modified to stop only after all solutions with same osts are heked to be already part of the Pareto front. VI. IMPLEMENTATION Floor maps are usually provided in CAD formats. Our ustom software let us onvert them to IndoorGML map and automatially add a navigation layer for wheelhairs, partitioned in navigable onvex ells. The planner has been implemented in Python making use of the following libraries: sipy for the COBYLA solver, networkx for the Yen s k-shortest path algorithm, and shapely for the omputation of geometri onstraints. The auray of the COBYLA algorithm an be tuned. We report in Setion VII-A a test for the trade-off between auray and omputational osts. The roboti wheelhair is ontrolled through a ROS interfae. The planner is integrated into move base, the ROS pakage for mobile robots navigation, as a global planner plugin. One optimized, the trajetory is sampled at fixed distanes and the resulting list of poses is passed to the move base loal planner. The same ROS interfae is used to ontrol a TurtleBot robot. VII. EXPERIMENTS We report experimental results that evaluate the trajetory optimization performane and illustrate one example of solution for the multi-objetive planner. All experiments are performed on a single ore modern arhiteture CPU. A. Syntheti map We begin by measuring the performane of the planner on a syntheti map, made by a suession of orners similar to a snake (see Figure 2). Two examples of trajetory optimization, shown on the top, onverge slower (top right) and faster (top left) to the optimal solution. Figure 3 (left) reports how the omputational ost grows almost quadratially with the length of the topologial plan. The optimization solver an be tuned for faster or more aurate results. Figure 3 (right) illustrates the trade-off between omputational ost and auray for paths of fixed length of 4 borders (i.e., with 18 degrees of freedom to optimize). For the experiments illustrated in the following, the planner is tuned for auray. B. Real building map We investigate the real world performane by testing the planner on the map of our building, whih is 120 meters long. The navigation layer was obtained as desribed in Setion III by inflating walls by 40 m and then deomposing the navigable spae into onvex ells. It ontains about 750 node (ells) and 770 edges (ell s boundaries). As detailed in Setion IV-E, the planner initialization preomputes all trajetories between onstrained boundaries, whih form the edges of the graph G opt. For our building, this requires about 3 minutes to ompute 2000 trajetories. Figure 4 (left) displays the full map (top left) and an exerpt of the navigation layer together with trajetories in G opt. Note how the urves originate perpendiularly from the middle of doors. The planner performane is higher than on a syntheti map. On the one hand, the trajetories are more onstrained (more doors and narrow passes). Therefore they an be split into hains of limited size. The total omputational ost beomes almost linear in the length of the path (see Figure 4 (enter)). On the other hand, at query time, we take advantage of G opt to ompute only the first and last part of a trajetory. Thank to this ahing, the omputational ost beomes almost onstant and remains well under 1 seond for almost all queries. This onfirms that the planner is well suited for online planning for robots moving in real buildings. C. Multi-objetive planning example Figure 4 (right) displays the solution for a single multiobjetive (C vs L) planning problem between two poses in our building. For this ase, the algorithm (Setion V) terminates after three paths have been evaluated and return a Pareto front omposed of two solutions. Only one additional

6 Fig. 2: Syntheti map with two examples of trajetory optimization. Darker olors mean further optimization (higher omputational ost). ratio ost to ratio best ost ost to best ost path length (number path length of boundaries (number of rossed) boundaries rossed) Fig. 3: Performane of trajetory on the syntheti map. Left: omputational ost for optimizing trajetories between random poses (50 per length) of inreasing (topologial) length. Average ± one standard deviation shaded in grey. Right: trade-off between omputational ost and urvature ost for 100 trajetories of length 4 between random poses. dominated solution is omputed. The planner give us the hoie between the longer (left) and urvier (right) trajetory. D. Robot navigation The robot loalizes itself with a depth sensor, on the same IndoorGML map used by the planner. We onfigure the move base loal planner to keep its ontribution to the final robot trajetory negligible. Figure 5 and the aompanying video show some of the trajetories performed by the TurtleBot robot moving autonomously in our building. We ompare our planner the default move base global planners: navfn, a grid searh planner, whih optimizes a navigation funtion. The two planners have different goals. In fat, navfn is not trying to find a differentiable trajetory and is meant to be used together with a loal planner that refines the trajetory. Still, it is interesting to note how qualitatively different the plans and the robot s traes are when using the two planners. Our planner looks for smooth trajetories that turn gently, making use of the free spae. They are easier to predit for a human observer and more similar to those produes by a skilled human driver. VIII. CONCLUSIONS We disussed how to take advantage of indoor GIS maps to: (i) build a topologial navigation graph, (ii) optimize a trajetory, (iii) perform multi-objetive path planning to selet short but omfortables trajetories for wheelhairs. We investigated the performane of the planner on syntheti maps. We showed that the planner is suitable for real time planning for robots in real world buildings. After the first trials with TurtleBots, whih we reported, we are urrently arrying out experiments with atual roboti wheelhairs. We would like to evaluate the user s omfort and the aeptane of the roboti wheelhair by people sharing the spae with it. The design of experiments that isolate the ontribution of the path planner is hallenging. For instane it requires that the ontroller, whih let the wheelhair follow the planned trajetory while avoiding dynamial obstales, work so reliable to be negligible. Additionally, we fous our urrent researh on more omplex planning problems, where we take into aount in addition to the trajetory geometry personal and soial osts and risks to traverse spaes. REFERENCES [1] Open Geospatial Consortium In., OGC IndoorGML v.1, [2] S. Gulati and B. Kuipers, High performane ontrol for graeful motion of an intelligent wheelhair, in 2008 IEEE International Conferene on Robotis and Automation (ICRA), [3] Alma, [4] Z. Liu, D. Chen, and G. von Wihert, 2D Semanti Mapping on Oupany Grids. in ROBOTIK, [5] A. Nühter and J. Hertzberg, Towards semanti maps for mobile robots. Robotis and Autonomous Systems, 2008.

7 C( ) [1/m] omputational ost [s] without ahe L( )[m] with ahe topologial length (number of rossed boundaries) Fig. 4: Left: part of our building IndoorGML map with the optimal trajetories forming the edges of G opt. Cells are olored by type (orridors green, rooms yellow, doors orange). Center: the average and standard deviation of the omputational ost to optimize 1000 random trajetories vs. the topologial length of the trajetory: full trajetory optimization at query time (squared blue markers) and optimization that uses pre-omputed G opt (irular green markers). Right: multi-objetive optimal paths between two poses (grey irles with arrow): Pareto optimal trajetories (solid lines) and the dominated solution (dashed line). Top Right: osts of the three trajetories Pareto front (blak irle), dominated solution (grey squares). Fig. 5: Left: qualitative omparison of the trajetories omputed by our planner (solid blak lines) and by the navfn planner (dashed grey lines). Center and right: the trajetories followed by the robot, whih suffers slightly from inauraies in loalisation and in atuation (enter: our planner, right: navfn planner). The trajetories are featured in the aompanying video too. [6] R. Li, L. Wei, D. Gu, H. Hu, and K. D. MDonald-Maier, Multilayered map based navigation and interation for an intelligent wheelhair, in ROBIO, [7] Y. Morales, A. Watanabe, and F. Ferreri, Inluding human fators for planning omfortable paths, in 2015 IEEE International Conferene on Robotis and Automation (ICRA), [8] J. M. M. Tur, C. Zinggerling, and A. C. Murtra, Geographial information systems for map based navigation in urban environments, Robotis and Autonomous Systems, [9] J.-C. Latombe, Exat Cell Deomposition, in Robot Motion Planning, [10] B. Chazelle and D. P. Dobkin, Deomposing a Polygon into its Convex Parts, STOC, [11] S. Ön and A. Yazii, A omparative study of smooth path planning for a mobile robot onsidering kinemati onstraints, in INISTA, [12] A. Piazzi, C. G. Lo Biano, and M. Romano, Smooth Path Generation for Wheeled Mobile Robots Using η 3 -Splines, in Motion Control, [13] H. Delingette, M. Hebert, and K. Ikeuhi, Trajetory generation with urvature onstraint based on energy minimization. IROS, [14] Y.-J. Ho and J.-S. Liu, Collision-free urvature-bounded smooth path planning using omposite Bezier urve based on Voronoi diagram, in CIRA, [15] J.-W. Choi, R. Curry, and G. Elkaim, Pieewise Bézier Curves Path Planning with Continuous Curvature Constraint for Autonomous Driving, in Mahine Learning and Systems Engineering, [16] S. Wang, L. Chen, H. Hu, and K. MDonald-Maier, Sensor-based dynami trajetory planning for smooth door passing of intelligent wheelhairs, in CEEC, [17] J. Guzzi, A. Giusti, L. M. Gambardella, G. Theraulaz, and G. A. Di Caro, Human-friendly robot navigation in dynami environments. in ICRA, [18] J.-H. Hwang, R. C. Arkin, and D.-S. Kwon, Mobile robots at your fingertip: Bezier urve on-line trajetory generation for supervisory ontrol, in IROS, [19] K. J. Li and J. Lee, Indoor spatial awareness initiative and standard for indoor spatial data, in IROS, [20] H. Prautzsh, W. Boehm, and M. Paluszny, Bézier and B-Spline Tehniques. Springer, [21] M. J. D. Powell, A Diret Searh Optimization Method That Models the Objetive and Constraint Funtions by Linear Interpolation, in Advanes in Optimization and Numerial Analysis, [22] J. Branke, K. Deb, K. Miettinen, and R. Słowiński, Eds., Multiobjetive Optimization. Springer, [23] J. Y. Yen, Finding the K-Shortest Loopless Paths in a Network, Management Siene, 1971.