Formation of Unmanned Vehicles

with Collision Avoidance Capabilities

Diogo Ruivodiogo.ruivo@tecnico.ulisboa.pt

Instituto Superior Tecnico, Universidade de Lisboa, Lisboa, Portugal

November 2015

Abstract

This paper presents a new motion control framework for the collision free formation of holonomicautonomous vehicles. The proposed framework is a modular one, comprehending the navigation,collision avoidance and vehicle’s formation tasks. Two methodologies for collision avoidance areconsidered, one using potential flow, the other velocity space clipping. These methods go a step furtherfrom a simple obstacle avoidance solution, accomplishing an agreement (deconfliction) between thedesired path and existing obstacles. Three types of formations are taken into consideration: looseformation without leader, loose formation with leader, and rigid formation with leader. The proposedsubsystems are first tested and validated in two dimensional holonomic vehicle models. The completeframework performance is demonstrated with a three dimensional scenario where five quadcopters information flight avoid existing obstacles.

Keywords: Collision Avoidance, Vehicles Formation, Potential Flow, Navigation, Holonomic Vehicle,Quadcopter

1. Introduction

New applications for autonomous vehicles are start-ing to appear given their increasing popularity out-side the research community. Tasks such as surveil-lance, monitoring, reconnaissance, search and res-cue, space and extraterrestrial exploration, to namebut a few, require the vehicle to autonomously fol-low a trajectory in an unknown or changing envi-ronment, while avoiding existing obstacles. Morecomplex scenarios might even require a cooperationbetween the vehicles to increase the autonomoussystem efficiency or area covered.

Collision avoidance is an important field of re-search within robotics. Several approaches havebeen proposed, from off-line path planning, to on-line path replanning. Brock and Khatib [1] presenta generalized solution on collision avoidance consid-ering a global dynamic window approach. A decen-tralized deconfliction solution for unmanned aerialvehicles (UAV’s) is introduced in [2–4]. Casqueiroet al. [5] use a velocity space clipping method to im-plement an obstacle avoidance system in a groundholonomic vehicle.

Although the formation control problem is morerecent, it has also attracted the attention of thescientific community. Tanner and Kumar [6] intro-duced a simple decentralized approach to deal with

various robots, while De Gennaro and Jadbabaie [7]apply a formation control for a cooperative multi-agent system using decentralized navigation func-tions.

In recent years, different solutions have been di-rectly implemented on quadcopter platforms. [8]presents a decentralized concept for outdoor flock-ing and formation flight, and [9] presents a decen-tralized approach with obstacle avoidance.

This work presents a modular framework to con-trol the different tasks of a holonomic vehicle,namely navigation, collision avoidance and groupformation. This framework is intended to be flex-ible to allow the adjustment of tasks, and expand-able for more tasks to be introduced if needed.

2. Motion Control Framework

The Motion Control framework is a high-level con-trol framework designed to accommodate and in-tegrate different tasks required of an autonomousvehicle during a single mission. Its design has amodular approach, where each task is managed bya different system. The resulting commands definedby each subsystem are then merged in an Integra-tion system. This approach allows to change eachsubsystem individually as well as to add other sub-systems, without compromising the already estab-

1

Figure 1: Motion Control Framework Diagram

lished ones, since only the Integration system needsto be redefined to accommodate the changes.

This framework assumes that the vehicle whereit is implemented is stable and has a velocity andheading control, corresponding to low and mid-levelcontrols, respectively. It is also assumed that thenecessary information needed for the system’s oper-ations (vehicle and environment states) is available.

In this work, the Motion Control framework pro-vides the necessary tools for a vehicle to follow adesired path while avoiding obstacles. The desiredpath may be constrained by a group formation re-quest. These tasks are managed by a NavigationSystem, an Anti-Collision system and a Group For-mation system, respectively. The proposed frame-work is described in figure 1.

In the following, the Navigation System is pre-sented, defining the reference type of motions thatcan be accomplished and how they are managed.The Anti-Collision system is presented in section 3,and the Group Formation system described in sec-tion 4. The Integration system is described withineach previous section, given its dependance on itsinput systems.

2.1. Navigation System

The Navigation system takes charge of basic motiontasks, such as path following or waypoint tracking.It requires information about the vehicle’s currentposition, ξ, as well as the reference mission.

The Navigation system provides the Integrationsystem with the reference velocity direction, DN ,and magnitude, SN and, in some scenarios, theheading, HN necessary to follow the reference mo-tion, working as a base for other systems.

Reference Mission The reference mission re-quired by the navigation system corresponds to aset of instructions on position and heading, withrestricted velocities. The predefined trajectory canbe a path to follow, a group of waypoints or a mixof both. While a path is a continuous line segmentin space, defined by nodes Ni, waypoints are pointsthat the vehicle should simply pass through or stopover, depending on the mission purpose.

