An Improved Fast Flocking Algorithm with Obstacle Avoidance for Multiagent Dynamic Systems

Flocking behavior is a common phenomenon in nature, such as flocks of birds and groups of fish. In order to make the agents effectively avoid obstacles and fast form flocking towards the direction of destination point, this paper proposes a fast multiagent obstacle avoidance (FMOA) algorithm. FMOA is illustrated based on the status of whether the flocking has formed. If flocking has not formed, agents should avoid the obstacles toward the direction of target. If otherwise, these agents have reached the state of lattice and then these agents only need to avoid the obstacles and ignore the direction of target. The experimental results show that the proposed FMOA algorithm has better performance in terms of flocking path length. Furthermore, the proposed FMOA algorithm is applied to the formation flying of quad-rotor helicopters. Comparedwith other technologies to perform the localization of quad-rotor helicopter, this paper innovatively constructs a smart environment by deploying somewireless sensor network (WSN) nodes using the proposed localization algorithm. Finally, the proposed FMOA algorithm is used to conduct the formation flying of these quad-rotor helicopters in the smart environment.


Introduction
Flocking is the behavior exhibited when a group of birds, called a flock, are foraging or in flight.There are similar behaviors such as the shoaling behavior of fish, the swarming behavior of insects, and the herd behavior of land animals [1].
Reynolds [2] firstly introduced three rules including flocking centering, obstacle avoidance, and velocity matching to imitate flocking by computer simulations.In [3,4], Reynolds further explains the three rules.Toner and Tu [5] and Shimoyama et al. [6] conduct researches about multiagent flocking.Toner adopts a continuous model to analyze multiagent flocking.Shimoyama et al. propose to describe behavior of multiagents based on particle system and use function rules of overall perception to consider the flocking of multiagent.Mogilner and Edelstein-Keshet [7,8] use a continuous model to research the flocking of multiagent, and they consider the practical sensing problems.
Jadbabaie et al. [9] conduct researches on the flocking behavior of multiagent based on the rule of the nearest agent and verify that the agents tend to move with the same direction under the assumption that all agents can detect each other.
Quadrotor helicopters have been widely used nowadays due to their unique advantages.Flocking has also been considered as an approach to control the behavior of unmanned air vehicles (UAVs).Flocking is a common technology in screensavers and has been successfully used in animations.Flocking has been widely used in many films [10] to generate crowding.Tim Burton's Batman Returns (1992) features flocking bats, and Disney's The Lion King (1994) includes a wildebeest stampede.Flocking behavior has been used for other interesting applications.For example, it has been applied to Internet multichannel radio stations [11], information visualization [12], and optimization tasks [13].
Lattice graph is used in order to describe the relationship between different agents.For multiagent dynamical systems, if the configurations of agents satisfy the constraint that ‖  −   ‖ = , ∀,  ∈   , where   and   represent the positions of agents  and , respectively, and   denotes the set of all of the agents, these agents form a lattice as shown in Figure 1(a); then,  is the goal distance (the length of side for the lattice).However, if the configurations of agents satisfy the constraint that − ≤ ‖  −  ‖− ≤ , ∀,  ∈   , then these agents form a quasilattice due to the movements of the agents.To form a lattice is the ideal status for the multiagent dynamical systems [2].

