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

,


Introduction
Mobile objects such as autonomous unmanned aerial vehicles (UAVs) and autonomous underwater vehicles (AUVs) [1][2][3][4][5][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][9][10].Our addressed problem is characterized by objects dynamically moving in large outdoor space denoted as four-dimension ( 1 ,  2 ,  3 , ) with obstacles, where ( 1 ,  2 ,  3 ) 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, girdbased 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][18][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.

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  () = ( 1 (),  2 (),  3 ()),  ∈ [0,  ], moving from  0 ∈ R 3 at time 0 to   ∈ R 3 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  0 = (0),   = (  ).We suppose obstacle obs  is a rigid object with the center   () = ( 1 (),  2 (),  3 ()),  = 1, 2, . . ., ,  ∈ [0,   ], where   (⋅),  = 1, 2, . . .,  are known continuous real vectorvalued functions, which are the paths of motion obstacles, and   is a given real number as final moving time of obs  .Mobile objects and all obstacles are simplified as spheres.The radius of  and obstacles can be denoted as   and   ,  = 1, 2, . . ., , 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 R 3 , are known as continuous real vector-valued functions as the boundaries of (), ẋ (), and ẍ () for all  ∈ [0,  ], respectively.
The reachable of (⋅) means mobile object  should follow the (⋅) from  0 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  ∞ [0,   ] (the set of highly smooth functions).
Set   is a given safety distance between  and obs  , which guarantees  and obs  are free of collision in the motion of .
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.

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][24][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),  1 ,  2 , . . .,  8 and  9 stand for large scalars to penalize the violation of the constraints. 1 and  2 are the lower and higher penalty coefficients of region boundary of the mobile object, respectively;  3 and  4 are the penalty coefficients of velocities boundary, respectively;  5 and  6 are the penalty coefficients of acceleration boundary, respectively;  7 and  8 are the penalty coefficients of reachable constraints, respectively; and  9 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   = 100 ( = 1, 2, . . ., 9).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.

Optimal Path for Mobile Objects in Four-Dimension with
Obstacles Using PSO.In PSO, with particles size , particle  (1 ≤  ≤ ) stands for one potential solution of the given problem in -dimensional space.Each particle  has three vectors at the th iteration, current 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  + 1, where  is a control factor controlling the magnitude of    ,   and   are positive acceleration coefficients, and  1 and  2 are uniform random numbers in [0, 1]; the lower equation gives the corresponding position   ( + 1) update of particle .Customarily, as in [26], we set the parameters to   =   = 1, and  = 0.8.
Basic flow of PSO algorithm named PSO basic 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 PSO basic ,  fitness and  represent fitness function min  pena and iteration number, respectively.Means of   and   , , , and  are the same as depictured above.Algorithm.PSO basic ( fitness , ,   ,   , , , ) Input: fitness function  fitness , particle size , acceleration coefficients   ,   , inertia factor , iteration number , and spatial dimension .Output: the latest position   and the optimal value of  fitness which represent the coefficients of   (⋅) and  0 , respectively.
(1) Initialize   and   of all particles in the swarm.With algorithm PSO basic , the optimal planned path expressions   (⋅) and length  0 of mobile object  can be achieved which corresponding to the value of   and  fitness (  ), respectively.

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  ∈ [0,   ].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.
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 obs  ,  = 1, . . ., 5 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.

Case Study with Stationary and Moving Obstacles in
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 PSO basic .For  ∈ [0, 10], 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.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.

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.

1 𝐴Figure 6 :
Figure 6: Planned paths of  are in the condensed form.

1 Figure 7 :Figure 8 :
Figure 7: Planned paths of  are in the discontinuous form.

Table 1 :
Information of obstacles Obs  .