Real-time path planning of unmanned aerial vehicle for target tracking and obstacle avoidance in complex dynamic environment

Aerospace Science and Technology 47 (2015) 269–279

Contents lists available at ScienceDirect

Aerospace Science and Technology www.elsevier.com/locate/aescte

Real-time path planning of unmanned aerial vehicle for target tracking and obstacle avoidance in complex dynamic environment Peng Yao a,b , Honglun Wang a,b,∗ , Zikang Su a,b a b

School of Automation Science and Electrical Engineering, Beihang University, Beijing 100191, China Science and Technology on Aircraft Control Laboratory, Beihang University, Beijing 100191, China

a r t i c l e

i n f o

Article history: Received 30 May 2015 Received in revised form 18 September 2015 Accepted 30 September 2015 Available online 8 October 2015 Keywords: Unmanned aerial vehicle (UAV) Three-dimensional (3D) real-time path planning Lyapunov Guidance Vector Field (LGVF) Interfered Fluid Dynamical System (IFDS) Varying receding-horizon optimization

a b s t r a c t According to the tactical requirements of unmanned aerial vehicle (UAV) for tracking target and avoiding obstacle in complex dynamic environment, a three-dimensional (3D) real-time path planning method is proposed by combing the improved Lyapunov Guidance Vector Field (LGVF), the Interfered Fluid Dynamical System (IFDS) and the strategy of varying receding-horizon optimization from Model Predictive Control (MPC). First, in order to track the moving target in 3D environment, the LGVF method is improved by introducing flight height into the traditional Lyapunov function, and the generated velocity can guide UAV converge gradually to the limit cycle in horizontal plane and the optimal height in vertical plane. Then, the IFDS method imitating the phenomenon of fluid flow is utilized to plan the collisionfree path. To achieve the mission of tracking moving target and avoid static or dynamic obstacle at the same time, the guidance vector field by LGVF is taken as the original fluid of IFDS. As the fluid system still remains stable under the influence of obstacles, the disturbed streamline from the interfered fluid can be regarded as the planned path. Third, as the quality of route is mainly influenced by the repulsive and tangential parameters of IFDS, the real-time suboptimal route can be planned by the varying receding-horizon optimization according to the predicted motion. The experimental results prove that the proposed hybrid method is applicable to various dynamic environments. © 2015 Elsevier Masson SAS. All rights reserved.

1. Introduction Path planning plays an important role in enhancing the ability of autonomous flight of unmanned aerial vehicle (UAV). By finding a global optimal route offline, the traditional two-dimensional (2D) path planning is popular in static or known environment [1–3]. But the actual flight environment of UAV is usually dynamic and unknown, where a feasible path should be planned online by dealing with various dynamic situations. Besides, compared to a 2D path, a three-dimensional (3D) route is more applicable to low-altitude and terrain-following flight. In addition, UAV needs to track static or moving target and avoid static or moving obstacles at the same time in many cases, but most methods only consider one above-mentioned tactical mission or assume the environment to be simple [4–8]. Therefore, the challenging real-time path planning technology in 3D complex environment is studied in this paper.

*

Corresponding author at: School of Automation Science and Electrical Engineering, Beihang University, Beijing 100191, China. Tel.: +86 10 82317546. E-mail address: [email protected] (H. Wang). http://dx.doi.org/10.1016/j.ast.2015.09.037 1270-9638/© 2015 Elsevier Masson SAS. All rights reserved.

Many studies have been carried out by researchers aiming at the problem of target tracking. The theory of dynamic programming is utilized in Ref. [7], where the distance between UAV and target keeps constant by minimizing the position error covariance while ignoring the relative angle. In Ref. [8], partially observable Markov decision process (POMDP) is employed to guide UAV trace the moving target. Lee et al. [9] propose a method based on the backstepping theory, where the path globally approximates to the target. Lawrence et al. [10] construct the Lyapunov guidance vector field (LGVF) centering at the target, where UAV will converge to the limit cycle over the moving target. Chen et al. [11] present the tangent guidance vector field (TGVF) to find the shortest path when the distance between UAV and target is bigger than the expected distance. But it is invalid when UAV is inside the expected cycle. Hence a hybrid method is developed in Ref. [12], where TGVF and LGVF are separately utilized when UAV is outside or inside the expected cycle. However, the above algorithms are mainly effective for 2D planning. According to the problem of obstacle avoidance, the current algorithms can be summarized to the following five kinds: heuristic search based method [1], e.g. A∗ algorithm and its improved

270

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

