Improved Leader Follower Formation Control for Multiple Quadrotors Based AFSA

In this paper, formation tracking in plane with equal height for all quadrotors is discussed. Two controllers are necessary. First, PID controller is used to ensure the tracking of the desired trajectory by the first quadrotor named leader. The formation of the quadrotors in plane is achieved by using the directed lyapunov controller. In order to improve the controller performances, the artificial fish swarm algorithm is used to ensure the dynamic optimization of the parameter controllers. When the desired shape formation is achieved, PID controller is used again to ensure the keeping of this formation shape. Finally, simulation results demonstrate the effectiveness of the proposed controllers compared to the ordinary controller and also compared to the static optimization by using the same algorithm.


Introduction
Quadrotors have become the interest of many researches in the word [1]- [4].Quadrotor can also perform solo mission where it can achieve good performances.This characteristic will become more interesting when it operate in a coordinated fashion such as formation and trajectory tracking.
In last decade, tracking formation control for multiple uavs has become the interest of many researches in the word.Based on separated saturations and a multi-agent consensus algorithm is developed to ensure the tracking formation control of mini quadrotor [5].In [6], 3D path-following of multiple quadrotors based lyapunov approach was considered.Integral backstepping controller is used to maintain a desired formation tracking control for multiple uavs is presented in [7].In [8] the authors investigate tracking controls for an arbitrary number of cooperating quadrotor unmanned aerial vehicles with a suspended load.In [9] Two controllers based on PID and sliding mode were used to ensure the tracking formation for quadrotors uavs.In [10], the synchronized position tracking controller is incorporated in formation flight control for multiple uavs.In [11] based on linear PD and sliding mode controller, flight formation control for leader follower quadrotor is presented, It is tested in real application.
Motivated by the different advantages of the quadrotor, the present paper studies the problem of leader follower formation control for multiple quadrotors.The present work is mainly based on [11], the control strategy is divided on two parts such as the tracking and formation tracking control.In the first part, PID controller is used to ensure the tracking of the desired trajectory by the first quadrotor named leader.This controller is also used to ensure the keeping formation by the followers.The second part is devoted for the formation tracking in plane with equal height ( ) for all quadrotors.
In order to achieve a good performance of time convergence of the controller proposed, Artificial Fish swarm Algorithm is used in this paper.AFSA was proposed in 2002 [12], it is inspired by the natural social behavior of fish schooling and swarm intelligence.This algorithm can achieve faster convergence speed and require few parameters to be adjusted.In literature, many works about optimization were presented [13]- [15]; however the tune of parameters is static.Different to the existing works in literature, the present paper use AFSA to tune the controller parameters dynamically.

Quadrotor Dynamic Model
The motion of quadrotor is controlled by varying the rotation speed of the four rotors to change the thrust and the torque produced by each one (Figure 1).In this paper, we consider the model dynamic based Newton-Euler approach.The dynamic model is presented as [16], [17]: (1)

, ,
Correspond to the relative position of the mass centre of the quadrotor with respect to an inertial coordinate frame, is the gravitational acceleration.is the length from the mass centre to the rotor, , , denotes the three Euler angles that represent the attitude of the quadrotor, namely roll-pitch-yaw of the quadrotor.
is the thrust force vector in the body system.
, and correspond to the control inputs of roll, pitch and yaw moments, respectively.

Controller Design
In this section, two controllers are designed to ensure the tracking and the formation for multiple quadrotors.

Tracking controller
A simple PID is designed to ensure the tracking of the desired trajectory by the first quadrotor named leader.It is also used to ensure the keeping of formation in x-y plane.The controller can be expressed as: (2)

With
and are the error and the derivation of error in i-direction.The error is defined as:

Formation Controller Design
The formation control by keeping a fixed distance and a fixed deviation ∆ between the leader and the i-th follower quadrotors (Figure 2).Now, we consider quadrotors.In our study we assumed that the quadrotors has the same translational dynamic model in plane as given by the following system [11]: Where: and are the velocity component in the and directions.
is the angular velocity for the yaw angle and , for the leader and the follower quadrotors.

