UAV formation control design with obstacle avoidance in dynamic three-dimensional environment

This paper considers the artificial potential field method combined with rotational vectors for a general problem of multi-unmanned aerial vehicle (UAV) systems tracking a moving target in dynamic three-dimensional environment. An attractive potential field is generated between the leader and the target. It drives the leader to track the target based on the relative position of them. The other UAVs in the formation are controlled to follow the leader by the attractive control force. The repulsive force affects among the UAVs to avoid collisions and distribute the UAVs evenly on the spherical surface whose center is the leader-UAV. Specific orders or positions of the UAVs are not required. The trajectories of avoidance obstacle can be obtained through two kinds of potential field with rotation vectors. Every UAV can choose the optimal trajectory to avoid the obstacle and reconfigure the formation after passing the obstacle. Simulations study on UAV are presented to demonstrate the effectiveness of proposed method.

A common problem with the artificial potential field method is the existence of local minima in the multi UAV system. It means that the repelling force is in the opposite direction of approaching UAV and the UAV will stick in local minima. A possible solution to this problem is to add a small disturbance in vertical direction when it is stuck (Chengqing et al. 2000). However, the UAV can not maintain the speed and this problem becomes complicated as the number of UAVs in the formation increases. In Masoud and Masoud (2002) the UAVs avoid the obstacles without containing local minimum points by using combination of the artificial potential fields and static fields. In this method, UAVs are navigated by generalized artificial potential field in an area which has known static obstacles.
In recent years, a number of various approaches for controlling a group of mobile robots to follow the track which aims to reach target point by avoiding obstacles in 2D (two-dimensional) space, see Rezaee and Abdollahi (2014), Hu and Feng (2010), Cui (2010). The collision and obstacle avoidance mechanism has been provided autonomously to ensure stability and robustness of the group by using this method. Furthermore, the tracks which aim to reach target points and avoid obstacles have been defined for the formation of UAVs in 3D (three-dimensional) space without effecting local minimums in Garcia-Delgado et al. (2012), Filippis et al. (2012). In Weihua and Go (2011), model predictive control (MMPC) method for UAVs formation coordination and obstacle avoidance for any shape and size of obstacles is introduced.
In this paper, the artificial potential field method is combined with rotational vectors and applied to formation control of UAVs. An UAV in formation is defined as the leader. The member UAVs follows the leader-UAV to track a moving target with obstacle avoidance in dynamic three-dimensional environment. By the attraction of the artificial potential field from the target, the leader will drive the formation to approach the target position. Each member-UAV will be connected with its neighbors and maintain in desired distance with neighbors. The moving trajectory of each member-UAV is controlled by the total potential field consisting of the attractive field of the leader and the repulsive fields of its neighbors. The potential fields with rotational vectors around the obstacles are divided into two kinds of potential fields, the potential fields parallel to x-y plane and the potential fields parallel to y-z plane. Each kind of potential field has two directions of rotational vectors. In the potential field, the rotational vectors adjust the direction of a UAV to lead it toward its target without being trapped in local minimum positions. This technique can choose the optimal path for each UAV and reconfigurate the formation for the swarm formation. The formation of UAVs track a moving target and keep the robustness and stability of the formation with obstacle and collision-avoidance by using this artificial potential field method.
The rest of paper is organized as follows: In second section, dynamics of unmanned vehicles model is defined. The proposed method of target tracking and formation configuration is presented in third section. Fourth section presented the obstacle avoidance method for unmanned vehicle formation. Fifth section gives the simulation results. Finally, some conclusions and ideas for future work are given in sixth section.

Unmanned vehicle dynamical equations and control
First, we consider UAV's dynamical equations which has been widely used in many literatures (Rezaee and Abdollahi 2013;Lin 2014;Wang and Xin 2013). The unmanned vehicle dynamical equations can be described by 3-DOF (degree of freedom) point mass model as follows (Wang and Xin 2013): where m is UAV's mass, D is the damping coefficient, T is the engine thrust, g is the gravitational acceleration, (ẋ,ẏ,ż) is the velocity vector of UAV at coordinate axis direction in 3D space, L is the lift force, α n is the flight path angle, β n is the heading angle, δ n is banking angle, V is the air speed which is assume to be be equal to the ground speed in this paper. Figure 1 is shown the UAV aerodynamic coordinate system model.
The control inputs of UAV is engine thrust T, lift force L, and the banking angle δ. The highly nonlinear UAV model (1) can be pre-linearized using feedback linearization as follows (Menon et al. 1999): where (u x , u y , u z ) is the virtual acceleration control inputs. The virtual control inputs are designed base on the linear model (2). The real control inputs can be obtained through the following equations (1)

