Batch Theta for path planning to the best goal in a goal set

Advanced Robotics ISSN: 0169-1864 (Print) 1568-5535 (Online) Journal homepage: http://www.tandfonline.com/loi/tadr20 Batch-Theta* for path planning to the best goal in a goal set Viet-Hung Dang, Nguyen Duc Thang, Hoang Huu Viet & Le Anh Tuan To cite this article: Viet-Hung Dang, Nguyen Duc Thang, Hoang Huu Viet & Le Anh Tuan (2015) Batch-Theta* for path planning to the best goal in a goal set, Advanced Robotics, 29:23, 1537-1550, DOI: 10.1080/01691864.2015.1073121 To link to this article: http://dx.doi.org/10.1080/01691864.2015.1073121 Published online: 08 Oct 2015 Submit your article to this journal Article views: 70 View related articles View Crossmark data Full Terms & Conditions of access and use can be found at http://www.tandfonline.com/action/journalInformation?journalCode=tadr20 Download by: [RMIT University Library] Date: 10 April 2016, At: 22:39 Advanced Robotics, 2015 Vol 29, No 23, 1537–1550, http://dx.doi.org/10.1080/01691864.2015.1073121 FULL PAPER Batch-Theta* for path planning to the best goal in a goal set Viet-Hung Danga∗ , Nguyen Duc Thangb , Hoang Huu Vietc and Le Anh Tuand a Institute of Research and Development, DuyTan University, Da Nang, Vietnam; b Department of Biomedical Engineering, International University, Vietnam National University-HiChiMinh City, Vietnam; c Department of Information Technology, Vinh University, Nghe An, Vietnam; d Department of Automotive Engineering, Vietnam Maritime University, Hai Phong, Vietnam Downloaded by [RMIT University Library] at 22:39 10 April 2016 (Received 20 November 2014; revised July 2015; accepted July 2015) The development of 3D cameras and many navigation-supporting sensors has recently enabled robots to build their working maps and navigate accurately, making path planning popular not just on computer graphics, but in real environments as well Pursuing the solutions for robot path planning, this paper presents a variant of searching method Theta* for choosing the best goal among given goals and the lowest-cost path to it, called Batch-Theta* The novelty lies at the proposed line-of-sight checking function during the searching process and the manner that the method handles the batch of goals during one search instead of repeatedly considering a single goal or blindly doing the exhausted search The analysis and simulations show that the proposed Batch-Theta* efficiently finds the lowest-cost path to the best goal in a given goal set under Theta*’s mechanism Keywords: shortest path; Batch-Theta*; Theta*; Lazy-Theta*; multi goals; goal set; line-of-sight Introduction Planning a path to a goal through a sequence of grid points has been studied and implemented for decades Yet, addressing the shortest path planning problem from a position to the nearest goal in a set of goals has not received much attention in spite of its importance in a wide range of applications It can be found in the tools for routing electronic circuits when the software wants to wire a point to one of the others that share the connectivity on a board It is also useful for most strategy games such as Age-Of-Empire, Heroes, Star-Craft, etc., in which the computer players must frequently make wise decisions to find a short path to the nearest resource mine from the house, choose where to retreat the troops for medical aid, select the nearest safe market to go for trading, or assign the workers to chase the nearest moving resource, etc (by nearest we mean having the shortest non-collision path in this paper) Because the computation for a real-time strategy game is huge and the task is performed regularly, even a slight improvement of reducing computational time is crucial Moreover, path planning to one among several goals is also necessary for robotics In specific applications, robots are designed to serve food and beverages, collect litter, or guide visitors in large sites like entertainment parks These robots must be capable of figuring out the shortest path to one among several desired positions such as restaurants, rubbish bins, or especially battery recharge stations For automatic controls of moving indoors, the vehicles may ∗ Corresponding author Email: dangviethungha@gmail.com © 2015 Taylor & Francis and The Robotics Society of Japan demand an optimal route through a set of targets selected by a user, which can be solved with a greedy manner via iterations of planning the path to the best target in the set and dropping it out Besides, the problem can be found in robots imposed by a coverage mission like cleaning, mowing, mine sweeping, or harvesting A workspace is usually decomposed into a grid where a robot needs to visit all of the accessible grid points Since this coverage mission is an NP-hard problem [1,2] (a vertex must be visited at least once), approaches for coverage path planning so far have been using strategy motions such as spiral motion [3–5] or boustrophedon motion [6–8] to cover the workspace part by part After one strategy motion, the robot has to choose the nearest unvisited grid point for the next strategy motion In another application of motion planning, a robot must find a set of consecutive connected configurations while avoiding blocked ones, usually by sampling-based motion planning methods.[9] In reality, although there are many target configurations, the robot needs to achieve only one Obviously, path planning to one among multiple goals is found in many important applications Constructing the path to the best goal in a goal set must rely on the well-developed path planning techniques to one goal, which can be classified into two types: static planning where the environment is assumed to be stable, and dynamic planning where the previous planning results are utilized for re-planning to deal with environment changes Downloaded by [RMIT University Library] at 22:39 10 April 2016 1538 V.-H Dang et al For static planning approaches, Dijkstra [10,11] is the simplest version that only uses cost values for the incremental search to the nearest goal A* [12,13] improves Dijkstra by adding heuristic evaluation to narrow the search space However, the result returned by A* is commonly a zigzag path through a set of neighboring grid points, which is not effective for robot routing and controlling Many A* variants have been developed to make the path found more suitable in practice, that means the path does not connect a set of adjacent points but a set of line-of-sight ones Some use A* for the rough path, then smooth it up as in A*PS [14,15] while others insert the smoothing process into the searching iterations like Theta*, consisting of BasicTheta* and Angle Propagation.[16,17] Theta* has then been upgraded to Lazy-Theta* [18] in which the computational cost is reduced because the line-of-sight calculation between a point and its parent is delayed until actually necessary instead of being computed as soon as a new point is added into the list of candidates to be expanded next Overall, optimality is not guaranteed by Theta* and several techniques have been introduced to improve Theta*’s path lengths, like re-expanding points, changing the weight of the heuristic value,[17] or using the visibility graphs for the true shortest path.[19](chapter 15),[17] Nevertheless, efforts of improving path length so far involve much additional computation.[17] Meanwhile, dynamic planning methods such as Lifelong Planning A* [20], D* [21], D* Lite [22], Dynamic Fringe saving A* [23], blocked A* [24], Filed D* [25], etc are mainly developed from A* In fact, the dynamic methods always require a static path planning in both of the initializing and re-planning phases for adapting to environment changes Therefore, although the solution of finding the best path to the nearest goal like the one proposed in this paper focuses on a static method, it can be utilized to improve other dynamic planning approaches as well when multiple goals are dealt with Conventionally, determining the lowest cost path to the best goal requires either an exhausted path search around the starting point until the nearest goal is found or the path search to each of the goals for later decision Both approaches are inefficient because the number of expanded points is high and many points are expanded more than once Obviously, it is necessary to investigate a path planning method that can mitigate this inefficiency while retaining the loop structure of the techniques listed above to avoid recursive search in big work spaces Specifically, at each loop a new point is expanded, the path to it is updated, and its neighbors are considered to be put in a waiting list for further expansion In addition, the method must aim at minimizing both path length results and searching time as all path planning approaches This paper presents a new approach that fulfills the aforementioned requirements, namely Batch-Theta*, to form a path from a starting point to one among goals in a known squared grid 2D map In such, all of the goals are considered in each loop and the smoothing phase is constructed in parallel via iterations Moreover, a new development of line-of-sight checking and parent indicating is proposed to improve the performance of Theta*’s variants in terms of path length, expanded space size and runtime Analysis in Section 3, simulations and discussions in Section show that Batch-Theta* gives better performance compared with other current best methods, which are reviewed in Section 2 Backgrounds of heuristic path planning methods 2.1 Heuristic path planning framework Generally, static path planning methods like Dijkstra, A*, Theta*, and their variants share the main structure of Algorithm 1, where the cost is commonly the path length since it is approximately proportional to other costs like fuel consumption or moving time These algorithms usually have to maintain two main lists including open_list and closed_list while exploring the work space They repeatedly expand one point after another and consider adding its neighbors into open_list for further expansion The closed_list stores all expanded points to ensure that no point is expanded more than once Besides, a point’s relationships to the expanded points and the information of the point’s cost f (s ) are also stored and updated for better paths through it The algorithms loop until the goal is reached or open_list is empty When the goal is found, by tracing back along the points’ relationships, the path is constructed The two functions U pdate Relation(s , par ent) and U pdateCost Estimation(s , f ) examine whether the current expanded s is a better parent for s by the measurement of f (s ) If yes, they replace the previous parent and the total cost estimation of s with s and f (s ), respectively In fact, A* is a generalized form of Dijkstra while Theta* [16,17] is an informed variant of A* Dijkstra chooses a point for expanding based only on the lowest cost from the starting point to it, denoted with g(s) and called g-value, which can be recursively presented by g (sstar t ) = g s = s= par ent(s ) g (s) + c s, s , (1) as illustrated in Figure 1, where c(s, s ) is the minimum cost to move the agent from s to s If the cost is evaluated by distance measurement, then c(s, s ) is the shortest path length from s to s Meanwhile, at the current point s, A*’s variants evaluate the cost from the starting point sstar t via s to the given goal sg This evaluation is conducted by taking not only the g-value, but also the cost estimation from s to sg , called heuristic function or h-value, denoted with h(s), into account A point’s cost now is the summation of these two costs f (s) = g(s) + h(s) (2) Advanced Robotics 1539 Algorithm 1: Heuristic path planning framework Data: sstar t , sgoal Result: f ound_ path, f ound_ path_length g(sstar t ) = 0; par ent (sstar t ) = sstar t ; open_list = sstar t ; f (sstar t ) = g(sstar t ) + h(sstar t , sgoal ); closed_list = ∅; while open_list = ∅ s = arg ( f (si )); Figure Two weaknesses of Theta*’s variants Red lines denote found paths and dashed lines optimal paths Downloaded by [RMIT University Library] at 22:39 10 April 2016 si ∈open_list open_list = open_list\ {s}; if s == sgoal then return [ f ound_ path, f ound_ path_length]; else closed_list = closed_list ∪ {s}; foreach s ∈ succ(s) if s ∈ / closed_list then if s ∈ / open_list then open_list = open_list ∪ {s }; par ent = U pdate Relation(s , par ent); f = U pdateCost Estimation(s , f ); end end return “No path found”; Since the path constructed by A* is through adjacent grid points, it is unnecessarily long and entails many robot heading changes Therefore, there have been proposals for smoothing the path, including post processing variants [14, 15] and Theta*’s variants.[17,18] Figure Cost function construction of common heuristic search algorithms, consisting of either real minimum cost g(s) or estimated cost h(s) or both Solid lines denote the direct connections while dashed lines denote relations which are not necessarily direct connections The heuristic function h(s) plays the role of guiding the direction of expanding during the search It has been proved that if h(s) is admissible, or h (s) ≤ c s, sg , (3) then the algorithm will find the lowest-cost path to the goal point sg [26] The straight line distance between two different positions would never be longer than a real path connecting them; thus, the Euclidean distance between s with coordinate (s.x, s.y) and sg with coordinate (sg x, sg y) is usually used as the h-value h (s) = Ed s, sg , (4) where Ed s, sg = (s.x − sg x)2 + (s.y − sg y)2 (5) 2.2 Theta* and Lazy-Theta* [16–18] Theta* upgrades A* by inserting the smooth mission into the search iterations It tries to choose the better parent for s from not only the direct neighbor s, but also the parent of s via line-of-sight detection This recursive assigning allows Theta* to determine the parent of s with some faraway ancestor and give a better path which does not pass through a sequence of adjacent points as usual but a sequence of line-of-sight ones According to [17], Theta* includes Basic-Theta* andAngle-Propagation Since BasicTheta*’s performance is better than that of the latter, it gets more attention and discussion in this paper Overall, parent indication based on line-of-sight in BasicTheta* has two weaknesses shown in Figure First, although point a is visible to point b, in order for b to get a as a parent, all the intermediate points expanded from a to b must have line-of-sight to a If this is not satisfied, b takes some intermediate point as its parent, i.e b1 Second, when constructing a path from a to c, Basic-Theta* expands c3 and chooses c3 as c’s parent since there is no line-of-sight between a and c Later on, c’s parent will be updated as c1 due to c1 ’s lower evaluated cost In other words, the path from a to c via c2 can never be constructed because a point’s parent is only either its direct neighbor or its direct neighbor’s parent Although Lazy-Theta* improves Theta*’s runtime and is the fastest among Theta*’s variants, it retains the two weaknesses mentioned When a point is added into open_list, Lazy-Theta* determines its cost as if it were visible to the parent of its direct neighbor The line-of-sight information is examined only when this point is expanded This implies while Basic-Theta* performs several line-of-sight checks for a new point, Lazy-Theta* does so only once Consequently, Lazy-Theta* is faster, but may return worse cost evaluations which increase the search space size and the path length 1540 V.-H Dang et al Downloaded by [RMIT University Library] at 22:39 10 April 2016 Figure w > does not guarantee admission condition Bold blue lines indicate the found paths while the thin blue lines denote the relations of expanded points In other words, the h-value is not computed by considering a specific goal but the entire set of goals h(si ) = Figure Example results of applying Basic-Theta* and lazyTheta* Bold blue lines present the found paths while the thin blue lines denote the relations of expanded points f (s) = g(s) + w.c(s, sg ) (6) However, the re-expanding approach increases computational cost remarkably Meanwhile, choosing w depends strongly on specific maps and tuning it for the shorter path length always needs much runtime [17] as a trade-off In addition, w > does not guarantee the admission condition as illustrated in Figure where the result path is lengthened with the mediate point (16,24) To overcome those drawbacks, this paper will introduce an idea of deciding a better parent for a point without both of the tuned parameter w and additional line-of-sight computation, in order to enhance our method’s efficiency Proposed Batch-Theta* 3.1 Cost evaluation In order to handle a set of goals in each expanding loop, we change the way that Theta*s variants estimate a point’s cost and choose the next point to be expanded from open_list as follows s = arg si ∈open_list g (si ) + sg ∈goal_set Ed si , sg (7) Ed si , sg (8) It is obvious that the proposed h(si ) is admissible for any pair of si and sg , where sg belongs to the given goal set Indeed, sg ∈goal_set The resulting path can also be improved by using a reexpanding approach without closed_list, or by reducing the contribution of the h-value via the parameter w [17]: sg ∈goal_set Ed si , sg ≤ Ed(si , sg ) sg ∈goal_set , (9) thus h(si ) = minsg ∈goal_set Ed si , sg is never overestimated from any si to any goal sg in the goal set As mentioned before, this leads to the fact that when some specific goal sg is reached, the constructed path from sstar t to sg is guaranteed to be the best to sg It is remarked that this cost evaluation is straightforward; however, the unsolved question in the literature with this cost evaluation is whether there exists a shorter collision-free path to another goal found by Theta*’s mechanism when the first goal is reached In this paper, Theta*’s mechanism is referred to as a search manner using (i) the cost evaluation in Equation (2) and (ii) the point’s parent indication of Basic-Theta* Following is the answer with detailed proofs theorem With h(si ) = minsg ∈goal_set Ed si , sg , the first reached goal is the one to which there is the shortest collision-free path from the starting point under Theta*’s mechanism Proof Note that our algorithm does not consistently explore a direction toward one goal, but changes the direction of expanding toward different goals during the search, making the g-value of the picked point inconsistently increase Therefore, we cannot prove the theorem based on the searching process as in Dijkstra, but on the result of this process by contradiction as follows When the first goal sg1 is reached, assume that there exists another path to goal sg j , ≤ j ≤ n in the goal set which is shortest, and of course is shorter than the one just found, or Downloaded by [RMIT University Library] at 22:39 10 April 2016 Advanced Robotics 1541 Figure Expanding behaviors of (a) Dijkstra, (b) Theta*, and (c) Batch-Theta* when trying to find the shortest path to a goal in the goal set g sg j < g sg1 (10) First of all, it can be concluded that j = 1, since the path to sg1 is shorter than any other paths to sg1 as just mentioned Therefore, (11) sg j = sg1 Let t0 be the moment right before sg1 is popped out of open_list for expanding According to Algorithm 1, at t0 , sg1 is in open_list and its parent has already been checked and updated so that the cost from the starting point to sg1 is minimal Combined with the fact that sg1 ∈ goal_set, sg1 ’s cost value is f (sg1 ) = g(sg1 ) + sg ∈goal_set Ed sg1 , sg = g(sg1 ) (12) Supporting illustration is in Figure where sstar t is denoted by the big white dot, the sg ∈ goal_set by big black dots and the intermediate points along the shortest path connecting sstar t and sg j by small white dots Let this best path be Pb and assume that the grid resolution is high so that intermediate point sk can be regarded as on Pb Before t0 , considering all the points sk along Pb including the both ends, we have: f (sk ) = g(sk ) + sg ∈goal_set Ed sk , sg sk ∈Pb (13) and f (sk ) ≤ g(sk ) + Ed sk , sg j ≤ g(sg j ) sk ∈Pb (14) Equations (11), (12) and (14) lead to: f (sk ) < f (sg1 ) before t0 sk ∈Pb (15) In addition, Batch-Theta* always considers sstar t to be first expanded, thus sstar t ’s neighbor on this path, called sk1 , have been in open_list before t0 According to Equation (15), (16) f (sk1 ) < f (sg1 ) thus sk1 must be picked up for expansion and its next neighbor on the path must be added to open_list before t0 By continuously inferring, all the points on the shortest path from sstar t to sg j including the ends must be picked and expanded before t0 That means sg j must be reached before sg1 , which is f alse under the fact that sg1 is the first reached goal Therefore, there does not exist any path shorter than the path found to the first reached goal under Theta*’s mechanism In other words, when applying Theta* to plan the paths to individual goals, the best resulting path length is the one found by Batch-Theta*, although this path is not optimal due to Theta*’s limitations With this cost evaluation, Batch-Theta* can reduce the search space size while expanding a point only once Figure illustrates the search space of Dijkstra, Theta*, and Batch-Theta* in gray Dijkstra’s expanded region is the circle around the starting point due to an exhaustive search and in Theta* many points are expanded more than once (overlapping gray portions) Meanwhile, Batch-Theta* only expands to equivalently good candidate goals, with smaller non-overlapping search space 3.2 Modified line-of-sight check This part proposes an improvement of Theta* through the line-of-sight checking process Specifically, when putting 1542 V.-H Dang et al Downloaded by [RMIT University Library] at 22:39 10 April 2016 Figure When the first goal is reached by Batch-Theta*, it is proved that the resulting path to it is the shortest or there cannot exist another shorter path under Theta*’s mechanism If sg j is the nearest goal, it must be reached first Figure Illustrations for candidate parent indication of candit_L O S() Red bold lines represent parent–child relations and dashed lines denote the line-of-sight checks Algorithm 2: Function candit_L O S(s1, s2) a point s into open_list, Batch-Theta* avoids considering two sub paths as Theta* does It just performs the line-ofsight check from s to the parent of s If available, it assigns the parent of s to be the parent of s Otherwise, it indicates the blocked cell and chooses one out of the two points of this cell to be the parent of s This new procedure of checking is executed by the function candit_L O S(si , s j ), which returns the candidate parents for si without extra lineof-sight computation Figure supports illustrations of the parent indication of this function When b0 , whose parent is a, is expanded and the neighbor b is inserted into open_list, candit_L O S (b, a) is revoked to detect obstacles along the straight line from b to a via a sequence of on-edge points (small white points) If some obstacle blocks the line, the function stops checking and locates the position where the line hits the obstacle Then, the two nearest grid points, i.e b1 and b2 , will be considered as the candidates for b’s parent Besides, when candit_L O S(c, a) is called, c is the impact-point, then c1 and c2 are c’s parent candidates Obviously, the process of checking two sub-paths of Basic-Theta* is included in our method and if a line-of-sight is not present then our method chooses the parent of s more wisely Note that candit_L O S() can help the search overcome the second weakness mentioned in Section 2.2 and Figure In that situation (see Figure 2), Batch-Theta* will assign c2 as the parent of c when doing the line-of-sight check from c to a and shortens the path found For line-of-sight checking, the conventional function LineO f Sight () (see Appendix A of [17]) works well based on the drawing-line algorithm in computer graphics, finding all cells that a line goes through with calculation on integers Nevertheless, utilizing this algorithm structure to find the impact location is difficult because it requires examining too many situations while LineO f Sight () performs several block-cell checks in one iteration Therefore, we come up with another structure for our own purpose (see Figure 8) Data: s1, s2 Result: {candit}, los los = tr ue; candit = ∅; if s1.x == s2.x | s1.y == s2.y then return; d x = (s2.x − s1.x); dy = (s2.y − s1.y); sx = sign(d x); sy = sign(dy); x0 = s1.x; y0 = s1.y; f = f = mar k = 0; while (x0! = s2.x) if grid(x0 + (sx − 1)/2, y0 + (sy − 1)/2) then los = f alse; block = (x0 + (sx − 1)/2, y0 + (sy − 1)/2); break; if abs( f + d x) > ( f + dy) then f = f + dy; x0 = x0 + sx; mar k = 1; else f = f + d x; y0 = y0 + sy; mar k = 2; if f == f + dy then f = f 1; x0 = x0 + sx; mar k = 0; end if !los then if mar k == then candit = {(x0, block.y), (x0, block.y + 1)}; else if mar k == then candit = {(block.x, y0), (block.x + 1, y0)}; else if sx == sy then candit = {(block.x, block.y + 1), (block.x + 1, block.y)}; else candit = {(block.x, block.y), (block.x + 1, block.y + 1)}; candit = candit ∩ succ(s1) Advanced Robotics Downloaded by [RMIT University Library] at 22:39 10 April 2016 Figure Illustrations for checked cell indication of candit_L O S() which is able to return the hit point position based on the edge points along the line checked The main idea is locating the on-edge points along the line checked Let x be the pace along the x axis when y increases and y be the pace along the y axis when x increases An on-edge point along the line checked then has the coordinates of (x, y), in which x takes the form of either (s1.x + m x ) or (s1.x + n x ∗ x), and y has the form of either (s1.y + m y ) or (s1.y + n y ∗ y) where m x , n x , m y , and n y are integers The current point C(x0, y0) is s1 at the beginning When drawing a line from s1 to s2, the next edge point N is indicated as the nearest edge point from C and then C replaces N repeatedly until the line drawn meets a blocked cell or reaches s2 Note that the function grid (i, j) returns tr ue if the cell at position (i, j), which covers the square with four corners {(i, j) , (i + 1, j) , (i, j + 1) , (i + 1, j + 1)}, is a blocked cell In fact, this idea of checking procedure is slow due to floating point calculations and may fail due to the errors caused by the floating point numerical system in computer Fortunately, it can be interpreted to work with integers, avoiding floating point numbers and their errors since x and y are the fractions of integers Algorithm presents the pseudo code of function candit_L O S() which deals with integers only The variable mar k stores the information of how the previously and currently checked cells contact each other When a blocked cell is detected, this information is used to find the impact edge and the candidate parents Obviously, LineO f Sight ()’s structure is more complicated than ours in which the total number of grid() checks is unchanged and the parent candidate indication occurs only once outside the loops with simple assignments One may argue that if the candidate parents are not yet expanded or not have parent–child relationships to previous expanded points, the proposed line-of-sight check would fail the searching process However, the algorithm expands the workspace gradually from the starting point and picks the lowest f -value point to expand next Hence, if a point (e.g b0 in Figure 7) is already expanded and its neighbors 1543 (e.g b) are about to be inserted to open_list, then the intermediate points along the line checked (e.g b2 ) are both already expanded and visible to the point’s parent (e.g a) The proof of this claim can be found in [17] (Lemma 1) That means the neighbors of these intermediate points (e.g b1 ) have at least appeared in open_list Moreover, our method indicates a point’s parent and its cost evaluation before adding it to open_list Consequently, when the function candit_L O S returns the candidates, each of them already has a parent and the respective evaluated cost Therefore, the search algorithm with this modified line-of-sight check is guaranteed to work properly 3.3 Batch-Theta* algorithm The algorithm is described with pseudo code inAlgorithm Although Batch-Theta* shares the framework of heuristic search, it possesses several differences First, it has a new version of the line-of-sight checking function to utilize the information of the impact-point and returns the parent candidates for s The resulting candit is a list of at most two candidate parents when los is f alse (no line-of-sight) and is empty when los is tr ue Function candit_L O S allows not only s and s’s parent but also the intermediate points along the line checked to be the parent of s while additional line-of-sight computation is mitigated Second, our proposed idea gives an added point a chance for choosing a better parent, which helps eliminate the two functions U pdate Relation(s , par ent) and U pdateCost Esti − mation(s , f ) A point’s parent and its related f -value are examined only once when being added to open_list Besides, better g-value evaluation decreases the f -value of points on the path found, leading to a lower number of expanded points and runtime Third, Batch-Theta* handles all goals in each iteration; thus, it compresses the search area while guaranteeing that the first reached goal is the best goal Simulations and discussions All algorithms presented in this section were implemented in a Matlab platform on a computer equipped with 2.4 GHz Core i5-2430M CPU and 4GB RAM For comparison, they were pruned to follow the same main structure and to minimize redundancy (no console printing and figure plotting) This paper chooses two competitors including Basic-Theta* and Lazy-Theta* for comparison due to two reasons On the one hand, according to Daniel et al.’s work (Figure 2, Tables and of [17]), the Basic-Theta* gives the best path length results compared to other suboptimal methods like FD*, AP Theta*, and A* PS while requiring less time than FD* and AP Theta* Although A* PS’s runtime is shorter than Basic-Theta*, its path length results are poor (nearly 5% longer than that of Basic-Theta* with low obstacle density); thus, it is not considered as a good competitor here On the 1544 V.-H Dang et al Algorithm 3: Batch-Theta* Data: sstar t , goal_set Result: f ound_ path, f ound_ path_length g(si ) = ∞; g(sstar t ) = 0; par ent (sstar t ) = sstar t ; open_list = sstar t ; (Ed(sstar t , sgoal )); f (sstar t ) = g(sstar t ) + sgoal ∈goal_set closed_list = ∅; while open_list = ∅ s = arg ( f (si )); Figure Batch-Theta* assigns parent for added points wisely Downloaded by [RMIT University Library] at 22:39 10 April 2016 si ∈open_list open_list = open_list\ {s}; if s ∈ goal_set then return [ f ound_ path, f ound_ path_length]; closed_list = closed_list ∪ {s}; foreach s ∈ succ(s) if (s ∈ / closed_list) & (s ∈ / open_list) then open_list = open_list ∪ {s }; [{candit}, los] = candit_L O S(s , par ent (s)); if los then par ent (s ) = par ent (s); else par ent (s ) = candit (1); if g(candit (1)) + Ed(candit (1), s ) > g(candit (2)) + Ed(candit (2), s ) then par ent (s ) = candit (2); g(s ) = g( par ent (s )) + Ed( par ent (s ), s ); (Ed(s , sg )); f (s ) = g(s ) + sg ∈goal_set end end return “No path found”; other hand, Lazy-Theta* is currently the fastest improved variant of Theta* that has the best trade-off for path length (see [18]) That means it reduces the computational time significantly with inconsiderable path length increment 4.1 Efficiency of proposed line-of-sight check This simulation demonstrates the efficiency of the proposed function candit_L O S Figure and Table are the results of applying Batch-Theta* for one goal to solve the path planning example in Figure The blue thin lines and the bold lines denote the parent–child relations among the expanded points and the resulting path, respectively Compared to the results in Figure 3, every expanded point in Figure has the optimal parent and its g-value is evaluated correctly As a consequence, the path length found by Batch-Theta* in this example is 19.1658, equal to that by Basic-Theta* and shorter than that by Lazy-Theta* which is 19.2070 Moreover, since appropriate evaluation of the g-value can be constructed more directly by BatchTheta*, the number of intermediate points can be reduced (see Table 1) Hence, the total computational time of BatchTheta* is less than those of both Basic-Theta* and Lazy- Theta* thanks to less line-of-sight computation as well as to the absence of updating the relations and estimated values for the points in open_list In this case, Batch-Theta* needs around 0.028 s while Basic-Theta* and Lazy-Theta* around 0.043 and 0.034 s respectively Extensively, we use the conventional map of 500 × 500 grid cells in which the obstacle cells are set randomly at a probability of 10, 20, and 30% alternately The starting point and the goal are also chosen arbitrarily as long as there is a path between them Applying Basic-Theta*, Lazy-Theta*, and Batch-Theta* (for one goal in this simulation set) yields the comparisons in Table It is seen that although Lazy-Theta* gives a slightly longer path length and higher number of expanded points compared to Basic-Theta*, its runtime is considerably lower Lazy-Theta* requires at most only 88.46% of the runtime of Basic-Theta* for the 30% obstacle case and 66.71% for the 10% obstacle case Meanwhile, equipped with the proposed line-of-sight check, Batch-Theta* even needs less time than Lazy-Theta* in all tests while its results of path length are even better than those of Basic-Theta* The reason is rechecking and updating the information of points in open_list are performed many times by Basic-Theta* and Lazy-Theta* before the points are dumped into closed_list This repeated procedure contributes most of Basic-Theta*’s and Lazy-Theta*’s computational time while being eliminated with our method As a result, it gives better cost evaluation in a direct manner and narrows the search space While Lazy-Theta* cannot guarantee to reduce the search space, our method always does with nearly 20% reduction for the 30% obstacle case and even with only about 50% reduction for the 10% obstacle case In addition, Batch-Theta* can yield shorter path length because it overcomes the second drawback of Basic-Theta*’s variants mentioned in Section 2.2 Obviously, the improvements by our candit_L O S function in terms of search space size and runtime are considerable while no trade-off of length is paid Actually, in some situations, Batch-Theta* may find longer paths than Basic-Theta* because it does not fully recheck for better parents However, these situations appear with a low probability and on average, the mean path length found by our method is shorter With higher obstacle density, Advanced Robotics 1545 Table Performance comparison between Basic-Theta*, Lazy-Theta*, and Batch-Theta* on the example in Figure Method Basic-Theta* Lazy-Theta* Batch-Theta* Path length (cell_edges) Expanded point number Time (s) 19.1658 19.2070 19.1658 38 35 33 0.0431 0.0343 0.0276 Table The average percentage results for maps of size 500 × 500 comparing to those of Basic-Theta* when constructing path to one goal #Goal Method Downloaded by [RMIT University Library] at 22:39 10 April 2016 Lazy-Theta* Batch-Theta* Lazy-Theta* Batch-Theta* Lazy-Theta* Batch-Theta* Path length percentage 10% obstacles 100.06 99.93 20% obstacles 100.24 99.98 30% obstacles 100.54 99.99 Expanded point number percentage Time percentage 98.53 51.38 66.71 41.82 111.48 70.60 85.84 67.34 112.30 80.74 88.46 81.38 Figure 10 Comparison among Basic-Theta*, Lazy-Theta*, and Batch-Theta* in one goal situation in terms of path length, runtime, and expanded space size when tuning the weight of h-value w 1546 V.-H Dang et al Table The average percentage results of path length comparing to those of Basic-Theta* after 200 trials for 500 × 500 cell map “LOS() BT*” is the abbreviation of Batch-Theta* with LineOfSight() function and “candit_LOS() BT*” is that of Batch-Theta* with candit_LOS() function #Goal Method Lazy-Theta* LOS() BT* candit_LOS() BT* Downloaded by [RMIT University Library] at 22:39 10 April 2016 Lazy-Theta* LOS() BT* candit_LOS() BT* Lazy-Theta* LOS() BT* candit_LOS() BT* 10% obstacles 100.05 100.00 99.95 20% obstacles 100.28 100.00 99.98 30% obstacles 100.57 100.00 99.99 100.03 100.00 99.95 100.03 100.00 99.94 100.03 100.00 99.94 100.03 100.00 99.93 100.33 100.00 99.99 100.34 100.00 99.99 100.30 100.00 99.97 100.29 100.00 99.98 100.63 100.00 99.99 100.60 100.00 99.99 100.58 100.00 99.98 100.54 100.00 99.99 Table The average percentage results of memory for expanded points comparing to those of Basic-Theta* after 200 trials for 500 × 500 cell map #Goal Method Lazy-Theta* LOS() BT* candit_LOS() BT* Lazy-Theta* LOS() BT* candit_LOS() BT* Lazy-Theta* LOS() BT* candit_LOS() BT* 10% obstacles 105.86 60.74 34.84 20% obstacles 111.93 61.72 44.22 30% obstacles 112.68 66.44 53.32 104.98 46.39 25.45 106.13 30.72 18.01 104.21 24.78 14.96 105.96 21.17 12.48 111.83 37.83 26.99 111.64 29.29 20.78 111.64 21.07 14.98 111.62 18.48 13.18 113.29 44.64 35.04 113.39 32.55 25.50 113.37 27.60 21.59 113.42 24.95 19.69 Table The average percentage results of runtime comparing to those of Basic-Theta* after 200 trials for 500 × 500 cell map #Goal Method Lazy-Theta* LOS() BT* candit_LOS() BT* Lazy-Theta* LOS() BT* candit_LOS() BT* 30% obstacles Lazy-Theta* LOS() BT* candit_LOS() BT* 10% obstacles 71.42 50.30 21.42 20% obstacles 87.99 54.60 31.20 65.49 33.98 14.12 69.92 21.77 9.48 65.67 17.09 7.75 69.54 14.64 6.58 87.77 29.31 16.13 88.01 21.82 11.87 87.73 14.98 8.16 87.57 13.03 7.07 88.82 56.18 41.17 89.50 33.41 23.55 89.22 22.96 15.87 89.49 18.86 13.03 89.25 16.70 11.59 Downloaded by [RMIT University Library] at 22:39 10 April 2016 Advanced Robotics 1547 Figure 11 Runtime results of Basic-Theta*, Lazy-Theta*, Batch-Theta* with LineO f Sight (), and Batch-Theta* with candit_L O S() (a) 10% obstacle case, (b) 20% obstacle case, and (c) 30% obstacle case the improvement in terms of path length by our method is reduced (99.93 to 99.98% and to 99.99%) It is because the probability of the second situation in Figure becomes lower when the obstacle density increases Since tuning the weight of heuristic value in the cost evaluation strongly influences the runtime and path length results of Basic-Theta* and Lazy-Theta*, we conduct another simulation set related to the tuning w in Equation (6) for both Basic-Theta* and Lazy-Theta* However, we also apply the tuning weight to Batch-Theta* by multiplying w to its h-value: h(si ) = w sg ∈goal_set Ed si , sg (17) In this simulation, the map is also 500 × 500 grid cells with 30% obstacles, which gives the least improvement of Batch-Theta* over Basic-Theta* and Lazy-Theta* as seen in Table w gets the value of 0.9, 0.95, 1.0, 1.05, and 1.1 alternately in this simulation set The results are in Figure 10, in which each data point is the average result of 200 trials Evidently, little changes of w results in significant changes of the closed point number and the runtime Tuning up w from to 1.05 and 1.1 can reduce the runtime of both Basic-Theta* and Lazy-Theta* about 10 times and 18 times respectively, which makes them faster than normal Batch-Theta*(with w = 1) When tuning down w for all three methods, Basic-Theta* finds shorter paths than BatchTheta* does However, tuning w cannot help Lazy-Theta* dominate Batch-Theta* in terms of path length Meanwhile, Batch-Theta* requires less runtime and lower closed point number at all w in the range between 0.9 and 1.1 Moreover, it’s found path length is equivalent to that of Basic-Theta* when w < and is shorter than both Basic-Theta* and Lazy-Theta* when w > It is also remarked that when w > 1, because the expanded area decreases rapidly, the results of all three methods are asymptotic to that of the greedy method without g-value The runtime then is small, but the found path length is bad Obviously, applying the tuning w to Batch-Theta* together with Basic-Theta* and Lazy-Theta* will maintain the advantages of Batch-Theta*, especially the shorter runtime 4.2 Efficiency of Batch-Theta* This subsection presents the results of applying BasicTheta*, Lazy-Theta*, and Batch-Theta* without the tuning weight w to deal with multiple goals Both Basic-Theta* and Lazy-Theta* take each goal in turn, find the path to it, and memorize the current best path and length During the searching process to a goal, they will stop for another goal if an expanded point’s f -value exceeds the current shortest length This helps Basic-Theta* and Lazy-Theta* save time by not fully constructing the paths that are surely longer than the current best one However, the big weakness of BasicTheta* and Lazy-Theta* for this multiple goal situation is that they inefficiently expand the entire workspace if an inaccessible goal is picked first Although Batch-Theta* does not have this problem, we place the goals to be accessible from the starting point in every trial The map size is 500 × 500 cells and a varied proportion of cells are randomly chosen as blocked cells as in the previous simulation The goal number for testing is from to Besides, we examine Batch-Theta* with both classic LineO f Sight () and candit_L O S() to highlight the efficiency of our proposed connecting scheme and cost evaluation separately For better table presentation, we use “LOS() BT*” as the abbreviation of Batch-Theta* with LineOfSight() function and “candit_LOS() BT*” as that of Batch-Theta* with candit_LOS() function Downloaded by [RMIT University Library] at 22:39 10 April 2016 1548 V.-H Dang et al Table presents the average percentage results of path length comparing to those of Basic-Theta* Each value in the table is obtained after 200 trials It is shown that the path lengths are equivalent for all three methods It is because for each trial, all of them find the same best goal, which verifies Theorem in this paper In other words, BatchTheta* guarantees that the first reached goal is the best goal under Theta*’s mechanism The minor differences in lengths originate from the different manners of checking line-of-sight discussed in Sections and 4.1 Table shows the percentage results of the maximum memory required for expanded points We use this criterion for comparisons because Basic-Theta* and Lazy-Theta* restart the search for each goal and clear their memories The memories they required for closed_list are less than the total number of expanded points Meanwhile, the required memory for Batch-Theta*’s closed_list is equal to the total number of expanded points Even so, our method needs at most 66.4% with LineO f Sight () and only 53.3% with candit_L O S() of Basic-Theta*’s memory while LazyTheta* requires more than Basic-Theta* This implies the total number of point expansions of Batch-Theta* is less than those of Basic-Theta* and Lazy-Theta* Evidently, when the number of goals increases, the chance of finding a shorter path is higher, and this decreases the explored area of Batch-Theta* On the other hand, additional goals contribute little to the chance for Basic-Theta* and LazyTheta* to reduce the explored size for each goal As a result, the average percentage memory size for expanded points required by Batch-Theta* decreases when the goal number increases With six goals, Batch-Theta* armed with candit_L O S() requires less than 20% compared to BasicTheta* and about 16% compared to Lazy-Theta* This paper also provides the comparisons of runtime because Lazy-Theta* can run faster than Basic-Theta* even with higher expanded point number and Batch-Theta* has a small additional computation for detecting and choosing parent candidates during line-of-sight checks Figure 11 shows the average results of runtime and Table presents the runtime percentage values compared to those of BasicTheta* Lazy-Theta* runs faster than Basic-Theta* while our method always needs less runtime even when classic LineO f Sight is used With proposed candit_L O S(), Batch-Theta*’s performance is improved significantly in all situations Figure 11 shows that when the number of goals increases, Batch-Theta* compresses the runtime by considerably narrowing the search space while Basic-Theta* and Lazy-Theta* must perform more computations for the added goals This explains the decreased runtime ratio of Batch-Theta* no matter which of LineO f Sight () or candit_L O S() is used In the worst situation where the goal number is and the obstacles take 30% the map, Batch-Theta* requires only 16.70 and 11.59% of Basic-Theta*’s runtime when using LineO f Sight () and candit_L O S(), respectively Remark that with less obstacle density, the chance for Batch-Theta* to determine the optimal parents is higher than for Basic-Theta* and Lazy-Theta* due to higher probability of the second situation in Figure Hence, the evaluated costs of many points are lower, making the search space more narrow and focused In other words, the less the obstacle density is, the more efficient Batch-Theta* is than Basic-Theta* and Lazy-Theta* in terms of path length, necessary memory, and runtime as seen in Tables 3–5 and Figure 11 Obviously, Batch-Theta* has all advantages of Dijkstra, Theta*, and Lazy-Theta* when looking for the best path to one of the goals It expands to the best goal(s) while guaranteeing that when the first goal is reached, the path to it is the best under Theta*’s mechanism It avoids both blindly expanding around the starting point and exhaustedly taking all goals in turn Thanks to the proposed line-of-sight check, our method requires less time than Lazy-Theta*, which is currently the fastest Theta* variant, while giving even shorter path length than Basic-Theta* which is the current best among suboptimal methods for path length Conclusions This article introduces a suboptimal path planning variant of Theta* to deal with the problem of finding the lowest-cost path to the best goal in a given set, called Batch-Theta* The approach uses a new line-of-sight checking technique and handles the batch of goals instead of considering each single goal in turn or blindly doing the exhausted search until a goal is found Both analysis and simulations lead to the conclusion that the proposed line-of-sight checking and cost evaluating help Batch-Theta* outperform Basic-Theta* and Lazy-Theta* even when dealing with the problem of finding a path to one goal They also show that when dealing with multiple goals, Batch-Theta* is more efficient than Basic-Theta* and Lazy-Theta* by reducing the expanded space, finding the best goal and the best path to it at the first reach while giving even shorter path length with less runtime Although Batch-Theta* is inspired by robot path planning in partially known workspace, it can be efficiently applied in other problems of finding the best path to the best goal in a goal set for computer games or circuit layout tools Disclosure statement No potential conflict of interest was reported by the authors Funding This research is funded by Vietnam National Foundation for Science and Technology Development (NAFOSTED) under [grant number 102.05-2013.11] Advanced Robotics Notes on contributors Downloaded by [RMIT University Library] at 22:39 10 April 2016 Viet-Hung Dang graduated as an electricelectronics engineer from HoChiMinh University of Technology in 2003, Vietnam He got his M.S from the same university in 2005 He received his Ph.D degree in Computer Science at Kyung Hee University, Korea in Feb 2012 after four years of pursuing He is now working for Institute of Research and Development in Duy Tan University, Vietnam His research interests are Machine Learning, Artificial Intelligence, and problems on Localization and Navigation Nguyen Duc Thang is now a lecturer of Department of Biomedical Engineering, International University, Vietnam National University-HCMC He obtained his PhD from Kyung Hee University, South Korea in 2011 His research interests include Biomedical Engineering, Machine Learning, and Artificial Intelligence [4] [5] [6] [7] [8] [9] [10] [11] Hoang Huu Viet received the B.S degree in Mathematics from Vinh University, Nghean, VietNam, in 1994, and the B.S and M.S degrees in Computer Science from Hanoi University of Technology, Hanoi, Vietnam, in 1998 and 2002, respectively He received the Ph.D degree in Computer Engineering from Kyung Hee University, Republic of Korea in August 2013 He is now a lecturer at Department of Information Technology, Vinh University, VietNam His current research interests include Artificial Intelligence, Reinforcement Learning, and Robotics [12] [13] [14] [15] [16] Le Anh Tuan graduated both B Eng and M Eng in Mechanical Engineering and Marine Machinery from Vietnam Maritime University in 2003 and 2007, respectively He received the Ph.D degree in Mechanical Engineering from Kyung Hee University, South Korea in 2012 Currently, he is the Head of Automotive Engineering Department, Vietnam Maritime University His interested research composes of Applied Nonlinear Control, Dynamics and Control of Industrial Machines [17] [18] [19] [20] [21] References [1] Hwang YK, Ahuja N Gross motion planning – a survey ACM Comput Surv (CSUR) 1992;24:219–291 [2] Gabriely Y, Rimon E Spanning-tree based coverage of continuous areas by a mobile robot Ann Math Artif Intell 2001;31:77–98 [3] Gonzalez E, Aristizabal PT, Alarcon MA Backtracking spiral algorithm: a mobile robot region filling strategy In: Proceeding of the 2002 International Symposium on [22] [23] [24] 1549 Robotics and Automation; 2002 Sep; Toluca, Mexico p 261–266 Gabriely Y, Rimon E Spiral-STC: an on-line coverage algorithm of grid environments by a mobile robot In: Proceedings of the IEEE International Conference on Robotics and Automation; 2002 May; Washington, DC, USA p 954–960 Gonzalez E, Alvarez O, Diaz Y, Parra C BSA: a complete coverage algorithm In: Proceedings of the IEEE International Conference on Robotics and Automation; 2005 Apr; Barcelona, Spain p 2040–2044 Choset H, Pignon P Coverage path planning: the boustrophedon cellular decomposition Proceedings of the International Conference on Field and Service Robotics; 1997 Dec; Canberra, Australia Choset H Coverage for robotics – a survey of recent results Ann Math Artif Intell 2001;31:113–126 Viet HH, Dang VH, Laskar MNU, Chung T BA*: an online complete coverage algorithm for cleaning robots Appl Intell 2013;39:217–235 Lopez AS, Zapata R, Lama MAO Sampling-based motion planning: a survey Comput Sistemas 2008;12:5–24 Dijkstra EW A note on two problems in connexion with graphs Numer Math 1959;1:269–271 Cormen TH, Leiserson CE, Rivest RL, Stein C Introduction to algorithms 3rd ed The MIT Press; 2009 Hart PE, Nilsson NJ, Raphael B A formal basis for the heuristic determination of minimum cost paths IEEE Trans Syst Sci Cybern 1968;4:100–107 Sislak D, Volf P, Pechoucek M Accelerated A* trajectory planning: grid-based path planning comparison In: Proceedings of ICAPS 2009 Workshop on Planning and Plan Execution for Real-World Systems; 2009 Sep; Thessaloniki, Greece p 74–81 Thorpe CE Path relaxation: path planning for a mobile robot In: Proceedings of the AAAI Conference on Artificial Intelligence; 1984 Aug; Austin, TX, USA p 318–321 Botea A, Muller M, Schaeffer J Near optimal hierarchical path-finding J Game Dev 2004;1:7–28 Nash A, Daniel K, Koenig S, Felner A Theta*: anyangle path planning on grids In: Proceedings of the AAAI Conference on Artificial Intelligence; 2007 Jul; Vancouver, Canada p 1177–1183 Daniel K, Nash A, Koenig S Theta*: any-angle path planning on grids J Artif Intell Res 2010;39:533–579 Nashand A, Koenig S, Tovey C Lazy Theta*: any-angle path planning and path length analysis in 3D In: Proceedings of the AAAI Conference on Artificial Intelligence; 2010 Atlanta, GA, USA p 147–154 Berg MD, Cheong O, Kreveld MV, Overmars M Computational geometry: algorithms and applications 3rd ed Dordrecht: Springer; 2008 Koenig S, Likhachev M, Furcy D Lifelong planning A* Artif Intell 2004;155:93–146 Stentz A Dynamic fringe-saving A* Technical Report No CMU-RI-TR-93-20 Pittsburgh (PA): Carnegie Mellon University, Robotics Institute; 1993 Aug Koenig S, Likhachev M D* lite In: Proceedings of the AAAI Conference on Artificial Intelligence; 2002 Jul; Alberta, Canada p 476–483 Sun X, Yeoh W, Koenig S Dynamic fringe-saving A* In: Proceedings of 8th International Conference onAutonomous Agents and Multiagent Systems (AAMAS 2009); 2009 May; Budapest, Hungary p 891–898 Yap P, Burch N, Holte R, Schaeffer J Block A*: databasedriven search with applications in any-angle path-planning 1550 V.-H Dang et al Downloaded by [RMIT University Library] at 22:39 10 April 2016 In: Proceedings of twenty-fifth AAAI Conference on Artificial Intelligence; 2011 San Francisco, CA, USA p 120–125 [25] Ferguson D, Stentz A Using interpolation to improve path planning: the field D* algorithm J Field Rob 2006;23: 79–101 [26] Russel SJ, Norvig P Artificial intelligence a modern approach 2nd ed Harlow, UK:Pearson Education; 2003  ... constructing path to one goal #Goal Method Downloaded by [RMIT University Library] at 22:39 10 April 2016 Lazy -Theta* Batch- Theta* Lazy -Theta* Batch- Theta* Lazy -Theta* Batch- Theta* Path length... The runtime then is small, but the found path length is bad Obviously, applying the tuning w to Batch- Theta* together with Basic -Theta* and Lazy -Theta* will maintain the advantages of Batch- Theta* ,... obstacle density, Advanced Robotics 1545 Table Performance comparison between Basic -Theta* , Lazy -Theta* , and Batch- Theta* on the example in Figure Method Basic -Theta* Lazy -Theta* Batch- Theta* Path