Development of Path Planning Algorithm Using Probabilistic Roadmap Based on Modified Ant Colony Optimization

Firas A. Raheem, Mohammed I. Abdulkareem

Control and Systems Department, University of Technology-Iraq, Baghdad, Iraq.

DOI: 10.4236/wjet.2019.74042                

Abstract

In this paper, a unique combination among probabilistic roadmap, modified ant colony optimization, and third order B-spline curve has been proposed to solve path planning problems in complex and very complex environments. This proposed approach can be divided into three stages. First stage involves constructing a random roadmap depending on the environment complexity using probabilistic roadmap algorithm. Roadmap can be constructed by distributing N nodes randomly in complex and very complex static environments then pairing these nodes together according to some criteria or conditions. The constructed roadmap contains a huge number of possible random paths that may lead to connecting the start and the goal points together. Second stage includes finding path within the pre-constructed roadmap. Modified ant colony optimization has been proposed to find or to search the best path between start and goal points, where in addition to the proposed combination, ACO has been modified to increase its ability to find shorter path. Finally, the third stage uses B-spline curve to smooth and reduce the total length of the found path in the previous stage. The results of the proposed approach ensure the feasible path between start and goal points in complex and very complex environments. Also, the path is guaranteed to be short, smooth, continuous and safe.

Keywords

Path Planning, Probabilistic Roadmap, Ant colony Optimization, B-Spline Curve

1. Introduction

Path planning is a process of obtaining reasonable, collision free route between start and goal point, where the need for path planning becomes important for fully or partially automated process. There are two types of environments according to how much information is known about environment. The environment considered as known when the location of obstacle(s) is defined previously while considered as unknown when there is no information about it [1] . Path planning can be divided into two categories: First category, according to time: On-line and Off-line. In On-line path planning, path computed during motion according to data coming from sensors but, in Off-line planning, the path computed according to environment model. Second category, according to environment: dynamic environment in which, environment contains moving and static obstacles while in static, environment contains only static obstacles [2] . Another category can be added depending on the type: global planning in which the path may be found from start to goal point before robot starts to move. Therefore, the information has to be fully known about the environment. Second type is local planning in which path cannot be fully obtained before robot starts to move because knowledge about environment is partially known or unknown. It is important to know path planning categories and environments types because good path planning leads to good robot navigation.

Global path planning starts from 1980 till these days and many algorithms have been proposed by researchers. Research leads to improve global planning year by year. At the beginning, path planning was focusing on finding path to goal only. Then it became bigger issue by not only reaching the goal but also considering optimization criteria [3] . There are a lot of algorithms used to solve path planning problems such as heuristic methods, meta-heuristic, and randomized method. A*, D*, and Dikstra can be considered as heuristic methods. A* algorithm is considered as a greedy, graph, and heuristic search algorithm that is able to find sub optimal but, not optimal path [4] . D* algorithm can be considered as dynamic A* which is able to reform the path according to new information coming from sensors [5] . Meta-heuristic methods include particle swarm optimization (PSO), and ant colony optimization (ACO) [6] . Also there are intelligent methods, like artificial potential field (APF), genetic algorithm (GA). APF is used in different types of robots where the field of forces is applied on robot. These forces are attractive force to goal and repulsive force from obstacles [7] . Randomized methods can be divided into two categories: rapidly exploring random tree (RRT) which is more suitable for dynamic environment and probabilistic roadmap (PRM) for static environment.

PRM algorithm has been applied successfully to very complex static environments. PRM computation can be divided into two phases: the preprocessing (construction) phase and the query phase. In the construction phase, a random roadmap constructed that contains many possible paths that can reach to goal point. On the other hand, the objective of the query phase is to select suitable path from the pre-constructed roadmap. In the next sections, each phase will be explained in detail.

The contributions of the presented work can be stated as follow:

1) A new approach has been proposed which consists of combining a random grid of roadmap, modified ant colony optimization, and third order B-spline curve to solve path planning problem in complex known environment.

2) Ant colony optimization has been modified to increase its ability to find shorter path.

3) The path found by modified ACO enhanced using B-spline curve to reduce the total length and also, to smooth the path.

