Efficient Robot Path Planning in the Presence of Dynamically Expanding Obstacles Konstantinos Charalampous, Angelos Amanatiadis, and Antonios Gasteratos Laboratory of Robotics and Automation, Department of Production and Management Engineering, Democritus University Of Thrace, 12 Vas. Sofias Str, GR-67100, Xanthi, Greece {kchara,agaster}@pme.duth.gr,
[email protected]
Abstract. This paper presents a framework for robot path planning based on the A* search algorithm in the presence of dynamically expanding obstacles. The overall method follows Cellular Automata (CA) based rules, exploiting the discrete nature of CAs for both obstacle and robot state spaces. For the search strategy, the discrete properties of the A* algorithm were utilized, allowing a seamless merging of both CA and A* theories. The proposed algorithm guarantees both a collision free and a cost efficient path to target with optimal computational cost. More particular, it expands the map state space with respect to time using adaptive time intervals in order to predict the necessary future expansion of obstacles for assuring both a safe and a minimum cost path. The proposed method can be considered as being a general framework in the sense that it can be applied to any arbitrary shaped obstacle. Keywords: robot path planning, obstacle avoidance, cellular automata.
1
Introduction
The task of path planning is a subject of active research in many scientific communities. Mainly, it addresses the design of a route from a starting point to a target point, given certain operational constraints and scenarios. Some of the most common and important parameters that a path planning algorithm must hold are the ability of obstacle avoidance, the operation in static or dynamic environments and computational efficiency. A collision free path is a non tradable property that every path planning algorithm must hold. Operating in static or dynamic environments refers to whether the world model remains unchangeable during time evolution or not, respectively. Computational efficiency is another desired property to build a path planning algorithm applicable to real time applications. The above requirements of path planning design have resulted in a large amount and variations of proposed solutions, that have led to major breakthroughs especially in car navigation [1], domestic robot assistance [2], sensor fusion [3], logistics [4], and search and rescue robots [5]. However, path planning challenge can not be characterized as a solved problem. G.C. Sirakoulis and S. Bandini (Eds.): ACRI 2012, LNCS 7495, pp. 330–339, 2012. c Springer-Verlag Berlin Heidelberg 2012
Efficient Robot Path Planning
331
Prior work on path planning yields different approaches dealing with most of the aforementioned constraints. Optimization approaches result in a set of inequalities and constraints which are being used to express the problem. However, in order to be accurately formed, the number of inequalities should be increased as well, producing complicated non linear functions that have to be optimized [6]. Extended use of Voronoi diagrams has be done in [7], [8] yet they lack computational efficiency. In [9] a more simple and efficient framework was introduced using Voronoi diagrams, which as yet do not satisfy the ability to deal with arbitrary shapes of mobile robots. Methods that imitate Dubin’s theorem have been proposed [10], where the exploitation of genetic algorithms provides an optimal solution in a static environments containing obstacles. Alternative approaches are based on heuristic functions [6], trying to prune possible untraversable paths considering the robot’s width [11]. A wave simulation technique (Distance Transform) in the configuration space has been introduced [12], which creates into the free space a wave that carries distances from goal. An extension to Distance Transform, Path Transform [13], adds distances from near obstacles into the calculation loops. Our algorithm guarantees a collision free path, among dynamically expanding obstacles, while it produces a path cost that is locally optimal in every time interval. The produced final trajectory lies in the neighborhood of the global optimum since it tries to attain the global optimum, in every iteration. The method can be characterized by low computational cost making it appropriate for real-time applications.
2
Cellular Automata
Automaton is a mathematical structure, described formally as a quintuple {I, Z, Q, δ1 , ω}
(1)
where each symbol is a set according to the following definition [14]: I is a set of inputs to the defined automaton Z is a set of outputs to the defined automaton Q is a set of internal states, known as accept states δ1 is a transition function defined as δ1 : I × Q → Q, that produces the corresponding internal state from the Cartesian product of an internal state and the set of inputs. ω is a function that produces the output from the Cartesian product of an internal state and the set of inputs, defined as ω : I × Q → Z. Cellular automata is a subcategory of automata. CA main characteristics are that they deal with time and space in a discrete level. Using the above definition each future state is an outcome from the exact previous one, yet this can be expanded, leading to a system with memory, producing outcomes by considering numerous previous states. The neighborhood of a cell is defined as a spacial region in which a cell explores its adjacecent neighboor. The size of the neighborhood is not bounded by any restriction, but it must be the same for
332
K. Charalampous, A. Amanatiadis, and A. Gasteratos
every cell. The most widely used types of neighborhoods are the Von Neumann and the Moore ones. Von Neumann’s is a diamond shaped neighborhood in a squared grid. Given a cell (x0 , y0 ) in a two-dimensional plane, and range r, Von Neumann neighborhood is defined as [15]: NxV0 ,y0 = {(x, y) : |x − x0 | + |y − y0 | ≤ r}
(2)
On the other hand, Moore’s results in a squared shaped neighborhood in a squared grid. Given a cell (x0 , y0 ) in a two-dimensional plane and range r, Moore neighborhood is defined as [15]: NxM0 ,y0 = {(x, y) : |x − x0 | ≤ r, |y − y0 | ≤ r}
3
(3)
A*
A* is a search algorithm widely used in computer science, mainly in path finding and graph traversal problems. A* was introduced as an extension of Dijkstra’s algorithm [16]. Originally it was proposed for two-dimensional grids, also known as state spaces, characterizing it as a discrete algorithm. A* is an algorithm that combines Dijkstra’s algorithm in the sense that can it find the shortest path, but also the Greedy Best-First-Search, in the sense that it uses a heuristic function. More precicely, A* follows the path that minimizes the cost function f (s), where: f (s) = g(s) + h(s)
(4)
Component g(s) gives the cost of the expanded path until the current state cell s. The h(s) component is a heuristic function providing with an assessment of cost from current state s to goal. If h(s) is an admissable function, i.e. h(s) never overestimates the true cost from current state to goal providing a value of less or equal to real cost, then A* acquires some very desirable properties. A* having an admissable function h(s) can guarantee that it will always find the shortest - optimal path [17]. Since A* was first introduced, many different heuristic functions have been proposed over the years depending on each problem’s nature. Among those a common parameter exists. That is, the trade-off between heuristic’s accuracy and time complexity in order to produce a result, in the form of an estimation. The challenge of making such search algorithms practical for real time implementations lies in an efficient heuristic function. Depending on each problem, the search strategy must be considered as the best trade off between global optimum path cost and fast convergence. The A* algorithm is a complete algorithm, since it always provides a path when a solution exists. The algorithm’s time complexity, as mentioned above, varies according to heuristic function h(s). Complexity can be polynomial if the following assumptions hold. State space is a tree, and for a given heuristic function h(s) the next condition is valid: (5) |h∗ (s) − h(s)| = O(log(h∗ (s)))
Efficient Robot Path Planning
333
where, h(s) is the heuristic’s value at state s, and h∗ (s) is the optimal function’s value at state s, that is the exact cost of state s to goal. This condition implies that the approximation error, which is the difference between the optimal function and the heuristic of interest, must be equal to the optimal solutions logarithmic value. However, the number of nodes could also be expanded exponentially, while deriving the path with the lower cost [18].
4
Proposed Approach
The proposed method is based on the A* algorithm beacuase it exploits the properties of CA theory. The purpose of this method is to derive a kinematically feasible path to a goal with fast convergence, while dynamic obstacle creation and expansion occurs. The fact that the A* search algorithm is applicable on two-dimensional grids, as well as the grounded CA properties of time and space discrete level of processing, allows a seamless merging of both CA and A* theories. As discussed before, the A* algorithm as initially proposed, holds some very intriguing properties, but it can only be applied on static and fully observable grid maps. In order to resolve the aforementioned disadvantage, the proposed approach builds on a seamless merging between A* and CA theories. The key idea is that since A* cannot be applied in dynamic grids, it will be applied on predicted grid maps where both obstacle expansion and creation properties are encapsulated. The grid map prediction is feasible due to the discrete nature of both A* and CA theories. However, it is imposed to provide a time step. This restriction is addressed with an adaptive step usage. The proposed method iteratively defines the time step of the grid map evolution. The time step is calculated as follows: disto (Oi , Oj ) ∗ re (i) 2 distr (r, Oi ) ∗ re (i) Vr (i) = 2 Ts = min(Do , Vr )
Do (i, j) =
i, j = 1, . . . , No , i = 1, . . . , No
i = j
(6) (7) (8)
where No is the number of obstacles, Oi defines the coordinates of the i − th obstacle, re (i) is the expansion rate of obstacle i, disto is an operator that calculates the Euclidean distance between two obstacles and Do (i, j) is a matrix. Given a number of obstacles No , the algorithm calculates the half distance between two obstacles and then multiplies it with the expansion rate of each obstacle. In every time step, the matrix Do (i, j) will contain the time in which an obstacle Oi will reach half the distance to the obstacle Oj . In (7), distr is an operator that calculates the naive Euclidean distance between an obstacle and robot. Vector Vr (i) ∈ RN0 stores the time in which an obstacle Oi will reach half the distance to the robot. In (8), min is an operator that finds the minimum time for any of the above events to happen defining the time step Ts . When the time step is defined, the grip map evolves Ts steps in time. Subsequently, A* is applied to the new grid map, moving the robot Ts time steps.
334
K. Charalampous, A. Amanatiadis, and A. Gasteratos
A new time step is then recalculated. This iteration is repeated until the robot reaches the goal. Time step calculation is justified given that in the predicted grid map obstacles will not collide, thus there will be always a kinematically feasible path for the robot between them. In case the time to impact between the robot and an obstacle is smaller than the collision time between obstacles, this time interval is selected as the new time step, ensuring that robot will avoid impact. In the proposed methodology, each obstacle expands according to Moore’s neighborhood, though this is not a constraint, since any type of neighborhood can be applied to the proposed strategy. However, Moore’s neighborhood can be considered as the worst case in which an obstacle might expand, in the sense that it covers a larger part of the grid map compared to other types of expansions. Various expansion rates can be applied to each different obstacle. The proposed method allows the possibility to add obstacles during execution, as well as to alter the goal’s position. This can be applied before every iteration, when the path is being recalculated, without introducing an extra computational burden. For ensuring a safe path calculation, the A* algorithm is being applied, at a current time interval t, using the predicted grid map of time interval t + Ts . In such a way, the A* treats the obstacles on the predicted grid map as ones on the current map. This obstacle projection in time guarantees that the calculated path will be always kinematically traversable by the robot. In order to reduce computational burden, the proposed method monitors if the following conditions hold: 1. That the imaginary line segment that joins two obstacles does not intersect with the robot path, 2. If an obstacle has a sufficiently greater expansion rate than another, leading at a time to the second obstacle to be overlaid from the first one, then only the first obstacle needs to be taken into account from time to onwards. The proposed method’s computational complexity varies according to the number and positioning of the obstacles. The following cases are introduced to show the A* algorithm’s varies. In the worst case, where all obstacles are taken into account, computational complexity is O((No )2 ), where No defines the number of obstacles. In the best case, where the robot distance from the goal can be traversed in less time than the time step Ts , then the complexity is reduced to that of A*. In the general case that k obstacles meet one of the above two aforementioned conditions, then only No − k obstacles need to be taken into account, thus the oeverall computational complexity is O((No − k)2 ).
5
Simulation Results
This section presents the environment as well as the simulation results of the proposed methodology. The simulation environment includes a robot, a point on the grid map marked as goal point, and expanding obstacles. The simulation depicted in Fig.1, was applied in a 100×100 grid, where both obstacles have their
Efficient Robot Path Planning
335
own expansion rates. The purpose was to demonstrate the ability of the proposed method to avoid expanding obstacles. As it can be seen, the robot’s initial course is curvy in order to avoid collision. After traversing between obstacles it follows a straight line, as a result of A*’s property. The second simulation depicted in
100
100
Goal Obstacles Obstacles Init.pos
90 80
Goal Obstacles Obstacles Init.pos
90 GOAL
80
70
70
60
60
50
50
40
GOAL
40
30
30
INITIAL POS.
20
INITIAL POS.
20
10
10 10
20
30
40
50
60
70
80
90
100
10
20
30
40
(a) 100
100
Goal Obstacles Obstacles Init.pos
90 80
50
60
70
80
90
100
(b) Goal Obstacles Obstacles Init.pos
90 GOAL
80
70
70
60
60
50
50
40
GOAL
40
30
30
INITIAL POS.
20
20
10
10 10
20
30
40
50
(c)
60
70
80
90
100
INITIAL POS.
10
20
30
40
50
60
70
80
90
100
(d)
Fig. 1. Two obstacle simulation with different expansion rates
Fig. 2 was applied in a 100×100 grid introducing four obstacles, each one holding its own expansion rate. In this simulation we can derive the main properties of the algorithm. Firstly, it is shown that it can calculate a safe path by taking into account the predicted grids. The option to implant obstacles after the simulation start is also illustrated in Fig. 2(a), where in the north east area there is no obstacle, but in Fig. 2(b), Fig. 2(c), and Fig. 2(d) an obstacle appears in the grid. Furthermore, the distance properties between the new obstacle and its adjacent is not taken into account for the adaptive step computation, since the imaginary line segment does not intersect with the robot’s path. In such a way the computational complexity of the algorithm is reduced. In the third simulation, as illustrated in Fig. 3, a 200 × 200 grid was utilized, with six obstacles of different expansion rates. Though there were six obstacles, the overall computation burden is not significantly increased compared to the previous simulations. This is due to the fact that only two of the imaginary line segments between obstacles intersect with the robot path. As a result, the rest of the obstacles are excluded from the adaptive step computation.
336
K. Charalampous, A. Amanatiadis, and A. Gasteratos
100
100
Goal Obstacle Obstacle Obstacles Init.pos
90 80 70
Goal Obstacle Obstacle Obstacles Init.pos
90 80 70
60
60
50
50 INITIAL POS.
40
INITIAL POS.
40
30
30
20
20
GOAL
10
GOAL
10 10
20
30
40
50
60
70
80
90
100
10
20
30
40
(a) 100
100
Goal Obstacle Obstacle Obstacles Init.pos
90 80 70
50
60
70
80
90
100
(b) Goal Obstacle Obstacle Obstacles Init.pos
90 80 70
60
60
50
50 INITIAL POS.
40
INITIAL POS.
40
30
30
20
20
GOAL
10
GOAL
10 10
20
30
40
50
60
70
80
90
100
10
20
30
40
(c)
50
60
70
80
90
100
(d)
Fig. 2. Four obstacle simulation with different expansion rates
200
200
180
180 GOAL
160
GOAL
160
140
140 INITIAL POS.
120
INITIAL POS.
120
100
100 Goal Obstacle Obstacle Obstacle Obstacle Obstacle Obstacle Init.pos
80 60 40 20 20
40
60
80
100
120
140
160
180
Goal Obstacle Obstacle Obstacle Obstacle Obstacle Obstacle Init.pos
80 60 40 20
200
20
40
60
80
(a)
100
120
140
160
180
200
(b)
200
200
180
180 GOAL
160
GOAL
160
140
140 INITIAL POS.
120
INITIAL POS.
120
100
100 Goal Obstacle Obstacle Obstacle Obstacle Obstacle Obstacle Init.pos
80 60 40 20 20
40
60
80
100
(c)
120
140
160
180
200
Goal Obstacle Obstacle Obstacle Obstacle Obstacle Obstacle Init.pos
80 60 40 20 20
40
60
80
100
120
140
160
180
(d)
Fig. 3. Six obstacle simulation with different expansion rates
200
Efficient Robot Path Planning 100
100
90
90
80
80
70
70
60
337
60 INITIAL POS.
50
GOAL
40
INITIAL POS.
50
GOAL
40 Goal Obstacle Obstacle Obstacle Obstacle Init.pos
30 20 10 10
20
30
40
50
60
70
80
90
Goal Obstacle Obstacle Obstacle Obstacle Init.pos
30 20 10
100
10
20
30
40
(a)
50
60
70
80
90
100
(b)
100
100
90
90
80
80
70
70
60
60 INITIAL POS.
50
GOAL
40
INITIAL POS.
50
GOAL
40 Goal Obstacle Obstacle Obstacle Obstacle Init.pos
30 20 10 10
20
30
40
50
(c)
60
70
80
90
100
Goal Obstacle Obstacle Obstacle Obstacle Init.pos
30 20 10 10
20
30
40
50
60
70
80
90
100
(d)
Fig. 4. Four obstacle simulation with additional two introduced during the simulation
In the last simulation, as depicted in Fig. 4, a 100 × 100 grid was used, where initially four obstacles were introduced. Subsequently, two more obstacles were added into the simulation. It can be seen that the robot initial position and the goal position are almost in a straight line. The A* part of proposed method is pointing to a path which is really close to a straight line. This path includes maneuvers in order to avoid obstacles, however the path marginally avoids collisions. The risk is not significant since the obstacles presented on the A* are predictions of their real size.
6
Conclusions
This paper proposes a framework for robot path planning in the presence of dynamically expanding obstacles. It guarantees a collision free and a lowest cost path to target without introducing significant computational complexity. The map prediction follows CA rules, exploiting the discrete nature of CAs for both obstacle and robot state spaces. The search strategy is based on the A* algorithm allowing a seamless merging of both CA and A* theories. The produced final trajectory lies in the neighborhood of the global optimum since it tries to attain the global optimum in every iteration. The method can be characterized by low computational cost, rendering it appropriate for real-time applications.
338
K. Charalampous, A. Amanatiadis, and A. Gasteratos
Acknowledgment. This work was supported by the E.C. under the FP7 research project for The Autonomous Vehicle Emergency Recovery Tool to provide a robot path planning and navigation tool, “AVERT”, FP7-SEC-2011-1-285092.
References 1. Pradalier, C., Hermosillo, J., Koike, C., Braillon, C., Bessiere, P., Laugier, C.: The CyCab: a car-like robot navigating autonomously and safely among pedestrians. Robotics and Autonomoys Systems 50(1), 51–68 (2005) 2. Balaguer, C., Gimenez, A., Huete, A.J., Sabatini, A.M., Topping, M., Bolmsjo, G.: The MATS robot: service climbing robot for personal assistance. IEEE Robotics & Automation Magazine 13, 51–58 (2006) 3. Kyriakoulis, N., Gasteratos, A., Amanatiadis, A.: Comparison of data fusion techniques for robot navigation. In: Advances in Artificial Intelligence, pp. 547–550 (2006) 4. Baohua, J., Liang, Z.: A Path Planning Method of Contingency Logistics Based on Max-Min Ant System. In: Proceedings of the 2010 Asia-Pacific Conference on Wearable Computing Systems, pp. 311–314 (2010) 5. Chien, S., Wang, H., Lewis, M.: Human vs. algorithmic path planning for search and rescue by robot teams. In: 54th Annual Meeting of the Human Factors and Ergonomics Society, pp. 379–383 (2011) 6. Hwang, Y., Ahuja, N.: Gross motion planning –A survey. ACM Comput. Surv. 24, 219–291 (1992) 7. Rao, N.S.V.: An algorithmic framework for navigation in unknown terrains. IEEE Trans. Comput., 37–43 (1989) 8. Sugihara, K.: An approximation of generalized Voronoi diagrams by ordinary Voronoi diagrams. Graphical Models and Image Processing 55, 522–531 (1993) 9. Tzionas, P.G., Thanailakis, A., Tsalides, P.G.: Collision-Free Path planning for a Diamond-Shaped Robot Using Two-Dimensional Cellular Automata. IEEE Robotics & Automation Magazine 13, 237–250 (1997) 10. Wang, C., Soh, Y.C., Wang, H., Wang, H.: A hierarchical genetic algorithm for path planning in a static enviroment with obstacles. In: Canadian Conference on Electrical and Computer Engineering, IEEE CCECE 2002, vol. 3, pp. 1652–1657 (2002) 11. Noborio, H., Naniwa, T., Arimoto, S.: A feasible motion planning algorithm for a mobile robot an a quadtree representation. In: Proc. IEEE Int. Conf. Robot. Automat., pp. 237–332 (1989) 12. Jahabin, M.R., Fallside, F.: Path planning using a wave simulation technique in the configuration space. In: Gero, J.S. (ed.) Artificial Intelligence in Engineering: Robotics and Processes. Computational Mechanics Publications, Southhampton (1988) 13. Zelinsky, A.: Using path transforms to guide the search for findpath in 2D. Int. J. Robot., 315–325 (1994) 14. Aleksander, I., Hanna, F.K.: Automata Theory: An engineerinng Approach. Crane Russak, New York (1975) 15. Gray, L.: A Mathematician Looks at Wolfram’s New Kind of Science. Not. Amer. Math., 200–211 (2003)
Efficient Robot Path Planning
339
16. Hart, P.E., Nilsson, N.J., Raphael, B.: A Formal Basis for the Heuristic Determination of Minimum cost paths. IEEE Trans. on Systems Science and Cybernetics, SSC 2004, pp. 100–107 (2004) 17. Dehter, R., Judea, P.: Generalized best-first search strategies and the optimality of A*. Journal of the ACM, 505–536 18. Pearl, J.: Heuristics: Intelligent Search Strategies for Computer Problem Solving. Addison-Wesley