Distance and angle controller
Let , be the x and y coordinates of the vector drawn from the mass center of the leader to the one of the follower, in the leader's body fixed frame.These two coordinates can be given by: We consider the orientation and the formation errors defined as: After some simplifications, we obtain [11]: Differentiating with respect to time and considering Equation (09) we have: To satisfy the Lyapunov stability condition, it is obvious to choose as follows: (11) Where is a diagonal positive matrix., , .Then < 0 is negative definite.

Optimization
In order to improve the controller proposed in (11), AFSA is applied to tune the controller parameters ( , , ) for better time convergence of the formation control errors.For this we defined (fitness function) as convergence criterion to evaluate the cost of the proposed algorithm.In this context the fitness function of the artificial fishes is defined as follows: ∆ (12) with: And ∆ are the distance and the orientation error between the leader and follower.The best fitness is the smallest fitness value among the artificial fishes which correspond to the best values of , .

Artificial Fish swarm Algorithm
The artificial fish individual state can be expressed as n dimension vector., … . Each artificial fish represents a solution to the optimization problem.In our case this solution given by AF represents a set of controller parameters which can make the function minimal.This algorithm can be presented as follow: The behavior of searching food (prey): We assume , the actual state and the next state of AF, respectively.This new state is given by this equation: ., with is the visual distance for the AF.The moving of AF from to will be taken place if the corresponding concentration food ( ) at state is more important than the food concentration ( ) at state .This step can be expressed by: The behavior of swarm: We assume as the number of the neighbors within the visual distance of the AF.The swarming of AF from to will be taken place if the corresponding concentration food ( ) at state c is more important than the food concentration ( ) at state and the swarm is not crowd ( ).Otherwise AF chooses to search food behavior.This step can be summarized by: The behavior of follow: In this behavior, The AF swarm from his actual state to the largest food concentration if the concentration of food (y >y ) is more important and the swarm is not crowd.Otherwise AF chooses to search food behavior.This step can be summarized by: Finally, the best fitness and the best fish correspond to this best function are selected.

Static Optimization
In this technique, for N iterations of algorithm one best fitness function is chosen.This function corresponds to the best controller parameters.These parameters will be injected in the controller (11).

Dynamic Optimization
In this technique and different to the static technique, for each iteration of algorithm a best function is selected and a best controller parameters correspond to this function are also selected and online injected in the controller (11).The same procedure will be repeated until the n-th iteration.

Results and Analysis
The proposed formation control has been simulated for the case of three quadrotors (one leader and two followers).The controllers' objectives are: First: The tracking of trajectory by the quadrotor leader described by: , sin , 5 0. Second: The formation and the keeping of formation by the followers described by the desired distance and deviation angle to the leader are given by: First follower 2 meters and ∆ 0 and for the second follower 4 meters and ∆ 0.    4, while the distance and angle errors between leader and follower quadrotors by using the controller (11) are depicted in Figure 5 and Figure 6, respectively.
By using static optimization as shown in Figures 7, 8 and 9, the results are improved comparatively to the Figures 4, 5 and 6.From Figures 10, 11 and 12, It is shown the results are considered perfect in term of error convergence (both distance and error between leader and follower quadrotors), this can be explain by the fact that in dynamic optimization case, the parameters are selected dynamically which ensure that the distance and angle errors are minimal for each iterration of the proposed algorithm.Compared to the static optimization case, only one fitness function is selected, this function is not necessary correspond to the good parameters for the iteration of the algorithm.

Conclusion
This paper addressed the problem of leader follower formation tracking control for multiple quadrotors.PID controller is used to track the desired trajectory by the leader, by using Simulation results demonstrate that the proposed AFSA algorithm is an effective tuning strategy of Lyapunov-based controller's parameter controls.AFSA algorithm leads to satisfying and efficient formation tracking performances in terms of the speed of convergence of the tracking errors and time achieving of the desired formation by tuning the controller parameters dynamically.

Figure 2 .
Figure 2. Position and orientation of the leader and follower quadrotors inplane[11]

Figure 3 .
Figure 3. Trajectories of leader and follower quadrotors in direction before optimization

Figure 8 .Figure 9 .Figure 12 .
Figure 8. Distance error between leader and follower quadrotors by using static optimization

TELKOMNIKA
Vol.13, No. 1, March 2015 : 85 -92 92 a direct lyapunov method a new controller is proposed to ensure the formation tracking in plane with equal height ( ) for all quadrotors, and then PID is used again to ensure the keeping of formation by the followers.