For the heading instructions, and for an holo-nomic vehicle, the heading variation can be definedseparately from the position motion. It can be fixed,set to make the vehicle face the next waypoint,match the path direction, or match the vehicle lin-ear velocity.

Velocity Direction The velocity direction, DN ,is defined by a processed navigation function, a sim-ulated potential field that is constructed aroundthe mission. This method as already proven itsworth [2–4, 6, 10]. Here, a simpler, and computa-tionally less demanding, interpretation is used, asthe vehicle only needs the negative gradient of thepotential field, named potential flow, to find its way.

The potential flow can be directly related to thedesired velocity of the vehicle, taking from there thevelocity direction. A representation of a path andwaypoint navigation function and potential flow canbe seen in figures 2 and 3, respectively.

21

y0

-16

4

x

2

0

0

2

1

(a) Path

-2

0

x22

0

y

0

1

2

-2

(b) Waypoint

Figure 2: Navigation Function Example

x-1 0 1 2 3 4 5 6

y

-1

0

1

2

(a) Path

x-2 0 2

y

-2

-1

0

1

2

(b) Waypoint

Figure 3: Potential Flow Function Example

There are two possibilities to proceed with thepotential flow calculations depending if the prede-fined mission is defined by a path or waypoints. Ineither case, DN is calculated as an unit vector thatis later given weight, or magnitude of importance,

2

WN , allowing it to be considered in other vehicletasks by the Integration system.

For the path mission case, a potential flowsmoothed in the path nodes, with a radius rp, isused, instead of the potential flow extracted directlyfrom the navigation function, in order to help di-rection changes in the path. The velocity direc-tion, DN , is obtained calculating the potential flowfield with a compromise between the path direc-tion, Dap, and the path following direction, Dpf .The closest point on path, Pop, is used to find thesedirections. With that point, the distance to path,dtp, and distance to path end, dpe, are also found.These distances are used as a proportion to mergethe path direction Dap, and the path following di-rection, Dpf , and later to calculate the referencespeed. The path direction Dap is obtained from

Dap =Pop − ξ‖Pop − ξ‖

(1)

The path following direction depends where theclosest point on path, Pop, is on the path. Using thedirection of segment i, diri. If the distance to thefirst node on segment i, Ni, is less than rP , then

Dpf =

(1

2− ‖Pop −Ni‖

2rP

)diri−1

+

(1

2+‖Pop −Ni‖

2rP

)diri (2)

or, if the distance to the last node on segment i,Ni+1, is less than rP , then

Dpf =

(1

2+‖Pop −Ni+1‖

2rP

)diri

+

(1

2− ‖Pop −Ni+1‖

2rP

)diri+1 (3)

otherwise,Dpf = diri (4)

To obtain the velocity direction, the importanceof each previously computed direction is taken intoaccount by evaluating the distance to the path

DN = Dpf + (Dap −Dpf )f(dtp) (5)

where f is a sigmoid and 0 ≤ f ≤ 1. In this way, theapproach direction is more relevant as the vehicle isfarther off course and the path follow direction takeshigher importance when the vehicle is on track.

When dealing with a waypoints reference mission,only the approach direction to the current waypointis needed,

DN =WP − ξ‖WP − ξ‖

(6)

The waypoints are sequentially followed by assign-ing the next waypoint as the vehicle approaches thecurrent one.

Speed The desired speed, SN , for the currentstate of the vehicle on the trajectory is given by afunction of the maximum desired speed, Smax, andthe distances to path and to path end, eq. (7a),or distance to stopping waypoint, eq. (7b). Forthe passing waypoint case, Smax is simply given byeq. (7c).

S = Vmissionf(k(dtp + dpe)) (7a)

S = Vmissionf(k(dwp)) (7b)

S = Vmission (7c)

where f is a sigmoid and 0 ≤ f ≤ 1, effectivelybounding the reference speed. k is a gain thatmakes the desired speed drop faster or slower as thedistance reaches zero. With the function f depen-dent of the distance, the vehicle speed is reduced tozero, coming to a full stop, as it reaches the interestpoint.

Heading In a 2D environment, the vehicle atti-tude corresponds to its heading, HN , and to theyaw value ψ. In a 3D environment, the vehicle atti-tude is defined by the three Euler angles, φ, θ andψ. Most of the times, the heading direction is givenby the path’s direction or having the vehicle turnedtoward the next waypoint. This only excludes themissions where the heading is fixed, Hf , and statedon the mission design. In a 3D environment, wherea second direction needs to be provided to fix alldegrees of freedom, the z direction is generally pro-vided.

