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

Authors: Jialiang Wang; Hai Zhao; Yuanguo Bi; Shiliang Shao; Qian Liu; Xingchi Chen; Ruofan Zeng; Yu Wang; Le Ha
Journal: Journal of Applied Mathematics
 (2014)

---

## Abstract

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. Compared with other technologies to perform the localization of quad-rotor helicopter, this paper innovatively constructs a smart environment by deploying some wireless 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.


---

## Body

## 1. 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∥
q
j
-
q
i
∥
=
d, ∀
i
,
j
∈
N
f, where q
j and q
i represent the positions of agents j and i, respectively, and N
f denotes the set of all of the agents, these agents form a lattice as shown in Figure 1(a); then, d is the goal distance (the length of side for the lattice). However, if the configurations of agents satisfy the constraint that -
δ
≤
∥
q
j
-
q
i
∥
-
d
≤
δ,  ∀
i
,
j
∈
N
f, 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].Figure 1
Architecture of lattice.


## 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(1)
u
i
=
u
i
α
+
u
i
β
+
u
i
γ
,
where α, β, and γ represent three different agents and u
i
α, u
i
β, and u
i
γ are the interaction mechanisms for the agent set named (
α
,
α
)
,
(
α
,
β
), and (
α
,
γ
), respectively, and u
i
γ is also the item of feedback control.The calculation of the three input parameters is given by(2)
u
i
α
=
c
1
α
∑
j
∈
N
i
α
φ
α
(
∥
q
j
-
q
i
∥
σ
)
n
i
,
j
+
c
2
α
∑
j
∈
N
i
α
a
i
j
(
q
)
(
p
j
-
p
i
)
,
u
i
β
=
c
1
β
∑
j
∈
N
i
β
φ
β
(
∥
q
^
i
,
k
-
q
i
∥
σ
)
n
^
i
,
k
+
c
2
β
∑
j
∈
N
i
β
b
i
,
k
(
q
)
(
p
^
i
,
k
-
p
i
)
,
u
i
γ
=
-
c
1
γ
σ
1
(
q
i
-
q
γ
)
-
c
2
γ
(
p
i
-
p
r
)
,
where σ
1
(
z
)
=
z
/
1
+
∥
z
∥
2, c
η
ν is a positive constant, and η
=
1
,
2, v
=
α
,
β
,
γ. (
q
r
,
p
r
) means the position and velocity of γ agent, respectively. Vectors n
i
,
j and n
^
i
,
k are the direction vectors used to describe the relation of interaction for these agents, and they are given by
(3)
n
i
j
=
q
j
-
q
i
1
+
ε
∥
q
j
-
q
i
∥
2
,
n
^
i
,
k
=
q
^
i
,
k
-
q
i
1
+
ε
∥
q
^
i
,
k
-
q
i
∥
2
.Supposing an obstacleO
k whose radius and center are R
k and y
k, respectively, and that there is an α agent whose status is (
q
i
,
p
i
), then the position q
^
i
,
k and the velocity p
^
i
,
k of β spherical agent (
(
i
,
k
)
∈
ν
α
×
ν
β
) are, respectively, defined by
(4)
q
^
i
,
k
=
μ
q
i
+
(
1
-
μ
)
y
k
,
p
^
i
,
k
=
μ
P
p
i
,
where μ
=
R
k
/
∥
q
i
-
y
k
∥, a
k
=
(
q
i
-
y
k
)
/
∥
q
i
-
y
k
∥,  and P
=
I
-
a
k
a
k
T.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 N
i
β
=
{
j
∈
v
β
:
∥
q
^
i
,
k
-
q
i
∥
<
r
′
} is met. Besides, the value of c
1
β and c
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 c
2
β
∑
j
∈
N
i
β
b
i
,
k
(
q
)
(
p
^
i
,
k
-
p
i
) while calculating the u
i
β will block the move path while α agent is moving away from obstacle.
(b)
It only obtains the velocity matching betweenα agent 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.


