#### 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).