Mathematical Problems in Engineering

Volume 2013, Article ID 613964, 9 pages

http://dx.doi.org/10.1155/2013/613964

## Path Planning for Mobile Objects in Four-Dimension Based on Particle Swarm Optimization Method with Penalty Function

^{1}Navigation College, Wuhan University of Technology, Wuhan, Hubei 430063, China^{2}Department of Mathematics, Islamic Azad University, Bojnourd Branch, Bojnourd 94186-54145, Iran^{3}Chutian College, Huazhong Agricultural University, Wuhan, Hubei 430205, China

Received 4 December 2012; Accepted 1 January 2013

Academic Editor: Yang Tang

Copyright © 2013 Yong Ma et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

We present one algorithm based on particle swarm optimization (PSO) with penalty function to determine the conflict-free path for mobile objects in four-dimension (three spatial and one-time dimensions) with obstacles. The shortest path of the mobile object is set as goal function, which is constrained by conflict-free criterion, path smoothness, and velocity and acceleration requirements. This problem is formulated as a calculus of variation problem (CVP). With parametrization method, the CVP is converted to a time-varying nonlinear programming problem (TNLPP). Constraints of TNLPP are transformed to general TNLPP without any constraints through penalty functions. Then, by using a little calculations and applying the algorithm PSO, the solution of the CVP is consequently obtained. Approach efficiency is confirmed by numerical examples.

#### 1. Introduction

Mobile objects such as autonomous unmanned aerial vehicles (UAVs) and autonomous underwater vehicles (AUVs) [1–6] have been applied for specific utilities in the engineering application field, especially in offshore drilling, spacewalk, and so forth. To guarantee the successful operation of above kinds of mobile objects, the problem of path planning for mobile objects in four-dimension (three spatial and one-time dimensions) becomes indispensable and ever-increasingly important. Therefore, we focus our investigations on the path planning for mobile objects in the presence of obstacles in four-dimension.

Path planning in a time-varying environment with static or moving obstacles is inherently hard [7, 8]. Even for a simple case in two dimensions, the problem is NP-hard and is not solvable in polynomial time [8–10]. Our addressed problem is characterized by objects dynamically moving in large outdoor space denoted as four-dimension with obstacles, where denotes the three-dimensional space and denotes the temporal dimension. Therefore, in this problem, we should consider the constraints of safety, velocity and acceleration brought by the influence among obstacles and objects, and the curse of dimensionality [2] brought by the time-varying motion in three spatial dimensions. This fact makes path planning for mobile object in four-dimension with obstacles a challenging research problem [9].

Generous excellent methods have been applied to path planning problems. Many recent researches have focused on the gird-based techniques [3, 5, 8]. However, lots of broken lines and excessive interrupted turnings have existed in the planned path with grid-based approaches [8]. Then, gird-based approaches violate the constraint of path smoothness [11]. In [12], Bzier-curve-based approach has been adopted to achieve smooth path, but index of their curve is restricted to one certain number. In [7, 13], parametrization-based method is proposed to fulfill the ideal path and their solution is obtained through software such as Lingo, Matlab with the embedded function. Due to the error of the embedded function of software itself, the resultant path in [7, 13] can be remarkably improved with some techniques.

In our problem, the goal is to minimize path length such that the path is smooth and safe. This goal is formulated as the calculus of a variation problem (CVP) whose variable is the path of the mobile object. Then using parametrization method [7, 13] and some calculations, the CVP is converted to the sequence of TNLPPs whose variable is a polynomial function with unknown constant coefficients. After some calculation, the TNLPP is equivalent to a conventional nonlinear programming problem (NLPP).