The general organization of this paper will be divided into eight sections including current section. The Second section gives brief description about related works and some related algorithms and methods, while the third section shows the general idea of the proposed algorithm. The fourth section explains the construction of the random roadmap and its algorithm. The fifth section shows the theories and modification of ant colony optimization (ACO), while the sixth section explains B-spline curve and how it can be used to enhance the path. The Seventh section shows simulation results of this paper, and finally the eighth section gives conclusions of the presented work

2. Related Work

In order to overcome path planning problems, many algorithms have been studied extensively. One of these algorithms is probabilistic roadmap (PRM) algorithm. Starting with L. E. Kavraki who used PRM to find path in high dimensional space. In other words PRM used with a robot has many degree of freedom in know static environment. L.E. Kavraki used uniform sampling to distribute nodes [8] . Valerie Boor proposed another strategy for sampling node called Gaussian sampler. The new strategy proposed to grantee better coverage of difficult part of space (narrow passage) [9] . Steven A. Wilmarth proposed another strategy for sampling nodes in narrow passages called medial axis. The basic concept of medial axis sampling is to sample a node randomly. The sampled node retracted to the medial axis between two obstacles, regardless if the sampled node is collision free or not [10] .

Roland Geraets list many advanced sampling strategies such as Gaussian, nearest contact, obstacle based, obstacle based*, medial axis and bridge test. In bridge sampling strategy two nodes sampled randomly and the distance between them is predefined. If the mid-point between two sampled nodes is collision free while two sampled nodes are not free then mid-point is added otherwise no node will be added. Obstacle based strategy sample a node randomly if it is collision free the node will be added, otherwise, in random direction the sampled node moves with pre-defined steps till it becomes collision free while in obstacle based* the sampled node will be discarded if its collision free [11] . In 2005, David Hsu propose a hybrid sampling strategy. This strategy use multiple sampling methods to take advantage of each one and reduce disadvantages of each one [12] . Safaa H. Shwail use A* algorithm with probabilistic roadmap to find near optimal path between start and goal points after that improved genetic algorithm used to enhance found path and make it more optimal [2] .

Generally, probabilistic roadmap (PRM) used with static environment but, Yunfei Zhang proposed a method to plan a path for mobile robot in unstructured and dynamic environment. The algorithm based on two phases: first phase, construct collision free roadmap and store it as graph; second phase is Q-learning, reinforcement learning method, is collaborated with PRM to obtain good path to catch goal. In this way, the robot use past experience to improve its performance in avoiding dynamic and static obstacles. This method called PRM-Q method [13] .

3. Proposed Approach

In this paper, a new combination between probabilistic roadmap algorithm, ant colony optimization, and B-spline curve has been proposed as shown in Figure 1. Mainly, the proposed approach divide path planning solution into three stages. First stage is construction of probabilistic roadmap which will be clarified in Section 4. Second stage is searching path by modified ant colony optimization, where Section 5 will show the theory and equations of ACO and how ACO has been modified, while the last stage is smoothing and enhancing path using B-spline curve. The third stage is discussed in detail in Section 6. At the end of the presented work, A comparison has been made between ACO and modified ACO as path search technique.

4. Probabilistic Roadmap

In this section, the preprocessing phase (construction of roadmap) has been explained in details. The main two element of roadmap (R) are edges and nods R = (N, E). N is a set of nodes distributed randomly to be collision free with obstacle while E is edge which can be defined as very simple, straight, and feasible (collision free) path. These edges or paths called local paths founded by not powerful planner but very fast planer called local planner. At the beginning, the roadmap R = (N, E) is empty. Frequently, random nodes are sampled and added to N if it is collision free. Now, for each new node q some nodes q’ selected from previously sampled nodes to connect them with new node by local planner. This process leads to find new edge E = (q, q’). The neighbors nodes Nq specified

Figure 1. Proposed method.

according to Equation (1) [8] . if the distance between current node and any node in R is less than or equal to pre-defined threshold, the node considered as neighbor otherwise; node can be neglected as shown in Figure 2 and the process repeated again for all nodes.

$Nq=\left\{q^{\prime }\in N|D\left(q,q^{\prime }\right)\le D_{\max }\right\}$ (1)