3. Anti-Collision System

The purpose of the Anti-Collision system is to pre-vent the vehicle from colliding with any obstaclesthat could come into its path. Instead of performinga simple avoidance, this system attempts an agree-ment (deconfliction) between the vehicle’s desiredpath and existing obstacles, reducing the impactcaused by the obstacle’s appearance. This systemworks online with information on the obstacle’s rel-ative position and velocity and information on thevehicle’s desired trajectory.

This section introduces the concept of vehiclesafety. Two different Anti-Collision methods arepresented, evaluated and compared. The firstmethod is based on Potential Flow functions andthe second is based on Velocity Space Clipping.

Vehicle Safety In order to evaluate the vehiclesafety while executing a mission and to define whena safety action should be taken, four safety zonesare defined around the obstacles. Defined by therespective radius, and in decreasing distance to theobstacle, they are observation, RO, warning, RW ,danger, RD, and collision, RC , zones. The vehicle

3

safety is evaluated by its distance to the obstacle,dO = ‖ξrO‖, where ξrO is the obstacle relative posi-tion.

The observation zone is where obstacles start tobe taken into account by the Anti-Collision system.When in the warning zone, the vehicle must alreadybe taking actions, and in the danger zone the Anti-Collision system takes priority over all other Motionsystems. To ensure a safe motion, the vehicle mustnever enter the collision zone.

3.1. Potential Flow Function Method

To calculate the potential flow value of deconflic-tion, PD, three components are used. The first onedeals with avoidance, keeping the obstacle at a dis-tance. The second component is a swirl in the po-tential flow function that makes a deconfliction be-tween the obstacle and the vehicle movements. Fi-nally, the third is a variable importance level thattakes into account the vehicle desired direction ofmotion and the relative velocity of the object.

The avoidance uses a navigation function sim-ilarly as the Navigation system. The navigationfunction gradient is zero until the obstacle is in thewarning zone, and is at its maximum, Amax, afterit reaches the danger zone . The avoidance poten-tial flow value, A, is a function of the dO dependentpotential flow function:

A =

0, ifDO > RW

AmaxξrODO

RW−DORW−RD

, if DO ≤ RW and DO ≥ RD

AmaxξrODO

, ifDO < RD(8)

A deconfliction is achieved by adding a swirlingmotion to the potential flow, as in works [2–4]. Thisswirl makes the vehicle initiate action sooner, mak-ing it evade the obstacle instead of simply keepinga distance. The effect of the swirl is dependent onthe obstacle distance, dO. It starts in the obser-vation zone and reaches its maximum, Swmax, inthe warning zone . The swirling effect is calculatedfrom

Sw =

0, if dO > RO

SwmaxRO−dORO−RW

ξrO×Swaxis‖ξrO×Swaxis‖

ξrO × Saxis,

if dO ≤ RW and dO ≥ RD

SwmaxξrO×Saxis‖ξrO×Swaxis‖

ξrO × Swaxis,

if dO < RW(9)

where Swaxis is the dynamic swirling axis. Theswirling rotation is made dynamic, since fixing itcould lead the vehicle to take a bigger path. Theswirling axis, Swaxis, is calculated using the normalbetween the direction wanted by the navigation sys-tem, DN , and the obstacle relative velocity, νrO

Swaxis = DN × νrO (10)

In the case both vectors are collinear, an arbitraryaxis is chosen, usually the z-axis. A case that couldcause conflict is if two vehicles with this system en-counter each other. In this case they would bothtry to pass in front the other, resulting in a unde-termined situation. To prevent this situation, theabsolute value of the swirling axis, |Saxis|, is usedwhen the obstacle is known to be moving.

Weight WD is considered in the potential flowvalue of deconfliction to minimize its influence oncethe danger of collision has passed. The weight WD

varies between zero and one, being zero when de-confliction is not needed and one when it is manda-tory. Inside the danger zone, WD takes the valueof one, as a collision may be imminent. Outside ofit, it takes a zero value when the relative speed isgreater than zero, meaning that the distance to theobstacle is increasing. Otherwise, it depends on theangle between ξrO and DN , weighting more whenthe obstacle is right in the vehicle path:

WD =

0, if dO > RD and Sr ≥ 0∣∣∣∣∣arccos

(ξrO·DN‖ξrO‖‖DN‖

)π

∣∣∣∣∣, if dO > RD and Sr < 0

1, if dO ≤ RD(11)

Merging all the mentioned effects results in the com-plete potential flow value of deconfliction

PD = WD(A+ S) (12)

A graphical representation of the potential flowvalue of deconfliction can be seen in fig. 4.

x [m]-1 0 1