In path planning problem, classic methods proved to be inefficient for high-dimension space, requiring considerably long time and huge storage memory. Consequently, heuristic methods are developed to cope with the curse of dimensionality in path planning problem. Many efficient metaheuristic approaches such as simulated annealing (SA), genetic algorithm (GA), ant colony optimization (ACO), taboo search (TS), and particle swarm optimization (PSO) are applied to path planning problems. PSO is one prevalent algorithm founded by Kennedy and Eberhart [14]. For the last decades, PSO has been extensively used in the field of path planning in two dimensions [15, 16]. PSO is a swarm intelligence method inspired by the collective behavior of birds flock. PSO algorithm is famous for its concise mechanism, fine convergence, and little computational time [17–19]. Up to the present time, little attention has been paid to path planning in four-dimension with PSO. To achieve the much better solution than [4, 13], we extend PSO to solve the above TNLPP.

As fitness function in two-dimension cannot be directly used in four-dimension, the foremost issue of PSO is how to determine the proper fitness function for our problem. The TNLPP is subject to path smoothness, safety criterion, and velocity and acceleration constraints, which can be classified as multiobject optimization problem. To devise the rational fitness function, penalty function is adopted to transform the constrained TNLPP to general unconstrained TNLPP. The constraints are placed into the fitness function via the penalty parameter in such a way as to penalize any violation of the constraints. Then, solution of the TNLPP can be attained with PSO. The advantage of our approach is that the paths for mobile objects in four-dimension can be achieved promptly and the quality of the paths is high in the perspective of path length and smooth. Within the algorithm based on PSO, the population can converge to the feasible region and reach the ideal fitness value effectively.

The rest of this paper is organized as follows. Section 2 describes the problem formulation. Section 3 introduces penalty function and PSO for path planning of mobile objects. Section 4 presents illustrative path planning examples for mobile objects in four-dimension using the proposed approach. Finally, Section 5 presents conclusions.

#### 2. Problem Formulation

Compared with former researches in [1, 6, 20, 21] that treated the objects as points, the actual shape of mobile objects and obstacles are taken into account, which would be more suitable for practicality. Consider a single rigid and free moving object with the center , , moving from at time 0 to at time in the presence of obstacles. Where is a continuously unknown differentiable real vector-valued function, which is the path of . is a given real number as final moving time of , and , . We suppose obstacle is a rigid object with the center ,, , where , are known continuous real vector-valued functions, which are the paths of motion obstacles, and is a given real number as final moving time of . Mobile objects and all obstacles are simplified as spheres. The radius of and obstacles can be denoted as and , , respectively. In Figure 1, the two smaller spheres are the start and final position of , respectively; the other two larger spheres stand for obstacles.

Planned path of should comply with the following constraints: the dynamic constraints of , the reachable and the smoothness of , and the safety distance between and obstacles. Satisfied with all above constraints, the goal is to minimize the length of .

To satisfy dynamic constraints of the mobile object , the velocity and acceleration of are restricted to certain rational regions. Thus, we suppose , , where , , , , , and all belong to , are known as continuous real vector-valued functions as the boundaries of ,, and for all , respectively.

The reachable of means mobile object should follow the from to . Then, criteria of dynamic and reachable are correspondingly given as below:

The smoothness of is satisfied inherently, that is, the planned path is introduced by a polynomial function belonging to (the set of highly smooth functions).

For the safety distance criterion, set where , , is the distance between object (or the planned path ) and obstacle at the moment . Set is a given safety distance between and , which guarantees and are free of collision in the motion of . Then, the distances between and obstacles are denoted as , where . Then, the safety criterion is listed as below:

Length of can be expressed as follows: Then, in the environment , to achieve the minimum path length of named , which is constrained by above criteria, for , the following CVP is defined:

Similar to the former work in [7, 13], for all , we set , where are polynomials of degree at most with unknown constant coefficients. Then, with parametrization method, by substituting for in the problems (5) and (6), the sequence of the TNLPP is obtained as follows: Suppose is the set of such that the CVP (5) and (6) is feasible and is the set of such that the TNLPP (7) and (8) is feasible. Also, we suppose and are not empty. Then, by the following theorem is proven that the sequence of the solutions of the problems (7) and (8) converges to the solution of the problems (5) and (6).

Theorem 1. *If and , Then . *

* Proof. *See [13].

When all constraints of are considered, such as bounded velocity and acceleration constraints, reachable of , and safety criterion, TNLPP (7) and (8) is transformed as follows:

Now, we partition the interval into equal parts as , set . Thus, by using a numerical integration method such as trapezoidal rule, for , problems (9) and (10) are converted to the following problem:

Theorem 2. *Solutions of the problems (11), (12), (9), and (10) are the same, if in problems (11) and (12) tends to infinity. *

* Proof. * See [22].

With parametrization method, our path planning problem in four-dimension passages from the CVP to TNLPP. When taking into account all constraints, the TNLPP is converted into NLPP. Thus, how to achieve the ideal solution of this NLPP becomes the first and foremost problem. In the following section, we propose the PSO with penalty function to resolve this NLPP.

#### 3. Determine the Optimal Path for Objects in Four-Dimension Based on PSO with Penalty Function

Generally, many softwares such as lingo, Matlab can be used to solve above NLPP with their embedded functions. In [7, 13], fine results have been reported with the embedded functions of above softwares. However, due to the error of the functions themselves, the obtained resolution which costs much computational time and spatial memory could not be the optimal one. Even in some complicated cases, no solution can be generated owing to insufficiency of the functions. Thus, to resolve the path planning problem in four-dimension taking on character of optimization function with multiple constraints, the PSO with penalty function is proposed.

##### 3.1. Multiconstrained NLPP Converts to Unconstrained NLPP with Penalty Function

In constrained optimization (CO) problems such as constrained TNLPP and NLPP, the search space consists of two kinds of points: feasible and unfeasible. Feasible points satisfy all the constraints, while unfeasible points violate at least one of them. Penalty function technique solves CO problems through a sequence of unconstrained optimization problems (UOPs) [23–25]. With proper penalty parameters, which could evaluate the degree of penalty, the constraints are deleted and added into the goal function. Thus, CO problems are changed into general UOP with penalty function.

Based on penalty function, the CO problems for path planning of mobile objects in four-dimension are converted to the following UOP. Then, for , expression of the UOP can be adopted as the fitness function of PSO as follows:

In (13), and stand for large scalars to penalize the violation of the constraints. and are the lower and higher penalty coefficients of region boundary of the mobile object, respectively; and are the penalty coefficients of velocities boundary, respectively; and are the penalty coefficients of acceleration boundary, respectively; and are the penalty coefficients of reachable constraints, respectively; and is the penalty coefficients of safety constraints. All the constraints should be satisfied well. Thus, on the one hand, the penalty function parameters would reflect the equal importance of each constraint, and on the other hand, the defined parameters can lead the population within the feasible region. The ideal value of penalty parameters should be specified moderately which is not too large or too small. Then, when any constraint violation occurred, the penalty function parameters are defined as (). Then, the focus of the above minimization problem becomes how to determine the polynomials of . After ascertainment of the fitness function for PSO, the following section presents the solution for the above problem based on PSO.

##### 3.2. Optimal Path for Mobile Objects in Four-Dimension with Obstacles Using PSO

In PSO, with particles size , particle stands for one potential solution of the given problem in -dimensional space. Each particle has three vectors at the th iteration, current position vector , velocity vector , and its own best position vector which represents the best objective function value or fitness value. One global best position vector is defined as the position of the best particle among the particles in the population. Figure 2 shows the iterative process of position vector in PSO.

When all the terms are stated, the standard PSO formula can be

In (14), the upper equation stands for the movement update of particle in dimension at the iteration number , where is a control factor controlling the magnitude of , and are positive acceleration coefficients, and and are uniform random numbers in ; the lower equation gives the corresponding position update of particle . Customarily, as in [26], we set the parameters to , and .

When it comes to our problem, the solution of corresponds to , where . We set the coefficients of correspond to . Thus, the dimension of which denoted as equals to all three indexes summation of , , and . For example, the highest sequential indexes of polynomials for , , and are 3, 4, and 5, respectively, then the value of is 12. With (11), after choosing proper parameters, the velocity and position update of particles can be implemented until the predefined conditions such as the generation number are satisfied. Thus, the achieved can be treated as the polynomial coefficients of , and the corresponding value of fitness function should be the optimal path length .