Formation control
In this section presents, an extended artificial potential field method is presented for a leader-follower formation of UAVs with obstacle avoidance in 3D environment.

Control algorithm for each member-UAV
The motion of the member-UAV is driven by the total artificial force that consists of two components as follows: where m is the mass of the nth UAV. The first component f nc is an attractive force to control the UAV to reach the spherical surface whose center is leader UAV. f nc can be described as: where where (x l , y l , z l ) is the coordinate of the leader UAV. k s is the gain coefficient.
The second component f n , which is the resultant force of repulsive forces of the UAVs, is created to arrange the unmanned vehicles evenly distributed on the spherical surface. We define leader-UAV as the center at x l , y l , z l and all UAVs have been negatively or positively charged. The repulsive forces affect on the unmanned vehicles which have identical electric charge. The control force that is defined in (5) keeping the UAVs on the spherical surface whose radius is r a and center is x l , y l , z l . The unmanned vehicle reaches the equilibrium point when the resultant of repulsive forces tangent to the spherical surface acting on an UAV is zero. It means that the distances between UAVs are equal. Then, the goal is achieved. The repulsive force between two UAVs is defined as follows: (3) where q n is electric quantity of the nth unmanned vehicle, q i is electric quantity of the ith unmanned vehicle, k r is the repulse constant coefficient, r ni is the distance between the ith and the nth unmanned vehicle. The amount of unmanned vehicle is N. Hence, the resultant of the repulsive forces from the other unmanned vehicles on the nth unmanned vehicle is given as follows: In Fig. 2, P is the initial position of UAV. P ′ is the position of UAV after move.
By decomposing the resultant force in three directions, component forces are in the x-axis direction, y-axis direction and the z-axis direction, respectively. It is shown in Fig. 2 as follows:  (2), the nth UAV's dynamical equations based on the virtual structure can be rewritten as: Given the above discussion, the repulsive force that is proportional to 1/r ni avoids the collision between unmanned vehicles. The control force (u x n , u x n , u x n ) in (10) leads the UAVs toward the equilibrium points on the spherical surface.

Control algorithm for the UAV-leader
The motion of the UAV-leader is driven by the attractive force to reach a moving target. The position of leader UAV p l = (x l , y l , z l ). Consider the target position is p t = (x t , y t , z t ) , the attractive force can be stated as follow: if r < d else where k t is the positive constants. d lt is the distance between target and the leader UAV.
d lt = (x t − x l ) 2 + y t − y l 2 + (z t − z l ) 2 .r t is defined as the range of the target.
The relative velocity among the leader and the target is added as a damping force to control the leader-UAV when it approaches the range of the target position. The damping force force is proposed as follows: where k m is the positive amplification coefficient. The control force is the resultant force of the attractive force and damp force

Stability analysis
In order to analyze the stability of the formation, we analyze the a UAV at the equilibrium point. It means that f n is zero. Lemma 3.1 provides a trajectory that UAV moves to a spherical surface whose center is (x c , y c , z c ) and radius is r a . It is stable when the UAV reaches the spherical surface. In other words velocity of UAV will be zero.

Lemma 1 The desire trajectory of UAV satisfies
where (x, y, z) � = (x c , y c , z c ).

Proof By inserting r
into Eq. (6). ϕ and θ are shown in Fig. 2. The equations can be achieved as follows: To prove the stability of UAV arriving at the spherical surface, it is obvious that θ and φ converge to zero when r = r a , we define the error ε = r − r a and choose the following Lyapunov function candidate: (12) The derivative of the Lyapunov function (4) is given by

Substituting (3) into (5), we have
Because (x, y, z) � = (x c , y c , z c ), it can be deuced that r > 0. It is obvious that V (ε) ≤ 0. Since V (ε) = 0 only if r = r a , it follows that velocity is non-increasing, θ = 0, φ = 0 and V (ε) is bounded. In other words, UAV's trajectory converges to the spherical surface. Unmanned vehicle will have no rotational motion when arrives at the spherical surface.

Obstacle avoidance
In this section, a novel method of obstacle avoidance for single UAV is presented. The strategy for formation obstacle avoidance is also proposed.