y [m

]

-1

0

1

Figure 4: Deconfliction Potential Flow

The integration of the complete Anti-Collisionsystem with other Motion Control systems is donein the Integration system. All obstacles potentialflow values are deconflicted with the direction re-quired by the Navigation system, resulting in a newdirection for the vehicle velocity, D, given by

D =DN +

∑PD

‖DN +∑PD‖

(13)

3.2. Velocity Space Clipping Method

The Velocity Space Clipping [5] is a method wherethe vehicle desired velocity is evaluated against anallowed space. The allowed velocity space is built

4

with information about the vehicle physical con-straints and surrounding obstacles position. A 2Dholonomic vehicle which has Vmax as its maximumvelocity in any direction has an initial allowed ve-locity space as seen in fig. 5. Two clips are made per

vx, m/s

-Vmax 0 Vmax

v y, m/s

-Vmax

0

Vmax

Figure 5: Vehicle velocity space

obstacle. The first is to bound the speed in the ob-stacle direction and the second to make the vehiclemove around the obstacle, creating a deconfliction.The first clip is done by reducing the allowed ve-locity projected in the obstacle direction, as it getscloser.

The second clip is made to achieve an effect sim-ilar to the swirling motion in the potential flowmethod. The direction of this clip is dependent onthe vehicle desired velocity direction and the ob-stacle relative position when it is first encountered.There is no clip before the vehicle reaches RO. Thenit starts at a zero value and only reaches half Vmax.

The clips made to a 2D holonomic vehicle velocityspace, with an obstacle closing in from North-East,are represented in fig. 6.

8x [m/s]

-Vmax 0 Vmax

8y [m

/s]

-Vmax

0

Vmax

(a) dO = RO

8x [m/s]

-Vmax 0 Vmax

8y [m

/s]

-Vmax

0

Vmax

(b) RO > dO >RW

8x [m/s]

-Vmax 0 Vmax

8y [m

/s]

-Vmax

0

Vmax

(c) dO = RW

Figure 6: 2D holonomic vehicle velocity space clip-ping

The implementation of this improved methodwith other Motion Control subsystems is done inthe Integration System. After all the cuts to thevelocity space have been made, the desired velocitydefined in other systems is evaluated to be insidethe allowed velocity space. If outside it, the desiredvelocity is changed to the closest value inside it (seefig. 7).

3.3. Anti-Collision Demonstration

This section demonstrates the performance of theAnti-Collision system with the different methodspresented. A representative test scenario was de-signed to evaluate the behavior of a ground holo-nomic vehicle in different environments.

vx, m/s

-Vmax 0 Vmax

v y, m/s

-Vmax

0

Vmax

(a) Inside

vx, m/s

-Vmax 0 Vmax

v y, m/s

-Vmax

0

Vmax

(b) Outside

vx, m/s

-Vmax 0 Vmax

v y, m/s

-Vmax

0

Vmax

(c) Outside

Figure 7: Space clipping. Desired velocity relativeto admissible space. Blue arrow: desired velocity;red arrow: new velocity.

Test Scenario The test scenario designed to eval-uate the Anti-Collision system has multiple staticobstacles.In this scenario, the holonomic ground ve-hicle must go from point (0,0)m to point (10,0)mwhile avoiding existing obstacles.

The vehicle speed is set to 1m/s. Obstacle safetyzones have diameters RO = 2m, RW = 1.5m,RD = 1m and RC = 0.5m. The Anti-Collisionsystem performance is evaluated through the maxi-mum distance to path, dtp, where minimal is better,and distance to obstacles, dO, where maximum isbetter.

Results Figure 8 demonstrates the Motion Con-trol system successful performance when the po-tential flow function method is used in the Anti-Collision system. The system manages to keep thevehicle at a safe distance from the obstacles, neverentering their danger zone, and keeping the vehicleclose to the path without great loss of velocity.

x [m]0 1 2 3 4 5 6 7 8 9 10

y [m

]

-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

(a) Top down view

8 [m

/s]

-1

0

1

xy

t [s]

9 [m

]

-20

0

20

t [s]0 2 4 6 8 10

A [º

]

-1

0

1

(b) Vehicle velocity, posi-tion and heading

dO

[m]

Rc

Rd

Rw

Ro

Obstacle 1Obstacle 2Obstacle 3

t [s]0 2 4 6 8 10

dtp

[m]

0

0.5

1

1.5

2

(c) Vehicle distance topath and distance to ob-stacle

Figure 8: Potential flow function method perfor-mance

Figure 9 depicts the Motion Control system per-formance when the Anti-Collision system uses thevelocity space clipping. The system again guaran-tees the vehicle safety. The vehicle never enters

