A Terrain-Based Path Planning for Mobile Robots with Bounded Curvature Paulo L. J. Drews Jr.1
Douglas G. Macharet2
Abstract— Recent advances in robotics have led to the development of autonomous vehicles that navigate in outdoor environments. A fundamental premise for a mobile robot to perform a navigation task in an efficient and safe way is to calculate the path previously to its execution. Several path planning algorithms are only concerned with finding a route between the initial and the goal position, without taking into account the characteristics of the vehicle that would eventually perform it. Another important issue to be considered is the cost of performing the path, since a shorter path cannot be the best choice depending on the terrain where the robot navigates. This paper presents a path planning methodology for nonholonomic vehicles with curvature constraints that takes into account the different characteristics of the terrain in the environment.
I. INTRODUCTION Path planning is a well studied problem in mobile robotics. Even though the literature on motion planning in past years has produced numerous important results, there is still room for improvements. Several strategies to obtain a path between two different positions can be found in the literature [1], [2], [3]. Among the simplest and most common are the visibility graph, cell decomposition and potential field path planners. However, a fundamental premise when planning a path for a robot is that this path is executable, i.e., during navigation, we must ensure that the vehicle’s movement restrictions is considered (i.e. nonholonomic constraints). A typical automobile, for example, cannot instantaneously move orthogonally to the wheel plane. Indeed, a large majority of of real world vehicles are nonholonomic. As a mobile robot navigates through an indoor environment, the condition of the floor has little (or no) relevance to its decisions, because it does not expect to find severe imperfections or abrupt changes in terrain type. This, however, is not the case with outdoor environments, where the terrain’s topography and surface types have a major bearing on the navigation and final path’s cost. Due to the raised issues, it is necessary to study techniques capable of generate trajectories that takes into account the characteristics of both the vehicles and the environment. Therefore, this paper proposes the use of an A*-based algorithm to the generation of trajectories for a robot with 1 Paulo L. J. Drews Jr. is with Computational Sciences Center, Universidade Federal do Rio Grande (FURG), Rio Grande, RS, Brazil. Email:
[email protected] 2 Douglas G. Macharet and Mario F. M. Campos are with the Computer Vision and Robotic Laboratory (VeRLab), Computer Science Department, Universidade Federal de Minas Gerais, Belo Horizonte, MG, Brazil. Email: {doug,mario}@dcc.ufmg.br
Mario F. M. Campos2
bounded curvature in an environment with multiple terrain types. II. RELATED WORK Motion-planning problem for autonomous vehicles is the subject of many investigations, and various approaches for single vehicle path planning in general environments can be found in literature [2], [4], [5]. The generation of paths for nonholonomic vehicles is a topic of great importance. Most of the works that address this problem are based in probabilistic methods such as Rapidlyexploring Random Trees (RRTs) [6], [7] and Evolutionary Algorithms (EA) [8], [9]. However, the main concern about these techniques is that they either neglect the paths’ feasibility (e.g. which satisfies the kinematic constraints of the vehicle) or path’s cost (shorter paths). During its navigation, the robot may encounter different types of surface (e.g. gravel, sand, asphalt, grass), specially in outdoor environments, that can affect the cost of the path being executed. The works which address this problem are based on the decomposition of the environment into discrete regions. In [10] a similar approach to the present method is proposed. They discretize the environment in cells and the time interval of each nonholonomic movements. After, a neighborhood tree is built and a Dijkstra algorithm is used to find the best path. Despite the cost to find the best path, it does not consider different terrain in the search. In [11] the environment is represented as a regular grid, and the A* algorithm is applied to find the optimal path accordingly to each cell cost. In [12] the environment is decomposed using the constrained Delaunay triangulation (CDT) and a continuous vector field is used to drive the robot through the regions. The work of Howard et al. [13] proposed a terrain-based path planning where a fuzzy set of terrain are defined. It generates a index called as traversability index that defined the difficulty to cross a terrain. The A* algorithm cost function is defined using this index together with a cost to goal function. In all of this work, the characteristic of the vehicle is neglected. The problem of path planning in multiple terrain also occurs in virtual environments as games. In [14] is proposed a method similar to ours, where the obstacle and the terrain cost is defined inside of the A* heuristic. The work of Wichmann and W¨unsche [15] compare some variations of the A* algorithm with multiple resolutions, as well as terrain constraints. Although, these methods consider a multi-terrain constraints, they do not consider nonholonomic restrictions.
III. METHODOLOGY
B. Cost to goal function
A. Theoretical formalization Our technique assumes full knowledge of the environment (obstacles and different types of terrain). The problem dealt in this work can be stated as, given a initial configuration, qstart = (xstart , ystart , θstart ) and a final position pgoal = (xgoal , ygoal ), find a safe path (with no collisions), minimizing its cost and taking into account the vehicle’s constraints. In this paper, we model the robot as a classical Dubins vehicle [16], which encompasses a large class of nonholonomic vehicles that range from Ackerman steering cars to fixed-wing airplanes [17]. The state of the vehicle is given by a configuration q = (x, y, θ), with q ∈ R2 × S1 = SE(2). The variables x and y represent the vehicle’s position in the plane of the axes XY , and θ is the orientation (heading) related to the X axis. The kinematic model of a Dubins vehicle is given by:
x˙ ν cos(θ) q˙ = y˙ = ν sin(θ) , ω θ˙
(1)
where ν (ν ∈ R+ ) is the linear velocity and ω the angular velocity (ω ∈ {−ν/ρ, 0, ν/ρ}), where ρ is the minimum curvature radius of the curve that the vehicle is capable to execute. We focused the present work on Unmanned Ground Vehicles (UGV) with Ackerman steering (car-like) models (Figure 1), which represents most of the vehicles nowadays.
Fig. 1.
Kinematic model of a car-like robot [2].
This model introduces two new variables, L represents the distance between the front and rear axles and φ denotes the steering angle. The minimum curvature radius can be ν L , and consequently dθ obtained by ρ = tan(φ) dt = L tan(uφ ), where uφ = {−φmax , 0, φmax }. In the present approach, we consider ν, L and φmax are constant and a priori known. Our proposal is divided in two steps: an heuristic to approximate the cost to goal function and the algorithm to find the path to goal based on A* search.
In order to obtain an approximation of the cost to goal function, we use a grid-based approach. This discrete representation supports an approximation of the dynamic of the vehicle using a fixed time step, dt. The parameters of the grid in SE(2) are defined based on the minimum curvature radius. Assuming a small dt, the relation between position and heading is linearized, therefor the spatial resolution is defined as a multiple of the angular resolution per quadrant. As proposed in the work of [18], it is possible to define: dθ = dt =
2π , 4i
L ∗ dθ, ν ∗ tan(φ) dt dx = , i
(2) (3) (4)
where, it is defined four possible orientation to the vehicle (dθ). Based on the kinematic model of the vehicle, it is possible to estimate dt. Considering the definition of dt, the value of dx is estimated. It defines the size of each cells in the grid. The variable i is the i-th iteration of the path search algorithm, it is explained in Sec. III-C. This representation allows a low-dispersion in the grid for the interpolation of the cost to goal function [18]. This interpolation is important during the A* search, the next step. In the proposed algorithm, one important feature is related to the neighborhood definition. It is based in the dynamics propagation, where a fixed number of possible orientation is defined. For each possible orientation, a grid is built in order to improve the quality of the cost to goal function and speedup the search in the next step. The collision detection is evaluate for each possible state. In the present work, we evaluate three possible neighborhood heuristics in order to build the cost to goal function. Firstly, a heuristic based in Manhattan model motion is used [2], it considers four neighbor for each possible orientation of the vehicle, as shown in the Figure 2(a). The second heuristic is based in the work of [18]. It defines the possible neighbors as an approximation of the possible positions and orientation of a Dubins vehicle, it is shown in the Fig. 2(b). Finally, a heuristic based in the Reed-Sheep vehicle as defined in [2]. The Figure 2(c) illustrates the possible neighborhood. The Figure 2 shown the possible neighborhood for each orientation, defined by different colors. The center position in the 3×3 grid is the interest point that we intend to find the neighborhood. The points with the same color of the center in the other positions are its neighbors, with their respective orientation. In our approach, a grid is built using a standard wavefront algorithm as defined in [3], where the dimensions are related with q. It is different from [18], where a linear implementation of the Dial’s algorithm was used based in the algorithm of Thorup [19]. Due its reduced computational cost compared to the A* search algorithm, the standard
(a) Manhattan Motion
Model (b) Dubins Model Motion
(c) Reed Sheep Model Motion Fig. 2. Heuristics in order to define the neighborhood to build a cost to goal function.
wavefront was used. The linear version can be used when the environment is large. C. Search in the state space In order to find the best path to goal, the states are explored based in the A* algorithm [20]. It is a complete algorithm, if the heuristic is admissible as described in [21], i.e., if it never overestimates the actual minimal cost of reaching the goal. In order to explore the configuration space, the algorithm uses a priority queue, where the cost to reach a state is based in two costs, described in the Equation 5. f (q) = t(q) ∗ g(q) + h(q),
(5)
where g(q) is the heuristic cost to goal function, defined in the last section. The distance h(q) is the total length from the start configuration (qstart ) to the actual configuration q, and f (q) is the approximation between the start configuration to the goal configuration, it is used as key in the priority queue. The t(q) is defined by the cost of cross a terrain. It can be estimated experimentally as described in [12], where the estimated vertical acceleration of each linear velocity is considered as a good approximation of the cost of the terrain. This values are normalized by the smaller one, in order to keep the heuristic admissible. There are two possibilities to build the terrain cost function. Firstly, a grid based approach where the main advantage is the computational cost to get t(q). Although, we need to build the grid, where it is necessary to test if all configurations q are inside the terrain areas. The second approach was used in the present paper, it tests for each possible state if this state is inside the terrain areas. Although, the computational cost to estimate it is dependent of the number of regions and the shape of them, this technique is more accurate because does not suffer with discrete approximation.
Given a current configuration q, the new states are generated by simulating the dynamic of the car, as described in Sec. III-A. It assumes that each action is applied continuously for the duration of the time step (dt). Following this approach for all possible actions, the set of next states is defined. One important characteristic of the proposed method is related to the goal configuration qgoal , where the orientation are not defined. Thus, the path planning algorithm only is able to find the goal position. Moreover, the goal position is relaxed as region goal, defined by the space discretization dx. Due the discretization process, a path to the goal could be not reachable even though there is. Thus, a multiresolution search is proposed, as described in [18]. The idea is iteratively increase the value of i in the Eq. 4. It changes the number of possible orientation (dθ) and, accordingly, the size of dt and dx, allowing the method to find new paths. This iterative step is repeated until some maximum resolution is reached. It allows the method to find a solution in a large amount of possibilities, but it does not guarantee the completeness of the method, i.e., the existence of a least one feasible solution cannot be discarded. IV. EXPERIMENTS The simulations were executed using the parameters of a real small radio controlled monster truck (Tamiya TXT-1), which is being used as the chassis for an under development robotic platform. The vehicle presents the following characteristics: ν = 1m/s, L = 0.33m and φmax = π6 . The proposed approach was tested in two possible scenarios, composed by obstacle and multi-terrain areas. In all results obtained, the qstart is shown by a pink triangle where is possible to see the initial orientation. The goal region is shown using a green rectangle. The white area is defined as unitary cost to cross, and a gray scale obstacle are defined as areas with known cost to cross, where lighter gray represents smaller cost than darker one. The blue curves represents the evaluated states and the small yellow square are the best path to the vehicle. Firstly, a set of distinct terrain are created in order to evaluate the planning generated. In these experiments, they were defined qstart = [1 5 0], where the position is defined in meters and the orientation in radian. The goal is defined as qgoal = [10 5], both in meters. It is important to remember that the algorithm only search a goal region, defined by qgoal and dx. The configuration space area is defined as 12 × 10 m2 . Initially, the environment is defined without obstacles and with only one distinct terrain that cover all configuration space, with unitary cost. The results obtained by the proposed algorithm is shown in Fig. 3. The time elapsed to reach the goal area was 8.08s, considering a constant velocity, as previously defined. Considering the three cost to goal function, all of them explored 31 states before to reach the goal area. In order to evaluate the quality of the result obtained, a Dijkstra algorithm was used, where the distance and time to reach the goal are the same, but exploring 1572 states.
10
10
9
9
8
8
7
7
6
6
5
5
4
4
3
3
2
2
1
0
1
0
2
4
6
8
10
12
0
0
2
4
6
8
10
12
(a) Result obtained using the pro- (b) Result obtained using Dijkstra posed algorithm. algorithm. Fig. 3. Experiment 1 - Environment without obstacle and only one distinct terrain with unitary cost.
Considering this space configuration, it was inserted an non-convex terrain in the center of the area, as shown in Fig.4. The three different cost to goal function were evaluated. Table I shows the quantitative results. It is possible to see that the Dubins heuristic obtain a best path than the other two heuristics. It is due the cost to goal function obtained, where the cost near the terrain is smaller than the others. The cost in the others two heuristic are higher due the proximity with the heavy terrain. The number of explored states are smaller to the Dubins heuristic, and similar for the others two heuristics. Considering the last environment, a new terrain area with t(q) = 2 was inserted. The location of this terrain area is inside the best path of the vehicle in the last situation. It makes that the algorithm change the path to a new one, as shown in Fig. 5. Due the environment’s symmetry, the new path is chosen in the top of the environment. As in the last result, the Dubins heuristic presents the best result with the same time and distance of the last one. The number of visited states are similar to the last results, with advantage to the Dubins heuristic, as shown in the second line of Table I. Finally, a new terrain area with t(q) = 3 was inserted, obtaining a set with four distinct terrains. This fact makes that the vehicle are not able to find the goal without cross one of these area with non-unitary cost. As expected, the path chosen by the algorithm crosses the terrain area with smaller cost. Figure 6 shows the results. It is possible to see that the vehicle explores almost all configurations in the terrain with unitary cost in all heuristics. After that, the vehicle explores the terrain area with cost t(q) = 2, reaching the goal. Table I shows the increased number of states explored by the algorithm in this case, where all heuristics explore approximately the same number of states, with a small advantage to the Manhattan heuristic. The smaller time and distance is acquired using Dubins heuristics, as in the last results. A second simulated test set are created in order to validate the proposed algorithm in a environment with obstacle and distinct terrain areas. In this experiment, it was defined qstart = [1 3 0], where the position is defined in meters and the orientation in radian. The goal configuration is defined as qgoal = [9 0.5], both in meters. The search area was defined as 10 × 4 m2 . In this case, only the Dubins heuristic.
Initially, two obstacles are inserted in the environment. In Figure 7(a), it is possible to see the path estimated by the algorithm with a small number of state explored. Table II shows the distance and time elapsed to complete the path. In Figure 7(b), a new terrain area with t(q) = 3 is inserted but the same path was estimated by the algorithm. It is due the fact that terrain area is out of the planned path. The second line in Table II shows that the number of states was reduced due the new terrain area. After that, a larger terrain area with t(q) = 3 was inserted. It makes the algorithm to change the path planned to other side of the environment. Due the restricted configuration space, it is possible to see, in Fig. 7(c), the best path included a circle near the start configuration. It is typical of Dubins vehicle model. In this case, as shown in Table II, the number of explored configuration increased, as well as the distance and time to reach the goal region. In the last configuration space simulated, the cost of the heavy terrain area was reduced to t(q) = 1.5. In this case, the best path crosses the terrain area, due the best cost-benefit of it. Figure 7(d) shows the results in this case. The last line in Table II shows that distance and time to reach the goal are the same of the environment with only obstacles. TABLE II E XPERIMENT 2. Instance Only obstacles Obstacles + small area Obstacles + large area Obstacles + large soft area
Time (s)
Length (m)
Explored States
8.08 8.08 13.46 8.08
8.98 8.98 14.36 8.98
30 28 104 48
V. CONCLUSION AND FUTURE WORK In this work, it was described an A*-based path planning algorithm for nonholonomic vehicles with bounded curvature navigating in an environment with multi-terrain types. Three different heuristics are evaluated, and the Dubins motion model heuristics showing the best results as expected. The capabilities of the proposed method to deal with obstacle and distinct terrain areas are presented. The method is able to efficiently find a plan close to optimal very in low dimensional environments. Despite the good results presented in the experimental results in simulated environment, the discretization of the environment affect the applicability of the proposed method. Regarding the optimal path, it is not guaranteed due the time and space discretization. In specific situation suboptimal path planning can be generated. Moreover, the complexity of the presented has a exponential dependency on the dimension of the configuration space [18]. Nevertheless, it relies on a efficient simulation of the dynamic of the vehicle. Future directions include the study of possible techniques for generating smoother variations of acceleration, since there is a discontinuity on the curvature profile on the curves’ junction, leading to abrupt lateral accelerations that can make the trajectory not feasible by an actual vehicle.
10
10
10
9
9
9
8
8
8
7
7
7
6
6
6
5
5
5
4
4
4
3
3
3
2
2
2
1
1
1
0
0
0
2
4
6
8
10
12
0
2
(a) Manhattan Model Motion Heuristic Fig. 4.
4
6
8
10
12
0
4
6
8
10
12
Experiment 1 with Two Distinct Terrains - Evaluation of the proposed heuristics in a environment with a non-convex terrain with t(q) = 4. 10
10
9
9
9
8
8
8
7
7
7
6
6
6
5
5
5
4
4
4
3
3
3
2
2
2
1
1
1
0
2
(c) Reed Sheep Model Motion Heuristic
10
0
0
(b) Dubins Model Motion Heuristic
2
4
6
8
10
12
0
0
2
(a) Manhattan Model Motion Heuristic
4
6
8
10
12
0
0
(b) Dubins Model Motion Heuristic
2
4
6
8
10
12
(c) Reed Sheep Model Motion Heuristic
Fig. 5. Experiment 1 with Three Distinct Terrains - Evaluation of the proposed heuristics in a environment with a non-convex terrain with t(q) = 4 and a rectangular area with t(q) = 2. 10
10
10
9
9
9
8
8
8
7
7
7
6
6
6
5
5
5
4
4
4
3
3
3
2
2
2
1
1
1
0
0
2
4
6
8
10
12
0
0
2
(a) Manhattan Model Motion Heuristic
4
6
8
10
12
0
(b) Dubins Model Motion Heuristic
0
2
4
6
8
10
12
(c) Reed Sheep Model Motion Heuristic
Fig. 6. Experiment 1 with Four Distinct Terrains - Evaluation of the proposed heuristics in a environment with a non-convex terrain with t(q) = 4, a rectangular area, in the bottom, with t(q) = 2 and a rectangular area, in the top, with t(q) = 3. TABLE I E XPERIMENT 1. Time (s)
Instance Two Distinct Terrains Three Distinct Terrains Four Distinct Terrains
Length (m)
Explored States
H1
H2
H3
H1
H2
H3
H1
H2
H3
13.46 13.46 13.46
12.56 12.56 12.56
13.46 13.46 13.46
14.36 14.36 14.36
13.47 13.47 13.47
14.36 14.36 14.36
516 465 716
267 263 722
454 456 732
Other important future direction includes the space coverage using Dubins vehicle, as well as, the extension of the
proposed method to multi-goal path planning. Furthermore, experiments in a real world scenario with a vehicle Tamiya
4 3.5
3
3
2.5
2.5 y (m)
y (m)
4 3.5
2 1.5 1
1
0.5
0.5
0
0
1
2
3
4
5 x (m)
6
7
8
9
0
10
4
4
3.5
3.5 3 2.5 y (m)
3 2.5 2 1.5
1
2
3
4
5 x (m)
6
7
8
9
10
2 1.5
1
1
0.5
0.5
0
0
(b) Environment with obstacles and a rectangular terrain area with t(q) = 3
(a) Environment with only obstacles
y (m)
2 1.5
0
1
2
3
4
5 x (m)
6
7
8
9
10
0
0
1
2
3
4
5 x (m)
6
7
8
9
10
(c) Environment with obstacles and a large rectangular terrain area with (d) Environment with obstacles and a large rectangular terrain area with t(q) = 3 t(q) = 1.5 Fig. 7.
Experiment 2 - Environment with obstacles and distinct terrain areas.
TXT-1 may also be conducted. R EFERENCES [1] R. Siegwart, I. R. Nourbakhsh, and D. Scaramuzza, Introduction to Autonomous Mobile Robots, 2nd ed. Cambridge, MA, USA: MIT Press, 2011. [2] S. M. LaValle, Planning Algorithms. New York, NY, USA: Cambridge University Press, 2006. [3] H. Choset, K. M. Lynch, S. Hutchinson, G. A. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press, 2005. [4] Y. Kuwata, A. Richards, T. Schouwenaars, and J. P. How, “Robust Constrained Receding Horizon Control for Trajectory Planning,” in Proceedings of the AIAA Guidance, Navigation and Control Conference, 2005. [5] M. Wzorek and P. Doherty, “Reconfigurable Path Planning for an Autonomous Unmanned Aerial Vehicle,” Proceedings of the International Conference on Hybrid Information Technology (ICHIT’06), vol. 2, pp. 242–249, Nov. 2006. [6] A. Alves Neto, D. G. Macharet, and M. F. M. Campos, “Feasible RRTbased path planning using seventh order B´ezier curves,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS’10), oct. 2010, pp. 1445–1450. [7] R. Pepy, A. Lambert, and H. Mounier, “Path Planning using a Dynamic Vehicle Model,” in Proceedings of Information and Communication Technologies (ICTTA’06), vol. 1, 2006, pp. 781–786. [8] J. M. de la Cruz, E. Besada-Portas, L. Torre-Cubillo, B. Andres-Toro, and J. A. Lopez-Orozco, “Evolutionary path planner for UAVs in realistic environments,” in Proceedings of the 10th annual Genetic and Evolutionary Computation Conference (GECCO’08). New York, NY, USA: ACM, 2008, pp. 1477–1484. [9] S. C. Yun, V. Ganapathy, and L. O. Chong, “Improved genetic algorithms based optimum path planning for mobile robot,” in Proceedings of the 11th International Conference on Control Automation Robotics Vision (ICARCV’10), dec. 2010, pp. 1565–1570. [10] J. Barraquand and J. Latombe, “Nonholonomic multibody mobile robots: Controllability and motion planning in the presence of obstacles,” Algorithmica, vol. 10, pp. 121–155, 1993. [11] Y. Guo, L. Parker, D. Jung, and Z. Dong, “Performance-based rough terrain navigation for nonholonomic mobile robots,” in Proceedings of the 29th Annual Conference of the IEEE Industrial Electronics Society (IECON’03), vol. 3, nov. 2003, pp. 2811–2816.
[12] G. A. S. Pereira, L. C. A. Pimenta, A. R. Fonseca, L. d. Q. Corra, R. C. Mesquita, L. Chaimowicz, D. S. C. de Almeida, and M. F. M. Campos, “Robot Navigation in Multi-terrain Outdoor Environments,” The International Journal of Robotics Research, vol. 28, no. 6, pp. 685–700, 2009. [13] A. Howard, H. Seraji, and B. Werger, “T*: a novel terrain-based path planning method for mobile robots,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA’02), 2002. [14] F. M. J¨onsson, “An optimal pathfinder for vehicles in real-world digital terrain maps,” The Royal Institute of Science, School of Engineering Physics, Stockholm, Sweden, Tech. Rep., 1997. [15] D. R. Wichmann and B. C. W¨unsche, “Automated route finding on digital terrains,” in Image and Vision Computing New Zealand IVCNZ, 2004. [16] L. E. Dubins, “On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents,” American Journal of Mathematics, vol. 79, no. 3, pp. 497–516, 1957. [17] G. Dudek and M. Jenkin, Computational Principles of Mobile Robotics, 2nd ed. New York, NY, USA: Cambridge University Press, 2010. [18] J. Mancilla and O. S. Plazas, “Optimal Planning with Differential Constraints using Heuristic-Based Search,” 2010, Unpublished manuscript. Available at: http://magma.cs.uiuc.edu/mancill1/Planning/planning project.html. [19] M. Thorup, “Undirected single source shortest paths in linear time,” in Proceedings of the 38th Annual Symposium on Foundations of Computer Science (FOCS’97). Washington, DC, USA: IEEE Computer Society, 1997, pp. 12–21. [20] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic determination of minimum cost paths,” IEEE Transactions on Systems, Science, and Cybernetics, vol. SSC-4, no. 2, pp. 100– 107, 1968. [21] R. Dechter and J. Pearl, “Generalized best-first search strategies and the optimality of A*,” Journal of the ACM, vol. 32, no. 3, pp. 505–536, jul 1985.
