A Fast On-line Global Path Planning Algorithm Based on Regionalized Roadmap for Robot Navigation *

Proceedings of the 20th World Congress The International Federation of Congress Automatic Control Proceedings of the 20th World The International Federation of Automatic Control Toulouse, France, July 2017 Proceedings of the 20th9-14, World Congress The International Federation of Automatic Control Toulouse, France, July 9-14, 2017 Available online at www.sciencedirect.com The International of Automatic Control Toulouse, France,Federation July 9-14, 2017 Toulouse, France, July 9-14, 2017

ScienceDirect

IFAC PapersOnLine 50-1Global (2017) 319–324 A Fast On-line Path Planning A Fast On-line Global Path Planning A Fast On-line Global Path Planning Algorithm Based on Regionalized Roadmap A Fast On-line Global Path Planning Algorithm Based on Regionalized Roadmap Algorithm for Based on Regionalized Roadmap   Robot Navigation Algorithm for Based on Regionalized Roadmap Robot Navigation  for Robot Navigation  for Robot Navigation Chaoliang Zhong ∗∗ Shirong Liu ∗∗ Botao Zhang ∗∗ Qiang Lu ∗∗

Chaoliang Zhong ∗ Shirong Liu ∗ Botao Zhang ∗ Qiang Lu ∗ ∗ ∗ ∗ Jian Wang Gao Chaoliang Zhong Shirong Liu Wu Botao Zhang Qiang Lu ∗ Qiuxuan ∗ Farong ∗ Jian Wang Farong Gao ∗ ∗ Qiuxuan Wu ∗ ∗ ∗ ∗ Chaoliang Zhong Liu Wu Botao Zhang Qiang Lu ∗ Jian WangShirong Qiuxuan Farong Gao ∗ Jian Wang ∗ Qiuxuan Wu ∗ Farong Gao ∗ ∗ Intelligent Mobile Robotics Laboratory of Hangzhou Dianzi Mobile Robotics Laboratory of Hangzhou Dianzi ∗ Intelligent University, Zhejiang, 310018, (e-mail: Intelligent Mobile,, Robotics of Hangzhou University, Zhejiang,Laboratory 310018, China China (e-mail: Dianzi ∗ Intelligent Mobile Robotics Laboratory of Hangzhou (zhongchaoliang,liushirong, University, , Zhejiang, 310018, China (e-mail: Dianzi (zhongchaoliang,liushirong, University, , Zhejiang, 310018, China billow,lvqiang,wangjian,wuqx,frgao)@hdu.edu.cn). (zhongchaoliang,liushirong, (e-mail: billow,lvqiang,wangjian,wuqx,frgao)@hdu.edu.cn). (zhongchaoliang,liushirong, billow,lvqiang,wangjian,wuqx,frgao)@hdu.edu.cn). billow,lvqiang,wangjian,wuqx,frgao)@hdu.edu.cn). Abstract: Abstract: Inspired Inspired by by the the ‘fine-to-coarse’ ‘fine-to-coarse’ way-finding way-finding strategy strategy that that human human utilized utilized in in the the process of navigation, the paper proposed a fast on-line global path planning algorithm based Abstract: Inspired by the ‘fine-to-coarse’ way-finding strategy that human utilized in the process of navigation, thethe paper proposed a way-finding fast on-line global path planning algorithm based Abstract: Inspired by ‘fine-to-coarse’ strategy that human utilized in the on regionalized roadmap. First, a regionalized roadmap(RRM) that has a multi-layered structure process of navigation, the paper proposed a fast on-line global path planning algorithm based on regionalized roadmap. First, a regionalized roadmap(RRM) that hasplanning a multi-layered structure process of navigation, the paper proposed a fast on-line global path algorithm based is for environments. Then, the FTC-A* algorithm designed onproposed regionalized roadmap. First, a regionalized roadmap(RRM) that has a multi-layered is for representing representing environments. Then, the RRM RRM based based FTC-A* algorithm is is structure designed onproposed regionalized roadmap. First, a regionalized roadmap(RRM) that has ayet multi-layered structure to plan an FTC-route(‘fine-to-coarse’ route) with being fine in vicinity coarse at a distance. is proposed for representing environments. Then, the RRM based FTC-A* algorithm is designed to plan an FTC-route(‘fine-to-coarse’ route) with being fine in vicinity yet coarse at a distance. is proposed for representing environments. Then, the RRM based FTC-A* algorithm is designed This algorithm can be applied to on-line global path planning in navigation system of mobile to plan an FTC-route(‘fine-to-coarse’ route) with being fine in vicinity yet coarse at a distance. This algorithm can be applied to on-line global path in navigation system mobile to plan anvehicles. FTC-route(‘fine-to-coarse’ route) with beingplanning fine in vicinity yet coarse at a of distance. robots or Finally, the simulation and physical experiments have been carried out This algorithm can be applied to on-line global path planning in navigation system of mobile robots or vehicles. Finally, the simulation and physical experiments have been carried out to to This algorithm can be proposed applied to on-line global path planning in navigation system of mobile show the efficacy of the path planning algorithm which can be applied to such occasions robots or vehicles. Finally, the simulation and physical experiments have been carried out to show the efficacy of the proposed path planning algorithm which can be applied to such occasions robots orefficacy vehicles. Finally, the simulation andalgorithm physical experiments been carried out to as large-scale environment and changes of show the of the proposed path planning which can behave applied to such occasions as large-scale environment and dynamic dynamic changes of the the destination. destination. show the efficacy of the proposed path planning algorithm which can be applied to such occasions as large-scale environment and dynamic changes of the destination. © IFAC (International of Automatic Hosting by Elsevier Ltd. All rights reserved. as 2017, large-scale environmentFederation and dynamic changesControl) of the destination. Keywords: Keywords: Navigation,Path Navigation,Path Planning,Regionalized Planning,Regionalized Roadmap Roadmap Keywords: Navigation,Path Planning,Regionalized Roadmap Keywords: Navigation,Path Planning,Regionalized Roadmap 1. but 1. INTRODUCTION INTRODUCTION but there there are are serious serious problems problems about about its its performance performance when the environment scale is very big, the 1. INTRODUCTION but there are serious problems about its because performance when the environment scale is very big, because the 1. INTRODUCTION but there are serious problems its performance number grids increases rapidly with the size of when theof environment scale is about very big, because the Robot navigation can be regarded as a process that comnumber of grids increases rapidly with the size of the Robot navigation can be regarded as a process that com- when the environment scale is very big, because the environment(Augustine et al. (2012)). Therefore, to reduce number of grids increases rapidly with the size of the bines global path planning with local motion planning. Robot navigation can be regarded as a process that comenvironment(Augustine et al. (2012)). Therefore, to reduce bines global path planning with local motion planning. number of grids increases rapidly with the size of the Robotglobal navigation can be environmental regarded as a process that comthe computation complexity, multi-layer map structure environment(Augustine et al. (2012)). Therefore, to reduce According to the priori map, global path bines path planning with local motion planning. computation complexity, multi-layer map to structure According to the priori environmental map, global path the environment(Augustine et al. (2012)). Therefore, reduce bines global planning with local motion planning. and hierarchical search algorithm are proposed, such the computation complexity, multi-layer map structure planning plans a path which is often consist of a series of According to path the priori environmental map, global path and hierarchical search algorithm are proposed, such as as planning plans a path which is often consist of a series of the computation complexity, multi-layer map structure According to the priori environmental map, global path HA*(Holte et al. (1996)), HPA*(Botea et al. (2004)) and hierarchical search algorithm are proposed, such as position nodes. Then the robot can move from one position planning plans Then a paththe which iscan often consist of a series of HA*(Holte et al. (1996)), HPA*(Botea et al. (2004)) and and position nodes. robot move from one position and hierarchical search algorithm are proposed, such as planning plans a the path which iscan often consist of a series of HA*(Holte CFA*(Lee and Yu (2009)). These algorithms improve their et al. (1996)), HPA*(Botea et al. (2004)) and to another along path before arriving at the end point. position nodes. Then the robot move from one position and Yu(1996)), (2009)). HPA*(Botea These algorithms improve their to another alongThen the path before arriving at the end point. CFA*(Lee HA*(Holte et al. et al. (2004)) and position nodes. the robot can move from one position efficiency through down the search CFA*(Lee and Yu narrowing (2009)). These their Therefore, global path planning functions as guide of to another along path before arriving at the point. through downalgorithms the size size of of improve search space. space. Therefore, globalthe path planning functions as a aend guide of efficiency CFA*(Lee and Yu narrowing (2009)). These their to another along the path before arriving at the end point. efficiency through narrowing downalgorithms the size of improve search space. the robot’s movements. However, when the robot begins Therefore, global path planning functions as a guide of The above path planning algorithms return to a complete the robot’s global movements. However,functions when theasrobot begins efficiency through narrowing down the size of search space. Therefore, path planning a guide of The above path planning algorithms return to a complete to move along the established path, it carries out local the robot’s movements. However, when robot global path. But in process navigation, a to move along the established path, it the carries outbegins local The above to a complete themove robot’s movements. However, when the robot path.path Butplanning in actual actualalgorithms process of ofreturn navigation, a comcommotion planning based on the environment to along theonline established path, itlocal carries outbegins local global The above path planning algorithms return toquickly a complete plete global path is not a must. It is enough to plan motion planning online based on the local environment global path. But in actual process of navigation, a comto move along the established path, it carries out local plete global path is not a must. It is enough to quickly plan information so that the robot is able to avoid any obstacle motion planning online based on the local environment global path. But isinnot actual process of that navigation, a comout an incomplete yet effective path can guide the information so that the robot is able to avoid any obstacle plete global path a must. It is enough to quickly motion planning online based on the local environment out an incomplete yet aeffective path that to can guideplan the to ensure safe and reliable movement. Still, if the target information so that the robot is able to avoid any obstacle plete global path is not must. It is enough quickly plan robot to move immediately to the right direction. Studies to ensure safe and reliable movement. Still, if the target out an incomplete yet effective path that can guide the information so and that changes the robot is ablethe to avoid any obstacle robot to move immediately to the right direction. Studies or the destination during motion, then the to ensure safe reliable movement. Still, if the target out an incomplete yetbeings effective path that canincomplete guide the find out that human are planning an or the destination changes during the motion, then the robot to move immediately to the right direction. Studies to ensure safe and changes reliable movement. if the target that human beingstoare planning an incomplete robot to employ a priori environment model to rapidly or thehas destination during the Still, motion, then the find robotout to move immediately themotion right direction. Studies path on-line to guide their own in the process robot has to employ a priori environment model to rapidly find out that human beings are planning an incomplete or thethe destination changes during the motion, the path on-line tohuman guide beings their own in an the incomplete process of of plan global path from the current position to the robot has to employ a priori environment model tothen rapidly find out that aretomotion planning navigation, which allows them quickly plan out plan the global path from the current position to the path on-line to guide their own motion in the process of robotdestination has to employ a priori environment model to rapidly navigation, which allows them tomotion quicklyinplan out paths paths new online, making the robot quickly move plan the global path from the current position to the path on-line to guide their own the process of and move to the destination under the circumstances of new destination online, making the robot quickly move navigation, which allows them to quickly plan out paths plan the global path from the the current position tomove the and move towhich the destination under the circumstances of by responding to the new instruction. On-line global path new destination online, making robot quickly navigation, allows them to quickly plan out paths big-scale environment and change of targets(Wiener and by responding to the new instruction. On-line global path and move to the destination under the circumstances of newresponding destination online, making the robot quickly move big-scale environment and change of targets(Wiener and planning holds strict limitation on the time taken by path by to the new instruction. On-line global and move to the destination underofIn the circumstances of Mallot (2003)Wiener et al. (2004)). the process of navplanning holds to strict limitation on the On-line time taken by path big-scale environment and change targets(Wiener and by responding the new instruction. global path (2003)Wiener etand al. (2004)). the process of navplanning. planning holds strict limitation on the time taken by path Mallot big-scale environment change ofIn targets(Wiener and igation, human has used the regionalized spatial model planning. Mallot (2003)Wiener et al. (2004)). In the process of navplanning holds strict limitation on the time taken by path igation, human has used regionalized spatial ofmodel planning. Mallot (2003)Wiener et al.the (2004)). In the process navand adopted the ‘fine-to-coarse’ way-finding strategy to igation, human has used the regionalized spatial model The on-line planning. adopted the ‘fine-to-coarse’ way-finding strategy to The on-line global global route route planning planning process process involves involves two two and igation, human has used the regionalized spatial model plan an incomplete route from the current position to the and adopted the ‘fine-to-coarse’ way-finding strategy to aspects: environment map and path planning algorithm. The on-line global route planning process involves two plan an incomplete route from the current position to the aspects: environment map and path planning algorithm. and adopted the ‘fine-to-coarse’ way-finding strategy to Thetoon-line global route planning process involves two plan destination, called ‘fine-to-coarse’ route(FTC-route). an incomplete route from the current position to the As map modeling, metric map(Rudan et al. (2010)), aspects: environment map and path planning algorithm. destination, called ‘fine-to-coarse’ As to map modeling, map metric map(Rudan et al. (2010)), plan an incomplete route from theroute(FTC-route). current position to the aspects: environment andmap(Rudan pathDellaert planning algorithm. destination, called ‘fine-to-coarse’ route(FTC-route). topological map(Ranganathan and (2011)) and As to map modeling, metric et al. (2010)), way-finding we topological map(Ranganathan and Dellaert (2011)) and Inspired called ‘fine-to-coarse’ route(FTC-route). As to map map(Ranganathan modeling,etmetric map(Rudan et al. (2010)), Inspired by by human human way-finding strategy, strategy, we seek seek to to build build a a the hybrid et al. topological and Dellaert and destination, robot navigation that integrates regionalized spathe hybrid map(Wu map(Wu et al. al. (2014)Schmuck (2014)Schmuck et(2011)) al. (2016)) (2016)) Inspired by humansystem way-finding strategy, we seek to build topological map(Ranganathan and Dellaert (2011)) and robot navigation system that integrates regionalized spa-a serve as the commonly-used map modeling methods. As the hybrid map(Wu et al. (2014)Schmuck et al. (2016)) Inspired by human way-finding strategy, we seek to build a tial representation and path to find serve as themap(Wu commonly-used map modelingetmethods. As robot navigation system that integrates regionalized the hybrid etmetric al. (2014)Schmuck al.has (2016)) tial representation and efficient efficient path planner planner to try try to to spafind one common type of map, grid map been serve as the commonly-used map modeling methods. As robot navigation system that integrates regionalized spaaa solution of global path planning problem. one common type of metricmap map, grid map has been representation and efficient planner to try toFirst, find servecommon asapplied the commonly-used modeling methods. As tial solution of on-line on-line global pathpath planning problem. widely into the area of modeling, one metric grid map map has been tial representation and efficient path planner to try toFirst, find we present a regionalized roadmap(RRM) to describe the widely appliedtype intoof the area map, of robot robot map modeling, awesolution of on-line global path planning problem. First, one common type of metric map, grid map has been present of a regionalized roadmap(RRM) to describe the widely applied into the area of robot map modeling, we  a solution on-line global path planning problem. First, search environment. Then, an innovative on-line global present a regionalized roadmap(RRM) to describe the This work was supported in part by the Natural Science  widely applied the area of robot modeling, search environment. Then, an innovative toon-line global This work was into supported in part by the map Natural Science we present a regionalized roadmap(RRM) describe the  Foundation of was Zhejiang Province under Grant LY17F030022 and incomplete path planning algorithm is proposed, called search environment. Then, an innovative on-line global This work supported in part by the Natural Science Foundation Zhejiang Province under Grant LY17F030022 and incomplete path planning algorithm is proposed, called  This workof ,and search environment. Then, an innovative on-line global LY16F030007 in part by the National Natural Science Founwas supported in part by the Natural Science Foundation Zhejiang Province Grant LY17F030022 and ‘fine-to-coarse’ A* algorithm(FTC-A*). on prior incomplete path algorithm isBased proposed, LY16F030007of ,and in part by the under National Natural Science Foun‘fine-to-coarse’ A*planning algorithm(FTC-A*). on the thecalled prior dation of China under Grant 61503108 andGrant 61375104. Foundation of ,and Zhejiang Province LY17F030022 and incomplete path algorithm isBased proposed, LY16F030007 in part the under National Natural Science Foun‘fine-to-coarse’ A*planning algorithm(FTC-A*). Based on thecalled prior dation of China under Grantby61503108 and 61375104. LY16F030007 ,and in part by the National Natural Science Foundation of China under Grant 61503108 and 61375104. ‘fine-to-coarse’ A* algorithm(FTC-A*). Based on the prior

dation of China under Grant 61503108 and 61375104. Copyright © 2017 IFAC 321 Copyright © 2017, 2017 IFAC 321 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) 321 Copyright © 2017 IFAC Peer review under responsibility of International Federation of Automatic Copyright © 2017 IFAC 321Control. 10.1016/j.ifacol.2017.08.053

Proceedings of the 20th IFAC World Congress 320 Chaoliang Zhong et al. / IFAC PapersOnLine 50-1 (2017) 319–324 Toulouse, France, July 9-14, 2017

regionalized spatial information, described by RRM, FTCA* algorithm plan a global incomplete ‘fine-to-coarse’ route(FTC-route). The route is temporarily stored and can be fetched by the target generator to produce the next closest goal for the goal executor to execute until the closest goal is reached. By updating robot’s location and then re-planning the FTC-route, the robot always have the ability to develop a detailed plan for movement decisions along the route. In navigation, mobile robots operate in the alternate mode of “plan-move-plan-move...”. They move to the next position according to the planned path and then plan the path from this position to the target position. Under the guidance of path planning, mobile robots finally reach the target position. The rest of the paper is organized as follows. In SectionII, a regionalized spatial representation model to describe the environment is presented, called regionalized roadmap(RRM). In Section III, inspired by human’s wayfinding mechanism, a FTC-A* algorithm is presented. Simulation and physical experiments are given in section IV. Finally, the conclusion is made in Section V.

2. REGIONALIZED ROADMAP Abundant research results(Wiener et al. (2004)Wiener and Mallot (2003)) have led to the hierarchical theories that the structure of spatial information in the memory system is regionalized and nested with multi-levels. Hierarchical theories demonstrated that the environments are divided into regions and smaller regions are grouped together to form superordinate regions at the next hierarchy level which yields a tree-like structure. But there are other relations between different spatial objects, making the whole spatial representation model more like a graph-like structure. This section proposes a new model, i.e. the regionalized roadmap(RRM), which is used to describe the environment representation. Fig. 1 uses the RRM to describe a simple environment. From this figure, one can see that the simple environment is divided into 4 big regions and each region consists of 4 places each, altogether 16 places in the environment, as shown in Fig.1(b). The places were interconnected by road. Fig.1(a) uses a RRM to describe the small environment. We can see from Fig.1(a) that RRM, which describes the environment in an abstract way, consists of two primary types of components: the spatial nodes and the relations between them. The spatial nodes denote the recognizable units in the space, such as bed, room, house, city, and so on. The RRM needs to be nested, which means that a spatial node can include other spatial nodes. For instance, the spatial node 17 includes 1, 2, 3 and 4, and itself is included in the node 21. Thus, RRM can be defined as follows. Definition 1. (RRM). Let C be a set of spatial objects. Then a spatial node O can be described as a couple O = (N, R), in which: (1) N = {O1 , O2 , · · · , Ok } is a set of spatial nodes included directly by O and this set is connected, and meets the condition that ∀i(Oi ∈ O) and O ∈ C. (2) R is a set of the relations owned by node O, and R ⊆ (N ∪ O) × C.

322

Fig. 1. An example of a regionalized roadmap of a simple environment A spatial node is denoted as O = (N, R), according to the Definition1, where N is a set of spatial nodes included directly by O and R is a set of relations owned by node O. O is called a parent node of each node in the set N . Each node in N can be called a child node of the node O. Therefore, it can be seen that RRM is a nested model. The expression R ⊆ (N ∪ O) × C allows relation R to be able to represent not only the mutual relations inside the region(just like there is a route linking spatial nodes 11 and 12), but also the relations with spatial nodes outside the region (like there is a road linking 19 and 20), thus the cross-regional connections can be described. The spatial nodes can be divided into two categories. In the first category, the node, whose child nodes are empty (N = ∅), is called place node, such as the node 11 in Fig.1(a). In the other category, the node, whose child noes are not empty (N = ∅), is called region node, such as the node 19. In this paper, the grid method is employed to divide the experimental environment and to build the regionalized roadmap. To build RRM, there are three main steps: first, the road information expressed in terms of discrete GPS points can be collected by driving the robot along the road; second, based on the road data collected in the environment, a road network is generated using Growing Neural Gas(GNG) algorithm; and third, the whole environment is divided into regions by grid segment method and represented into a RRM with two-layer structures in this paper. The sub-layer is the road network, and the uplayer is the connective relation layer of regions. 3. “FINE-TO-COARSE” ROUTE AND ITS PLANNING ALGORITHM Previous sections focus on representation of regionalized spatial environments. The following subsections integrate its application into robot navigation system. This section starts with a description of the ‘fine-to-coarse’ route(FTCroute), and then presents a innovative FTC-A* algorithm to plan the FTC-route.

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Chaoliang Zhong et al. / IFAC PapersOnLine 50-1 (2017) 319–324

3.1 “Fine-to-Coarse” Route

321

Algorithm 1 The pseudocodes of the FTC-A* algorithm

Studies on the cognitive mechanism of human navigation show that human beings use regionalized spatial representations in their navigation, and influenced by the regionalized spatial representation, the ‘fine-to-coarse’ wayfinding strategy is mainly used by human in the process of navigation(Wiener and Mallot (2003)Nayak et al. (2011)). Different with the traditional route planning method, the strategy plans a route with being fine in vicinity yet coarse at a distance, called fine-to-coarse route (FTC-route). As shown in Fig.2, a FTC-route from the current start position 11 to the final goal 6 can be planed as a route of 11-9-17-18. Except the place-place connection (P-P connection, i.e. 11-9), FTC-route planning also should take place-region connection (P-R connection, i.e. 9-17), regionregion connection (R-R connection, i.e. 17-18) or regionplace connection (R-P connection) into consideration.

Fig. 2. A fine-to-coarse route with being fine in vicinity yet coarse at a distance 3.2 FTC-A* Algorithm Path can be described as a sequence of nodes that need to be visited in order, which can be wrote as (n1 → n2 → · · · → ni ). Inspired by the way-finding strategy of ‘fineto-coarse’ in human navigation, the paper proposes an FTC-A* algorithm to plan an FTC-route based on the A* algorithm. FTC-A* can effectively utilize the nested spatial structure to generate a FTC-route that is fine in vicinity yet coarse at a distance. The pseudocodes of the FTC-A* algorithm can be found in Algorithm 1. FTC-A* algorithm has three inputs: S representing the current start node, T the place of destination, RRM the regionalized roadmap. This derivative algorithm is different from A* algorithm mainly in the following aspects: • Line 8 is the termination condition of FTC-A* algorithm, meaning that if the BestN ode is the destination T , or T is inside the regional spatial object BestN ode, then this algorithm will consider that it has found a FTC-route. • A* algorithm gives a detailed route and can guide the robot to move to the target. But FTC-A* algorithm takes region-connectivity into account to gain a route with being fine in vicinity yet coarse at a distance and it is not based on place-connectivity alone. • FTC-A* algorithm can be regarded as an extension of A* algorithm. If RRM degenerates into a regular planimetric environmental model, then FTC-A* will degenerate into a general A* algorithm. 323

Require: Regionalized roadmap, RRM ; Current start node, S; Target node, T ; Ensure: The node list of the best FTC-route, F tcRoute; 1: add S to OpenList; 2: while OpenList is not empty do 3: remove best node BestN ode from OpenList; 4: add BestN ode to ClosedList; 5: if OpenList is empty then 6: return failure; 7: end if 8: if BestN ode is T or T is in the region object BestN ode then 9: return construct route and save it in F tcRoute; 10: end if 11: for each successor SuccessorN ode of BestN ode do 12: set BestN ode’s previous node to P reN ode; 13: if P reN ode = N U LL and SuccessorN ode’s parent object P arentN odeS is not equal to P arentN odeB which is the parent object of BestN ode then 14: set the P arentN odeS to SuccessorN ode; 15: end if 16: if SuccessorN ode is in ClostList, continue; 17: if SuccessorN ode is not in OpenList, then add itset BestN ode as the previous node of SuccessorN ode, and calculate the cost of SuccessorN ode; 18: if SuccessorN ode is in OpenList, then set N ewCost to sum of BestN ode cost and cost of moving from BestN ode to SuccessorN ode, and then whether N ewCost is less than SuccessorN ode cost or not is judged. if yes, set BestN ode as the previous node of SuccessorN ode, and calculate the cost of SuccessorN ode; 19: end for 20: end while

• Line 13 is the key to generating a ‘fine-to-coarse’ route and lowering the size of searching space. The adoption of the idea to search in bottom-up way makes FTC-A* tend to search in the more coarsegrained spatial information. The condition of jump to upper layer consists of two parts. When both conditions are met at the same time, replace the successor node SuccessorN ode with its parent object P arentN odeS , and the route search will be raised to an upper level. 4. SIMULATION AND EXPERIMENTS A simulation case and a physical experiment have been carried out to validate the feasibility and effectiveness of the algorithm. We also present three indexes to evaluate the performance of the online planning. 4.1 On-line Planning Performance Metrics During the movement of the robot, the destination might change and thus replan a new path from the current position to the new destination. That’s why global online path planning becomes a problem that must be considered. But online path planning is highly demanding on planning time, especially when it involves in a large problem size, frequently changing destination and limited memory space and CPU resource. Hence, to investigate the on-line planning performance of the planning algorithms, we defines

Proceedings of the 20th IFAC World Congress 322 Chaoliang Zhong et al. / IFAC PapersOnLine 50-1 (2017) 319–324 Toulouse, France, July 9-14, 2017

three performance metrics: memory usage, planning time and path length. • Memory usage: The sum of OpenList size and ClostList size indicates the memory usage of an algorithm. • Planning time: It refers to the time consumed (measured in milliseconds) from staring planning to completing planning. Though FTC-A* is used to plan incomplete paths while A* plans complete paths, we can still use this metric to measure the response capability of a robot to the new task commands. • Path length: The mileage of traveled path from the robot’s initial position to the final destination point. 4.2 Performance Comparison in A Large-scale Complex Environment In this section, the planning performance and effect of FTC wayfinding strategy are analyzed and compared with the A* and Hierarchical A* by planning four different paths in a large-scale complex environment. The Simulation Environment As presented in Fig.3, a 18km*13km large-scale complex environment is created, which includes 5000 place nodes and is divided into 12 regions as A, B, C, D, E, F, G, H, I, J, K, and L. Four black dots signify four different starting places as Start 1, Start 2, Start 3 and Start 4. And the yellow five-pointed star is the destination. The robot moves to the destination from different starts respectively. By comparing and analyzing FTC-A*, A* and HA* on the three performance metrics as memory usage, planning time and path length, it can be verified that FTC-A * algorithm have excellent planning performance and effect.

plans the route on the upper layer first and then refines the route on the next layer, so it is hard to ensure optimal path length. For example, Fig.4(d) shows that the route planed by HA*on the upper layer (the connection layer of area nodes) is the best one, however, the one refined is not the optimal, which is 0.99km longer than that by A* and 0.69km longer than that by FTC-A*. It can be seen from the results of the four plans that FTC strategy is difficult to ensure optimal final route. Table I shows that when Start 3 is the start point, the length of the final route by FTC-A* is 19.82km, 0.28km longer than that by A*, and 0.22km longer than that by HA*. However, FTC-A* manifests huge advantages on the other two performance metrics as memory usage and planning time. For example, when Start 1 is the starting point, the biggest memory usage of FTC-A* is 720 nodes, while that of A* and HA* are 2423 and 1825 nodes respectively, which are 3.37 and 2.53 times that of FTC-A*. The longest time that FTC-A* algorithm takes is 7.15ms, while that of A* and HA* algorithms are 69.29ms and 31.03ms, being 9.69 and 4.34 times of that of FTC-A* algorithm. While the average time of FTC-A* algorithm is only 1.74ms, far less than those of A* and HA* algorithm, which presents that FTC-A* algorithm has excellent response performance.

Fig. 4. Four planned routes using A*,HA* and FTCA* algorithms in an large-scale complex environment with 5,000 places. The red line represents the motion trajectory driven by FTC wayfinding strategy and the blue one and black one are the paths planed by A* algorithm and HA* algorithm, respectively. 4.3 Physical Experiment in Campus Environment

Fig. 3. An simulation environment with the size 18km × 13km. The environment consists of 5000 places and is divided into 12 regions. Four black dots signify four different starting places and yellow star is the destination. Results Figure4 presents the routes from Start 1, Start 2, Start 3 and Start 4 to the destination. Table I shows the performances and their comparison of the three algorithms in the 4 routes. It can be seen from Fig.4 that routes of FTC-A*and A* are almost overlapped and their lengths are almost the same. When starting from Start 4, the route based on FTC-A* algorithm even overmatches that based on HA*, which is caused by the fact that HA* is a kind of algorithm difficult to ensure optimal path length. HA* 324

During autonomous navigation, the mobile robot usually encounters the issue of target changes when the robot is moving. At that time, the mobile robot has to plan rapidly a reasonable and feasible route from current position to the new destination or target, and it is done online. For comparison of the online planning performance of the A*, HA*, and FTC-A* algorithms, we made a mobile robot’s navigation experiment where A* and HA* also need to replan their routes when the PowerBot robot moves to a place node. Physical Robot Platform We installed the GNSS/IMU integrated navigation system (KY-INS100 Kit) on the PowerBot mobile robot. The KY-INS100 Kit can provide high precise outdoor positioning and heading data. The PowerBot robot is also equipped with sonar sensors, bump sensors, a laser rangefinder and cameras. The computing

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Chaoliang Zhong et al. / IFAC PapersOnLine 50-1 (2017) 319–324

323

Table 1. Comparison of computing performance between the three algorithms in a large-scale complex environment* . Tasks Start 1

Start 2

Start 3

Start 4 *

PM

A*

HA*

MU(nodes) PT(ms) PL(km) MU(nodes) PT(ms) PL(km) MU(nodes) PT(ms) PL(km) MU(nodes) PT(ms) PL(km)

2423 69.29 24.61 2177 65.11 22.06 1677 53.54 19.54 1884 59.51 20.69

1825 31.03 24.61 1630 30.23 22.06 1313 23.51 19.60 1475 24.04 21.68

FTC-A* Max Avg 720 273 7.15 1.74 25.12 707 256 7.03 1.70 22.41 639 206 6.74 1.68 19.82 431 166 5.62 1.14 20.99

Fig. 6. Connective relation layer of region nodes. The black dot is the location of the region node.

PM, MU, PT and PL are the abbreviations of “performance metrics”, “memory usage”, “Planning time” and “path length”, respectively.

platform for the navigation system is a laptop with a CPU of i5-2540M. The Campus Environment and Its Regionalized Roadmap The Xiasha Campus of Hangzhou Dianzi University was used as the outdoor experimental environment as illustrated in Fig.5, where the red line is the road network composed by 1000 place nodes, and each node contains latitude and longitude data. The whole experimental environment is divided into 100 regions by grid segment method. Based on RRM, this environment is represented as a two-layer structure that is provided for the mobile robot as the priori environment knowledge. The sub-layer is the road network, and the up-layer is the connective relation layer of regions, as shown in Fig.6, where the black dot is the location of the region node. The black dot in Fig.5 is the start location; red star refers to destinations of Goal. Every time the robot moves to a closest goal, suppose the destination will be changed, thus the robot must re-plan its route from current position to the destination. And then, the online planning performance of the A*, HA*, and FTC-A* algorithms can be compared.

performances. The memory usage and planning time of these three algorithms during the whole navigation are shown in Fig.8 and Fig.9, respectively. The horizontal axis represents the total 162 place nodes that the robot passes when moving from the start to the end in sequence. From Fig.8 and Fig.9, we can see that the memory usage and planning time of FTC-A* algorithm, compared with A*and HA* algorithms, are all significantly reduced. From Fig.8, it can be seen that influenced greatly by the scale of entire environmental nodes, the memory usage of A*and HA* algorithms are on the decline. However, the memory usage of FTC-A* algorithm is greatly affected by each individual region nodes, maintaining a stable trend during the entire navigation. Figure 9 indicates that similar to memory usage, the planning time of FTC-A* algorithm has little relation with the scale of the entire environmental nodes, but is seriously affected by the scale of individual region nodes. Compared with A*and HA* algorithms, the planning time of FTC-A* algorithm is remarkably reduced. By comparing Fig.8 with Fig.9, we can notice that the planning time of HA* is even higher than that of A* when the memory usage of HA* and A* are close. This is because HA* plans path on the upper level and then fines it on the next level. This process increases the computing consumption, thus increasing the planning time. All in all, the simulation and physical experiments show that FTC-A* algorithm can limit the researching space to one specific region, thus notably reducing the memory usage and planning time and improving researching efficiency, therefore it can be applied to occasions in which the destinations are under dynamic changes.

Fig. 5. Real-world experimental environment. The environment consists of 1000 place nodes and is divided into 100 regions. Black circle is the starting location. The red start is the destination location. Results The robots moving trajectory from the start point to the end point is represented by red line in Fig.7. In this experiment, the moving routes based on A*, HA* and FTC-A* algorithms almost are all the same, but three algorithms have very big difference in terms of computing 325

Fig. 7. The movement trajectory of the robot in the realworld environment.

Proceedings of the 20th IFAC World Congress 324 Chaoliang Zhong et al. / IFAC PapersOnLine 50-1 (2017) 319–324 Toulouse, France, July 9-14, 2017

Fig. 8. Comparison of Memory Usage during navigation by three algorithms. The horizontal axis represents the place nodes that the robot passes in turn, and the vertical axis the memory usage during each planning.

Fig. 9. Comparison of Planning Time during navigation by three algorithms. The horizontal axis represents the place nodes that the robot passes in turn, and the vertical axis the planning time during each planning. 5. CONCLUSION References(Wiener and Mallot (2003)) puts forward for the first time the idea that human beings use the wayfinding strategy of ‘fine-to-coarse’ in navigation from the perspective of biological spatial cognition. However, there is no a explicit computing model. In this paper, some consideration is given to how to carry out effective and rapid online path planning in a large-scale environment. We drawn some lessons from Reference(Wiener and Mallot (2003)) and come up with a explicit computing model that makes a effective and significant attempt to rapidly solve online path planning by putting the way-finding strategy of ‘fine-to-coarse’ into application. Compared with the current A* and HA* algorithms with good performance, it is shown that the FTC-A* is featured by the advantages of small search space, short computation time, light memory load and little delay in motion response, which can be applied to such occasions as large-scale complex environment and dynamic changes of the destination. REFERENCES Augustine, M., Ortmeier, F., Mair, E., Burschka, D., Stelzer, A., and Suppa, M. (2012). Landmark-tree map: a biologically inspired topological map for long-distance robot navigation. In Robotics and Biomimetics (ROBIO), 2012 IEEE International Conference on, 128– 135. IEEE. Botea, A., Mller, M., and Schaeffer, J. (2004). Near optimal hierarchical path-finding. Journal of Game Development, 1, 7–28. 326

Cetin, O., Zagli, I., and Yilmaz, G. (2013). Establishing obstacle and collision free communication relay for uavs with artificial potential fields. Journal of Intelligent & Robotic Systems, 69(1-4), 361–372. Daniel, K., Nash, A., Koenig, S., and Felner, A. (2010). Theta*: Any-angle path planning on grids. Journal of Artificial Intelligence Research, 39(1), 533–579. Holte, R., Perez, M., Zimmer, R., and Macdonald, A. (1996). Hierarchical A*: Searching Abstraction Hierarchies Efficiently. In Proceedings of the National Conference on Artificial Intelligence, volume 1, 530–535. MIT Press Cambridge,MA,USA, August 4-8,Portland, Oregon. Huang, H.P. and Chung, S.Y. (2004). Dynamic visibility graph for path planning. In Intelligent Robots and Systems, 2004.(IROS 2004). Proceedings. 2004 IEEE/RSJ International Conference on, volume 3, 2813–2818. IEEE. Koenig, S., Likhachev, M., and Furcy, D. (2004). Lifelong planning a*. Artificial Intelligence, 155(1), 93–146. Lee, J.Y. and Yu, W. (2009). A coarse-to-fine approach for fast path finding for mobile robots. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, 5414–5419. Moon, C.b. and Chung, W. (2015). Kinodynamic planner dual-tree rrt (dt-rrt) for two-wheeled mobile robots using the rapidly exploring random tree. IEEE Transactions on Industrial Electronics, 62(2), 1080–1090. Nayak, S., Mishra, V., and Mukerjee, A. (2011). Towards a cognitive model for human wayfinding behavior in regionalized environments. In AAAI Fall Symposium: Advances in Cognitive Systems. Park, B., Choi, J., and Chung, W.K. (2012). An efficient mobile robot path planning using hierarchical roadmap representation in indoor environment. In Proceedings of 2012 IEEE International Conference on Robotics and Automation, 180–186. Ranganathan, A. and Dellaert, F. (2011). Online probabilistic topological mapping. The International Journal of Robotics Research, 1–17. Rudan, J., Tuza, Z., and Szederk´enyi, G. (2010). Using lms-100 laser rangefinder for indoor metric map building. In Industrial Electronics (ISIE), 2010 IEEE International Symposium on, 525–530. IEEE. Schmuck, P., Scherer, S.A., and Zell, A. (2016). Hybrid metric-topological 3d occupancy grid maps for largescale mapping. IFAC-PapersOnLine, 49(15), 230–235. Wiener, J.M. and Mallot, H.A. (2003). ’fine-tocoarse’route planning and navigation in regionalized environments. Spatial Cognition and Computation, 3(4), 331–358. Wiener, J.M., Schnee, A., and Mallot, H.A. (2004). Use and interaction of navigation strategies in regionalized environments. Journal of Environmental Psychology, 24(4), 475–493. Wu, H., Tian, G.h., Li, Y., Zhou, F.y., and Duan, P. (2014). Spatial semantic hybrid map building and application of mobile service robot. Robotics and Autonomous Systems, 62(6), 923–941.