5

the obstacles warning zone, taking the path aroundthem. The downside of this approach is the distancefrom path (see fig. 9(c)), as it presents higher valuesthan with the potential flow method (see fig. 8(c)).

x [m]0 1 2 3 4 5 6 7 8 9 10

y [m

]

-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

(a) Top down view

8 [m

/s]

-1

0

1

xy

t [s]

9 [m

]

-20

0

20

t [s]0 5 10 15

A [º

]

-1

0

1

(b) Vehicle velocity, posi-tion and heading

dO

[m]

Rc

Rd

Rw

Ro

Obstacle 1Obstacle 2Obstacle 3

t [s]0 5 10 15

dtp

[m]

0

0.5

1

1.5

2

(c) Vehicle distance topath and distance to ob-stacle

Figure 9: Velocity space clipping method perfor-mance

The potential flow method keeps a good balancebetween a safe distance from the obstacle and lessdistance from the reference mission. This methodalso works independently of the vehicle type, be itholonomic or non-holonomic.

The velocity clipping method presents very goodresults when dealing with static obstacles, with em-phasis on the vehicle safety. But it fails with movingobstacles. This method proves to be computation-ally expensive, as for each obstacle it encounters itmust do two clips on a space, after which it mustfind if the desired velocity is inside the allowed spaceand, if it is not, find the closest point in the allowedspace.

Comparing both approaches, the potential flowmethod proves to be better in versatility and over-all performance, being also less computationally ex-pensive. The better performance of the potentialflow method may be due to its affinity with theNavigation system, since they are both based onthe same type of navigation functions, making theirintegration easier.

4. Group Formation

The objective of the Group Formation system is tohave a larger number of vehicles working for thesame goal in an orderly fashion. This improves per-formance on certain tasks and creates redundancy,making the overall system more robust.

Two types of formations are considered, a looseformation and a rigid formation. The major differ-ence between the loose and rigid formations is theshape of the formation. While the loose formationhas no shape requirements, in the rigid formationthe shape is pre-established. This also changes theamount of information needed to run the differentformations. For the loose formation, only informa-tion on the closest vehicles is needed to keep a co-hesive group. The rigid formation, on the otherhand, needs the position of every vehicle, in orderto calculate the overall position of the shape.

The velocity associated with this system has amaximum speed alteration per vehicle, SG, so thatthere is room for the vehicle to speed up to catch thegroup and slow down if it is too far ahead. The im-portance of this system relative to the other systemsin the Integration system is given by the weightWF , which is lower than the one given to the Anti-Collision system.

The formations are accomplished using a poten-tial flow function, simplified from a navigation func-tion, similar to those used in works [6] and [7].

This section introduces the methods used to man-age loose and rigid formations, and describes theadaptation of the Integration system to accommo-date this system. A demonstration of the systemperformance follows.

4.1. Loose Formation

In loose formation, the displacement of the vehiclesis not important, since the objective is to keep thegroup together with a safe distance between vehi-cles.

The desired spacing between vehicles is set previ-ously as RG, along with a minimum safe distance,RD, and a maximum search distance, RO. Theminimum safe distance serves to signal the vehi-cle that priority action should be taken to distanceitself from others, while the maximum search dis-tance prompts the vehicle to get closer to vehiclesinside it, up to a certain number. A vehicle does notattempt to keep a fixed distance from every othervehicle. Instead it only cares about up to the sixclosest ones, the maximum stable formation whilekeeping a safe distance. If no vehicle is within themaximum search distance, the search should be ex-panded outside it, to ensure no vehicle is left un-connected. A mechanism is set in place that guar-antees that all active vehicles are connect with eachother, and that they do not form small unconnectedgroups. Or, in the case of a follow-the-leader, thatthey are all connected to the leader.

To apply this system, navigation and potentialflow functions are used to represent the other ve-hicles in space (see fig. 10). The navigation func-tion is like a ditch around the vehicle, trapping the

6

21

0

x [m]-1

-2-2

-1

y [m]

0

1

1.5

0.5

1

2

(a) Vehicle navigationFunction

x [m]-2 -1.5 -1 -0.5 0 0.5 1 1.5 2

y [m

]

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

(b) Vehicle potential flowfunction

Figure 10: Loose formation vehicle management

close ones and keeping them at a distance. Theposition of the vehicle in consideration is PA andthe distance to it is dA = ‖PA − ξ‖. Taking thatinformation into consideration, the velocity neededto rearrange the position between two vehicles, orinteraction velocity, νint, can be calculated from

Vint =

SGf(dA −RG), if dA > RG

−SG RG−dARG−RD

DA, if dA ≤ RG and dA > RD