Olfati-Saber Algorithm of Obstacle Avoidance for Multiagent Dynamical System
Olfati-Saber proposed an algorithm of obstacle avoidance [14].In this algorithm, obstacle is regarded as a moving agent, and system merges this velocity and the velocity of that agent entering the region of obstacle and then regards the merged velocity as next-time velocity for the agent.The magnitude of velocity, but not its direction, is considered while merging velocity.Besides, obstacle is still regarded as one agent while it is moving away from the obstacle, so this will disturb the behavior of the agent and even the process of obstacle avoidance for these agents.
In the algorithm, the system input is given by where , , and  represent three different agents and    ,    , and    are the interaction mechanisms for the agent set named (, ), (, ), and (, ), respectively, and    is also the item of feedback control.
The calculation of the three input parameters is given by where  1 () = / √ 1 + ‖‖ Supposing an obstacle   whose radius and center are   and   , respectively, and that there is an  agent whose status is (  ,   ), then the position q, and the velocity p, of  spherical agent ((, ) ∈ ]  × ]  ) are, respectively, defined by where  =   /‖  −   ‖,   = (  −   )/‖  −   ‖, and  =  −      .If the interaction mechanism between the two agent sets named (, ) and (, ) is fixed.Analyzing the interaction mechanism of (, ) agent, there are two parts for the mutual effect; the first one is repulsive interaction between  agent and  agent, and the second part is the velocity matching between them.The two parts exist all the time under the condition of  agent entering the perception region of  agent, which means that the neighbor condition of  agent obstacle that    = { ∈ V  : ‖q , −   ‖ <   } is met.Besides, the value of   1 and   2 does not change all the time.After analyzing the existing algorithm, we know that the algorithm has the following flaws.
(a) It only judges the distance of  agent and obstacle.While the distance is smaller than the distance perception, then the system will execute the process of obstacle avoidance, but without considering the velocity direction of  agent, which means that the velocity direction of  agent does not move toward the obstacle; under this case, if  agent can detect the obstacle, the factor of  and  agent, which means that the system aims to change the velocity direction of  agent, till it tends to the same with  agent; thus, the final velocity direction of  agent is uncertain.So this is not beneficial for the form of flocking and access to target point.
The major contribution of this paper is to propose an improved algorithm for obstacle avoidance, which aims to make the agents fast form flocking and approach the destination point; at the meantime, all agents can effectively avoid the obstacles.In addition, this paper innovatively constructs a smart environment to implement the localization of quadrotor helicopters, and the proposed FMOA algorithm is used to conduct the formation flying of quadrotor helicopters in the smart environment.

Fast Multiagent Obstacle Avoidance (FMOA) Algorithm
The proposed algorithm is considered under the status of whether the flocking has formed.If there exists at least one agent having the same velocity with  agent and the distance between them reaches the given value , then it means that the flocking has formed.
If the action range of  agents is , then the dangerous radius of an obstacle is  dan =   +  safe, where   denotes the radius of obstacle and  safe represents the predefined safe region of obstacle.Besides, using   denotes the vector between  agent and the center of obstacle as Figure 2 shows, and the tangent vector between  agent and dangerous region of obstacle is defined as   1 and   2 , respectively.
After agents detect the dangerous region of obstacle (while   ≤ ), the system judges the direction of its velocity; if it moves within the region of obstacle (while det[ 1 ,   ] < 0 ∧ det[ 2 ,   ] > 0), the system will execute the process of obstacle avoidance.Otherwise, no obstacle exists.
While  agent enters the dangerous region of obstacle, the system regards the obstacle as  agent; the magnitude and direction of its velocity for  agent are discussed by the status of whether flocking has formed.
System can judge whether  agent enters the dangerous region of  agent by calculating the value of det[ 1 ,   ], det[ 2 ,   ], and det[  , ], which are defined by where So the above judgment can be simplified to judge its value.If vector    ≥ 0, it means that the velocity vector is above the median vector as Figure 3(a) shows; oppositely, if vector    < 0, it represents that the velocity vector is under the median vector as Figure 3(b) shows.

Flocking Has Not Formed.
The target point  is the final goal for  agent.So, while agents avoid obstacles, the target point  is the guided direction for these agents.
(a) While  agent is located in the region as Figure 4(a) shows, then the velocity direction of  agent should be altered to the tangential direction of obstacle as  1 , which makes it avoid obstacle with the least deviation:   (b) While  agent is located in the region as Figure 4(b) shows, then the velocity direction of  agent should be altered to the target point.In this case, system does not change the velocity direction of  agent, because the obstacle will not affect the movement of  agent.
(c) While  agent is located in the region as Figure 4(c) shows, then the velocity direction of  agent should be altered to the tangential direction.The velocity of  agent is given by (d) While  agent is located in the region as Figure 4(d) shows, then the velocity direction of  agent should be altered to the tangential direction.The velocity of  agent is given by 3.2.Flocking Has Formed.After flocking is formed,  agent has the same velocity with its neighbors, so if it moves toward the direction of obstacle, its next-time velocity should be changed to the tangential direction of the obstacle.The velocity of  agent is given by If it does not move toward the direction of obstacle, then it should just follow its neighbors, because the obstacle will not affect the movement of the  agent at all.System continuously adjusts the movement of agents according to the following conditions: whether there exist the neighbors with the same velocity and the distance between the agent and its neighbor is , whether the direction of moving agent is toward the obstacle, and whether target agent is located in the region of two tangent lines formed by agent and obstacle.Then, system will adjust the velocity of agents according to the above different cases, and the whole algorithm can be described as Figure 5 shows.