Obstacle avoidance
We consider the trajectory tracking problem of UAV in dynamic 3-D environment. The position of UAV is available. We assume that obstacles can be detected by the UAV vision sensor. Without loss of generality, the obstacle can be considered as a rectangular solid are its vertices which parallel to x-, y-, z-axis respectively. The potential field covers the obstacles with the minimum volume that satisfies The potential field method depends on repulsive force. When the repulsive force is in the opposite direction of approaching UAV, the UAV will stick in a local minimum position. To avoid this condition, rotational vectors are added to adjust the direction of the UAV. The potential fields with rotational vectors which cover this ellipsoid can be divided into two kinds of potential fields: the potential field which is parallel to x-y plane with rotational vectors and the potential field which is parallel to y-z plane with rotational vectors. It is depicted in Fig. 3. The desired trajectories which are effected by two kinds of potential fields with rotational vectors respectively are shown in Fig. 3.
Where γ n and φ n are the velocity angles that take affect respectively in two kinds of potential fields.
The trajectories of the UAV which only be effected by the rotational vectors in the potential fields parallel to x-y plane can be divided into two directions: clockwise direction and counterclockwise direction.
The trajectories of UAV which only be effected by the rotational vectors in the potential fields parallel to y-z plane can be divided into two directions: upward direction and downward direction.

Obstacle avoidance path optimization
In this section, the strategy of avoidance obstacles for formation is studied and the control force for single UAV is presented. We desire the rotational vectors effect when The potential fields with rotational vectors around an obstacle an UAV enter the range of the obstacle and be enlarged when the UAV is closer to the obstacle. The range of obstacle is denoted by r v . The distance between the UAV to the obstacle is satisfying The control force for avoidance obstacle can be stated as follow: Therefore the control force can be stated as follows: if r a < r o else where f desire = f x l , f y l , f z l for leader-UAV and f desire = f x nc + f x n , f y nc + f y n , f z nc + f z n for member UAVs. Based on the trajectories in clockwise and counterclockwise direction which are effected by potential field with rotational vectors on x-y plane, the control force for avoidance obstacle can be designed as follows: or where k o is the gain coefficient. The rotational vectors in clockwise and counterclockwise direction are depicted in Fig. 4. ρ n is the angle between line linking the UAV and the center of gravity of the obstacle and the horizontal axis. χ n is the direction of potential field vector. φ n is the move direction of UAV. It can be obtained as follows: φ n = arctan ẏ,ẋ

Simulation result
This section shows the performance of the proposed approaches in different scenarios. In Scenario 1, the leader UAV tracks the target position by using provided control algorithm.

Scenario 1
Consider the target as a moving point in the free space. The initial position is (30,20,10), and the initial position of the leader is (0, 0, 0). The trajectory of target depends on (13) can be described as f xt = 10, f yt = 10 × sin (1/8x v ), f zv = 2. The simulation result in Fig. 6 shows that while reaching to the trajectory of the target the leader UAV always kept stable.

Scenario 2
Consider an obstacle with vertices located in (20 ± 2, 20 ± 3, 10 ± 3). The leader UAV switches to obstacle avoidance mode when it is close to the obstacle. The leader UAV selects the optimal trajectory, therefore it can catch up the target rapidly with a smoothly trajectory. Figure 7 shows the scenario of leader UAV flying with obstacle avoidance.
The initial position of the leader UAV is p L = (1, 1, 5.5) and the obstacle with vertices is located in p o = (20 ± 2, 20 ± 3, 10 ± 3). Without loss the generality, we set M = 1, D = 1, k r = 5, k s = 5, k t = 2, k m = 1.5, r a = 2. Simulation results depicted in Fig. 8 show that the organization of five UAVs is influenced by obstacle in 3D environment. The swarm avoids obstacle effectively and reconfigure the formation after avoiding the obstacle. The algorithm of the formation control, the leader-following motion of the formation, the obstacles avoidance, the collision avoidance between UAVs in the formation and the stability of the formation while moving are verified by simulation.

Conclusion
In this paper, an formation control approach has been addressed for multi-UAV systems to track a moving target under the drive of a UAV-leader based on the artificial potential field method combined with rotational vectors. The UAVs were able to configure the formation easily and quickly based on the attractive artificial potential field and move to the targets position. The repulsive forces kept UAVs from collision with each other during the maneuver. The effectiveness of the algorithm was verified through the simulations. Furthermore, exploring more features of the proposed method, we will focus on controlling the formation of UAVs in specific shape as the future direction of this research.