−SGDA, if dA ≤ RD(14)

where

DA =PA − ξ‖PA − ξ‖

(15)

and f is a sigmoid that respects 0 ≤ f ≤ 1, effec-tively not letting the speed exceed SG.

4.2. Rigid Formation

In rigid formation, the displacement of the vehiclesmatters and is predefined. This shape is alignedwith the motion direction and defined by a set ofnodes that form connected segments.

The Group Formation system receives the refer-ence mission for the leader to follow and the vehiclesmust encounter their place inside it. The formationshape is positioned on space dependent of the leaderposition, placing the leader in a key-point. The for-mation direction must match the leader referencevelocity direction. The vehicles end up filling theformation shape, if it is not too large, with a ho-mogeneous density. This occurs since vehicles areattracted to the formation shape and repelled fromeach other, keeping a minimum distance.

To accomplish the first step, navigation and po-tential flow functions are defined around the forma-tion shape to attract the vehicles (see fig. 11). Inorder for this to be possible, the closest point onthe shape, PS , is encountered and used to calculatea velocity reference, Vshape, needed to guide the ve-hicle onto the formation shape:

Vshape = SGf(‖PS − ξ‖)PS − ξ‖PS − ξ‖

(16)

where f is a sigmoid that respects 0 ≤ f ≤ 1, sothat the speed does not exceed SG.

10.5

x [m]

0-0.5

-1-1y [m]

0

0

0.5

1

1

(a) Formation shape navi-gation function

x [m]-1 -0.5 0 0.5 1

y [m

]

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

(b) Formation shape po-tential flow function

Figure 11: Rigid formation shape management

For the second step to be accomplished, naviga-tion and potential flow functions are also used (seefig. 12), but to perform a repellent effect. The posi-tion of the other vehicles, PA, is used to rearrangethe direction of the reference velocity inside the In-tegration system with the potential flow FA givenby:

FA =

0, if dA > RW

−SGRW−dARW−RD

DA, if dA ≤ RW , dA > RD

−SGDA, if dA ≤ RD

(17)

21

0

x [m]-1

-2-2

-1

y [m]

0

1

1

0.5

0

1.5

2

(a) Vehicle navigationfunction

x [m]-2 -1.5 -1 -0.5 0 0.5 1 1.5 2

y [m

]

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

(b) Vehicle potential flowfunction

Figure 12: Rigid formation vehicle management

4.3. Integration System

In order to make the Group Formation systemwork in conjunction with the Navigation and Anti-Collision systems, some modifications in the Inte-gration system are needed.

The Integration system is still using DN and SN

from the Navigation system. But their value is de-pendent on the type of formation and if there is aleader or not. After merging the existing systems,the resultant direction and speed are DA and SA,respectively.

Loose Formation For a loose formation withleader, SN and DN correspond to the leader posi-tion, otherwise they are dependent of each vehicle.SN and DN are merged with Vint to create the newreference speed, SA, given by:

SA = ‖SNDN +∑

Vint‖ (18)

and the new direction, DA, given by:

DA = SNDN +WF

SG

∑Vint +

∑PD (19)

7

if the Anti-Collision system is implemented with thepotential flow function, or by:

DA = SNDN +WF

SG

∑Vint (20)

if the Anti-Collision system uses velocity space clip-ping.

Rigid Formation For a rigid formation, SN andDN correspond to the leader position. The speedsare merged with

SA = ‖SNDN + Vshape‖ (21)

while the directions are merged with

DA = SNDN +WF

SGVshape +

∑FA +

∑PD (22)

if the Anti-Collision system is implemented with thepotential flow method, or

DA = SNDN +WF

SGVshape +

∑FA (23)

if the Anti-Collision system uses velocity space clip-ping.

4.4. Group Formation Demonstration

This section demonstrates the Group Formationsystem performance. Several vehicles are given apath to follow in formation. Proving that this sys-tem works requires that the vehicles hold formationduring operation while avoiding obstacles. In looseformation, the vehicles must keep a safe distance ofaround 0.75m from their closest partners. In anygiven situation, if that distance drops bellow 0.5mthe vehicles are in danger, and if it drops bellow0.25m it means the vehicles have collided. The ve-hicles starting position is close to the position theywill occupy when in formation. Two scenarios arepresented and compared in the sequence: loose for-mation with leader and rigid formation.

Figure 13 shows the successful mission execu-tion by seven vehicles when in loose formation withleader (vehicle 1). The vehicles manage to followthe path while avoiding the existing obstacles (inthis case, other vehicles), as their closest distance isnever less than 0.25m - see table 1.

As opposed to the loose formation without leadercase, the vehicles are not all trying to be in the path,but rather focused on following the leader. Oncethey get into formation there is only the need tokeep pace with the leader.