Simulation Experiment Test
In the experiments, Matlab 7.0 is used as the simulation software.System randomly generates 100 agents in the given region as Figure 6(a) shows.The red circle represents the obstacles, and the smaller black circles represent the agents.Besides, the execution parameters of communication distance, lattice length, and safe distance of obstacle are set as 8.8 m, 7 m, and 2 m separately.
For each agent, its arrow describes the direction of the velocity, and the length of the arrow means the magnitude of the velocity.At any certain time, if agents form lattice, then blue lines are added between these agents.These following pictures show the simulation process of FMOA flocking with avoiding obstacles for these agents.
As Figure 6 shows, we can know that agents can effectively avoid obstacles and form flocking toward the direction of destination point.Besides, in order to test the performance of the proposed FMOA algorithm, we tested its flocking path and compared it with Olfati-Saber algorithm.Flocking path length means the total length of paths that all of the agents move from the initial status to the final flocking status after avoiding all obstacles.For the flocking path, the less its value is, the less time the agents spend to reach the destination point with the proposed FMOA algorithm, since these agents have the same magnitude of velocity.In addition, the time complexity of the proposed FMOA algorithm is still ( 2 ), which is the same as that of Olfati-Saber algorithm.These experiments are executed with different obstacles and parameter settings.As the experiment results of Figure 7 show, the proposed FMOA algorithm has better efficiency than Olfati-Saber algorithm.

The Application of the Proposed FMOA Algorithm
Nowadays, there are many researchers focusing on the research about localization of quadrotor helicopters.Compared with other solutions used to locate the positions of quadrotor helicopters by GPS, laser finder range, and ultrawideband, this paper innovatively constructed smart indoor environment by deploying WSN nodes.

Current Techniques about Localization of Quadrotor Helicopters
(a) Laser Range Finder.Abraham Bachrach and others in MIT focus on the research on embedding laser range finder to the hardware platform of quadrotor helicopter, so as to conduct localization of the quadrotor helicopter and explore unknown area.For example, one of their publications [15] focused on the implementation of exploring unknown environment by embedding laser range finder to the quadrotor helicopter.
(b) Ultra-Wideband (UWB) Signal.In order to solve the localization of quadrotor helicopter in indoor environment, Damien B and others in MIT propose to deploy beacons in the test environment.One of their papers [16] targets the algorithm of using some known beacons (at least three beacons are needed) to accurately locate the positions of unknown agents in the indoor environment.
(c) Global Position System (GPS).Ruijie He and others in MIT have conducted research of embedding GPS devices into the hardware platform of quadrotor helicopter, so as to adopt the quadrotor helicopter to implement object tracking and path planning, and so forth.One of their publications [17] focuses on the design of light-weight quadrotor helicopter and implementation of tracking objects.In fact, GPS can work well outdoors, but in the indoor environment, it usually cannot be used to implement the localization.

The Construction of Smart Environment.
In order to locate the position of the quadrotor helicopter in indoor environment, wireless sensor network (WSN) nodes are deployed in the smart environment.Accordingly, the wireless communication is enabled for each quadrotor helicopter so as to form a WSN node, so quadrotor helicopters can communicate with the WSN nodes deployed in the smart environment.
Based on the analysis of localization error [18], this paper proposes a fast localization algorithm as follows.
Step 1. Calculate the distance between two nodes (including all quadrotor helicopters).
Step 2. Unknown nodes send broadcast packet periodically, and each packet includes the information format {ID,   , (, , )}, where ID is the identification of the unknown node,   is the time when the reference node sends the packet, and (, , ) is the coordinate value of an unknown node.
Step 3. Reference node  receives the localization packet and then calculates the distance   between certain reference node  and the unknown node.
Step 4. For these  packets, according to the  4   set, the system performs the following judgments repeatedly.
(a) Use cos  , = ( 2  +  2  −  2  )/2    to calculate the included angle formed by the unknown node and any two reference nodes (for the set composed of the four ones), where   and   represent the distances between the unknown node and the two reference nodes, respectively, and   is the distance between the two reference nodes.Choose the reference nodes whose six included angles are all within the range [70.5 ∘ − , 70.5 ∘ + ] and store them in the set ; then,  = +1 (the initial value of  is 0), and return to Step 4; otherwise, go to the next step.
(b) While both of the following conditions are satisfied, then  =  + 1. (1) The included angle formed by any two reference nodes (for the set composed of the three reference nodes) and the unknown node (vertex of the included angle) is within the range [/3 − , /3 + ], where  is the angle threshold.(2) The line connected by the fourth reference node and the unknown node and the plane formed by the other three reference nodes will form an included angle.When the included angle is within the range [/2 − , /2 + ], where  is the angle threshold, then return to Step 4; otherwise, go to the next step.
Step 5. Finally, calculate all of the elements in the set , and obtain their average value  avg .Then,  avg is the estimated position of the unknown node.