methods (D∗ and D∗ Lite methods); optimization algorithm [2,3,6, 13], e.g. mixed integer linear programming (MILP), intelligent algorithms (e.g. Particle Swarm Optimization, Gravitational Search Algorithm, Genetic Algorithm, etc.); probabilistic programming [14], e.g. Rapidly-exploring Random Tree (RRT) and Probabilistic Roadmaps (PRM); potential field based method [15–17], e.g. Artificial Potential Field (APF), Virtual Force (VF) and the method based on stream function; Roadmap based method [18], e.g. Voronoi Map. In order to avoid the moving threats, APF is improved by introducing the relative velocity into the potential-energy function [19]. Model Predictive Control (MPC) is utilized in Refs. [20,21] to plan a suboptimal path in real time. These methods may be efficient in planning a real-time 2D path, but their computational efficiency will decrease substantially when utilized in 3D environment. Besides, the quality of path e.g. smoothness is unsatisfactory. Therefore, a novel path planning method based on Interfered Fluid Dynamical System (IFDS) is proposed in our previous work to plan a collision-free path [22,23]. This method will not change the stability of the original system, i.e. the planned path can avoid various obstacles or threats and reach the target eventually. However, the target tracking mission as well as the real-time optimization of the route is not taken into consideration. The actual flight environment of UAV is complicated, where UAV should avoid static obstacles or moving threats and track moving target at the same time. Compared to standard path planning in simple environment, this technology is much more complicated. In Refs. [12,24], TGVF and LGVF are combined for target tracking, and only some simple rules are then implemented for avoiding circle obstacles in 2D environment. In Ref. [25], the POMDP method is utilized where the features of target tracking and obstacle avoidance can be incorporated into this POMDP framework by plugging in their appropriate models. And the state space, the action space, the conversion strategy and the objective function can all be formulized, but it is only suitable for 2D environment. Under the constraints of terrain occlusions and airspace limitations, Shaferman et al. [28] utilized the genetic method to optimize the target-tracking route in urban environment, while the future motion of target is ignored. On the basis of above analysis, a single method cannot usually meet the requirements of target tracking and obstacle avoidance simultaneously. And the computation efficiency and the route quality of most methods are still unsatisfactory in 3D environment. Therefore, a three-dimensional dynamic path planning method is proposed in this paper by combing LGVF and IFDS, and the strategy of varying receding-horizon optimization from MPC is also utilized to obtain the suboptimal path in real time. First, on the basis of the standard LGVF, the flight height is introduced into the Lyapunov function which proves to be stable. Taking into account UAV dynamic constrains, the velocity of the guidance vector field will guide UAV to track static and moving target in 3D environment. Second, the guidance vector field by LGVF is taken as the original fluid of IFDS, which imitates the phenomenon of fluid flow. And the disturbed velocity can guide UAV avoid obstacles and track target at the same time. Third, inspired by the MPC theory, the suboptimal route is obtained by adjusting the repulsive and tangential parameters of IFDS in the varying receding horizon. In the objective function, the sub-objective functions which respectively describe performances of target tracking and obstacle avoidance are incorporated. The rest of this paper is organized as follows. Section 2 models the problem of path planning. The improved LGVF is then described in Section 3. In Section 4, the IFDS method is introduced. The varying receding-horizon optimization strategy is illustrated in Section 5. In Section 6, the experimental results are analyzed. Finally Section 7 draws the conclusion of this paper.

Fig. 1. The process of EKF.

2. Modeling of path planning 2.1. Problem description In this paper, the mission of target tracking and obstacle avoidance are considered simultaneously for the dynamic path planning problem, which can be regarded as the real-time optimization problem under constraints. The future motion of threat or target should be predicted according to the real-time detection information by UAV. Then the constraints of path planning are confirmed on the basis of UAV maneuvering characteristics and environmental information. Finally the route satisfying the certain optimization index under constraints is generated quickly. In the planning space C , any point can be expressed as P = (x, y , z). The initial position of target is P t0 = (xt0 , y t0 , zt0 ), the final point of target P tf = (xtf , y tf , ztf ), the initial state i.e. the start point of UAV P 0 = (x0 , y 0 , z0 ), and the final state P f = (xf , y f , zf ). The process of path planning is finding the flyable waypoints and connecting them to form the route. 2.2. State prediction by Extended Kalman Filter (EKF) We assume that the acceleration speed of target or threat is variable, so the traditional Kalman Filter does not apply to state prediction. In this paper, EKF [26] is utilized to predict the future motion of target or threat. The position and velocity of target or threat are taken as the motion state i.e. X k = [xk , yk , zk , v xk , v yk , v zk ]T ; the position is taken as the corresponding observed value i.e. Z k = [xk , yk , zk ]T . Hence the state equation and observation equation of EKF are expressed as follows:



X k +1 = f ( X k ) + w k Z k +1 = h ( X k ) + v k

(1)

where f and h are the state equation and observation equation of moving threat or target; w k and v k are the zero-mean Gaussian process noise and observation noise i.e. p ( w ) ∼ N (0, Q ) and p ( v ) ∼ N (0, R ). Q and R are the corresponding covariance matrixes. Fig. 1 describes the process of EFK. The state matrix F k and observation matrix H k are the Jacobian matrix calculated by the partial derivative of f and h respectively:

⎡ ⎢

∂ f1 ∂ xk

···

. Fk = ⎢ ⎣ ..

∂ f6 ∂ xk

···

∂ f1 ∂ v zk

⎤

.. ⎥ ⎥ . ⎦,

∂ f6 ∂ v zk

⎡ ⎢

∂ h1 ∂ xk

···

∂ h1 ∂ v zk

···

∂ h3 ∂ v zk

. Hk = ⎢ ⎣ ..

∂ h3 ∂ xk

⎤

.. ⎥ ⎥ . ⎦

(2)

2.3. UAV performance constraints UAV performance should be modeled as the constraints of path planning to ensure the feasibility of route. In this paper, UAV is assumed to fly at the constant cruising speed V 0 . The minimum and

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

maximum flight path angle are θmin and θmax ; the minimum and maximum altitude are hmin and hmax ; the maximum UAV turn rate is ωmax (ωmax ∈ R + ); the sampling time is  T . For any waypoint P k = (xk , yk , zk ) ∀k = 1, 2, . . . , K , the heading path angle of UAV is expressed as ψk and the flight–path angle θk . To satisfy UAV dynamic constraints, the following conditions should be satisfied:



|ψk+1 − ψk | ≤ ωmax ·  T θmin ≤ θk ≤ θmax hmin ≤ zk ≤ hmax

(3)

Besides, UAV reconnaissance capability should be considered. In this paper, UAV is assumed to detect the underneath circle area with radius R r . Hence the detecting area can be defined as follows:



SP = P 

(x − xk )2 + ( y − yk )2 ≤ R r

(4)

2.4. Environmental constraints In this paper, the target is assumed to be a mass point, the velocity of which satisfies |u t | < V 0 . There are mainly two modes for the mission of tracking ground target: standoff tracking, which means that UAV should be above the target with an optimal height, and the horizontal distance between UAV and target remains constant; persistent tracking, i.e. maintaining the target within the sensor coverage range of UAV. By the former method, the target can be in the central field of view of airborne camera eventually and the reconnaissance efficiency of UAV is higher. Therefore, the standoff tracking is utilized in this paper. For the problem of obstacle avoidance, both static obstacles e.g. terrain and dynamic threats e.g. moving radar or firepower exist in the planning space. The velocity of moving threat needs to meet |u o | < V 0 with the appropriate accelerated speed. The obstacles or threats can be equivalent to the standard convex polyhedrons e.g. cone, cylinder, sphere or cuboid:

 Γ (P ) =