## 3. 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 d, then it means that the flocking has formed.If the action range ofα agents is r, then the dangerous radius of an obstacle is d
dan
=
R
k
+
r
_
safe, where R
k denotes the radius of obstacle and r
_
safe represents the predefined safe region of obstacle. Besides, using d
q 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 d
t
1 and d
t
2, respectively.Figure 2
The description of agent and dangerous area.After agents detect the dangerous region of obstacle (whiled
′
≤
r), the system judges the direction of its velocity; if it moves within the region of obstacle (while det
⁡
⁡
[
d
t
1
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
p
i
]
>
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
⁡
⁡
[
d
t
1
,
p
i
], det
⁡
⁡
[
d
t
2
,
p
i
], and det
⁡
⁡
[
d
q
,
p
], which are defined by
(5)
det
⁡
⁡
[
d
q
,
p
]
=
det
⁡
⁡
[
d
x
p
x
d
y
p
y
]
=
d
x
p
y
-
d
y
p
x
=
[
d
x
,
d
y
]
[
p
y
-
p
x
]
,
where [
d
x
,
d
y
]
[
p
y
-
p
x
]
=
d
q
T; thus, T
=
[
p
y
-
p
x
].So the above judgment can be simplified to judge its value. If vectord
q
T
≥
0, it means that the velocity vector is above the median vector as Figure 3(a) shows; oppositely, if vector d
q
T
<
0, it represents that the velocity vector is under the median vector as Figure 3(b) shows.The relative position relationship between agent and obstacle.
(a)
(b)


### 3.1. 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 p
β
1, which makes it avoid obstacle with the least deviation:
(6)
p
α
1
=
{
∥
p
i
∥
σ
·
(
q
i
-
q
γ
)
∥
(
q
i
-
q
γ
)
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
1
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
q
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
≥
0
}
∥
p
i
∥
σ
·
(
q
i
-
q
γ
)
∥
(
q
i
-
q
γ
)
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
q
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
≤
0
}
.Different conditions between agents while flocking has not formed.
(a)
(b)
(c)
(d)(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
(7)
p
α
2
=
{
∥
p
i
∥
σ
·
d
t
1
∥
d
t
1
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
1
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
q
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
>
0
}
∥
p
i
∥
σ
·
d
t
2
∥
d
t
2
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
q
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
>
0
}
.(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
(8)
p
α
3
=
{
∥
p
i
∥
σ
·
d
t
1
∥
d
t
1
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
1
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
>
0
}
∥
p
i
∥
σ
·
d
t
2
∥
d
t
2
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
2
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
>
0
}
.


### 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
(9)
p
α
4
=
{
∥
p
i
∥
σ
·
d
t
1
∥
d
t
1
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
1
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
q
,
p
i
]
>
0
}
∥
p
i
∥
σ
·
d
t
2
∥
d
t
2
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
q
,
p
i
]

<

0
∧
det
⁡
⁡
[
d
t
2
,
p
i
]

>

0
}
.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 isd, 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.Figure 5
The workflow of FMOA algorithm.


## 3.1. 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 p
β
1, which makes it avoid obstacle with the least deviation:
(6)
p
α
1
=
{
∥
p
i
∥
σ
·
(
q
i
-
q
γ
)
∥
(
q
i
-
q
γ
)
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
1
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
q
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
≥
0
}
∥
p
i
∥
σ
·
(
q
i
-
q
γ
)
∥
(
q
i
-
q
γ
)
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
q
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
≤
0
}
.Different conditions between agents while flocking has not formed.
(a)
(b)
(c)
(d)(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
(7)
p
α
2
=
{
∥
p
i
∥
σ
·
d
t
1
∥
d
t
1
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
1
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
q
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
>
0
}
∥
p
i
∥
σ
·
d
t
2
∥
d
t
2
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
q
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
>
0
}
.(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
(8)
p
α
3
=
{
∥
p
i
∥
σ
·
d
t
1
∥
d
t
1
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
1
,
p
i
]
>
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
>
0
}
∥
p
i
∥
σ
·
d
t
2
∥
d
t
2
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
2
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
t
1
,
d
γ
]
<
0
∧
det
⁡
⁡
[
d
t
2
,
d
γ
]
>
0
}
.