where D_max is predefined threshold, q is new generated node, q’ is neighbor node, and D(q, q’) is Euclidean distance Equation (2) [8] .

$D\left(q,q^{\prime }\right)=\Vert q-q^{\prime }\Vert$ (2)

Another way can be used to specify node neighbors is to choose nearest K number of nodes to the current node. The last step in roadmap construction is related to local planner to check if the edge is collision free or not. Edge divided into X steps starting from start point to end point and with each step the collision checked with obstacles. Figure 3 shows the flow chart of random roadmap construction. After roadmap construction, a path search technique is needed to find suitable path from constructed roadmap. Next section proposes a technique that can find suitable path.

Figure 2. Selecting neighbor nodes.

Figure 3. Construction of random roadmap.

5. Ant Colony Optimization ACO

In the world of ants, it is well known that ants are capable to obtain the shortest path to food using swarm intelligence. In 1992, ant colony optimization (ACO) has been proposed by M. Dorigo to simulate behavior of the ants to find solution for optimization problems. When the ants travel to find food, they deposit chemical matter called pheromones. Pheromones attract other ants to food source. When more ants travel to specific food source, the quantity of pheromones on the path increased. The shortest path to food source contains more pheromones, because more ants travel through this path. The pheromone is chemical matter that evaporates over time, thus it reduces the chance of taking path with low pheromone level, where pheromone level in shortest path will increase because, deposition process of pheromone overcome evaporations process [14] .

Consider a network, which is in our case the constructed roadmap, the probability that ant K travels from node i to next node j can be calculated by Equation (3) [14] .