x − x0

2p

 +

a

y − y0

2q

 +

b

z − z0

2r (5)

c

where a, b, c and p , q, r determine the size and shape of obstacle respectively, and (x0 , y 0 , z0 ) is the center of obstacle. Γ ( P ) = 1 is the surface equation; Γ ( P ) > 1 refers to the area outside the obstacle; Γ ( P ) < 1 is the region inside the obstacle. Suppose there are W obstacles or threats in the planning space. The planned route should avoid all the obstacles or threats, hence any planned waypoint P k should be within the free region C free i.e. P k ∈ C free . The free region is defined as:



C free = P  Γ w ( P ) > 1, ∀ w = 1, . . . , W

(6)

Besides, when UAV is too close to any obstacle, UAV may crash into it because of the limit of UAV maneuverability. Hence the dilatation coefficients λa , λb , λc greater than 1 can be introduced into the obstacle equation:

 Γ (P ) =

x − x0

λa · a

2p

 +

y − y0

λb · b

2q

 +

z − z0

λc · c

2r (7)

3. Target tracking based on improved LGVF LGVF is proposed in Ref. [10] for the problem of standoff target tracking in 2D environment. It is assumed that the target moves on the ground and UAV flies at the constant height. The desired velocity of UAV is determined by the Lyapunov function, which ensures the distance between UAV and target to be a constant R, i.e. UAV converges to a limit cycle with radius R above the target eventually.

271

However, sometimes the height of UAV is varying in 3D environment, especially when UAV with the terrain-following property needs to avoid obstacles at the same time. Hence the standard LGVF should be improved to apply to 3D environment: UAV will gradually converge to not only the limit cycle with radius R in horizontal plane, but also the optimal height H above the target in vertical plane. 3.1. Tracking the static target Suppose UAV tracks the static ground target P t = (xt , y t , zt ) with the constant cruising speed V 0 . By introducing the flight height, the Lyapunov function can be redefined as follows:

2 1  2 2 1 2 r − R2 + h − H2 (8) 2 2   where r = x2r + y 2r = (x − xt )2 + ( y − y t )2 is the horizontal distance between UAV and target, and h = zr = z − zt the vertical V (P ) =

distance. Based on Eq. (8), the guidance vector field is obtained, where the velocity u ( P ) is defined as follows:

⎡

⎤