When in rigid formation, the mission is againsuccessfully executed (in this case by five vehicles),with the path being followed - see fig. 14(a) - andthe vehicles keeping a safe distance between them-selves - see table 2.

x [m]-2 0 2 4 6 8 10

y [m

]

-2

-1

0

1

2

3

4

Vehicle 1Vehicle 2Vehicle 3Vehicle 4Vehicle 5Vehicle 6Vehicle 7

Figure 13: Top-down view of loose formation withleader motion

Minimum distance between vehicles [m]

2 3 4 5 6 7

1 0.640 0.674 0.729 0.633 0.708 0.6152 0.690 1.241 1.325 1.226 0.6963 0.675 1.105 1.395 1.1934 0.676 1.275 1.4535 0.709 1.1536 0.700

Table 1: Loose formation with leader motion sce-nario: minimum distance between vehicles

One thing to note is that the outer vehicleshave more difficulty keeping up on mission corners.When the cornering occurs (at around six and four-teen seconds in the simulation), the distance toshape of vehicles 3 and 5 increases significantly -see figs. 14(a) and 14(b). This is due to the differ-ence in velocity needed to do the turn and when theturning point is inside the formation.

x [m]-2 0 2 4 6 8 10

y [m

]

-2

-1

0

1

2

3

4

Vehicle 1Vehicle 2Vehicle 3Vehicle 4Vehicle 5

(a) Top-down view

0 2 4 6 8 10 12 14 16 18 20

Lead

er P

ositi

on [m

]

0

5

10

xy

time [s]0 2 4 6 8 10 12 14 16 18 20

Dis

tanc

e to

Sha

pe [m

]

0

0.1

0.2

0.3

0.4

(b) Leader position and vehicles distance to forma-tion shape

Figure 14: Rigid formation motion scenario

Both types of formation performed as expected.The path was followed, and although there was con-flict between the vehicles, no collisions occurred.For the rigid formation case, the number of vehicleson the mission must be considered when designingthe formation shape, and especially its size. The

8

Minimum distance between Vehicle [m]

2 3 4 5

Veh

icle 1 0.720 1.464 0.719 1.457

2 0.654 0.985 1.6293 1.568 1.9774 0.652

Table 2: Rigid formation motion scenario: mini-mum distance between vehicles

vehicles will not accomplish a formation shape thatis not safe, one where the distance between vehiclesneeds to be too low in order to be maintained.

5. Results

To test the complete framework, two scenarios aredefined where the vehicles’ dynamics is that of astabilized quadcopter. Scenario 1 defines a pathto be followed by 5 quadcopters in an environmentwith static obstacles. Scenario 2 defines the samepath, but the 5 quadcopters have to avoid a movingobstacle. In both scenarios, the quadcopters are toexecute the mission in formation, namely the looseformation with leader and the rigid formation.

If the quadcopter distance to an obstacle dropsbellow 1m, then it has entered its danger zone. Ifthat distance drops bellow 0.5m, it is consideredthey have collided. If the distance between quad-copters drops bellow 0.25m, it is considered thequadcopters have collided.

5.1. Loose Formation with Leader

The quadcopters must maintain a distance of 1mbetween each other, and that distance should notdrop bellow 0.5m.

Scenario 2 - Moving Obstacles Figure 15shows the effect of the moving obstacle to the quad-copters formation. The quadcopters break the for-mation to avoid colliding with the obstacle, but theyquickly return to their places. Again, there is nocollision between the quadcopters - see table 3 - orbetween the quadcopters and the obstacle - see ta-ble 4.

Figure 15: Loose formation with leader: scenario 2- moving obstacle

Minimum distance [m]

Quadcopter

2 3 4 5

Quad.

1 0.733 1.000 0.784 0.8952 0.843 1.210 0.9963 0.919 0.7494 0.871

Table 3: Loose formation with leader: scenario 2 -moving obstacle, minimum distance between quad-copters

Minimum distance [m]

Quadcopter

1 2 3 4 5

Obst. 1 0.783 1.318 1.432 0.600 0.947

Table 4: Loose formation with leader: scenario 2 -moving obstacle, minimum distance between quad-copters and obstacle

5.2. Rigid Formation

In a arrowhead formation with the leader at its tip,the quadcopters should maintain a minimum dis-tance between each other of 0.5m.

Scenario 1 - Static Obstacles Although thequadcopters come close to each other while avoid-ing the obstacles, table 5 shows they do not collide.Figure 16 depicts how the vehicles safely avoid thefirst obstacle with relative ease, but have to deviatemore from the desired formation to avoid the sec-ond obstacle. Quadcopters 2 and 3 get close to eachother, but still no collision occurs, either betweenquadcopters - table 5 - or between quadcopters andobstacles - table 6.