The Proposed Algorithm Used in Formation Flying of
Quadrotor Helicopters.In order to apply the proposed algorithm (FMOA), ArduCopters are used as the quadrotor helicopter platform in the constructed smart indoor environment.Real-time data and images are transmitted between ArduCopters and remote PC.Video frames are sent to remote PC by Video Transmitter Kit; accordingly, the process (such as flight attitude, navigation, and obstacles avoidance) is handled in remote PC [19][20][21][22][23][24][25][26], and then flight commands are sent to ArduCopters through Radio Telemetry Kit [27].The workflow of the whole quadrotor helicopter system is shown as in Figure 8.
The algorithm of quadrotor helicopters in the same height is presented as follows.
Step 1. Quadrotor helicopters all hover in the smart room.
Step 2. Input variables of  and  are sent by remote PC.
Step 3. Choose a front quadrotor helicopter, so as to make sure that all other quadrotor helicopters reach the anticipated formation under the shortest path.
Step 4. Choose other two quadrotor helicopters near the front one as the second layer ones.
Step 5.The other three quadrotor helicopters are processed as the third layer ones.
Step 6. Quad-rotor helicopters obtain the coordinates of their current poison by the WSN nodes, and send these coordinates to remote PC.
Step 7. Remote PC sends calculated destination point to each quadrotor helicopter by WSN nodes.
Step 8.Each quadrotor helicopter flies to the final destination position by the proposed FMOA algorithm.
(b) Quadrotor Helicopters in the Different Height.From the analysis of Figure 9(a), while the height is  for the three layers of these quadrotor helicopters and the height of the highest quadrotor helicopter is  0 , then the destination points for ( 1 ,  1 ,  1 ), ( 2 ,  2 ,  2 ), ( 3 ,  3 ,  3 ), ( 4 ,  4 ,  4 ), ( 5 ,  5 ,  5 ), and ( 6 ,  6 ,  6 ) are calculated by the input variables  0 , ,  1 , and  2 .This destination information is very important for each helicopter flying from current position to these positions, respectively.After obtaining all of the positions of quadrotor helicopter, their formation control is conducted by the proposed FMOA algorithm (see Figure 10).
The algorithm of quadrotor helicopters in the same height is presented as follows.
Step 1. Quadrotor helicopters all hover in the smart room.
Step 2. Input variables of  0 , ,  1 , and  2 are sent by remote PC.
Step 3. Choose a front quadrotor helicopter with the height  0 , so as to make sure that all other quadrotor helicopters reach the anticipated formation under the shortest path.
Step 4. Choose other two quadrotor helicopters near the front one as the second layer ones with the height  0 − .
Step 5.The other three quadrotor helicopters are processed as the third layer ones with the height  0 − 2.
Step 6. Quad-rotor helicopters obtain the coordinates of their current poison by the WSN nodes, and send these coordinates to remote PC.
Step 7. Remote PC sends calculated destination point to each quadrotor helicopter by WSN nodes.
Step 8.Each quadrotor helicopter flies to the final destination position by the proposed FMOA algorithm.
The proposed FMOA algorithm can successfully conduct the formation flying of these quadrotor helicopters.As to the

𝛽 2 ∑
∈    , ()( p, −   ) while calculating the    will block the move path while  agent is moving away from obstacle.(b) It only obtains the velocity matching between  agent

Figure 2 :
Figure 2: The description of agent and dangerous area.

Figure 3 :Figure 4 :
Figure 3: The relative position relationship between agent and obstacle.
2,  ]  is a positive constant, and  = 1, 2, V = , , .(  ,   ) means the position and velocity of  agent, respectively.Vectors  , and n, are the direction vectors used to describe the relation of interaction for these agents, and they are given by 2.