ux V0  u( P ) = ⎣ u y ⎦ = 2 + R 2 )2 + λ2 (h 2 − H 2 )2 r · ( r uz

⎤ −xr · (r 2 − R 2 ) − y r · (2r R ) 2 2 × ⎣ − y r · (r − R ) + xr · (2r R ) ⎦ −λ · r · (h2 − H 2 ) ⎡

(9)

where λ is a positive constant determining the convergence rate in vertical plane. Then the velocity u ( P ) is considered as the desired UAV velocity, the magnitude of which is still equal to the cruising speed i.e. |u ( P )| = V 0 . The derivative of V ( P ) with respect to time t is:





⎡

⎤

α V α V α V ⎣ ux ⎦ · uy = , , dt αx α y α z u

dV

z

=

−2r V 0 · (r − R ) − 2λhV 0 · (h2 − H 2 )2  (r 2 + R 2 )2 + λ2 (h2 − H 2 )2 2

2 2

(10)

UAV is above the target i.e. h > 0, so the inequation dV /dt ≤ 0 holds. When the conditions r = R and h = H hold, we can infer dV /dt = 0. It should be noticed that when r = 0 and h = H , the equation dV /dt = 0 holds too. But this position should be eliminated as u ( P ) is nonsense in this situation. Therefore, the former situation (r = R, h = H ) is the only minimum of LGVF. Based on the Lasalle invariance principle, UAV will gradually converge to the limit cycle in horizontal plane and the optimal height in vertical plane, i.e. r → R, h → H . The desired path is then obtained by the successive integration of u ( P ). Fig. 2 and Fig. 3 illustrate the desired UAV paths from a series of start points (x0 ∈ [−6, 6], y 0 ∈ [−6, 6], z0 ∈ {1.0, 1.5}), by the improved and traditional LGVF respectively. It is assumed that the target point is P t = (0, 0, 0) km, the radius of limit cycle R = 3 km and the optimal height H = 0.5 km. Wherever UAV starts, by the improved LGVF, the route will always guide UAV gradually converge to the limit cycle in horizontal plane and the optimal height in vertical plane. However, by the traditional LGVF, although UAV will converge to the limit cycle in horizontal plane, it only flies at the initial height in the vertical plane, which is apparently unreasonable.

272

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

Fig. 2. Paths by improved LGVF.

Fig. 3. Paths by traditional LGVF.

Next we need to analyze the feasibility of path, mainly considering the turn rate and flight–path angle. The heading angle of any planned waypoint is:

    uy −xr · (r 2 − R 2 ) − y r · (2r R ) = arctan ψ = arctan ux − y r · (r 2 − R 2 ) + xr · (2r R )

(11)

Hence we can calculate the turn rate:

ψ˙ = 4V 0 · 

r2 + R 2

Rr 2

· (r 2 + R 2 )2 + λ2 (h2 − H 2 )2 (r 2 + R 2 )2

(12)

When UAV is on the limit cycle with the optimal height i.e. h = H and r = R, we can obtain the maximum value i.e. ψ˙ max = V 0 / R. To ensure the feasibility of planned path, the condition V 0 / R ≤ ωmax i.e. R ≥ V 0 /ωmax should be fulfilled. The flight–path angle is:

 θ = arcsin

uz



V0

 = arcsin 



−λ · (h2 − H 2 ) (r 2 + R 2 )2 + λ2 · (h2 − H 2 )2

(13)

When h = hmax and r = R, we obtain the minimum value



i.e. θ1 = arcsin

−λ·(h2max − H 2 )

R 4 +λ2 ·(h2max − H 2 )2



. When h = hmin and r = R,



the maximum value i.e. θ2 = arcsin

−λ·(h2min − H 2 ) R 4 +λ2 ·(h2min − H 2 )2



is calcu-

lated. The condition θmin ≤ θ1 < θ < θ2 ≤ θmax should be satisfied to ensure the feasibility of path. Hence we can infer λ ≤ min

− tan θmin · R 2 (h2max − H 2 )

θmax · R , − tan 2 2 (hmin − H )

2

.

To ensure the path to be feasible for UAV with a stable controller, the appropriate R and λ should be chosen from their respective value range.

3.2. Tracking the moving target Suppose the velocity of moving target is u t = [utx , ut y , utz ]T . By revising the desired UAV velocity from Eq. (9), we obtain the relative Lyapunov guidance vector field:

⎡

⎤

⎡

⎤

ux utx u ( P ) = α ⎣ u y ⎦ + ⎣ ut y ⎦ uz utz

(14)

where α is called the velocity correction coefficient, and the velocity of UAV relative to target is α [u x , u y , u z ]T . The derivative of V ( P ) with respect to time t is:

dV dt

=

−2α rC · (r 2 − R 2 )2 − 2α λhC · (h2 − H 2 )2  (r 2 + R 2 )2 + λ2 (h2 − H 2 )2

(15)

The desired velocity by Eq. (14) should remain the constant V 0 , so the following condition holds:



u 2x + u 2y + u 2z

2

· α 2 + 2(u x utx + u y ut y + u z utz ) · α   2 2 2 + utx + ut2y + utz − V 02 = 0

(16)

As the velocity of target is less than that of UAV, there must be a positive solution of Eq. (16). Hence dV /dt ≤ 0 holds from Eq. (15). From the analysis in Section 3.1, UAV can still converge to the horizontal limit cycle and the optimal height in the relative coordinate system, but the relative velocity is α times as big as that from Eq. (9). Fig. 4 illustrates the process of tracking a moving target in the ground coordinate system and in the relative coordinate system respectively. It is assumed that the target moves from the

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

273

Fig. 4. Tracking of a moving target by UAV.

Fig. 5. The illustration of the fluid from IFDS.

initial point P t = (0, 0, 0) km by the sinusoidal motion in the horizontal plane, and the initial point of UAV is P 0 = (6, 6, 2) km, the radius of limit cycle R = 3 km and the optimal height H = 0.5 km. By introducing the target velocity, UAV can track the moving target successfully. Next we analyze the feasibility of path. The desired heading α u +u angle is Ψ = arctan( α uy +u t y ), and the turn rate proves to meet x

tx

Ψ˙ ≤ (α 2 + α )ψ˙ , where ψ˙ is the turn rate by Eq. (12). Hence we can V ·(α 2 +α ) infer R ≥ 0 ω . The flight path angle is θ = arcsin( α u zV+utz ), max 0 and λ shall satisfy the following condition through derivation: λ ≤

2 2 χ min − min · 2 R 2 , − χmax · 2 R 2 , where χmin = 2 α 2 −χmin

(hmax − H )

2 α 2 −χmax

(hmin − H )

sin θmin − V zt / V 0 and χmax = sin θmax − V zt / V 0 . As long as the value of R and λ are reasonable referring to the above conditions, the planned path will be feasible. 4. Obstacle avoidance by IFDS 4.1. The description of IFDS In our previous work [22,23], the Interfered Fluid Dynamic System (IFDS) is proposed to solve the problem of obstacle avoidance, which imitates and formulates the phenomenon of fluid flow by extracting and broadening the hydromechanical properties. Compared to the traditional methods of obstacle avoidance, the biggest advantage of IFDS is its high computation efficiency. The basic procedures of IFDS are as follows. First, the initial fluid field is determined, of which the streamlines can be taken as UAV paths in the absence of obstacles. Second, when there are static obstacles in planning space, the influence of obstacles on the initial fluid is expressed by the modulation matrix. Hence the dis-

turbed fluid is obtained, of which the streamlines i.e. the paths will guide UAV avoid obstacles and reach destination. Besides, when dynamic threats exist in the planning space, the relative initial fluid and the relative interfered fluid are determined by introducing the motion information of threat. In the relative fluid field, the planned path will be calculated by the previously-mentioned static method. In our previous work [22,23], the mission of target tracking is not considered, i.e. UAV only needs to reach the target with avoiding obstacles. Hence the sink flow is taken as initial fluid of IFDS, the velocity of which is defined as follows:

u( P ) = −



V 0 (x−xt ) d( P )

V 0 ( y− yt ) d( P )

V 0 ( z − zt ) d( P )

T

(17)

where V 0 is UAV cruising  speed, d( P ) the distance between UAV and target d( P ) = (x − xt )2 + ( y − y t )2 + ( z − zt )2 . All the streamlines in the original fluid are the straight lines pointing at the target ([5, 5, 0]), as shown in Fig. 5(a). Suppose there are some static obstacles in planning space, then the interfered fluid is obtained by the above procedures, where all the streamlines can avoid obstacles and reach target, as shown in Fig. 5(b). 4.2. LGVF+IFDS method In this paper, the problem of target tracking and obstacle avoidance are considered simultaneously. The guidance vector field by LGVF has the fluid characteristics, so it can replace the sink fluid as the original fluid of IFDS, shown in Fig. 6(a). That is to say, the velocity u ( P ) by Eq. (14) can be taken as the initial fluid velocity. What’s more, the IFDS method will not change the stability of fluid system, so the streamlines in the modified fluid by LGVF+IFDS will

274

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

Fig. 6. The illustration of the fluid from LGVF+IFDS. Table 1 Pseudo code of LGVF+IFDS. Path planning algorithm based on LGVF+IFDS 1. while (! reach target) 2. Update the motion information of moving target 3. Calculate the initial fluid velocity u ( P ) by Eq. (14) of LGVF 4. Update the information of dynamic threat: model Γ w ( P ), velocity v w ( P ), w ∈ 1... W 5. for each obstacle, calculate ⎧ W =1 ⎨1

W  (Γi ( P )−1) (Γi ( P )−1)+(Γ w ( P )−1) W = 1 i =1,i = w  δΓ w ( P ) δΓ w ( P ) δΓ w ( P ) T normal vector n w ( P ) = , δ y , δz  δΓδwx( P ) T δΓ w ( P ) tangent vector t w ( P ) = δ y , − δx , 0 ω w ( P )n w ( P )n w ( P )T τ ω w ( P )t w ( P )n w ( P )T modulation matrix M w ( P ) = I − + 1 1 |Γ w ( P )| σ w t w ( P )

n w ( P )

|Γ w ( P )| ρ w n w ( P )T n w ( P )

6.

the weighting coefficient

7.

the

8.

the

9.

the

ωw ( P ) =

⎩

10.

end for

11.

¯ (P ) = Calculate the overall modulation matrix M

12.

Compute the overall velocity of threats v ( P ) =

W  w =1

W 

w =1

M w (P )

ω w ( P ) exp( −Γλww( P ) ) v w ( P )

13. Calculate the correction factor β and the disturbed fluid velocity u¯ ( P ) by Eq. (19) 14. Obtain the next waypoint P k+1 = P k + u¯ ( P ) ·  T , where  T is the time step 15. end while

track target and avoid obstacles well at the same time, shown in Fig. 6(b). The streamline can be taken as the planned path. By this way, two methods are combined skillfully. Based on the computing method in [22,23], the modulation ma¯ ( P ) and the threat kinematic velocity v ( P ) are confirmed. trix M Then we will calculate the interfered fluid velocity i.e. UAV velocity:





¯ ( P ) u( P ) − v ( P ) + v ( P ) u¯ ( P ) = M

(18)

In this paper, UAV is assumed to fly at the constant velocity V 0 , so the velocity correction coefficient β is introduced into the interfered fluid velocity:





¯ ( P ) u( P ) − v ( P ) + v ( P ) u¯ ( P ) = β M

(19)

β can be confirmed by solving the equation |u¯ ( P )| = ¯ ( P )(u ( P ) − v ( P )) + v ( P )| = V 0 . As the condition | v ( P )| < V 0 |β M meets, there must be a real solution. Based on the above analysis, the pseudo code of LGVF+IFDS method is shown in Table 1. The streamlines i.e. paths by LGVF+IFDS have the following features: (a) UAV can still track the target i.e. u¯ ( P ) · u ( P ) ≥ 0. (b) UAV can avoid all the obstacles or threats. (c) No local minimum exists outside the obstacle i.e. u¯ ( P ) = 0.

The velocity u ( P ) of the initial streamline, calculated by LGVF, has proved to guide UAV track target well. Therefore, to ensure the disturbed streamline by LGVF+IFDS can still track target, the disturbed velocity u¯ ( P ) should meet the inequation u¯ ( P ) · u ( P ) ≥ 0. And the remaining proof (a) can be seen in Refs. [22,23], so is the proof (b) and (c). Reactive parameters ρ w and σ w can be called the repulsive and tangential parameter of the w-th obstacle respectively. It is important to note that, the bigger ρ w or σ w is, the more drastically and earlier UAV avoids the obstacle. 5. Varying receding-horizon optimization strategy 5.1. The description of rolling optimization The above-mentioned method only considers the current environment information and plans one-step route without considering the future motion of target or threat, so some unfeasible situations e.g. large angle changes may exist in the planned path. Besides, as the selection of reactive parameters is aimless or blind, the optimality of path cannot be ensured. To resolve the problem, the rolling optimization strategy from MPC is utilized to adjust the path by LGVF+IFDS. Then reliable local path in the future will be confirmed ahead by the real-time optimization, which will avoid unfeasible situations effectively.

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

275

Fig. 8. Illustration of threat judgment.

when UAV falls into the obstacle, A w (k) is the positive infinity; when UAV is far away from the obstacle i.e. L at > 2 km, A w (k) can be zero. Then A (k) is defined as:

Fig. 7. Illustration of rolling optimization strategy.

The illustration of this method is shown in Fig. 7. Suppose that a N-step path { P k , · · · , P k+ N } is planned ahead when UAV arrives at P k at the time tk . As the prediction accuracy will decrease when the prediction step increases, UAV will only fly along P k P k+1 , i.e. only one step of path is executed. In the process of executing the path P k P k+1 , a N-step path { P k+1 , · · · , P k+ N +1 } with the start point P k+1 is planned: first, the future motion of threat or target is predicted by EKF according to the detected information; then the local path is calculated by LGVF+IFDS; then the repulsive and tangential parameters should be adjusted to find the optimal local path under various constraints. To ensure the path to be planned in real time, the time of local path planning (Tp) must be less than that of executing the path P k P k+1 (Tf, i.e.  T ). When UAV arrives at P k+1 at the time tk+1 , the first step of the newly planned route is then executed. Repeat these above steps until the mission is completed. 5.2. The optimizing index of path

(20)

where λ1 , λ2 , λ3 are the weighting factors of these three indexes respectively satisfying λ1 + λ2 + λ3 = 1. The initial velocity u ( P ) by LGVF guides UAV track the moving target under a certain optimal index. Therefore the deviation of the disturbed path from the initial one can be treated as the index of target tracking:

T (k) =

k+ N −1 



u ( P i ), u¯ ( P i )

(21)

i =k

where u ( P i ), u¯ ( P i ) is the angle between u ( P i ) and u¯ ( P i ). The bigger the angle is, the poorer traceability the path has. The threaten degree of the w-th obstacle to the k-th waypoint P k can be defined as follows:



A w (k) =

0 1 L at

+∞

L at ≥ 2 0 < L at < 2 L at ≤ 0

(22)

where L at is the distance between UAV and obstacle surface. When UAV is in the influence region i.e. 0 < L at < 2 km, the threaten degree is 1/ L at according to the UAV maneuvering characteristics;

W k +N 

A w (i )

(23)

w =1 i =k

Besides, S (k) should be introduced to evaluate the smoothness of path:

S (k) =

1 N

k+ N −1 





μ1 · |ψi+1 − ψi | + μ2 · |θi+1 − θi |

(24)

i =k

where ψi and θi are the heading angle and flight–path angle, and μ1 and μ2 are the corresponding weight coefficients. It is noticed that the ranges of indexes T (k), A (k) and S (k) are different, so they should be normalized before the calculation of J (k). To obtain the optimal N-step path, the reactive parameters of all the obstacles or threats (especially those nearer to UAV) are adjusted to obtain the combination of parameters:

U=

The evaluation function J (k) can be utilized to judge the superiority and inferiority of local path. Usually the smaller J (k) is, the higher the route quality is. Although the initial path by LGVF is under UAV dynamic constraints, it is not guaranteed that the disturbed path by LGVF+IFDS is flyable. Therefore, the feasibility of route should be judged: if the path is beyond any constrained boundary defined in Section 2.3, the route is unfeasible and the value of J (k) will be a positive infinity. Otherwise, J (k) includes the index of target tracking T (k), obstacle avoidance A (k) and path smoothness S (k):

J (k) = λ1 T (k) + λ2 A (k) + λ3 S (k)

A (k) =



  = arg min J (k) ρ1 , σ1 · · · ρW , σW

ρ ,σ

(25)

5.3. Adaptive adjustments to time domain The steps of local planning, i.e. the time domain of optimization, is the crux of the proposed method. If N is too small, the path is of poor optimization as the future motion is not fully considered. When N is too big, the calculation time will increase violently. Therefore some adaptive adjustments to N are utilized in this paper. The time domain can be adapted according to the intensity of obstacles. When there are fewer obstacles or threats in the detecting area S P , UAV can easily avoid them. Hence we can reduce N appropriately to avoid the unnecessary computation of the future path. When there are numerous obstacles or threats in the detecting area, it is challenging to plan a safe route. As a result, N should increase to consider the multi-step motional tendency of threats or target in the future. Besides, the time domain can be adapted by judging whether UAV encounters the moving threats. As shown in Fig. 7, when the moving threat approaches to UAV i.e. v o · V OA > 0, N should increase, as the motion of threat has a considerable effect on the planned result. When the threat moves away from UAV i.e. v o · V OA < 0, N could decrease, as the influence of threat is negligible. In Fig. 8, P k is the current position of UAV, O tk the current position of threat, O t k+1 and O t k+2 the predicted position of threat, and v o the current velocity of threat. 6. Simulation The proposed method is simulated in MATLAB R2011a on the computer with CPU processor of Intel Core i5 and dominant frequency of 2.5 GHz. The necessary parameters of path planning are listed in Table 2. As the calculation and storage of intelligent

276

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

Table 2 Parameters of path planning. Parameter

Value

Sampling time,  T (s) Cruising velocity, V 0 (km/s) Maximum UAV turn rate, ωmax (deg/s) Minimum/Maximum flight–path angle, θmin (deg), θmax (deg) Minimum/Maximum UAV altitude, hmin (km), hmax (km) Range of repulsive parameter ρ Range of tangential parameter σ Range of planning steps N Detecting radius, R r (km) Obstacle expansion parameter, λa , λb , λc Weight values of path cost, λ1 , λ2 , λ3 Weight values of lateral/longitudinal smoothness, μ1 , μ2

1 0.15 30 −30, 30 0.2, 5 [0.1, 5] [0.1, 5] [1, 10] 3 1.1, 1.1, 1.1 0.5, 0.3, 0.2 0.5, 0.5

Fig. 11. State variables of paths with λ = 2 and R = 1, 2, 3 km.

Fig. 9. Paths of target tracking with λ = 2 and R = 1, 2, 3 km.

Fig. 10. Paths of target tracking with R = 2 km and λ = 0.1, 2, 5.

optimization method are too large, which is unfit for online application, the denumerable set of reactive parameters is utilized to seek the sub-optimal solution. 6.1. Target tracking Suppose there are no obstacles or threats in planning space. For the target tracking problem, the velocity by LGVF is taken as that of UAV. Suppose that the target moves from the start point (0, 0, 0) km along the sinusoid in x– y plane, the start point of UAV is (6, 6, 1) km, and the optimal height H is 0.5 km. The planned paths, with λ = 2 and R = 1, 2, 3 km, are shown in Fig. 9. The paths with R = 2 km and λ = 0.1, 2, 5 are shown in Fig. 10. All the paths will converge to the respective limit cycle and optimal

Fig. 12. State variables of paths with R = 2 km and λ = 0.1, 2, 5.

height, but the shapes of paths are different with various selections. The state variables of paths with different parameters are listed in Fig. 11 and Fig. 12. It is obvious that the turn rate and the flight– path angle of all the paths are within their respective range. The smaller R is, the larger the turn rate, the lesser the horizontal velocity component, and the more the vertical velocity component. As a result, the flight–path angle is bigger, so UAV approaches to

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

277

Fig. 13. Paths with different reactive parameters.

Fig. 14. Path avoiding two moving threats.

the optimal height more quickly. λ has no influence on UAV horizontal motion; on the other hand, the larger λ is, the larger the flight–path angle, hence UAV will converge to the optimal height more quickly. 6.2. Obstacle avoidance 6.2.1. Static obstacle Suppose the target moves along the x-axis with the initial point (0, 0, 0) km, and the start point of UAV is (0, 2, 1) km. The parameters of target tracking are as follows: H = 0.5 km, R = 2 km, λ = 2. When there are no obstacles in planning space, the LGVF method can generate the initial path. Suppose three static obstacles, i.e. one sphere, one cylinder and one cone, are constructed in the planning space, then the LGVF+IFDS method is employed to plan path. By selecting different repulsive and tangential parameters, the correspondingly interfered paths are obtained, shown in Fig. 13. It is obvious that all the interfered paths can avoid obstacles when tracking the target at the same time. Besides, the larger the repulsive or tangential parameter is, the earlier and more violently UAV avoids obstacles. 6.2.2. Dynamic obstacle Suppose there are two moving obstacles in planning space. One is a sphere threat of the radius 2 km with the initial position (1, 0, 0) km, whose motion information is: x = 1 + 0.025 · t, y = sin(0.02π · t ) + sin(0.01π · t ) and z = 0. The other is a cylinder obstacle with size parameters a = b = 1.5 km and the ini-

Fig. 15. Distance between UAV and threat surface.

tial point (16, −2.5, 0) km, the motion information of which is: x = 16 − 0.03 · t, y = −2.5 + sin(0.01π · t ) and z = 0. The repulsive and tangential parameters of obstacles are ρ = 1.0, σ = 1.0. On the basis of the dilatation parameter of obstacle, the safe distances of sphere and cylinder are D safe1 = 0.2 km, D safe2 = 0.15 km respectively. Fig. 14 illustrates the planned path and Fig. 15 shows the distance between UAV and the sphere surface i.e. D UAV-threat1 , and that between UAV and cylinder surface i.e.

278

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

Fig. 16. Estimated sphere threat position errors.

D UAV-threat2 . It is obvious that D UAV-threat1 is bigger than D safe1 and D UAV-threat2 is bigger than D safe2 , meaning that UAV avoids these two moving threats successfully. 6.3. Varying receding-horizon optimization A scenario is constructed to simulate the real UAV flight environment as follow. The overall flying time of UAV is 1000 s. Some static obstacles exist randomly in the planning space, the size of which is 25 km × 25 km × 5 km. Besides, one cylinder threat moves by the following model: x = 10 + 0.015 · t, y = 9 − 0.05 · t and z = 0, t ∈ [0, 500]; one sphere obstacle moves with S-shaped motion: x = 5 + 0.025 · t, y = 13 + 0.01 · t + sin(0.02π · t ) and z = 0, t ∈ [0, 350]. Suppose the target moves around the coordinate origin circularly in the horizontal plane, whose motion model is x = 20 cos(0.0005π · t ), y = 20 sin(0.0005π · t ) and z = 0, t ∈ [0, 1000]. The future motion of target or threat should first be predicted by EKF, based on the current observation information and history information. In this scenario, both the target and threats move on the x– y plane, so the motion state and the observed value can be simplified as X k = [xk , yk , v xk , v yk ]T and Z k = [xk , yk ]T respectively. Suppose the covariance matrixes Q and R of EKF are as follows:

⎡

0 ⎢0 ⎢ Q =⎣ 0 0

⎤

0 0 0 0 0 0 ⎥ ⎥ 0 0.001 0 ⎦ 0 0 0.001

 R=

0.01 0

0 0.01



Take the moving sphere obstacle as an example, its estimated position errors by EKF are illustrated in Fig. 16. As the estimated position error remains small, the prediction results can be utilized in the receding-horizon optimization. If one-step planning strategy is utilized i.e. N = 1, only the current environment information is considered without considering the future motion of target or threat. Therefore some large changes of angle may exist in the planned path, which is shown in Fig. 17. When the rolling optimization strategy based on adaptive adjustment is utilized, the future motion of threat or target is fully considered with a proper time domain. Therefore the route is much smoother, and it satisfies the constraint conditions, shown in Fig. 17. Besides, the path can track target and avoid obstacle at the same time. The time domain of optimization i.e. the steps of local path planning is displayed in Fig. 18. The paths can be evaluated quantitatively in Table 3. min D UAV-threat1 and min D UAV-threat2 mean the minimum distance between UAV and the surface of the first or the second threat respectively. Suppose K waypoints { P 1 , · · · , P K } are planned in the whole process, then the global smoothness GS and the local  K −2 1 smoothness LS can be defined [27]. GS = K − k=1 (μ1 · |ψk+1 − 2 ψk | + μ2 · |θk+1 − θk |) means the average change of the path angle, while LS = maxkK=−12 (μ1 · |ψk+1 − ψk | + μ2 · |θk+1 − θk |) is the maximum angle change of the whole path. From Table 3, when the single-step strategy is used, the planning time is very short (0.002 s). However, the local smoothness is too large, and the turn rate ψ˙ and the flight path angle θ are out of their respective ranges, meaning that the path is unfeasible. If the varying-step strategy is employed, the path is much smoother. Although the average planning time increases significantly to 0.86 s, it is still shorter than the flight time ( T = 1 s). 7. Conclusion This paper focuses on the dynamic path planning of UAV in three-dimensional complex dynamic environment. The good performance of target tracking from LGVF and the advantage of obstacle avoidance from IFDS are first combined effectively to meet the various mission requirements, and the varying receding-horizon optimization is then utilized to adjust the repulsive and tangential parameters in real time. And this proposed hybrid method has the advantage of simple principle, high computational efficiency and strong practicality. The relevant simulations have demonstrated that the planned route can guide UAV track target and avoid obstacles at the same time under the certain optimization criteria.

Fig. 17. Path in the complex dynamic environment. (For interpretation of the references to color in this figure, the reader is referred to the web version of this article.)

P. Yao et al. / Aerospace Science and Technology 47 (2015) 269–279

Fig. 18. Time domain of local optimization. Table 3 Performances of paths with different N.

Average time of local planning (s) min D UAV-threat1 (km) min D UAV-threat2 (km) LS (deg) GS (deg) θ (deg) ψ˙ (deg/s)

N =1

Self-adaptive N

0.002 0.19 0.17 70.68 2.93 [−33, 45] [−83, 61]

0.86 0.22 0.20 27.86 3.05 [−25, 22] [−19, 21]

Conflict of interest statement The authors declared that they do not have any conflicts of interest to this work. Acknowledgements The authors would like to express their acknowledgment for the support from the National Natural Science Foundation of China (No. 61175084), Program for Changjiang Scholars and Innovative Research Team in University (No. IRT13004). Appendix A. Supplementary material Supplementary material related to this article can be found online at http://dx.doi.org/10.1016/j.ast.2015.09.037. References [1] H.I. Yang, Y.J. Zhao, Trajectory planning for autonomous aerospace vehicles amid known obstacles and conflicts, J. Guid. Control Dyn. 27 (6) (2004) 997–1008. [2] W.R. Zhu, H.B. Duan, Chaotic predator–prey biogeography-based optimization approach for UCAV path planning, Aerosp. Sci. Technol. 32 (2014) 153–161. [3] F. Ahmed, K. Deb, Multi-objective optimal path planning using elitist nondominated sorting genetic algorithms, Soft Comput. 17 (7) (2013) 1283–1299. [4] J. Karimi, S.H. Pourtakdoust, Optimal maneuver-based motion planning over terrain and threats using a dynamic hybrid PSO algorithm, Aerosp. Sci. Technol. 26 (1) (2013) 60–71.

279

[5] W. Liu, Z. Zheng, K.Y. Cai, Adaptive path planning for unmanned aerial vehicles based on bi-level programming and variable planning time interval, Chin. J. Aeronaut. 26 (3) (2013) 646–660. [6] V. Roberge, M. Tarbouchi, G. Labonte, Comparison of parallel genetic algorithm and particle swarm optimization for real-time UAV path planning, IEEE Trans. Ind. Inform. 9 (1) (2013) 132–141. [7] S.A.P. Quintero, F. Papi, D.J. Klein, et al., Optimal UAV coordination for target tracking using dynamic programming, in: 49th IEEE Conference on Decision and Control, Dec. 2010, IEEE, 2010, pp. 4541–4546. [8] S. Ragi, E.K.P. Chong, Dynamic UAV path planning for multitarget tracking, in: 2012 American Control Conference, Jun. 2012, IEEE, 2012, pp. 3845–3850. [9] S.O. Lee, Y.J. Cho, M. Hwang-Bo, et al., Stable target-tracking control for unicycle mobile robots, in: 2000 IEEE/RSJ International Conference on Intelligent Robots and Systems, Oct.–Nov. 2000, IEEE, 2000, pp. 1822–1827. [10] D.A. Lawrence, E.W. Frew, W.J. Pisano, Lyapunov vector fields for autonomous unmanned aircraft flight control, J. Guid. Control Dyn. 31 (5) (2008) 1220–1229. [11] H.D. Chen, K.C. Chang, C.S. Agate, A dynamic path planning algorithm for UAV tracking, in: Proceedings of the International Society for Optical Engineering, 2009, SPIE Paper 7336: B1–10. [12] H.D. Chen, K.C. Chang, C.S. Agate, UAV path planning with tangent-plusLyapunov vector field guidance and obstacle avoidance, IEEE Trans. Aerosp. Electron. Syst. 49 (2) (2013) 840–856. [13] Y. Zhang, J. Chen, L.C. Shen, Real-time trajectory planning for UCAV air-tosurface attack using inverse dynamics optimization method and receding horizon control, Chin. J. Aeronaut. 26 (4) (2013) 1038–1056. [14] Y. Kuwata, J. Teo, G. Fiore, et al., Real-time motion planning with applications to autonomous urban driving, IEEE Trans. Control Syst. Technol. 17 (5) (2009) 1105–1118. [15] M.A.K. Jaradat, M.H. Garibeh, E.A. Feilat, Autonomous mobile robot dynamic motion planning using hybrid fuzzy potential field, Soft Comput. 16 (1) (2012) 153–164. [16] Z.N. Dong, R.L. Zhang, Z.J. Chen, et al., Study on UAV path planning approach based on fuzzy virtual force, Chin. J. Aeronaut. 23 (3) (2010) 341–350. [17] J. Sullivan, S. Waydo, M. Campbell, Using stream functions for complex behavior and path generation, AIAA Guidance, Navigation and Control Conference and Exhibit, AIAA, 2003, pp. 1–9. [18] H. Choser, J. Burdick, Sensor-based exploration: the hierarchical generalized Voronoi graph, Int. J. Robot. Res. 19 (2) (2000) 96–125. [19] S.S. Ge, Y.J. Cui, Dynamic motion planning for mobile robots using potential field method, Auton. Robots 13 (3) (2002) 207–222. [20] X.G. Gao, J. Ren, D. Chen, Developing an effective algorithm for dynamic UAV path planning with incomplete threat information, Proc. Inst. Mech. Eng., G J. Aerosp. Eng. 226 (4) (2012) 413–421. [21] X.G. Peng, D.M. Xu, Intelligent online path planning for UAVs in adversarial environments, Int. J. Adv. Robot. Syst. 9 (3) (2012) 1–12. [22] P. Yao, H.L. Wang, Z.K. Su, UAV feasible path planning based on disturbed fluid and trajectory propagation, Chin. J. Aeronaut. 28 (4) (2015) 1163–1177. [23] P. Yao, H.L. Wang, C. Liu, 3-D dynamic path planning for UAV based on interfered fluid flow, in: IEEE Chinese Guidance, Navigation and Control Conference, Aug. 2014, IEEE, 2014, pp. 997–1002. [24] H.D. Chen, K.C. Chang, C.S. Agate, Tracking with UAV using tangent-plusLyapunov vector field guidance, in: 12th International Conference on Information Fusion, Jul. 2009, IEEE, 2009, pp. 363–372. [25] S. Ragi, E.K.P. Chong, UAV path planning in a dynamic environment via partially observable Markov decision process, IEEE Trans. Aerosp. Electron. Syst. 49 (4) (2013) 2397–2412. [26] Y. Bar-Shalom, X.R. Li, T. Kirubarajan, Estimation with Applications to Tracking and Navigation: Theory, Algorithm and Software, John Wiley and Sons Inc., New York, 2001. [27] Z. Zheng, S.J. Wu, W. Liu, et al., A feedback based CRI approach to fuzzy reasoning, Appl. Soft Comput. 11 (1) (2011) 1241–1255. [28] V. Shaferman, T. Shima, Unmanned aerial vehicles cooperative tracking of moving ground target in urban environments, J. Guid. Control Dyn. 31 (5) (2008) 1360–1371.