## 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
(9)
p
α
4
=
{
∥
p
i
∥
σ
·
d
t
1
∥
d
t
1
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
t
1
,
p
i
]
<
0
∧
det
⁡
⁡
[
d
q
,
p
i
]
>
0
}
∥
p
i
∥
σ
·
d
t
2
∥
d
t
2
∥
σ
,
{
d
′
≤
r
}
∩
{
det
⁡
⁡
[
d
q
,
p
i
]

<

0
∧
det
⁡
⁡
[
d
t
2
,
p
i
]

>

0
}
.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 isd, 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.Figure 5
The workflow of FMOA algorithm.


## 4. 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 Figure6(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.Simulation process of the proposed FMOA algorithm with avoiding obstacles.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)
(j)
(k)
(l)
(m)
(n)
(o)
(p)
(q)
(r)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 Figure6 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 O
(
n
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.Figure 7
Experimental results about flocking path.


## 5. 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 ultra-wideband, this paper innovatively constructed smart indoor environment by deploying WSN nodes.


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


### 5.2. 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
,
T
s
,
(
a
,
b
,
c
)
}, where ID is the identification of the unknown node, T
s is the time when the reference node sends the packet, and (
a
,
b
,
c
) is the coordinate value of an unknown node.Step
3. Reference node i receives the localization packet and then calculates the distance L
i between certain reference node i and the unknown node.Step
4. For these N packets, according to the C
N
4 set, the system performs the following judgments repeatedly.(a) Usecos
⁡
⁡
a
i
,
j
=
(
L
i
2
+
L
j
2
-
L
i
j
2
)
/
2
L
i
L
j to calculate the included angle formed by the unknown node and any two reference nodes (for the set composed of the four ones), where L
i and L
j represent the distances between the unknown node and the two reference nodes, respectively, and L
i
j 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 H; then, K
=
K
+
1 (the initial value of K is 0), and return to Step 4; otherwise, go to the next step.(b) While both of the following conditions are satisfied, thenK
=
K
+
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 H, and obtain their average value H
avg. Then, H
avg is the estimated position of the unknown node.


### 5.3. 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–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.Figure 8
Workflow of the whole quadrotor helicopter system.(a)Quadrotor Helicopters in the Same Height. From the analysis of Figure 9(a), the values of destination points (
x
1
,
y
1
), (
x
2
,
y
2
), (
x
3
,
y
3
), (
x
4
,
y
4
), (
x
5
,
y
5
), and (
x
6
,
y
6
) are (
0
,
L
)  (
-
L
·
tan
⁡
β
,
0
), (
L
·
tan
⁡
β
,
0
), (
-
2
L
·
tan
⁡
β
,
0
), (
0
,
-
L), and (
2
L
·
tan
β
,
0
), respectively. These values are very important for each helicopter flying from the current position to these destination positions, respectively. There are totally 23 WSN nodes placed in the smart environment as shown in Figure 9(b). After obtaining the position set of all of the quadrotor helicopters, their formation flying is controlled by the proposed FMOA algorithm.The model of formation control for quadrotor helicopters under the condition of different height.
(a)
Mathematical model
(b)
Practical application in smart environmentThe 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 L and c 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 H for the three layers of these quadrotor helicopters and the height of the highest quadrotor helicopter is H
0, then the destination points for (
x
1
,
y
1
,
z
1
), (
x
2
,
y
2
,
z
2
), (
x
3
,
y
3
,
z
3
), (
x
4
,
y
4
,
z
4
), (
x
5
,
y
5
,
z
5
), and (
x
6
,
y
6
,
z
6
) are calculated by the input variables H
0, H, L
1, and L
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 model of formation control for quadrotor helicopters under the condition of different height.
(a)
Mathematical model
(b)
Practical application in smart environmentThe 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 H
0, H, L
1, and L
2 are sent by remote PC.Step
3. Choose a front quadrotor helicopter with the height H
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 H
0
-
H.Step
5. The other three quadrotor helicopters are processed as the third layer ones with the height H
0
-
2
H.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 process of obstacle avoidance, the methods of optical flow and image entropy are used for these quadrotor helicopters with single camera. For the future work, we will focus on the research about the believability of the emergent behaviour [28].


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