$p_{ij}^k=\{\begin{array}{l}\frac{{\left({\tau }_{ij}^k\right)}^{\alpha }{\left({\eta }_{ij}^k\right)}^{\beta }}{{{\displaystyle \sum }}_{i\in N_i^k}{\left({\tau }_{ij}^k\right)}^{\alpha }{\left({\eta }_{ij}^k\right)}^{\beta }}\text{if}j\in N_i^k\\ 0\text{if}j\notin N_i^k\end{array}$ (3)

${\tau }_{ij}^k$ refers to pheromone levels, while ${\eta }_{ij}^k$ represents Euclidean distance between node i (current node) and node j (next possible node). α and β is weight of pheromone level and distance respectively between two nodes. When β equal to zero then the probability of choosing node j depends on nothing but pheromone level. In other words, when α equal to zero then the probability depends only on heuristic distance (the closest node to current node). K is ant number while $N_i^k$ is set of neighboring nodes. As mentioned before, the pheromone evaporate overtime and this process can be expressed in Equation (4) [14] . Evaporation rate denoted by ρ (0 ≤ ρ < 1) where

${\tau }_{ij}=\left(1-\rho \right){\tau }_{ij}$ (4)

It is clearly that when ρ equal to zero, no evaporation happens, on the other hand, when ρ equal to one, pheromone is fully evaporated. After evaporation process, the pheromone level updated by additional levels deposited by ants according to Equation (5) [14]

${\tau }_{ij}={\tau }_{ij}+{\displaystyle {\sum }_{k=1}^m\text{Δ}{\tau }_{ij}^k}$ (5)

where m is population, and $\text{Δ}{\tau }_{ij}^k$ can be calculated by Equation (6) where $C^k$ reward of ant k for choosing path [14] .

$\text{Δ}{\tau }_{ij}^k=\frac{1}{C^k}$ (6)

Modified Ant Colony Optimization

Normally, probability of the node to be selected depends on the level of pheromones ( ${\tau }_{ij}$ ) and Euclidian distance from current node to next possible node ( ${\eta }_{ij}$ ) as noticed in Equation (3). The new modification is that the probability of the next possible node depends on another factor which is the length of virtual path from next possible node to goal node. The length of virtual can be measured by Euclidean distance. Equation (3) can be rewritten again involving new term where the new Equation (7) is shown below:

$p_{ij}^k=\{\begin{array}{l}\frac{{\left({\tau }_{ij}^k\right)}^{\alpha }{\left({\sigma }_{ij}^k\right)}^{\beta }}{{{\displaystyle \sum }}_{i\in N_i^k}{\left({\tau }_{ij}^k\right)}^{\alpha }{\left({\sigma }_{ij}^k\right)}^{\beta }}\text{if}j\in N_i^k\\ 0\text{if}j\notin N_i^k\end{array}$ (7)

where ${\sigma }_{ij}^k$ can be defined by Equation (8):

${\sigma }_{ij}={\eta }_{ij}+E{d}_{jg}$ (8)

where $E{d}_{jg}$ is Euclidian distance of virtual path between next possible node j and goal node. The virtual distance added to probability equation to make the selection of the next node to be more intelligent. All remaining steps and equation of ACO stay as it without any changing. Figure 4 shows simple example of possible case can be solved by modified ACO. If it is assumed that node B has been already chosen then there are three possible nodes can be selected D, C, and A. The integer numbers beside thick black lines are the distance ( ${\eta }_{ij}$ ), while the fractional numbers are pheromone level ( ${\tau }_{ij}$ ). The dashes green lines present the distance of virtual paths from next possible node to goal node.

6. B-Spline Curve

Generally, the output of the most path planning algorithm is straight lines segment. Actually, path with straight line segments is undesirable because of discontinuity issue, robot mechanical wear, localization error, and slipping. Also, robot needs to stop at each turning edge, then turn, and lastly move forward again which can be considered as another disadvantage. Moreover Dubin’s, Reed, and Shepp’s paths, generate smooth paths by combining lines and circles but, the path was discontinuous [15] .

Figure 4. Simple case of modified ACO.

B-Spline and Bezier curve, as shown in Figure 5, are the most famous spline curve used in computer aided design. Each type of those curve use control points to control their shape and they were studied to use them in path planning. Bezier curve has unwanted properties like, number of control point will determine curve order. In other words, high number of control points leads to increase order complexity. Whereas B-spline curve order independent on number of control points which give advantage over Bezier curve [16] . The control points of B-spline curve are extracted from the basic path found by ACO or modified ACO, where basic path is nothing but selected node from roadmap connected together by straight lines. A B-spline curve has n control points $P_i\left(i=0,1,\cdots ,n\right)$ and k order may be defined by Equation (9) [17] .

$p\left(t\right)={\displaystyle {\sum }_{i=0}^n{p}_i{N}_{i,k}\left(t\right)},t_{k-1}\le t\le t_{n+1}$ (9)

where $N_{i,k}\left(t\right)$ is basis function of B-spline with k order defined over the knot vector $T=\left\{t_0,t_1,\cdots ,t_k,\cdots ,t_n,\cdots ,t_{n+k}\right\}$ . Basis function can be obtained by the following recursive de Boor-Cox Equation (10) [16] .

$N_{i,1}\left(t\right)=\{\begin{array}{l}1,t_i\le t\le t_{i+1}\\ 0,\text{otherwise}\end{array}$ (10a)

$N_{i,k}\left(t\right)=\frac{t-t_i}{t_{i+k}-t_i}{N}_{i,k-1}\left(t\right)+\frac{t_{i+k}-t}{t_{i+k}-t_{i+1}}{N}_{i+1,k-1}\left(t\right)$ (10b)

$t_i=\{\begin{array}{l}0,i<k\\ 1+i-k,k\le i\le n\\ 2+n-k,i>n\end{array}$ (10c)

At the end of this section, all needed theories and equation for proposed approach has been explained. Figure 6 shows the flow chart of the proposed combination of these methods.

7. Simulation and Results

In this section the proposed approach is applied on given environment and task. Environment range X = [−10 10], Y = [−10 10] and start point = [−9 9], while goal point = [9 −9]. Start and goal points are placed diagonally to force the path face larger possible number of obstacles as shown in Figure 7.

Figure 5. Example of B-Spline and Bezier curve.

Figure 6. PRM based on modified ACO flow chart.

Figure 7. Given environment.

Firstly, a random roadmap has been constructed for the given environment. As explained previously, this roadmap contains huge number of paths that can connect start and goal points. Two path search methods have been used to find best path. First search method is ACO while the second one is modified ACO. After finding basic path either by ACO or modified ACO, the path is enhanced using B-spline curve. The length of the path obtained by ACO is found to be 30.92 m and the constructed road map is shown in Figure 8 while, the length of the enhanced path by B-spline curve is equal to 29.7264 m as shown Figure 9. The convergence to the best path using ACO is shown in Figure 10.

Figure 8. Constructed roadmap.

Figure 9. Path found by ACO and enhanced path by B-spline curve.

Figure 10. Convergence to the best solution using ACO.

For the same environment and constructed roadmap, modified ACO is used to find basic path within pre-constructed roadmap. The length of the path found by modified ACO is equal to 29.1711 m and the constructed roadmap is shown in Figure 11, while the length of the enhanced path by B-spline curve is 27.9639 m as shown in Figure 12. The convergence to the best path using modified ACO is shown in Figure 13.

From the results it can be noticed that modification of ACO increases the ability of classical ACO to find better path, where modified ACO found a path shorter than the path found by ACO by 1.7489 m. Also, path enhancement reduce the total length of the path, where the length of the path found by ACO reduced by 1.1936 m. in the other hand, the length of the path found by modified ACO has been enhanced by 1.2072 m. Table 1 summarizes all results and shows parameters of PRM, ACO, Modified ACO.

8. Conclusions

This paper proposes to use the combination of probabilistic roadmap (PRM) and modified ACO technique to solve path planning problems for complex, very complex, static, and known environment. ACO has been modified by changing the way of calculating the probability of the next possible nodes, where the estimated distance from next possible node to goal node has been considered in probability equation. The proposed method solves the path planning problem by dividing it into three stages. In first stage, PRM used to model environment by constructing a collision free random roadmap where this roadmap contains more than one possible path to reach goal, here the first stage finishes. Second stage is path search stage by using modified ACO to obtain the basic path within roadmap. Found path by modified ACO is represented by straight lines

Figure 11. Constructed roadmap.

Figure 12. Path found by modified ACO and enhanced path by B-spline curve.

Figure 13. Convergence to the best solution using modified ACO.

Table 1. Summerized results.

therefore, the path needs to be enhanced. Third stage is shortening and smoothing the path found by modified ACO. To prove the effectiveness of modification, both ACO and modified ACO have been used as path search method.

From results, following points can be noticed:

1) The modification of ACO has been approved, where the length of the path found by modified ACO is shorter than the length of the path found by ACO by 6%.

2) Path has been effectively enhanced in both cases, ACO case and modified ACO case using B-spline curve. The effect on the path’s length in enhancement stage is increased or decreased depending on the quality of the path found in second stage.

The main advantage of merging a random roadmap with the modified ACO is to increase the possibilities of obtaining shorter path, where using ACO with traditional square grid reduces the possibilities to cover all possible paths specially, in critical regions. In addition to enhancing the length of found path, producing smooth path using B-spline curve is much better for robot navigation due to discontinuity issue.

It is clear that the proposed method can be used in static known environment and the path is globally planned before robot moves. Also, it is noticed that the quality of the path depends on probability of the node distribution. But, at the same time, probabilistic roadmap algorithm can ensure to find several paths to the goal regardless of the complexity of the environment. The complexity of the roadmap depends on complexity of the environment. In other words, when the environment is complex, more nodes should be distributed, which leads to the increase number of ways that may be selected to reach the goal. So, the complexity of roadmap is increased at this case. In the end, the approach has been simulated and the path has been approved to be short, smooth, continuous, and collision free.

Finally some points or ideas can be suggested as future work such as:

1) Apply proposed method in the presented work on a real mobile robot.

2) Enhance construction of roadmap by making construction of roadmap more intelligent.