Basic flow of PSO algorithm named is given as below, from which we can obtain the optimal solution of our path planning problem for mobile objects in four-dimension promptly. In , and represent fitness function and iteration number, respectively. Means of and , and are the same as depictured above.

*Algorithm.* **Input:** fitness function , particle size , acceleration coefficients , , inertia factor , iteration number , and spatial dimension . **Output:** the latest position and the optimal value of which represent the coefficients of and , respectively.(1) Initialize and of all particles in the swarm. (2); .(3)**if** the particle with minimum , then , .(4)**end if**(5).(6)**for each** particle in swarm (7) Update the with (14).(8)**if**, then .(9)**end if**(10)**if **, then .(11)**end if**(12);(13)**end for each**(14).

With algorithm , the optimal planned path expressions and length of mobile object can be achieved which corresponding to the value of and , respectively.

#### 4. Experimental Results

Simulations are provided to validate the effectiveness of the proposed PSO with penalty function, and path planning for mobile objects in four-dimension under different circumstances is discussed. We set , where is interpreted as control function which shows the speed of object in the direction of at the moment . In the following figures, the scales are the same and all quantities conform to a given unit system, for instance, meters, per second, and so forth. In this paper, all the computations were run on a PC with CPU Intel Core2 Duo and 2 GB of RAM, and all the codes are written in Matlab 7.0 software.

##### 4.1. Case Study with Stationary Obstacles in Four-Dimension

To prove the validity of the proposed approach, we test it using the case data in [13] and give the experiment results. Consider the mobile object in a space , in the presence of five stationary obstacles with the following information: , , , , where . Table 1 gives the raw information data list of the obstacles.

With our proposed approach, the path length is 1.819 while in [13] the value is 1.844, and their approach was based on the embedded functions of softwares such as Matlab, lingo, and so forth. As shown in Figure 3, the planned path is smooth and conflict-free with the five obstacles. The control function denoted by velocity changes on time is illustrated in Figure 4, wherein the curves of velocity change meet the requirements of velocity constraints. The distances between and , are shown in Figure 5, and all the curves reveal that the distances satisfy the predefined safety distances.

Results of this example demonstrate that our approach fits for path planning problems of mobile objects in four dimensions with static obstacles.

##### 4.2. Case Study with Stationary and Moving Obstacles in Four-Dimension

Circumstance settings of three dimensions with static and moving obstacles are given as follows.(1)Parameters of mobile object : radius , initial points of center position , final points of center position .(2)Boundary conditions: velocity boundary . Time boundary . Safety distance , , .(3)Parameters of obstacles: radius . Center position , , . Path of moving obstacle denoted by is listed as follows: After all constraints related to the motion of static and moving obstacles are taken into account, with transformation of parametrization method, the problems are resolved by algorithm . For , paths of mobile object are given as below: From two different angles of view, the condensed and discontinuous planned paths of sequentially showed in Figures 6 and 7 are conflict-free with all obstacles moving and stationary in environment of time-varying three dimensions. In our approach, the navigation path length of is 93.84 which corresponding to the best fitness value of PSO. Compared with [13], with which method the planned path length is 96.56.

The PSO curve of fitness is shown in Figure 8, with the proposed approach, the set of solving space converges to the best value within the predefined iteration number 1000 in 5 s. Planned paths in Figures 6 and 7, and fitness curve in Figure 8 verify that the method of PSO with penalty function can successfully deal with the mobile objects path planning in four-dimension with moving obstacles.

Velocities of and the distances between and , are shown in Figures 9 and 10, respectively. As all velocities and the distances are limited to their corresponding boundaries, consequently all the velocity constraints and safety distance criterion are satisfied. Results of this example indicate that the proposed approach is valid for mobile objects path planning with static or moving obstacles in four dimensions.

#### 5. Conclusion

