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

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 𝑥(𝑡)=(𝑥₁(𝑡),𝑥₂(𝑡),𝑥₃(𝑡)), 𝑡∈[0,𝑡_𝑓], 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 𝑥₀=𝑥(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 vector-valued 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.

Figure 1 

Mobile object 𝐴 and obstacles obs𝑘, 𝑘=1,2,…,𝑞.

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 𝑥(⋅)∈𝑋={𝑥(𝑡)|𝑥(𝑡)∈𝐶¹(0,𝑡_𝑓), 𝑎(𝑡)≤𝑥(𝑡)≤𝑏(𝑡),𝑐(𝑡)≤𝑥̇(𝑡)≤𝑑(𝑡),𝑒(𝑡)≤𝑥̈(𝑡)≤𝑓(𝑡),𝑥(0)=𝑥₀,𝑥(𝑡_𝑓)=𝑥_𝑓,𝑡∈[0,𝑡_𝑓]}, where 𝑎(𝑡), 𝑏(𝑡), 𝑐(𝑡), 𝑑(𝑡), 𝑒(𝑡), and 𝑓(𝑡) all belong to ℜ³, 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 𝑥₀ to 𝑥_𝑓. Then, criteria of dynamic and reachable are correspondingly given as below: 𝑥(⋅)∈𝑋. (1)

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

For the safety distance criterion, set 𝜑_𝑘(𝑥(𝑡))=𝑥(𝑡)−𝛼_𝑘(𝑡)₂−𝑟_𝐴+𝑟_𝑘, (2) where 𝜑_𝑘(𝑥(𝑡)), 𝑘=1,2,…,𝑞, is the distance between object 𝐴 (or the planned path 𝑥(⋅)) and obstacle 𝑘 at the moment 𝑡. Set 𝑑_𝑘 is a given safety distance between 𝐴 and obs_𝑘, which guarantees 𝐴 and obs_𝑘 are free of collision in the motion of 𝐴. Then, the distances between 𝐴 and obstacles obs_𝑘 are denoted as (𝑑₁,…,𝑑_𝑘,…,𝑑_𝑞), where 𝑘=1,2,…,𝑞. Then, the safety criterion is listed as below: 𝜑_𝑘(𝑥(𝑡))≥𝑑_𝑘. (3)

Length of 𝑥(⋅) can be expressed as follows: 𝐼₀𝑥𝑡_𝑓=^𝑡𝑓₀𝑥̇²₁(𝑡)+𝑥̇²₂(𝑡)+𝑥̇²₃(𝑡)𝑑𝑡=^𝑡𝑓₀||𝑥̇(𝑡)||₂𝑑𝑡. (4) Then, in the environment 𝑋, to achieve the minimum path length of 𝐴 named 𝐼₀, which is constrained by above criteria, for 𝑘=1,…,𝑞,𝑡∈[0,𝑡_𝑓], the following CVP is defined: min𝐼₀𝑥𝑡_𝑓=^𝑡𝑓₀||𝑥̇(𝑡)||₂𝑥(⋅)𝜑𝑑𝑡s.t.∈𝑋_𝑘(𝑥(𝑡))≥𝑑_𝑘. (5)(6)

Similar to the former work in [7, 13], for all 𝑡∈[0,𝑡_𝑓], we set 𝑝_𝑛(𝑡)=(𝑝_1𝑛(𝑡),𝑝_2𝑛(𝑡),𝑝_3𝑛(𝑡)), where 𝑝_𝑗𝑛(⋅),𝑗=1,2,3 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: min𝐼₀𝑝_𝑛𝑡_𝑓=^𝑡𝑓₀𝑝̇_𝑛(𝑡)₂𝑝𝑑𝑡s.t.^𝑛(⋅)𝜑∈𝑋_𝑘𝑝_𝑛(𝑡)≥𝑑_𝑘𝑘=1,…,𝑞,𝑡∈0,𝑡_𝑓. (7)(8) 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  𝜂=inf_𝑄𝐼₀(𝑥(𝑡_𝑓)) and 𝜂(𝑛)=inf_𝑄𝑛𝐼₀(𝑝_𝑛(𝑡_𝑓)), Then  𝜂=lim_𝑛→∞𝜂(𝑛).

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: min^𝑡𝑓₀𝑝̇_𝑛(𝑡)₂𝑝𝑑𝑡s.t.^𝑛(𝑡)≥𝑎(𝑡)𝑡∈0,𝑡_𝑓𝑝_𝑛(𝑡)(𝑡)𝑝̇≤𝑏𝑛=1,2,…_𝑛(𝑡)≥𝑐_𝑖(𝑡)𝑝̇_𝑛(𝑡)≤𝑑_𝑖(𝑡)𝑝̈_𝑛(𝑡)≥𝑒_𝑖(𝑡)𝑝̈_𝑛(𝑡)≤𝑓_𝑖(𝑡)𝑝_𝑛(0)=𝑥₀,𝑝_𝑛𝑡_𝑓=𝑥_𝑓𝜑_𝑘𝑝_𝑛(𝑡)≥𝑑_𝑘𝑘=1,…,𝑞. (9)(10)

Now, we partition the interval [0,𝑡_𝑓] into 𝑀 equal parts as ℎ=𝑡_𝑓/𝑀, set 𝐸(𝑡)=||𝑝̇_𝑛(𝑡)||₂. Thus, by using a numerical integration method such as trapezoidal rule, for 𝑘=1,…,𝑞, problems (9) and (10) are converted to the following problem: ℎmin2∑^𝑀_𝑚=0𝐸(𝑚ℎ)−𝐸(0)−𝐸𝑡_𝑓2𝑝s.t.^𝑛()()𝑝𝑚ℎ≥𝑎𝑚ℎ𝑚=0,1,…,𝑀._𝑛()()𝑝̇𝑚ℎ≤𝑏𝑚ℎ𝑛=1,2,…,_𝑛()()𝑝̇𝑚ℎ≥𝑐𝑚ℎ_𝑛()()𝑝̈𝑚ℎ≤𝑑𝑚ℎ_𝑛()()𝑝̈𝑚ℎ≥𝑒𝑚ℎ_𝑛()()𝑝𝑚ℎ≤𝑓𝑚ℎ_𝑛(0)=𝑥₀,𝑝_𝑛𝑡_𝑓=𝑥_𝑓𝜑_𝑘𝑝_𝑛(𝑚ℎ)≥𝑑_𝑘 (11)(12)

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: min𝐹_pena=ℎ2∑^𝑀−1_𝑚=1𝐸(𝑇)−𝐸(0)−𝐸𝑡_𝑓2+𝑐₁^𝑀_𝑚=0max0,𝑎𝑡^′−𝑝_𝑛𝑡^′+𝑐₂^𝑀_𝑚=0max0,𝑝_𝑛𝑡^′−𝑏𝑡^′+𝑐₃^𝑀_𝑚=0max0,𝑐𝑡^′−𝑝̇_𝑛𝑡^′+𝑐₄^𝑀_𝑚=0max0,𝑝̇_𝑛𝑡^′−𝑑𝑡^′+𝑐₅^𝑀_𝑚=0max0,𝑒𝑡^′−𝑝̈_𝑛𝑡^′+𝑐₆^𝑀_𝑚=0max0,𝑝̈_𝑛𝑡^′−𝑓𝑡^′+𝑐₇max0,𝑝_𝑛(0)−𝑥₀+𝑐₈max0,𝑝_𝑛𝑡_𝑓−𝑥_𝑓+𝑐₉^𝑞𝑀_𝑘=1𝑚=0max0,𝑑_𝑘−𝜑_𝑘𝑝_𝑛𝑡^′. (13)

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 𝑐_𝑖=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.

3.2. 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 𝑢_𝑖(𝑙)=(𝑢¹_𝑖(𝑙,𝑢²_𝑖(𝑙),…,𝑢^𝑑_𝑖(𝑙)), 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.

Figure 2 

The sketch of PSO implements.

When all the terms are stated, the standard PSO formula can be 𝑣^𝑗_𝑖(𝑙+1)=𝑤𝑣^𝑗_𝑖(𝑙)+𝑐_𝑝𝑟₁𝑝^𝑗_𝑖−𝑢^𝑗_𝑖(𝑙)+𝑐_𝑔𝑟₂𝑝^𝑗_𝑔−𝑢^𝑗_𝑖(𝑙),𝑢^𝑗_𝑖(𝑙+1)=𝑢^𝑗_𝑖(𝑙)+𝑣^𝑗_𝑖(𝑙+1)𝑗=1,2,…,𝑑. (14)

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 𝑟₁ and 𝑟₂ 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.

When it comes to our problem, the solution of 𝑥_𝑖 corresponds to 𝑝_𝑛(⋅), where 𝑝_𝑛(⋅)=(𝑝_1𝑛(⋅),𝑝_2𝑛(⋅),𝑝_3𝑛(⋅)). We set the coefficients of 𝑢_𝑖 correspond to (𝑝_1𝑛(⋅),𝑝_2𝑛(⋅),𝑝_3𝑛(⋅)). Thus, the dimension of 𝑢_𝑖 which denoted as 𝑑 equals to all three indexes summation of 𝑝_1𝑛(⋅), 𝑝_2𝑛(⋅), and 𝑝_3𝑛(⋅). For example, the highest sequential indexes of polynomials for 𝑝_1𝑛(⋅), 𝑝_2𝑛(⋅), and 𝑝_3𝑛(⋅) 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 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 𝐼₀, respectively.(1) Initialize 𝑢_𝑖 and 𝑣_𝑖 of all particles in the swarm. (2)𝑓_fitness(𝑢^𝑗_𝑖); 𝑢^𝑗_𝑖,𝑣^𝑗_𝑖→𝑝_𝑖.(3)if the particle 𝑖 with minimum 𝑓_fitness, then 𝑣_𝑖, 𝑢_𝑖→𝑝_𝑔.(4)end if(5)𝑙=1.(6)for each particle 𝑖 in swarm 𝐰𝐡𝐢𝐥𝐞𝑙≤𝑁𝐝𝐨(7) Update the 𝑣^𝑗_𝑖,𝑥^𝑗_𝑖 with (14).(8)if𝑓_fitness(𝑢_𝑖(𝑡))≤𝑝_𝑖, then 𝑝_𝑖=𝑓_fitness(𝑢_𝑖(𝑡)).(9)end if(10)if 𝑝_𝑖≤𝑝_𝑔, then 𝑝_𝑔=𝑝_𝑖.(11)end if(12)𝑙=𝑙+1;(13)end for each(14)𝐫𝐞𝐭𝐮𝐫𝐧𝑢_𝑖,𝑓_fitness(𝑢_𝑖).

With algorithm PSO_basic, the optimal planned path expressions 𝑝_𝑛(⋅) and length 𝐼₀ of mobile object 𝐴 can be achieved which corresponding to the value of 𝑢_𝑖 and 𝑓_fitness(𝑢_𝑖), 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 𝑡∈[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.

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: 𝑟=0.01,𝑥₀=(0,0,0), 𝑥_𝑓=(1,1,1), (0,0,0)≤(𝑥₁(𝑡),𝑥₂(𝑡),𝑥₃(𝑡))≤(1,1,1), (0,0.2,0.3)≤(𝑣₁(𝑡),𝑣₂(𝑡),𝑣₃(𝑡))≤(7.5,4,3.5), where 𝑡∈[0,1]. Table 1 gives the raw information data list of the obstacles.

Table 1 

Information of obstacles Obs_𝑘.

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.

Figure 3 

Planned path in the presence of obstacles.

Figure 4 

Velocity-time curve.

Figure 5 

Distance curve.

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 100∗100∗100 with static and moving obstacles are given as follows.(1)Parameters of mobile object 𝐴: radius 𝑟=1.5, initial points of center position 𝑥₀=(70,80,20), final points of center position 𝑥_𝑓=(20,35,85).(2)Boundary conditions: velocity boundary (−7,−5.5,0)≤(𝑣₁,𝑣₂,𝑣₃)≤(0,0,8). Time boundary 𝑇_max=10. Safety distance 𝑑₁=3, 𝑑₂=5, 𝑑₃=4.(3)Parameters of obstacles: radius 𝑟₁=5,𝑟₂=3,𝑟₃=4. Center position 𝛼₁=(30,50,40), 𝛼₂=(55,70,26), 𝛼₃₀=(42,20,51). Path of moving obstacle Obs₃ denoted by 𝛼₃(𝑡)=(𝛼₁₃(𝑡),𝛼₂₃(𝑡),𝛼₃₃(𝑡)),𝑡∈[0,10] is listed as follows: 𝛼₁₃(𝑡)=42+0.058𝑡−0.4011𝑡²+0.0175𝑡³,𝛼₂₃(𝑡)=20+5.0098𝑡−0.0236𝑡²−0.011𝑡³−0.00158𝑡⁴,𝛼₃₃(𝑡)=51−3.98𝑡+0.677𝑡²−0.130𝑡³+0.0070𝑡⁴. (15)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: 𝑥₁(𝑡)=70−5.26325𝑡+0.0263𝑡²,𝑥₂(𝑡)=80−3.726𝑡−0.2798𝑡²+0.0202𝑡³,𝑥₃(𝑡)=20+8.015𝑡−0.3326𝑡²+0.0557𝑡³−0.0038𝑡⁴. (16) 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.

Figure 6 

Planned paths of 𝐴 are in the condensed form.

Figure 7 

Planned paths of 𝐴 are in the discontinuous form.

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.

Figure 8 

PSO curve of fitness.

Velocities of 𝐴 and the distances between 𝐴 and obs_𝑘, 𝑘=1,2,3 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.

Figure 9 

Velocities-time curve of 𝐴 in 4D.

Figure 10 

Distance curve.

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