3) Try to find better modification for ACO or find better search method.

Conflicts of Interest

The authors declare no conflicts of interest regarding the publication of this paper.

References

[1]	Raheem, F.A. and Hameed, U.I. (2019) Interactive Heuristic D* Path Planning Solution Based on PSO for Two-Link Robotic Arm in Dynamic Environment. World Journal of Engineering and Technology, 7, 80-81. https://doi.org/10.4236/wjet.2019.71005
[2]	Safa, H.S. and Alia, K. (2014) Probabilistic Roadmap, A*, and GA for Proposed Decoupled Multi-Robot Path Planning. Iraq Journal for Applied Physics, 10, 3.
[3]	Sariff, N.B. and Buniyamin, N. (2010) Ant Colony System for Robot Path Planning in Global Static Environment. 9th WSEAS International Conference on System Science and Simulation in Engineering, Iwate, 4-6 October 2010, 192.
[4]	Raheem, F.A. and Hussain, A.A. (2017) Applying A* Path Planning Algorithm Based on Modified C-Space Analysis. Al-Khwarizmi Engineering Journal, 13, 125.
[5]	Raheem, F.A. and Hameed, U.I. (2018) Path Planning Algorithm Using D* Heuristic Method Based on PSO in Dynamic Environment. American Scientific Research Journal for Engineering, Technology, and Sciences, 49, 258.
[6]	Gigras, Y. and Gupta, K. (2014) Metaheuristic Algorithm for Robotic Path Planning. International Journal of Computer Applications, 85, 26-29. https://doi.org/10.5120/14822-3056
[7]	Raheem, F.A. and Badr, M.M. (2017) Development of Modified Path Planning Algorithm Using Artificial Potential Field (APF) Based on PSO for Factors Optimization. American Scientific Research Journal for Engineering, Technology, and Sciences, 37, 317-328.
[8]	Kavraki, L., Svestka, P. and Overmars, M.H. (1996) Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces. IEEE Transaction on Robotics and Automation, 12, 566-570. https://doi.org/10.1109/70.508439
[9]	Boor, V., Overmars, M.H. and Van Der Stappen, A.F. (1999) The Gaussian Sampling Strategy for Probabilistic Roadmap Planners. IEEE International Conference on Robotics and Automation, Detroit, 10-15 May 1999, 1018.
[10]	Wilmarth, S.A., Amato, N.M. and Stiller, P.F. (1999) MAPRM: A Probabilistic Roadmap Planner with Sampling on the Medial Axis of the Free Space. IEEE International Conference on Robotics and Automation, Detroit, 10-15 May 1999, 1024.
[11]	Geraerts, R. and Overmars, M.H. (2006) Sampling and Node Adding in Probabilistic Roadmap Planners. Robotics and Autonomous Systems, 54, 165-173. https://doi.org/10.1016/j.robot.2005.09.026
[12]	Hsu, D., Sánchez-Ante, G. and Sun, Z. (2005) Hybrid PRM Sampling with a Cost-Sensitive Adaptive Strategy. Proceedings of the 2005 IEEE International Conference on Robotics and Automation, Barcelona, 18-22 April 2005, 3874.
[13]	Zhang, Y., Fattahi, N. and Li, W. (2013) Probabilistic Roadmap with Self-Learning for Path Planning of a Mobile Robot in a Dynamic and Unstructured Environment. IEEE International Conference on Mechatronics and Automation, Takamatsu, 4-7 August 2013, 1074. https://doi.org/10.1109/ICMA.2013.6618064
[14]	Brand, M., Masuda, M., Wehner, N. and Yu, X.H. (2010) Ant Colony Optimization Algorithm for Robot Path Planning. International Conference on Computer Design and Applications, Qinhuangdao, 25-27 June 2010, Vol. 3, 436-437. https://doi.org/10.1109/ICCDA.2010.5541300
[15]	Elbanhawi, M., Simic, M. and Jazar, R.N. (2014) Continuous-Curvature Bounded Trajectory Planning Using Parametric Splines. Frontiers in Artificial Intelligence and Applications, Chania, Volume 262, 514.
[16]	Elbanhawi, M., Simic, M. and Jazar, R.N. (2015) Continuous Path Smoothing for Car-Like Robots Using B-Spline Curves. Journal of Intelligent & Robotic Systems, 80, 25-26. https://doi.org/10.1007/s10846-014-0172-0
[17]	Tai, C.L., Hu, S.M. and Huang, Q.X. (2003) Approximate Merging of B-Spline Curves via Knot Adjustment and Constrained Optimization. Computer-Aided Design, 35, 894. https://doi.org/10.1016/S0010-4485(02)00176-8