Level Set Based Path Planning Using A Novel Path Optimization Algorithm For Robots
Level Set Based Path Planning Using A Novel Path Optimization Algorithm For Robots
Level Set Based Path Planning Using A Novel Path Optimization Algorithm For Robots
Xiao-Guang Zhang1, Wei Zhang1,#, Hui Li1,2, Ming-Qin Liu1, and Sungki Lyu3,#
1 School of Mechanical and Electrical Engineering, China University of Mining & Technology, 1, Daxue Road, Xuzhou, Jiangsu, 221116, China
2 School of Mechanical and Electrical Engineering, ZaoZhuang University, 1, Beian Road, Zaozhuang, Shandong, 277160, China
3 School of Mechanical and Aerospace Engineering, ReCAPT, Gyeongsang National University, 501, Jinju-daero, Jinju-si, Gyeongsangnam-do, 52828, Republic of Korea
# Corresponding Authors / E-mail: 18052127939@163.com, TEL +86-18052127939, ORCID: 0000-0002-5131-1621
E-mail: sklyu@gnu.ac.kr, TEL: +82-55-772-1632, ORCID: 0000-0001-9803-0272
KEYWORDS: Path optimization, Active contour, Path planning, Level set, Distance field
In order to decrease the path length and control the minimum distance between the path and the obstacles when the level set based
path planning algorithm is adopted , a new path optimization algorithm named elastic particle is proposed in this paper. Firstly, the
iteration expression of optimization algorithm is deduced by active contour theory. Secondly, to ensure the convergence of algorithm,
the relation among each item in the algorithm expression is analyzed and its convergence condition is determined. At last, level set
algorithm is improved so that the smoothness of the initial path and the convergence speed of the algorithm are improved. In addition,
a method named the nearest boundary distance is put forward to accelerate the operation speed of the algorithm. What’s more,
memory pool and binary sort tree are adopted in the code to further reduce the running time of this algorithm. The optimal values
of the algorithm parameters are analyzed via the simulation experiment,and its result demonstrates that the new algorithm has greatly
optimized the path of algorithm-level set and guaranteed fast running speed and high reliability.
Manuscript received: July 28, 2017 / Revised: March 19, 2018 / Accepted: June 15, 2018
to further optimize the initial path by analyzing the active contour theory.
The basic principle of this algorithm is that the path is considered as a
set of ordered particles. Each particle is balanced under the action of
internal and external force so that the path converges to a predetermined
position. In the construction of external force, in order to accelerate the
running speed of this algorithm, memory pool and binary sort tree are
used for the implementation of the resolving distance field problem. To
accelerate the convergence speed of optimizing algorithm, bilinear
interpolation and four-order Range-Kutte method are taken into the
velocity express in Eikonal function to solve the initial path.
∂C ∇P
------- = – (β C′)′ + ∇g (3) ∇g = Frep = –δ ----------- (8)
∂t ∇P
The initial path can be optimized by Eq. (3). The first item is the In which δ is the magnitude of the repulse force. ∇P/ ∇P is the
internal force which can be classified as interaction force between normalized potential field, which is the direction of the repulse force.
particles, and the second item is the external force exerted by the Hence, in order to guarantee the converge of algorithm, the path
external environment on the particles. expression must meet the following equation:
Eq. (3) is discretized through finite differences method and the
δ
recursion formulae of path can be got C″ ≤ –--- Frep ( d ) (9)
β
n+1 n n n n n n
Ci = Ci – Δt (β ( ( Ci – Ci – 1) – ( Ci + 1 – Ci ) ) + ∇g ( Ci ) ) (4) Eq. (9) can also be rewritten as follows
δ ⎧ ∇P ⎫ δ
Where i represents the ith particle, n is the iterative number, C is C″ ≤ --- sup ⎨ ----------- ⎬ = --- (10)
0
β Ω ⎩ ∇P ⎭ β
the initial path which is obtained by the previous algorithm. C consists 0
n n
of m particles, Co is the start point, Cm is the destination. Therefore, Eq. (10) can guarantee the convergence of our algorithm.
Let g(C) = −P(C), the P(C) can be seen as the repulse potential in
the artificial potential algorithm, ∇P ( Ci ) is the repulse force acting on
the ith particle. So the equation can be modified as: 3. The Solving Method of Distance Field
n+1 n n n n n
Ci = Ci – Δt (β ( ( Ci – Ci – 1 ) – ( Ci + 1 – Ci ) ) – Frep ) (5) In section 2, the distance from the point in free space to the obstacle
INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 19, No. 9 SEPTEMBER 2018 / 1333
Γ(c) represents the distance field. There are two main methods
currently for getting the solution of distance field set: the direct method
and the Eikonal equation method.
(1) The direct method is easy to operate and realize, which is
described in Eq. (12):
k k k k
di = min {di1, di2, …, dM }
(12)
1 2 N
di = min {di , di , …, dij }
Where i is the ith grid point in free space, j is the jth boundary point, Fig. 2 The initial status of computing distance field set
k is the kth obstacle. The complexity of this method is O(MN), it requires
a great number of computations.
(2) Eikonal equation method gets the distance distribution through
solving Eikonal equation shown as Eq. (13):
1
∇T = --- (13)
V
Fig. 5 Another updating method when there exists two rational Fig. 6 Simulation map (For estimating the influence of β and Δt on
distance values the convergence of path optimization algorithm)
min(a, b). The distance value of this point will be updated by the evolution stage. External force does not work, the path evolution is
following methods: completely controlled by internal force according to Eq. (16)
As shown in Fig. 4, q is the point with shortest distance selected 2
∂C ∂ C
from the narrow band set, the distance of p is to be updated. If a==b, ------- = --------2- (16)
∂t ∂p
S is the nearest boundary point of the point with rational value in
1
seq = argmin(d , d ).
1 2 Eq. (17) is gradient descent flow of Eq. (16). Eq. (17) illustrates that
As shown in Fig. 5, q is the point with shortest distance selected decreasing the energy means the decrease of length of C(p). The path
from the narrow band set, the distance of p is to be updated. If a ≠ b , will evolve to a straight line.
S is the nearest boundary point of the point with rational value in
1 When there is a particle ei, which satisfies d(ei) ≤ rth, the path
direction x, S is the nearest boundary point of the point with rational
2 optimization is in the constrained evolution stage. In this stage, the
value in direction y. Set seq = argmin(a, b), so the nearest boundary position of particle will be gradually stable. Therefore, just selecting
point of point p is Sseq and the distance of point p is ||p−Sseq||. appropriate β and δ can ensure that the path will converge to the desired
position. In this paper, we propose the following method to determine
β and δ.
4. The Acquisition of Obtaining Initial Path
δ
--- = sup { C″ }n – 1 (18)
β n Ω
Suppose there is a velocity gradient area whose radius is Rth (Rth >
rth) around the forbidden space. In the algorithm of this paper, the map Where n is current iteration.
is a binary double-type picture, and the area with value 0 refers to free
area and the area with value 1 refers to forbidden area. Below velocity
function is proposed according to these information: 6. Experiment and Discussion
max ( Rth – d ( x, y )0 )
V ( x, y ) = 1 – -------------------------------------------
- (15) 6.1 Influence of Δt and β on convergence performance of path
Rth
optimization algorithm
Where d(x, y) is the distance of the point from free space to In Eq. (5), Δt and β are two important parameters. It is intuitive that
forbidden space. Eq. (15) is used to compute the solution of the Eikonal the convergence capability of path optimization algorithm is related to
equation. In the new method, the minimum distance between the path these two parameters. We will discuss how Δt and β impact on the
and the forbidden space is Rth. convergence performance of path optimization algorithm in this part.
Fig. 6 is the simulation map. Main parameters are set as follows: R = th
5. Convergence Analysis of the Optimization Algorithm Figs. 7 and 9 show the change of path length when β = 1 and Δt is
varying. Figs. 8 and 10 show the change of path length when Δt = 0.1
By the previous analysis, the path optimization consists two stages: and β is varying. Simulation experiments show that increasing Δt and
free evolution stage and constrained evolution stage. Suppose C is the β is conducive to decreasing the path length and accelerating the
initial path, e is a particle in the path, d represent the distance field, and convergence speed. There are two main reasons: (1) The increment of
n is the number of particles. Δt and β causes larger internal force, accordingly, the convergence
When rth < d(ei) ≤ Rth, i = 1, 2, …, n, initial path will be in the free speed is increased. (2) The larger Δt and β is, the longer movement
INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 19, No. 9 SEPTEMBER 2018 / 1335
Fig. 7 The change of length of path using Fig. 6(a) (Varying Δt, while Fig. 9 The change of length of path using Fig. 6(b) (Varying Δt, while
β = 1) β = 1)
Fig. 8 The change of length of path using Fig. 6(a) (Varying β, while Fig. 10 The change of length of path using Fig. 6(b) (Varying β,
Δt = 0.1) while Δt = 0.1)
distance of particle will be, which will cause the smoothness and length
of the path decreases. However, when the value exceeds permitted
value, the feasible path may not be obtained. So we set β = 1, Δt = 0.1 Fig. 11 The distance distribution between every point on the path and
in this paper. the forbidden region when adopting Fig. 19(a)
Fig. 13 The path obtained by our algorithm when adopting Fig. 19(a)
Fig. 16 Comparison of the time used for several distance field solving
Fig. 14 The scene of simulation map algorithms
of obstacle remains unchanged, while the size of free space is modified. 7. Conclusions
Under different size of maps, algorithm runs 50 times and record the
average running time. Fig. 15 is the parameters of this experiment, the In this paper, a new path optimization algorithm is proposed. Main
results are shown as Fig. 16. It can be seen that the memory pool and conclusions draw by previous analysis are presented as follows:
binary sort tree improve the speed of the stablishing distance field set (1) Compared with traditional Eikonal method, the nearest boundary
algorithm. The results meet the design goal of the algorithm. point propagation method can obtain the distance field distribution with
the minimum amount of time, which mainly owing to the adoption of
6.4 Running time of the whole algorithm memory pool and binary sort tree in this algorithm.
The running time of establishing distance field set is discussed in (2) A gradual change speed for Eikonal can reduce the oscillation
the previous section. In this section the running time of the whole phenomenon of initial path obtained by common gradient descent and
algorithm will be investigated. Fig. 17 is the simulation parameters. accelerate the convergence speed of the optimization algorithm. From
Other parameters are: β = 1, Δt = 0.1, R = 5, r = 3. The iteration
th th the simulation results, the error between minimum distance to obstacle
number is 5000. Algorithm runs 100 times under different scene and for every point and the expected is 1.41%, and it basically meets the
records the average running time. The results are shown as Fig. 18. The requirements.
establishment of the distance field set and the establishment of the (3) The convergence condition of the optimization algorithm can
global cost are only related to the map, which is independent of the ensure the convergence performance of the path. The final path
path in the scene. The simulation shows that the algorithm in this paper converges to the desired position under the influence of this
has high real-time performance. optimization algorithm. The simulation results show that the proposed
INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 19, No. 9 SEPTEMBER 2018 / 1337
2. Qin, Y.-Q., Sun, D.-B., Li, N., and Cen, Y.-G., “Path Planning for
Mobile Robot Using the Particle Swarm Optimization with Mutation
Operator,” Proc. of International Conference on Machine Learning
and Cybernetics, pp. 2473-2478, 2004.
4. Fu, Y., Ding, M., and Zhou, C., “Phase Angle-Encoded and
Quantum-Behaved Particle Swarm Optimization Applied to Three-
Dimensional Route Planning for UAV,” IEEE Transactions on
Systems, Man and Cybernetics, Part A: Systems and Humans, Vol.
42, No. 2, pp. 511-526, 2012.
Fig. 17 Start point and destination of the scene for evaluating running 5. Xue, Y. and Liu, H., “Optimal Path Planning in Complex Indoor
time Environment Based on Improved PSO,” Journal of Computational
Information Systems, Vol. 7, No. 6, pp. 2158-2165, 2011.
8. Cheng, C.-T., Fallahi, K., Leung, H., and Chi, K. T., “A Genetic
Algorithm-Inspired UUV Path Planner Based on Dynamic
Programming,” IEEE Transactions on Systems, Man, and
Cybernetics, Part C (Applications and Reviews), Vol. 42, No. 6, pp.
1128-1134, 2012.
Fig. 18 The results of operating time 9. Wang, X. C., Zhou, M. G., Liu, X. Y., and Fan, Y. C., “The Research
on Robot Global Path Planning Based on Simulated Annealing
Dissipative Ant System,” Proc. of 7th World Congress on Intelligent
algorithm can reduce the average time by 59.9% compared with the Control and Automation, pp. 5764-5769, 2008.
traditional algorithm.
10. Miao, H. and Tian, Y.-C., “Robot Path Planning in Dynamic
Environments Using a Simulated Annealing Based Approach,” Proc.
of International Conference on Control, Automation, Robotics and
ACKNOWLEDGEMENT
Vision, 2008.
The authors are grateful to the anonymous referees for their valuable 11. Duan, H., Yu, Y., Zhang, X., and Shao, S., “Three-Dimension Path
comments and suggestions for improving the presentation of this paper. Planning for UCAV Using Hybrid Meta-Heuristic ACO-DE
Project Funded by the Priority Academic Program Development of Algorithm,” Simulation Modelling Practice and Theory, Vol. 18, No.
Jiangsu Higher Education Institutions (PAPD). Production, Prospective 8, pp. 1104-1115, 2010.
Joint Research Projects in Jiangsu Province (BY2016026-02).
12. Zhang, G.-l. and Jia, H.-m., “Global Path Planning of AUV Based
on Improved Ant Colony Optimization Algorithm,” Proc. of IEEE
International Conference on Automation and Logistics (ICAL), pp.
REFERENCES
606-610, 2012.
1. Ferguson, D., Likhachev, M., and Stentz, A., “A Guide to Heuristic- 13. Wei, Z., Luo, Q., and Su, X., “A Novel Mobile Robot Path Planning
1338 / SEPTEMBER 2018 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 19, No. 9
14. Xu, B., Stilwell, D. J., and Kurdila, A. J., “Fast Path Re-Planning
Based on Fast Marching and Level Sets,” Journal of Intelligent &
Robotic Systems, Vol. 71, Nos. 3-4, pp. 303-317, 2013.
15. Petres, C., Pailhas, Y., Patron, P., Petillot, Y., Evans, J., and Lane, D.,
“Path Planning for Autonomous Underwater Vehicles,” IEEE
Transactions on Robotics, Vol. 23, No. 2, pp. 331-341, 2007.
17. Kimmel, R. and Sethian, J. A., “Optimal Algorithm for Shape from
Shading and Path Planning,” Journal of Mathematical Imaging and
Vision, Vol. 14, No. 3, pp. 237-244, 2001.
18. Pal, A., Tiwari, R., and Shukla, A., “Modified A* Algorithm for
Mobile Robot Path Planning,” in: Soft Computing Techniques in
Vision Science, Patnaik, S., Yang, Y. M., (Eds.), Springer, pp. 183-
193, 2012.
20. Witt, J. and Dunbabin, M., “Go with the Flow: Optimal AUV Path
Planning in Coastal Environments,” Proc. of Australian Conference
on Robotics and Automation, 2008.
21. Liu, H., Stoll, N., Junginger, S., and Thurow, K., “A Floyd-Dijkstra
Hybrid Application for Mobile Robot Path Planning in Life Science
Automation,” Proc. of IEEE International Conference on Automation
Science and Engineering (CASE), pp. 279-284, 2012.
22. Shaikh, E. A. and Dhale, A., “AGV Path Planning and Obstacle
Avoidance Using Dijkstra's Algorithm,” International Journal of
Application or Innovation in Engineering & Management (IJAIEM),
Vol. 2, No. 6, pp. 77-83, 2013.