## 5.2. 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
,
T
s
,
(
a
,
b
,
c
)
}, where ID is the identification of the unknown node, T
s is the time when the reference node sends the packet, and (
a
,
b
,
c
) is the coordinate value of an unknown node.Step
3. Reference node i receives the localization packet and then calculates the distance L
i between certain reference node i and the unknown node.Step
4. For these N packets, according to the C
N
4 set, the system performs the following judgments repeatedly.(a) Usecos
⁡
⁡
a
i
,
j
=
(
L
i
2
+
L
j
2
-
L
i
j
2
)
/
2
L
i
L
j to calculate the included angle formed by the unknown node and any two reference nodes (for the set composed of the four ones), where L
i and L
j represent the distances between the unknown node and the two reference nodes, respectively, and L
i
j 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 H; then, K
=
K
+
1 (the initial value of K is 0), and return to Step 4; otherwise, go to the next step.(b) While both of the following conditions are satisfied, thenK
=
K
+
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 H, and obtain their average value H
avg. Then, H
avg is the estimated position of the unknown node.


## 5.3. 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–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.Figure 8
Workflow of the whole quadrotor helicopter system.(a)Quadrotor Helicopters in the Same Height. From the analysis of Figure 9(a), the values of destination points (
x
1
,
y
1
), (
x
2
,
y
2
), (
x
3
,
y
3
), (
x
4
,
y
4
), (
x
5
,
y
5
), and (
x
6
,
y
6
) are (
0
,
L
)  (
-
L
·
tan
⁡
β
,
0
), (
L
·
tan
⁡
β
,
0
), (
-
2
L
·
tan
⁡
β
,
0
), (
0
,
-
L), and (
2
L
·
tan
β
,
0
), respectively. These values are very important for each helicopter flying from the current position to these destination positions, respectively. There are totally 23 WSN nodes placed in the smart environment as shown in Figure 9(b). After obtaining the position set of all of the quadrotor helicopters, their formation flying is controlled by the proposed FMOA algorithm.The model of formation control for quadrotor helicopters under the condition of different height.
(a)
Mathematical model
(b)
Practical application in smart environmentThe 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 L and c 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 H for the three layers of these quadrotor helicopters and the height of the highest quadrotor helicopter is H
0, then the destination points for (
x
1
,
y
1
,
z
1
), (
x
2
,
y
2
,
z
2
), (
x
3
,
y
3
,
z
3
), (
x
4
,
y
4
,
z
4
), (
x
5
,
y
5
,
z
5
), and (
x
6
,
y
6
,
z
6
) are calculated by the input variables H
0, H, L
1, and L
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 model of formation control for quadrotor helicopters under the condition of different height.
(a)
Mathematical model
(b)
Practical application in smart environmentThe 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 H
0, H, L
1, and L
2 are sent by remote PC.Step
3. Choose a front quadrotor helicopter with the height H
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 H
0
-
H.Step
5. The other three quadrotor helicopters are processed as the third layer ones with the height H
0
-
2
H.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 process of obstacle avoidance, the methods of optical flow and image entropy are used for these quadrotor helicopters with single camera. For the future work, we will focus on the research about the believability of the emergent behaviour [28].


## 6. Conclusion

On the base of the existing Olfati-Saber algorithm, this paper proposed a fast multiagent obstacle avoidance algorithm to make the multiagent system effectively avoid obstacles and fast form flocking towards the direction of goal target. One of the contributions of this paper is that the direction of agent velocity is comprehensively considered while avoiding obstacles. This paper analyzed the proposed algorithm theoretically under the conditions of whether the flocking is formed, and the subsequent experimental results show that the performance of FMOA is better than Olfati-Saber algorithm in terms of flocking path. In addition, this paper constructed a smart environment by innovatively deploying WSN nodes in indoor environment to locate the positions of all of the quadrotor helicopters; the proposed FMOA algorithm can successfully conduct the formation flying of quadrotor helicopters.



---

*Source: 659805-2014-06-24.xml*