Path planning problems for mobile objects in four-dimension with static and moving obstacles, which have been paid little attention, are introduced. We have formulated the problem as one TNLPP. Particle swarm optimization method with penalty function is proposed for the problems. Results of several numerical examples have verified the effectiveness of our approach. With the presented approach, the resultant path is collision-avoidance with all other obstacles, smooth and much shorter than other methods. Based on the work of this paper, in future works, we will focus on the multiobject path planning problems in four-dimension with stationary and moving obstacles.

#### Acknowledgments

The authors wish to sincerely thank the Editor-in-Chief and anonymous reviewers for their constructive and valuable comments which lead to the better presentation of this paper. The authors (Y. Ma and Y. Xu) are partially supported by National Science Foundation of China (no. 51109173).

#### References

- E. Frazzoli, Z. H. Mao, J. H. Oh, and E. Feron, “Resolution of conflicts involving many aircraft via semidefinite programming,”
*Journal of Guidance, Control, and Dynamics*, vol. 24, no. 1, pp. 79–86, 2001. View at Publisher · View at Google Scholar · View at Scopus - W. Powell,
*Approximate Dynamic Programming: Solving the Curses of Dimensionality*, vol. 703, Wiley-Blackwell, 2007. - Y. Kim, D.-W. Gu, and I. Postlethwaite, “Real-time path planning with limited information for autonomous unmanned air vehicles,”
*Automatica*, vol. 44, no. 3, pp. 696–712, 2008. View at Publisher · View at Google Scholar · View at MathSciNet - Y. Wang, D. M. Lane, and G. J. Falconer, “Two novel approaches for unmanned underwater vehicle path planning: constrained optimization and semi-infinite constrained optimization,”
*Robotica*, vol. 18, no. 2, pp. 123–142, 2000. View at Publisher · View at Google Scholar · View at Scopus - C. Pêtrès, Y. Pailhas, P. Patrón, Y. Petillot, J. Evans, and D. Lane, “Path planning for autonomous underwater vehicles,”
*IEEE Transactions on Robotics*, vol. 23, no. 2, pp. 331–341, 2007. View at Publisher · View at Google Scholar · View at Scopus - P. P. Y. Wu, D. Campbell, and T. Merz, “Multi-objective four-dimensional vehicle motion planning in large dynamic environments,”
*IEEE Transactions on Systems, Man, and Cybernetics B*, vol. 41, no. 3, pp. 621–634, 2011. View at Publisher · View at Google Scholar · View at Scopus - Y. Ma, H. Wang, and M. Zamirian, “A novel approach for multiple mobile objects path planning: parametrization method and conflict resolution strategy,”
*Physics Letters A*, vol. 376, no. 4, pp. 377–386, 2012. View at Publisher · View at Google Scholar - H. Wang, Y. Ma, Y. Xie, and M. Guo, “Mobile robot optimal path planning based on smoothing A* algorithm,”
*Journal of Tongji University*, vol. 38, no. 11, pp. 1647–1655, 2010. View at Publisher · View at Google Scholar · View at Zentralblatt MATH · View at Scopus - Y. Guo and L. E. Parker, “A distributed and optimal motion planning approach for multiple mobile robots,” in
*Proceedings of the IEEE International Conference on Robotics and Automation (ICRA ’02)*, vol. 3, pp. 2612–2619, IEEE, May 2002. View at Scopus - J. E. Hopcroft, J. T. Schwartz, and M. Sharir, “On the complexity of motion planning for multiple independent objects; pspace-hardness of the warehouseman’s problem,”
*International Journal of Robotics Research*, vol. 3, no. 4, pp. 76–88, 1984. View at Google Scholar · View at Scopus - T. Berglund, A. Brodnik, H. Jonsson, M. Staffanson, and I. Söderkvist, “Planning smooth and obstacle-avoiding B-spline paths for autonomous mining vehicles,”
*IEEE Transactions on Automation Science and Engineering*, vol. 7, no. 1, pp. 167–172, 2010. View at Publisher · View at Google Scholar · View at Scopus - I. Skrjanc and G. Klancar, “Optimal cooperative collision avoidance between multiple robots based on bernstein-bézier curves,”
*Robotics and Autonomous Systems*, vol. 58, no. 1, pp. 1–9, 2010. View at Publisher · View at Google Scholar - M. Zamirian, A. V. Kamyad, and M. H. Farahi, “A novel algorithm for solving optimal path planning problems based on parametrization method and fuzzy aggregation,”
*Physics Letters A*, vol. 373, no. 38, pp. 3439–3449, 2009. View at Publisher · View at Google Scholar · View at Zentralblatt MATH · View at Scopus - J. Kennedy and R. Eberhart, “Particle swarm optimization,” in
*Proceedings of the IEEE International Conference on Neural Networks*, vol. 4, pp. 1942–1948, December 1995. View at Scopus - M. Saska, M. Macas, L. Preucil, and L. Lhotská, “Robot path planning using particle swarm optimization of ferguson splines,” in
*Proceedings of the IEEE Conference on Emerging Technologies and Factory Automation (ETFA ’06)*, pp. 833–839, IEEE, 2006. - A. W. Mohemmed, N. C. Sahoo, and T. K. Geok, “Solving shortest path problem using particle swarm optimization,”
*Applied Soft Computing Journal*, vol. 8, no. 4, pp. 1643–1653, 2008. View at Publisher · View at Google Scholar · View at Scopus - Y. Tang, Z. Wang, and J. Fang, “Controller design for synchronization of an array of delayed neural networks using a controllable probabilisticpso,”
*Information Sciences*, vol. 181, no. 20, pp. 4715–4732, 2011. View at Publisher · View at Google Scholar - Y. Tang, Z. Wang, and J. Fang, “Feedback learning particle swarm optimization,”
*Applied Soft Computing*, vol. 11, no. 8, pp. 4713–4725, 2011. View at Publisher · View at Google Scholar - Y. Tang, Z. Wang, and J. A. Fang, “Parameters identification of unknown delayed genetic regulatory networks by a switching particle swarm optimization algorithm,”
*Expert Systems with Applications*, vol. 38, no. 3, pp. 2523–2535, 2011. View at Publisher · View at Google Scholar · View at Scopus - R. Abiyev, D. Ibrahim, and B. Erin, “Navigation of mobile robots in the presence of obstacles,”
*Advances in Engineering Software*, vol. 41, no. 10-11, pp. 1179–1186, 2010. View at Publisher · View at Google Scholar · View at Scopus - M. A. P. Garcia, O. Montiel, O. Castillo, R. Sepúlveda, and P. Melin, “Path planning for autonomous mobile robot navigation with ant colony optimization and fuzzy cost function evaluation,”
*Applied Soft Computing Journal*, vol. 9, no. 3, pp. 1102–1110, 2009. View at Publisher · View at Google Scholar · View at Scopus - J. Stoer and R. Bulirsch,
*Introduction to Numerical Analysis*, vol. 12 of*Texts in Applied Mathematics*, Springer, New York, NY, USA, 3rd edition, 2002. View at MathSciNet - J. A. Joines and C. R. Houck, “On the use of non-stationary penalty functions to solve nonlinear constrained optimization problems with GA's,” in
*Proceedings of the 1st IEEE Conference on Evolutionary Computation*, pp. 579–584, June 1994. View at Scopus - J. Yang, Y. Chen, J. Horng, and C. Kao, “Applying family competition to evolution strategies for constrained optimization,” in
*Proceedings of the 6th International Conference on Evolutionary Programming*, pp. 201–211, Springer, 1997. - K. Parsopoulos and M. Vrahatis, “Particle swarm optimization method for constrained optimization problems,” in
*Intelligent Technologies-Theory and Application: New Trends in Intelligent Technologies*, pp. 214–220, IOS Press, 2002. View at Google Scholar - C. F. Juang and Y. C. Chang, “Evolutionary-group-based particle-swarm-optimized fuzzy controller with application to mobile-robot navigation in unknown environments,”
*IEEE Transactions on Fuzzy Systems*, vol. 19, no. 2, pp. 379–392, 2011. View at Publisher · View at Google Scholar · View at Scopus