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

Authors: Yong Ma; M. Zamirian; Yadong Yang; Yanmin Xu; Jing Zhang
Journal: Mathematical Problems in Engineering
 (2013)

---

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


---

## Body

## 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 (x1,x2,x3,t) with obstacles, where (x1,x2,x3) denotes the three-dimensional space and t 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 pathx(·) 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. Section2 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 A with the center x(t)=(x1(t),x2(t),x3(t)), t∈[0,tf], moving from x0∈ℜ3 at time 0 to xf∈ℜ3 at time tf in the presence of obstacles. Where x(·) is a continuously unknown differentiable real vector-valued function, which is the path of A. tf is a given real number as final moving time of A, and x0=x(0), xf=x(tf). We suppose obstacle obsk is a rigid object with the center αk(t)=(α1k(t),α2k(t),α3k(t)),k=1,2,…,q, t∈[0,tk], where αk(·), k=1,2,…,q are known continuous real vector-valued functions, which are the paths of motion obstacles, and tk is a given real number as final moving time of obsk. Mobile objects and all obstacles are simplified as spheres. The radius of A and obstacles can be denoted as rA and rk, k=1,2,…,q, respectively. In Figure 1, the two smaller spheres are the start and final position of A, respectively; the other two larger spheres stand for obstacles.Figure 1
Mobile objectA and obstacles obsk, k=1,2,…,q.Planned pathx(·) of A should comply with the following constraints: the dynamic constraints of A, the reachable and the smoothness of x(·), and the safety distance between A and obstacles. Satisfied with all above constraints, the goal is to minimize the length of x(·).To satisfy dynamic constraints of the mobile objectA, the velocity and acceleration of A are restricted to certain rational regions. Thus, we suppose x(·)∈X={x(t)|x(t)∈C1(0,tf), a(t)≤x(t)≤b(t),c(t)≤x˙(t)≤d(t),e(t)≤x¨(t)≤f(t),x(0)=x0,x(tf)=xf,t∈[0,tf]}, where a(t), b(t), c(t), d(t), e(t), and f(t) all belong to ℜ3, are known as continuous real vector-valued functions as the boundaries of x(t),x˙(t), and x¨(t) for all t∈[0,tf], respectively.The reachable ofx(·) means mobile object A should follow the x(·) from x0 to xf. Then, criteria of dynamic and reachable are correspondingly given as below:
(1)x(·)∈X.The smoothness ofx(·) is satisfied inherently, that is, the planned path is introduced by a polynomial function belonging to C∞[0,tf] (the set of highly smooth functions).For the safety distance criterion, set(2)φk(x(t))=||x(t)-αk(t)||2-(rA+rk),
where φk(x(t)), k=1,2,…,q, is the distance between object A (or the planned path x(·)) and obstacle k at the moment t. Set dk is a given safety distance between A and obsk, which guarantees A and obsk are free of collision in the motion of A. Then, the distances between A and obstacles obsk are denoted as (d1,…,dk,…,dq), where k=1,2,…,q. Then, the safety criterion is listed as below:
(3)φk(x(t))≥dk.Length ofx(·) can be expressed as follows:
(4)I0(x(tf))=∫0tf‍x˙12(t)+x˙22(t)+x˙32(t)dt=∫0tf‍||x˙(t)||2dt.
Then, in the environment X, to achieve the minimum path length of A named I0, which is constrained by above criteria, for k=1,…,q,t∈[0,tf], the following CVP is defined:
(5)minI0(x(tf))=∫0tf‍||x˙(t)||2dt(6)s.t.{x(·)∈Xφk(x(t))≥dk.Similar to the former work in [7, 13], for all t∈[0,tf], we set pn(t)=(p1n(t),p2n(t),p3n(t)), where pjn(·),j=1,2,3 are polynomials of degree at most n with unknown constant coefficients. Then, with parametrization method, by substituting pn(·) for x(·) in the problems (5) and (6), the sequence of the TNLPP is obtained as follows:
(7)minI0(pn(tf))=∫0tf‍||p˙n(t)||2dt(8)s.t.{pn(·)∈Xφk(pn(t))≥dkk=1,…,q,t∈[0,tf].
Suppose Q is the set of x(·) such that the CVP (5) and (6) is feasible and Q(n) is the set of pn(·) such that the TNLPP (7) and (8) is feasible. Also, we suppose Q and Q(n) 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η=infQI0(x(tf)) and η(n)=infQnI0(pn(tf)), Then  η=limn→∞η(n).Proof.
See [13].When all constraints ofA are considered, such as bounded velocity and acceleration constraints, reachable of x(·), and safety criterion, TNLPP (7) and (8) is transformed as follows:
(9)min∫0tf‍||p˙n(t)||2dt(10)s.t.{pn(t)≥a(t)t∈[0,tf]pn(t)≤b(t)n=1,2,…p˙n(t)≥ci(t)p˙n(t)≤di(t)p¨n(t)≥ei(t)p¨n(t)≤fi(t)pn(0)=x0,pn(tf)=xfφk(pn(t))≥dkk=1,…,q.Now, we partition the interval[0,tf] into M equal parts as h=tf/M, set E(t)=||p˙n(t)||2. Thus, by using a numerical integration method such as trapezoidal rule, for k=1,…,q, problems (9) and (10) are converted to the following problem:
(11)minh[2∑m=0ME(mh)-E(0)-E(tf)]2(12)s.t.{pn(mh)≥a(mh)m=0,1,…,M.pn(mh)≤b(mh)n=1,2,…,p˙n(mh)≥c(mh)p˙n(mh)≤d(mh)p¨n(mh)≥e(mh)p¨n(mh)≤f(mh)pn(0)=x0,pn(tf)=xfφk(pn(mh))≥dkTheorem 2.
Solutions of the problems (11), (12), (9), and (10) are the same, if in problems (11) and (12) M 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, fort′=mh, expression of the UOP can be adopted as the fitness function of PSO as follows:
(13)minFpena=h[2∑m=1M-1E(T)-E(0)-E(tf)]2+c1∑m=0M‍max(0,a(t′)-pn(t′))+c2∑m=0M‍max(0,pn(t′)-b(t′))+c3∑m=0M‍max(0,c(t′)-p˙n(t′))+c4∑m=0M‍max(0,p˙n(t′)-d(t′))+c5∑m=0M‍max(0,e(t′)-p¨n(t′))+c6∑m=0M‍max(0,p¨n(t′)-f(t′))+c7max(0,||pn(0)-x0||)+c8max(0,||pn(tf)-xf||)+c9∑k=1q‍∑m=0M‍max(0,dk-φk(pn(t′))).In (13), c1,c2,…,c8 and c9 stand for large scalars to penalize the violation of the constraints. c1 and c2 are the lower and higher penalty coefficients of region boundary of the mobile object, respectively; c3 and c4 are the penalty coefficients of velocities boundary, respectively; c5 and c6 are the penalty coefficients of acceleration boundary, respectively; c7 and c8 are the penalty coefficients of reachable constraints, respectively; and c9 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 ci=100 (i=1,2,…,9). Then, the focus of the above minimization problem becomes how to determine the polynomials of Pn(t). 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 sizes, particle i(1≤i≤s) stands for one potential solution of the given problem in d-dimensional space. Each particle i has three vectors at the lth iteration, current position vector ui(l)=(ui1(l,ui2(l),…,uid(l)), velocity vector vi(l)=(vi1(l),vi2(l),…,vid(l)), and its own best position vector pi=(pi1,pi2,…,pid) which represents the best objective function value or fitness value. One global best position vector pg=(pg1,pg2,…,pgd) is defined as the position of the best particle among the s particles in the population. Figure 2 shows the iterative process of position vector ui(l) in PSO.Figure 2
The sketch of PSO implements.When all the terms are stated, the standard PSO formula can be(14)vij(l+1)=wvij(l)+cpr1(pij-uij(l))+cgr2(pgj-uij(l)),uij(l+1)=uij(l)+vij(l+1)j=1,2,…,d.In (14), the upper equation stands for the movement update of particle i in dimension j at the iteration number l+1, where w is a control factor controlling the magnitude of vij, cp and cg are positive acceleration coefficients, and r1 and r2 are uniform random numbers in [0,1]; the lower equation gives the corresponding position ui(l+1) update of particle i. Customarily, as in [26], we set the parameters to cp=cg=1, and w=0.8.When it comes to our problem, the solution ofxi corresponds to pn(·), where pn(·)=(p1n(·),p2n(·),p3n(·)). We set the coefficients of ui correspond to (p1n(·),p2n(·),p3n(·)). Thus, the dimension of ui which denoted as d equals to all three indexes summation of p1n(·), p2n(·), and p3n(·). For example, the highest sequential indexes of polynomials for p1n(·), p2n(·), and p3n(·) are 3, 4, and 5, respectively, then the value of d 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 ui can be treated as the polynomial coefficients of pn(·), and the corresponding value of fitness function should be the optimal path length I0.Basic flow of PSO algorithm namedPSObasic 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 PSObasic, ffitness and N represent fitness function minFpena and iteration number, respectively. Means of cp and cg,s,d, and w are the same as depictured above.Algorithm.
PS
O
basic
(
f
fitness
,
s
,
c
p
,
c
g
,
w
,
N
,
d
)
Input: fitness function ffitness, particle size s, acceleration coefficients cp, cg, inertia factor w, iteration number N, and spatial dimension d.
Output: the latest position ui and the optimal value of ffitness which represent the coefficients of pn(·) and I0, respectively.
(1)
Initializeui and vi of all particles in the swarm.
(2)
f
fitness
(
u
i
j
); uij,vij→pi.
(3)
if the particle i with minimum ffitness, then vi, ui→pg.
(4)
end if
(5)
l
=
1.
(6)
for each particle i in swarm whilel≤Ndo
(7)
Update thevij,xij with (14).
(8)
if
f
fitness
(
u
i
(
t
)
)
≤
p
i, then pi=ffitness(ui(t)).
(9)
end if
(10)
if
p
i
≤
p
g, then pg=pi.
(11)
end if
(12)
l
=
l
+
1;
(13)
end for each
(14)
r
e
t
u
r
n
u
i
,
f
fitness
(
u
i
).With algorithmPSObasic, the optimal planned path expressions pn(·) and length I0 of mobile object A can be achieved which corresponding to the value of ui and ffitness(ui), respectively.


## 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, fort′=mh, expression of the UOP can be adopted as the fitness function of PSO as follows:
(13)minFpena=h[2∑m=1M-1E(T)-E(0)-E(tf)]2+c1∑m=0M‍max(0,a(t′)-pn(t′))+c2∑m=0M‍max(0,pn(t′)-b(t′))+c3∑m=0M‍max(0,c(t′)-p˙n(t′))+c4∑m=0M‍max(0,p˙n(t′)-d(t′))+c5∑m=0M‍max(0,e(t′)-p¨n(t′))+c6∑m=0M‍max(0,p¨n(t′)-f(t′))+c7max(0,||pn(0)-x0||)+c8max(0,||pn(tf)-xf||)+c9∑k=1q‍∑m=0M‍max(0,dk-φk(pn(t′))).In (13), c1,c2,…,c8 and c9 stand for large scalars to penalize the violation of the constraints. c1 and c2 are the lower and higher penalty coefficients of region boundary of the mobile object, respectively; c3 and c4 are the penalty coefficients of velocities boundary, respectively; c5 and c6 are the penalty coefficients of acceleration boundary, respectively; c7 and c8 are the penalty coefficients of reachable constraints, respectively; and c9 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 ci=100 (i=1,2,…,9). Then, the focus of the above minimization problem becomes how to determine the polynomials of Pn(t). 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 sizes, particle i(1≤i≤s) stands for one potential solution of the given problem in d-dimensional space. Each particle i has three vectors at the lth iteration, current position vector ui(l)=(ui1(l,ui2(l),…,uid(l)), velocity vector vi(l)=(vi1(l),vi2(l),…,vid(l)), and its own best position vector pi=(pi1,pi2,…,pid) which represents the best objective function value or fitness value. One global best position vector pg=(pg1,pg2,…,pgd) is defined as the position of the best particle among the s particles in the population. Figure 2 shows the iterative process of position vector ui(l) in PSO.Figure 2
The sketch of PSO implements.When all the terms are stated, the standard PSO formula can be(14)vij(l+1)=wvij(l)+cpr1(pij-uij(l))+cgr2(pgj-uij(l)),uij(l+1)=uij(l)+vij(l+1)j=1,2,…,d.In (14), the upper equation stands for the movement update of particle i in dimension j at the iteration number l+1, where w is a control factor controlling the magnitude of vij, cp and cg are positive acceleration coefficients, and r1 and r2 are uniform random numbers in [0,1]; the lower equation gives the corresponding position ui(l+1) update of particle i. Customarily, as in [26], we set the parameters to cp=cg=1, and w=0.8.When it comes to our problem, the solution ofxi corresponds to pn(·), where pn(·)=(p1n(·),p2n(·),p3n(·)). We set the coefficients of ui correspond to (p1n(·),p2n(·),p3n(·)). Thus, the dimension of ui which denoted as d equals to all three indexes summation of p1n(·), p2n(·), and p3n(·). For example, the highest sequential indexes of polynomials for p1n(·), p2n(·), and p3n(·) are 3, 4, and 5, respectively, then the value of d 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 ui can be treated as the polynomial coefficients of pn(·), and the corresponding value of fitness function should be the optimal path length I0.Basic flow of PSO algorithm namedPSObasic 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 PSObasic, ffitness and N represent fitness function minFpena and iteration number, respectively. Means of cp and cg,s,d, and w are the same as depictured above.Algorithm.
PS
O
basic
(
f
fitness
,
s
,
c
p
,
c
g
,
w
,
N
,
d
)
Input: fitness function ffitness, particle size s, acceleration coefficients cp, cg, inertia factor w, iteration number N, and spatial dimension d.
Output: the latest position ui and the optimal value of ffitness which represent the coefficients of pn(·) and I0, respectively.
(1)
Initializeui and vi of all particles in the swarm.
(2)
f
fitness
(
u
i
j
); uij,vij→pi.
(3)
if the particle i with minimum ffitness, then vi, ui→pg.
(4)
end if
(5)
l
=
1.
(6)
for each particle i in swarm whilel≤Ndo
(7)
Update thevij,xij with (14).
(8)
if
f
fitness
(
u
i
(
t
)
)
≤
p
i, then pi=ffitness(ui(t)).
(9)
end if
(10)
if
p
i
≤
p
g, then pg=pi.
(11)
end if
(12)
l
=
l
+
1;
(13)
end for each
(14)
r
e
t
u
r
n
u
i
,
f
fitness
(
u
i
).With algorithmPSObasic, the optimal planned path expressions pn(·) and length I0 of mobile object A can be achieved which corresponding to the value of ui and ffitness(ui), 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 setv(t)=x˙(t), where v(t) is interpreted as control function which shows the speed of object A in the direction of x(t) at the moment t∈[0,tf]. 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 A in a space ℜ3, in the presence of five stationary obstacles with the following information: r=0.01,x0=(0,0,0), xf=(1,1,1), (0,0,0)≤(x1(t),x2(t),x3(t))≤(1,1,1), (0,0.2,0.3)≤(v1(t),v2(t),v3(t))≤(7.5,4,3.5), where t∈[0,1]. Table 1 gives the raw information data list of the obstacles.Table 1
Information of obstaclesObsk.
Obs
k
(
α
1
k
(
t
)
,
α
2
k
(
t
)
,
α
3
k
(
t
)
)
r
k
d
k
Obs1
(0.5, 0.5, 0.5)
1/6
0.1
Obs2
(0.3, 0.3, 0.1)
1/5
0.15
Obs3
(0.8, 0.2, 0.6)
1/5
0.11
Obs4
(0.2, 0.2, 0.8)
1/5
0.12
Obs5
(0.2, 0.8, 0.4)
1/5
0.2With 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 A and obsk, k=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 dimensions100*100*100 with static and moving obstacles are given as follows.(1)
Parameters of mobile objectA: radius r=1.5, initial points of center position x0=(70,80,20), final points of center position xf=(20,35,85).(2)
Boundary conditions: velocity boundary(-7,-5.5,0)≤(v1,v2,v3)≤(0,0,8). Time boundary Tmax=10. Safety distance d1=3, d2=5, d3=4.(3)
Parameters of obstacles: radiusr1=5,r2=3,r3=4. Center position α1=(30,50,40), α2=(55,70,26), α30=(42,20,51). Path of moving obstacle Obs3 denoted by α3(t)=(α13(t),α23(t),α33(t)),t∈[0,10] is listed as follows:
(15)α13(t)=42+0.058t-0.4011t2+0.0175t3,α23(t)=20+5.0098t-0.0236t2-0.011t3-0.00158t4,α33(t)=51-3.98t+0.677t2-0.130t3+0.0070t4.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 PSObasic. For t∈[0,10], paths of mobile object A are given as below:
(16)x1(t)=70-5.26325t+0.0263t2,x2(t)=80-3.726t-0.2798t2+0.0202t3,x3(t)=20+8.015t-0.3326t2+0.0557t3-0.0038t4.
From two different angles of view, the condensed and discontinuous planned paths of A 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 A 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 ofA are in the condensed form.Figure 7
Planned paths ofA are in the discontinuous form.The PSO curve of fitness is shown in Figure8, 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 ofA and the distances between A and obsk, k=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 ofA in 4D.Figure 10
Distance curve.


## 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 A in a space ℜ3, in the presence of five stationary obstacles with the following information: r=0.01,x0=(0,0,0), xf=(1,1,1), (0,0,0)≤(x1(t),x2(t),x3(t))≤(1,1,1), (0,0.2,0.3)≤(v1(t),v2(t),v3(t))≤(7.5,4,3.5), where t∈[0,1]. Table 1 gives the raw information data list of the obstacles.Table 1
Information of obstaclesObsk.
Obs
k
(
α
1
k
(
t
)
,
α
2
k
(
t
)
,
α
3
k
(
t
)
)
r
k
d
k
Obs1
(0.5, 0.5, 0.5)
1/6
0.1
Obs2
(0.3, 0.3, 0.1)
1/5
0.15
Obs3
(0.8, 0.2, 0.6)
1/5
0.11
Obs4
(0.2, 0.2, 0.8)
1/5
0.12
Obs5
(0.2, 0.8, 0.4)
1/5
0.2With 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 A and obsk, k=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 dimensions100*100*100 with static and moving obstacles are given as follows.(1)
Parameters of mobile objectA: radius r=1.5, initial points of center position x0=(70,80,20), final points of center position xf=(20,35,85).(2)
Boundary conditions: velocity boundary(-7,-5.5,0)≤(v1,v2,v3)≤(0,0,8). Time boundary Tmax=10. Safety distance d1=3, d2=5, d3=4.(3)
Parameters of obstacles: radiusr1=5,r2=3,r3=4. Center position α1=(30,50,40), α2=(55,70,26), α30=(42,20,51). Path of moving obstacle Obs3 denoted by α3(t)=(α13(t),α23(t),α33(t)),t∈[0,10] is listed as follows:
(15)α13(t)=42+0.058t-0.4011t2+0.0175t3,α23(t)=20+5.0098t-0.0236t2-0.011t3-0.00158t4,α33(t)=51-3.98t+0.677t2-0.130t3+0.0070t4.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 PSObasic. For t∈[0,10], paths of mobile object A are given as below:
(16)x1(t)=70-5.26325t+0.0263t2,x2(t)=80-3.726t-0.2798t2+0.0202t3,x3(t)=20+8.015t-0.3326t2+0.0557t3-0.0038t4.
From two different angles of view, the condensed and discontinuous planned paths of A 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 A 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 ofA are in the condensed form.Figure 7
Planned paths ofA are in the discontinuous form.The PSO curve of fitness is shown in Figure8, 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 ofA and the distances between A and obsk, k=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 ofA 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.



---

*Source: 613964-2013-02-14.xml*