Minimum distance [m]

Quadcopters

2 3 4 5

Quad.

1 0.910 0.905 0.794 1.3952 0.437 1.188 1.7703 1.313 1.9464 0.597

Table 5: Rigid formation: scenario 1 - static obsta-cles, minimum distance between quadcopters

6. Conclusions

This paper presented a motion control frameworkto manage high-level tasks on an autonomous ve-hicle. It integrates the different possible subsys-tems responsible for mission following (Navigation),obstacle avoidance (Anti-Collision) and formationmanagement (Group Formation). Two solutions for

9

(a) Top-down view

9 [m

]

-5

0

5

10

15xyz

time [s]0 5 10 15 20

Dis

tanc

e to

sha

pe [m

]

0

0.5

1

1.5

2

(b) Leader position and quadcopters distance toformation shape

Figure 16: Rigid formation: scenario 1 - static ob-stacles

Minimum distance [m]

Quadcopters

1 2 3 4 5

Obst.1 1.953 2.592 3.246 1.338 0.8842 1.003 1.167 0.925 1.658 2.253

Table 6: Rigid formation: scenario 1 - static obsta-cles, minimum distance between quadcopters andobstacles

collision avoidance are presented, with the poten-tial flow method outperforming the velocity spaceclipping one. Besides being applicable to differentvehicle configurations, the potential flow methodachieves a better balance between mission accom-plishment and obstacles avoidance. A method ableto manage different types of group formations ofholonomic vehicles was also introduced, and demon-strated for the loose formation with leader andrigid formation cases. The complete framework,considering the best collision avoidance method,was tested for the two types of group formations.The vehicles were simulated with a stabilized quad-copter model. The results demonstrate the func-tionality of each subsystem, and the validity of theproposed framework. Future work includes the gen-eralization of the group formation management toboth holonomic and non-holonomic vehicles, andthe experimental validation of the framework.

References

[1] Oliver Brock and Oussama Khatib. High-speednavigation using the global dynamic windowapproach. In Robotics and Automation, 1999.Proceedings. 1999 IEEE International Confer-

ence on, volume 1, pages 341–346. IEEE, 1999.

[2] Amirreza Rahmani, Kunihiko Kosuge, TakashiTsukamaki, and Mehran Mesbahi. Multipleuav deconfliction via navigation functions. InAIAA Guidance, Navigation and Control Con-ference, 2008.

[3] Prachya Panyakeow and Mehran Mesbahi. De-centralized deconfliction algorithms for unicy-cle uavs. In American Control Conference(ACC), 2010, pages 794–799. IEEE, 2010.

[4] Prachya Panyakeow and Mehran Mesbahi. De-confliction algorithms for a pair of constantspeed unmanned aerial vehicles. Aerospaceand Electronic Systems, IEEE Transactionson, 50(1):456–476, 2014.

[5] A. Casqueiro, D. Ruivo, A. Moutinho, andJ. Martins. Improving teleoperation with vi-bration force feedback and anti-collision meth-ods. In ROBOT 2015, 2nd Iberian RoboticsConference, Advances in Intelligent Systemsand Computing Series. Springer, 2015.

[6] Herbert G Tanner and Amit Kumar. To-wards decentralization of multi-robot navi-gation functions. In Robotics and Automa-tion, 2005. ICRA 2005. Proceedings of the2005 IEEE International Conference on, pages4132–4137. IEEE, 2005.

[7] Maria Carmela De Gennaro and Ali Jadbabaie.Formation control for a cooperative multi-agent system using decentralized navigationfunctions. In American Control Conference,2006, pages 6–pp. IEEE, 2006.

[8] Gabor Vasarhelyi, Csaba Viragh, Gergo So-morjai, Norbert Tarcai, T Szorenyi, Tamas Ne-pusz, and Tamas Vicsek. Outdoor flockingand formation flight with autonomous aerialrobots. In Intelligent Robots and Systems(IROS 2014), 2014 IEEE/RSJ InternationalConference on, pages 3866–3873. IEEE, 2014.

[9] Weihua Zhao and Tiauw Hiong Go. Quad-copter formation flight control combining mpcand robust feedback linearization. Journal ofthe Franklin Institute, 351(3):1335–1355, 2014.

[10] David C Conner, Alfred Rizzi, Howie Choset,et al. Composition of local potential func-tions for global robot control and navigation.In Intelligent Robots and Systems, 2003.(IROS2003). Proceedings. 2003 IEEE/RSJ Interna-tional Conference on, volume 4, pages 3546–3551. IEEE, 2003.

10