safe autonomous agent formation operations via obstacle

Asian Journal of Control, Vol. 00, No. 0, pp. 1–11, Month 0000Published online in Wiley InterScience (www.interscience.wiley.com) DOI: 10.1002/asjc.0000

Safe Autonomous Agent Formation Operations Via Obstacle Collision

Avoidance

Mohammad Deghat, Iman Shames, Brian D. O. Anderson

ABSTRACT

In this paper, we consider the problem of flocking and shape-orientation control of multi-agent systems with inter-agent and obstaclecollision avoidance. We first consider the problem of forcing a set ofautonomous agents to form a desired formation shape and orientationwhile avoiding inter-agent collision and collision with convex obstacles, andfollowing a trajectory known to only one of the agents, namely the leader ofthe formation. Then we build upon the solution given to this problem and solvethe problem of guaranteeing obstacle collision avoidance by changing the sizeand the orientation of the formation. Changing the size and the orientation ofthe formation is helpful when the agents want to go through a narrow passagewhile the existing size or orientation of the formation does not allow this.We also propose collision avoidance algorithms that temporarily change theshape of the formation to avoid collision with stationary or moving nonconvexobstacles. Simulation results are presented to show the performance of theproposed control laws.

I. Introduction

Providing a safe environment for simultaneousoperation of humans and robots has been the subjectof research for the last few years. Additionally,formation control of multi-agent systems continuesto be an expanding area of research, see [2–6], andproviding such safe environments for human-agentsystem interactions has become an important researchquestion. An important element that one needs to take

M. Deghat and B. D. O. Anderson are with The AustralianNational University and National ICT Australia, Canberra, Aus-tralia. I. Shames is with Department of Electrical and ElectronicEngineering, University of Melbourne, Melbourne, Australia,e-mail: Mohammad.Deghat, [email protected],[email protected].

This work is supported by NICTA, which is funded by theAustralian Government as represented by the Department ofBroadband, Communications and the Digital Economy andthe Australian Research Council through the ICT Centre ofExcellence program. The conference version of this paper hasbeen presented in [1]. The current work contains followingkey additions: it considers inter-agent collision avoidance andinvestigates the case where the obstacles are moving.

into account while designing control laws to ensure thesafety in such environments is to guarantee obstacleavoidance for the formation of mobile agents. Theseobstacles can be human workers in the region ofoperation of the robots or can be natural barriers wherean unmanned vehicle is operating.

There are two general approaches to achieveobstacle collision avoidance in the literature: (i)planning-based approaches, and (ii) behavior-basedapproaches. Planning based approaches have their rootsin classical control theory, and are highly dependent onexact mathematical world models. In these methods,the agent follows a calculated path guaranteeing thatno collision occurs between itself and any obstaclesin the environment. The most important assumptionmade in these methods is the availability of an exactmodel for the environment, which may neither bepractical nor realistic in most situations. In contrast, inmost of the behavior-based approaches, e.g. [7–13], themotion of an agent is determined by taking into accountdifferent behaviors, such as avoid-moving-obstacles,avoid-stationary-obstacles, and move-to-goal. Theseapproaches do not usually rely on a strong a priori

c© 0000 John Wiley and Sons Asia Pte Ltd and Chinese Automatic Control SocietyPrepared using asjcauth.cls [Version: 2008/07/07 v1.00]

2 Asian Journal of Control, Vol. 00, No. 0, pp. 1–11, Month 0000

knowledge about the environment; however, they mightcause the agent to seek to follow contradicting goals,for example move-to-goal behavior may force the agentto move on a collision course with an obstacle. In suchcases, the agent might trap in a local minimum when theattractive force to the goal is equal to and in the oppositedirection of the repulsive force from the obstacles.Although there are some studies in the literature whichguarantee a local-minimum-free navigation function,see e.g. [14–16], they usually consider the case wherethe obstacles are convex. But for nonconvex obstacles,an agent either needs to have some knowledge about thegeometric shape of the obstacle to avoid it or it has tofollow the boundary of the obstacle until the motion togoal is again possible.

Recently, due to much attention being investedin multi-agent systems, collision avoidance methodssynthesizing the two approaches have been sought.Because of the challenging nature of multi-agentsystems the classical approaches to deal with collisionavoidance are rendered impractical, and researchershave moved to solve this problem by taking thevirtues of both of the abovementioned approaches. Forexample, the approaches proposed in [17–21] fall in thiscategory; they have elements of both planning-basedand behavior-based approaches. This paper also fallsin this category.

Similarly to some planning based approaches, weuse a navigation function that forces the agents togo to their desired positions to achieve the desiredformation shape and orientation and follow the leader,and also to avoid collision with convex obstacles andother agents. We assume that the agents can detectwhether an obstacle is convex or nonconvex and thetrajectory of the leader is planned such that it does notintersect with any obstacle. All obstacles considered inthis paper are assumed to be of finite extent. We furtherassume that each agent in the formation only knows therelative position of its neighbor agents and can detect anobstacle within a given range.

The main contribution of this paper over multi-agent control algorithms like [22] that guarantee shapeand orientation control of multi-agent systems with aleader is that it guarantees inter-agent and collisionavoidance as well as the motion control objectivesin [22]. There are other results in the literature thatstudy collision avoidance in multi-agent systems, e.g[23, 24]. An advantage of the control algorithmsproposed in this paper over these results is that theagents are able to follow a leader by just measuringthe relative position of their neighbor agents, ratherthat relative position and velocity. Compared to other

collision avoidance algorithms, this paper providesseveral collision avoidance techniques to (i) avoid inter-agent collision and collision with convex obstaclesusing a navigation function, (ii) change the size andorientation of the formation to go through a narrowpassage while the existing size or orientation of theformation does not allow this, (iii) temporarily changethe shape of the formation to avoid collision with astationary or moving nonconvex obstacle.

The rest of this paper is organized as follows. Inthe next section, after introducing some preliminariesand assumptions, we explain an algorithm proposedearlier in [1, 22] that forces the agents to form aformation with a desired shape within a bounded-timeand follow a trajectory which is unknown to all butone, the leader of the formation. In Section III weintroduce several obstacle avoidance algorithms. Bymodifying the algorithm proposed in Section II, we firstpropose an algorithm that achieves the abovementionedgoals and also avoids inter-agent collision and collisionwith convex obstacles. In Subsection 3.2, we proposean algorithm to control the motion of a formationof autonomous agents by rotating and scaling theformation. Then in Subsections 3.3 and 3.4, we proposeobstacle collision avoidance strategies that force anagent in the formation, that is in danger of collisionwith a stationary or moving nonconvex obstacle, tonavigate around the obstacle without requiring thewhole formation to change the formation size orthe orientation. Simulation results are presented inSection IV to show the applicability of the proposedcontrol methods. Finally, concluding remarks and futureresearch directions are presented in Section V.

II. Formation Shape Control and MotionCoordination Algorithm

Consider an n-agent formation of single-integratorpoint agents and let G(V, E ,A) be the underlyingsensing graph of this formation, where V = in1 isthe vertex set with i ∈ V corresponding to node i, Eis the edge set of the graph, and A ∈ Rn×n is theadjacency matrix with non-negative entries. In thispaper we consider the case where the edges in the graphare directed. The directed edge i, j ∈ E connects ito j if node i senses the relative position of j, wherethe corresponding entry in the adjacency matrix Aij isone. The out-degree of node i is deg (i) =

∑j∈Ni

Aij ,where Ni = j ∈ V : i, j ∈ E is the neighborhoodset of i. The degree matrix ∆ (G) ∈ Rn×n is a diagonal

c© 0000 John Wiley and Sons Asia Pte Ltd and Chinese Automatic Control SocietyPrepared using asjcauth.cls

3

matrix defined as

∆ij =

deg (i) , i = j0 , i 6= j

and the Laplacian of G is defined as L(G) = ∆−A.The problem of interest in this section is to

force the agents of the formation to form a desiredshape and orientation within a bounded time and movealong a desired trajectory while keeping the shape andorientation intact. We assume that one of the agents inthe formation is the leader and others are followers,and there is always a path from each of the agents tothe leader. Without loss of generality, we take agent 1to be the leader of the formation and other agents tobe followers. The followers can measure the relativepositions of their neighbors, which might be the leaderor other followers, in their local coordinate frames.Therefore, the control law proposed in this paper isdistributed and each agent only uses local information.

We make the following assumption on theconnectivity of the sensing graph G.

Assumption 1. The leader has a directed path from allfollowers at each time instant.

Let si(t) denote the position of agent i inRm,m = 2, 3 at time t and assume the single-integratorkinematics for the motion of agent i:

si(t) = υi(t) (1)

where υi(t) is the control input. Since the motions ineach of the dimensions are decoupled, for the rest ofthis paper we only introduce the control laws for the x-coordinate of the position of agent i, xi(t). Hence, wehave

xi(t) = υxi(t) (2)

where υxi(t) is the x-coordinate of υi(t). The desiredformation is represented in terms of displacementsfrom x1(t). Thus the desired position of agent i at timet is x?i (t) = x1(t) + δxi for all i ∈ 2, · · · , n whereδxi is a predefined constant scalar.

Assume the leader’s motion is governed by

x1(t) = f(t) (3)

where f(t) ∈ R is an arbitrary continuous function.Define

Fsi(t) = −n∑

j=1

Aij(t)(xi(t)− xj(t)

)+ uxi(t) (4)

where

uxi(t) = −n∑

j=1

Aij(t)(δxi − δxj

)(5)

for i ∈ 2, · · · , n, and Ai1 is one if the leader’sposition is available to follower i and is zero otherwise.Let α, β be positive constant scalars. Inspired by [25],we consider the following control law which appearedin [1, 22] to control the positions of the followers (i ∈2, · · · , n) to achieve the desired formation shape andorientation and follow the leader.

υxi(t) = α Fsi(t) + β sign (Fsi(t)) (6)

Remark 1. The evaluation of uxi in (5) only dependson local information, that is the information about thedesired relative position of an agent from its neighbors.

Theorem 1. Under Assumption 1, the control law (6)forces the agents to form the desired formation shapeand orientation while moving along the trajectorydefined by the motion of the leader in (3) within abounded time for motions satisfying |f(t)| < β for allt > 0 given that si(0) ∀i = 1, · · · , n are bounded.

Proof. See [1, 22].

Remark 2. It can be shown by a slight modificationof the result in [22] that the convergence time to thedesired shape under control law (6) is upper boundedby

t =

maxi∈2,··· ,n

|xi(0)− x1(0)− uxi|

(β − fmax)

where fmax = maxt|f(t)|.

III. Obstacle Avoidance Scheme Via FormationShape Modification

The control algorithm (6) in the previous sectiondoes not guarantee inter-agent collision and collisionwith obstacles. In this section, we study obstaclecollision avoidance algorithms to control a formationof agents to move safely in an obstacle field. Wefirst consider convex obstacles in Subsection 3.1and propose a control algorithm to avoid inter-agent collision and collision with such obstacles. InSubsection 3.2, we propose an algorithm based onthe algorithm in Subsection 3.1 to make the agents



in the formation change the scale and orientation ofthe formation temporarily so that the formation cango through a narrow passage. Then we study the casewhere one of the agents in the formation navigatesaround a nonconvex obstacle to avoid collision.

3.1. Avoiding inter-agent collision and collisionwith convex obstacles

Let oik(t) be the closest point on a convex obstaclek to agent i at time t and oxik

(t) be the x-coordinate ofoik(t). Let d be a positive distance at which an agentwhich is in the danger of collision with other agentsor obstacles starts modifying its trajectory to avoidcollision. Inspired by the obstacle avoidance algorithmsin [7] and [26], we consider the following control law tocontrol the position of the followers (i ∈ 2, · · · , n):

υxi(t) =α

(Fsi(t) +

n∑j=1

faij(t) +

n∑k=1

foik(t)

)

+β sign

(Fsi(t) +

n∑j=1

faij (t) +

n∑k=1

foik(t)

)

(7)

where

faij=

xi−xj

‖si−sj‖4

(1

‖si−sj‖2 −1d2

), ‖si − sj‖ ≤ d

0 , ‖si − sj‖ > d

foik =

xi−oxik

‖si−oik‖4

(1

‖si−oik‖2 −1d2

), ‖si − oik‖ ≤ d

0 , ‖si − oik‖ > d

(8)

The function Fsi(t) makes agent i move with the samevelocity as the leader and in the desired relative positionwith respect to its neighbors. The repulsive functionsfaij

(t) and foik(t) push agent i’s trajectory away fromagent j and obstacle k, respectively when agent i is intheir influence region. Note that the effect of faij

(t) andfoik(t) is local, i.e. when the distance between agenti and agent j (resp. the distance between agent i andobstacle k) is more than d, then faij

(t) (resp. foik(t)) iszero.

These repulsive functions guarantee that agent idoes not collide with its neighbor agents and obstacles,as these functions push agent i away from themwhen agent i is approaching them. It should be notedthat these repulsive functions generate some undesiredequilibria and thus the agents might trap into someof these equilibria if they start from certain initial

conditions. In other words, there are some initialconditions from which the agents cannot achieve thedesired formation shape and orientation and cannotfollow the leader. This, however, is not a problem from apractical point of view especially when the obstacles arepoint obstacles as there are just certain initial conditionsthat cause problem and if the agents start from a slightlydifferent initial positions, they might not trap in anyundesired equilibrium.

We will explain later in this section how to modifythe control law to avoid collision with nonconvexobstacles by changing the orientation or the size ofthe formation or temporarily changing the shape of theformation to make sure that agents can pass nonconvexobstacles. To do so, we assume that the agents areable to check whether they are approaching a convexor a nonconvex obstacle. This may require havingspecific sensors or having a priori knowledge about theobstacles in the filed.

Corollary 1. Under Assumption 1, the control law (7)forces the agents to form the desired formation shapeand orientation from almost all initial conditions whileavoiding inter-agent collision and collision with convexobstacles and moving along the trajectory defined bythe motion of the leader in (3) for motions satisfying|f(t)| < β for all t > 0.

3.2. Stationary Obstacle Avoidance Scheme ViaFormation Contraction

Consider the case where the agents in a formationare supposed to go through a narrow passage while theexisting size of the formation does not allow this. Apossible solution is to reduce the inter-agent distances,i.e. contract the formation and move through a narrowcorridor. In this case we assume that each of the agentsknows the diameter of the smallest circle that encirclesthe formation. They can either receive the informationabout the diameter of the smallest circle from the leaderwho knows the path along which the formation movesand might have more knowledge about the environmentthan other agents, or estimate a value for the diameterof the smallest circle and then share their values withtheir neighbors and agree on a final value. A possiblealgorithm for agreeing on the size of the formationis that each agent i that detects the passage estimatesthe factor σi necessary to reduce the diameter by thatfactor to ensure a safe passage through the narrowcorridor. Then the agents broadcast their correspondingscaling values to their neighbors. We assume herethat the communication graph G is different from thesensing graph G and is connected and undirected. If


5

an agent receives two unequal scaling factors fromits neighbors, it discards the smaller one and retainsthe larger one, and consequently broadcasts this largervalue. The reason for this behavior is that the larger thecontraction value, the more conservative the trajectorywill be in terms of collision avoidance. The value thatall the agents agree on will be the scaling factor σ ∈R+. The algorithm used to select this scaling factor ispresented in Algorithm 1.

Algorithm 1 Scaling Factor Selection at Agent `

initiate: Let LG be the diameter of graph G;Set σ`;k ← 1;while k ≤ LG do

Broadcast σ` to all the agents in N`;Receive σi for all i ∈ N`;σ` ← max

i∈N`∪`σi;

k ← k + 1;end while

Then by modifying the vector uxi in (5) for each ofthe followers as

uxi(t) = −

n∑j=1

Aij(t)(δxi − δxj

)σ

(9)

the desired distances between the leader and followerswould be divided by σ and therefore the formationwould be scaled.

The whole formation can also rotate around theleader to go through a narrow passage. This can be doneby rotating all the vectors δxi. For example in 2D-space,we can rotate the followers by modifying (5) into

uxi(t) = −n∑

j=1

Aij(t)(δxiRot

− δxjRot

)(10)

where[δxiRot

δyiRot

]= −

[cos(θ) − sin(θ)sin(θ) cos(θ)

] [δxiδyi

],

θ is the angle of rotation and .Rot denotes the rotatedvector. This angle of rotation can be calculated similarlyto the way the formation calculates σ.

Proposition 1. Let LG be the diameter of thecommunication graph G. After LG steps σi, ∀i ∈2, · · · , n are equal under Algorithm 1.

Proof. Without loss of generality assume σm =max

i∈2,··· ,nσi. Each agent j receives the value σm

and replaces the value of σj by this after k = Ljm

steps, where Ljm is the shortest path between j andm in the communication graph. Furthermore, sinceLjm ≤ LG , after at most LG steps, σi are equal ∀i ∈2, · · · , n.

Although the algorithms proposed in this subsec-tion can change the size and orientation of a formationto go through a narrow passage, they can also be appliedto avoid collision with nonconvex obstacles. However,in many situations changing the shape of the formationto avoid collision with obstacles is indeed unnecessary.In the next subsections, we take the idea of avoidingcollision with nonconvex obstacles by temporarilychanging the shape of the formation and apply it only tothose parts of the formation that are in danger of havinga collision.

3.3. Stationary Obstacle Avoidance Scheme ViaLocal Formation Shape Modification

We assume in this subsection that agent 1, that isthe leader of the formation, visits a set of waypointsin R2 (the three-dimensional case is a trivial extensionof this case.). Let W = wiwi=1 with wi ∈ R2 bethe ordered set of the w waypoints that 1 shouldvisit. Moreover, assume that 1 travels between anyconsecutive pair of waypoints, wj and wj+1, on astraight line. Furthermore, assume that the environmentwhere the agents operate contains o nonconvexstationary obstacles which can be placed in imaginarynon-overlapping circles with centers at coi and radii roi ,and max

i=1,··· ,oroi = rmax. We formally define the problem

that we study in this subsection here.

Problem 1. How can one design a set of control lawsto force the followers 2 · · · , n to achieve a desiredshape and orientation and avoid inter-agent collision,collision with stationary or moving convex obstacles,and collision with stationary nonconvex obstacles whilethe leader is moving between a pair of consecutivewaypoints wj and wj+1 on a straight line?

We address this problem in the rest of thissubsection. To propose a solution to Problem 1, weintroduce a control strategy for each of the agents thattakes priority for avoiding collision with nonconvexobstacles in the environment through temporary localmodification of the shape of the formation. After acollision with an obstacle is avoided by an agent, this



agent reverts to the shape preservation strategy to returnthe formation to its desired shape.

To address Problem 1, we first make the followingassumption on the trajectory of the leader.

Assumption 2. The distance between the straight linesconnecting any consecutive pairs of waypoints and theobstacles (whether or not convex) is more than ε : 0 <ε 1.

Assumption 2 ensures that the leader does notcollide with any obstacle. Note that this assumption canbe relaxed if the leader re-plans the waypoints as itdetects an impending collision with an obstacle on itspath.

We next define a collision course flag Ωi(t) foreach i:

Ωi(t) =

1∃j such that dij(t) ≤ roj + ε

& ρij(t) ≤ d0 otherwise

(11)

where dij(t) is the distance of coj to the ray originatingfrom si(t) and along the vector vi(t) at time t, ρij(t)is the shortest distance between si(t) and the line thatgoes through coj and is perpendicular to vi(t) at time tas shown in Fig. 1, and d > rmax is a positive scalar.

The reason for introducing Ωi(t) comes from thefact that the collision avoidance can be achieved byeach of the agents through local manipulation of theshape of the formation. When Ωi(t) = 1, it means thatif agent i were to continue its motion along its currenttrajectory, it would become too close to the nonconvexobstacle ahead and possibly collide with it. Considering(11) and Fig. 1, it can be seen that when the agent ismoving toward the obstacle such that dij(t) ≤ roj + ε

and ρij ≤ d the course flag goes to one.

Let the time when the value of Ωi(t) switches from0 to 1 be denoted by τi. Define uoi (t) = [uoxi, u

oyi]> to be

a perpendicular vector to vi(t) with a magnitude of roj −dij(t) + ε1 and its direction parallel to a perpendicularvector departing from coj towards the abovementionedray where ε1 : ε1 < ε is a positive constant scalar. Anexample for uoi (τi) when Ωi(t) changes from zero toone is depicted in Fig. 1.

If Ωi(t) = 1, we use the following control law tocontrol the x-coordinate of i

υxi(t) =α (Fsi + ∆iiuoxi)

+β sign (Fsi + ∆iiuoxi)

(12)

where ∆ii is the out-degree of node i that is∑j∈Ni

Aij . Additionally, for all j ∈ Ni, Aji is set

Fig. 1. For all the points inside the dashed area Ωi is equal to 1.

to be zero and uxj is modified accordingly for theperiod that Ωi(t) = 1. The interpretation of this actionon the neighbors is that when agent i is undergoing anevasive action to avoid a collision with the obstacle,the neighbors stop following it to prevent furtherdeterioration of the formation shape.

We summarize the approach presented here tosolve Problem 1 in Algorithm 2. However, first we makethe following assumption.

Assumption 3. There is no time t at which thereexists an agent ` such that Ωi(t) = 1 and Ω`(t) = 1for all ` ∈ Ni. Furthermore, the size of the imaginarycircle around the nonconvex obstacle is such that whenthe collision course flag for an agent is 1, all convexobstacles which are in the vicinity of the nonconvexobstacle and might collide with the agent are inside theimaginary circle.

If we apply Algorithm 2 sequentially when theleader visits each of its waypoints we can solve theproblem of moving the formation from a starting pointto an end point while avoiding any collision withthe obstacles along the way. The following corollaryformally states the result of this subsection.

Corollary 2. Algorithm 2 solves Problem 1 underAssumptions 2-3.

3.4. Moving Obstacle Avoidance Via LocalFormation Shape Modification

The problem that we address in this subsection isformalized in what comes next.

Problem 2. How can one design a set of control laws toforce the followers 2 · · · , n in the formation to achievea desired shape and orientation and avoid inter-agent collision, collision with stationary or movingconvex obstacles, and collision with external moving


7

Algorithm 2 Motion Control and Collision Avoidancewith Nonconvex Stationary Obstacles

if Ωi(t) = 1 thenA ← A;υxi ← −α

(Fsi + ∆iiu

oxi

)− βsign

(Fsi + ∆iiu

oxi

);

for j ∈ Ni doAji ← 0;

uxj := −n∑

k=1Ajk(t)

(δxj − δxk

);

F sj := −n∑

k=1Ajk(t)

(xj(t)− xk(t)

)+ uxj ;

υxj ← −α(F sj +

n∑k=1

fajk +n∑

k=1fojk

)− βsign

(F sj +

n∑k=1

fajk +n∑

k=1fojk

);

end forelseυxi ← −α

(Fsi +

n∑k=1

faik +n∑

k=1

foik

)− βsign

(Fsi +

n∑k=1

faik +n∑

k=1

foik

);

for j ∈ Ni do

υxj ← −α(Fsj +

n∑k=1

fajk +n∑

k=1

fojk

)− βsign

(Fsj +

n∑k=1

fajk +n∑

k=1

fojk

);

end forend if

nonconvex obstacles while the leader is moving betweena pair of consecutive waypoints wj and wj+1 on astraight line?

We assume that the nonconvex obstacles movewith constant velocities. This enables the agents toestimate the velocity of the obstacles easily. Let voj bethe velocity of the j-th obstacle. Assume that it takesτij > 0 units of time from the obstacle detection timeτi for the path of obstacle j to cross that of the i-thagent. Now consider the distance between the obstacleand agent pair at that time, i.e., τi + τij . If ‖si(τi +τij)− coj(τi + τij)‖ ≤ roj + ε then there is a potentialfor collision between agent i and obstacle j, and oneof the following cases holds:

Case 1: Agent i collides with the front-half of obstacle jand avoids collision easier if it increases its speedalong vi(τi).

Case 2: Agent i collides with the back-half of obstacle jand avoids collision easier if it decreases its speedalong vi(τi).

If the first case holds, then as before uoi is set to bea vector with roi + ε− ‖sj(τi + τij)− coj(τi + τij)‖ as

magnitude and the direction of vi(τi) until the timewhen by setting uoi to zero no collision is possible.An example for the situation where Case 1 happens isdepicted in Fig. 2.

Fig. 2. A possible scenario with moving obstacles.

If the second case happens, uoi is a vector withthe same magnitude as the previous case, however, itsdirection is now −vi(τi). Again, this continues to beapplied to the agents until the distance between obstaclej and agent i starts to increase. We set Ωi(t) = 1 for allt that there is potential for the collision of agent i withan obstacle. The algorithm for this scenario is the sameas Algorithm 2.

For the cases where there are both moving andstationary obstacles available in the environment, weintroduce another binary variable µj associated with thej-th obstacle. The variable µj is equal to 1, if the j-thobstacle is a moving obstacle and if µj = 0 it means thatthis obstacle is stationary. Then the avoidance schemeintroduced in Subsection 3.3 is applied to stationaryobstacles and the scheme in Subsection 3.4 is appliedto the moving ones.

IV. Simulations

In the first simulation, we consider a formation of5 agents controlled by (6) where 1 is the leader andmoves on a sinusoidal trajectory. The desired formationshape is a square. The desired position of the leaderis at the center of the square and the desired positionof other four agents are at the corners. The agents’trajectories are depicted in left-hand side subfigure ofFig. 3 and the minimum distance between the agentsand the sum of the 2-norm of the relative position errorbetween the neighbor agents are respectively shown assolid blue line and dashed black line in the right handside subfigure. It can be seen that the agents achievethe desired formation shape after a bounded time andthe formation follows the trajectory of the leader, but



the minimum distance between the agents goes to zeroat t = 0.1sec. To avoid inter-agent collision, we controlthe followers using the control law in (7). We assumedd in (8) to be 0.5m. All initial conditions are the sameas before. It can be seen in Fig. 4 that both the motioncontrol objective and inter-agent collision avoidancegoal are achieved.

In the next simulation, we assume that there arethree stationary circle shaped obstacles with the radiusof 0.1m. We use the control law in (7). The resultsare shown in Fig. 5. According to the right hand sidesubfigure, the agents achieve the desired shape andorientation quickly after they pass an obstacle.

In the next scenario, we consider the case wherethe agents reach a consensus on the contraction factorσ necessary to reduce the size of the formation whichis explained in Subsection 3.2. Each agents i adjustsits ui along the lines of (9) where σ = 4. This resultsin the formation going through a narrow passage. Theagents’ trajectories in this scenario are presented inFig. 6 and the the inter-agent distances are shown inFig. 7. Then we consider the case where a formation of5 agents is moving on a straight line in an obstacle fieldusing Algorithm 2. The agents’ trajectories in this caseare depicted in Fig. 8. Finally, we present simulationsfor the case where the agents in the formation avoidcollision with a set of moving obstacles using thescheme presented in Subsection 3.4. The obstaclesmove on trajectories perpendicular to those of theagents, and have a radius equal to 0.25m. The obstacles’trajectories are such that if the agents do not changetheir speed, the agents will collide with the obstacles.The distances between the agents and the obstacles andbetween each pair of the agents are depicted in Fig. 9and Fig. 10, respectively. According to Fig. 10, theagents achieve the desired formation shape after passingthe obstacles.

V. Conclusions and Future Directions

In this paper, we introduced methods to modify theshape of the formations should the need arise duringthe course of motion of the formation to avoid collisionwith obstacles. The information needed to implementthese control methods is local to the agents and hencecan be implemented locally.

Possible future directions include consideringmore realistic agent kinematic models and providingsimilar guarantees for such agent models, andintroducing acceleration and turning rate constraints foragents.

−5 0 5 10 15 20

0

5

10

15

20

25

x (m)

y (

m)

Agents Trajectories

s

1(t)

s2(t)

s3(t)

s4(t)

s5(t)

Fig. 6. Formation contraction to allow motion through a narrowpassage.

0 1 2 3 4 50

1

2

3

4

5

6

7

Time (sec)

Dis

tance

Inter−agent Distances

||s1(t)−s

2(t)||

||s1(t)−s

3(t)||

||s1(t)−s

4(t)||

||s1(t)−s

5(t)||

||s2(t)−s

3(t)||

||s2(t)−s

4(t)||

||s2(t)−s

5(t)||

||s3(t)−s

4(t)||

||s3(t)−s

5(t)||

||s4(t)−s

5(t)||

Fig. 7. Inter-agent distances while the formation is moving through anarrow passage.

REFERENCES

1. I. Shames, M. Deghat, and B. D. O. Anderson,“Safe formation control and coordination withobstacle avoidance,” in 18th IFAC WorldCongress, 2011, pp. 11252–11257.

2. Q. Hui and W. M. Haddad, “Distributed nonlinearcontrol algorithms for network consensus,” Auto-matica, vol. 44, no. 9, pp. 2375–2381, September2008.

3. N. Moshtagh and A. Jadbabaie, “Distributedgeodesic control laws for flocking of nonholo-nomic agents,” IEEE Transactions on AutomaticControl, vol. 52, no. 4, pp. 681–686, April 2007.

4. E. W. Justh and P. S. Krishnaprasad, “Equilibriaand steering laws for planar formations,” Systems


9

−8 −6 −4 −2 0 2 4 6 8 10

−6

−4

−2

0

2

4

6

x (m)

y (

m)

s1(t)

s2(t)

s3(t)

s4(t)

s5(t)

0 1 2 3 4 5 6 7 80

2

4

6

8

10

12

Time (sec)

Dis

tance (

m)

Fig. 3. The left figure shows the trajectories of the agents controlled by (6) that achieve a desired formation shape while following a sinusoidal pathonly known to the leader. The right figure shows the minimum distance between the agents (solid blue line) and the sum of the 2-norm of therelative position error between the neighbor agents (dashed black line).

−8 −6 −4 −2 0 2 4 6 8 10

−6

−4

−2

0

2

4

6

x (m)

y (

m)

s1(t)

s2(t)

s3(t)

s4(t)

s5(t)

0 1 2 3 4 5 6 7 80

2

4

6

8

10

12

Time (sec)

Dis

tance (

m)

Fig. 4. The left figure shows the trajectories of the agents controlled by (7) that achieve a desired formation shape while following a sinusoidal pathonly known to the leader. The right figure shows the minimum distance between the agents (solid blue line) and the sum of the 2-norm of therelative position error between the neighbor agents (dashed black line).

−8 −6 −4 −2 0 2 4 6 8 10

−6

−4

−2

0

2

4

6

x (m)

y (

m)

s

1(t)

s2(t)

s3(t)

s4(t)

s5(t)

0 1 2 3 4 5 6 7 80

2

4

6

8

10

12

Time (sec)

Dis

tance (

m)

Fig. 5. The left figure shows the trajectories of the agents controlled by (7) that avoid collision with three stationary circle shaped obstacles with theradius of 0.1m and achieve a desired formation shape while following a sinusoidal path only known to the leader. The right figure shows theminimum distance between the agents (solid blue line) and the sum of the 2-norm of the relative position error between the neighbor agents(dashed black line).



0 5 10 15 20

−8

−6

−4

−2

0

2

4

6

8

x (m)

y (

m)

s1(t)

s2(t)

s3(t)

s4(t)

s5(t)

Fig. 8. Stationary obstacle avoidance via local formation shapemodification.

0 1 2 3 4 50

2

4

6

8

10

12

14

16

18

20

TIme (sec)

Dis

tance

Distance Between the Agents and the Obstacles

Fig. 9. Distances between the agents and the obstacles when the agentstemporarily modify the formation shape to avoid collision withmoving nonconvex obstales.

0 1 2 2 4 50

0.5

1

1.5

2

2.5

3

3.5

4

4.5

Time (sec)

Dis

tan

ce

Inter−agent Distances

Fig. 10. Inter-agent distances when the agents temporarily modifythe formation shape to avoid collision with moving nonconvexobstales.

& Control Letters, vol. 52, no. 1, pp. 25–38, May2004.

5. M. Basiri, A.N. Bishop, and P. Jensfelt, “Dis-tributed control of triangular formations withangle-only constraints,” Systems & ControlLetters, vol. 59, no. 2, pp. 147–154, February2010.

6. T. H. Summers, C. Yu, S. Dasgupta, and B. D.O. Anderson, “Control of minimally persistentleader-remote-follower and coleader formationsin the plane,” IEEE Transactions on AutomaticControl, vol. 56, no. 12, pp. 2778–2792, 2011.

7. Oussama Khatib, “Real-time obstacle avoidancefor manipulators and mobile robots,” TheInternational Journal of Robotics Research, vol.5, no. 1, pp. 90–98, 1986.

8. T. Balch and R. C. Arkin, “Behavior-basedformation control for multirobot teams,” IEEETransactions on Robotics and Automation, vol. 13,pp. 926–939, Dec. 1998.

9. R. Dougherty, V. Ochoa, Z Randies, and C. Kitts,“A behavioral control approach to formation-keeping through an obstacle field,” in IEEEAerospace Conference, Big Sky, MT, USA, March2004.

10. M. Egerstedt and H. Xiaoming, “Formation con-strained multi-agent control,” IEEE Transactionson Robotics and Automation, vol. 17, pp. 947 –951, Dec. 2001.

11. C Yu, B. Fidan, I. Shames, S. Sandeep, andB. D. O. Anderson, “Collision free coordinationof autonomous multiagent system,” in EuropeanControl Conference, Athens, Kos, Greece, 2007,pp. 900–907.

12. L. Yin, Y, Yin, and C. J. Lin, “A new potentialfield method for mobile robot path planning inthe dynamic environments,” Asian Journal ofControl, vol. 11, no. 2, pp. 214–225, 2009.

13. C. Song, G. Feng, and Y. Wang, “Decentralizeddynamic coverage control for mobile sensornetworks in a non-convex environment,” AsianJournal of Control, vol. 13, no. 1, pp. 1–9, 2013.

14. E. Rimon, “A navigation function for a simplerigid body,” in IEEE International Conference onRobotics and Automation, 1991.

15. E. Rimon and D. E. Koditschek, “Exact robotnavigation using artificial potential functions,”IEEE Transactions on Robotics and Automation,vol. 8, pp. 501–518, Dec. 1992.

16. Seyed Mohammad Khansari-Zadeh and Aude Bil-lard, “A dynamical system approach to realtimeobstacle avoidance,” Autonomous Robots, vol. 32,no. 4, pp. 433–454, 2012.

17. I. Shames, C. Yu, B. Fidan, and B.D.O. Anderson,“Externally excited coordination of autonomous


11

formations,” in Mediterranean Conference onControl & Automation, 2007, pp. T33–003.

18. G.M. Hoffmann and C.J. Tomlin, “Decentralizedcooperative collision avoidance for accelerationconstrained vehicles,” in 47th IEEE Conferenceon Decision and Control, 2008. CDC 2008, 2008,pp. 4357–4363.

19. S. Mastellone, D.M. Stipanovic, C.R. Graunke,K.A. Intlekofer, and M.W. Spong, “Formationcontrol and collision avoidance for multi-agentnon-holonomic systems: Theory and experi-ments,” The International Journal of RoboticsResearch, vol. 27, no. 1, pp. 107, 2008.

20. D.V. Dimarogonas, S.G. Loizou, K.J. Kyriakopou-los, and M.M. Zavlanos, “A feedback stabiliza-tion and collision avoidance scheme for multipleindependent non-point agents,” Automatica, vol.42, no. 2, pp. 229–243, 2006.

21. I. Shames, B. Fidan, and B. D. O. Anderson,“Close Target Reconnaissance with GuaranteedCollision Avoidance,” International Journal ofRobust and Nonlinear Control, vol. 21, no. 16, pp.1823–1840, 2011.

22. Y. Cao, W. Ren, and Z. Meng, “Decentralizedfinite-time sliding mode estimators and theirapplications in decentralized finite-time formationtracking,” Systems & Control Letters, vol. 59, no.9, pp. 522–529, 2010.

23. Housheng Su, Xiaofan Wang, and Zongli Lin,“Flocking of multi-agents with a virtual leader,”IEEE Transactions on Automatic Control, vol. 54,no. 2, pp. 293–307, 2009.

24. M Ani Hsieh, Vijay Kumar, and Luiz Chaimowicz,“Decentralized controllers for shape generationwith robotic swarms.,” Robotica, vol. 26, no. 5,pp. 691–701, 2008.

25. Y. Cao and W. Ren, “Distributed coordinatedtracking via a variable structure approach - parti: Consensus tracking,” in American ControlConference, 2010, pp. 4750–4755.

26. Eduardo G Hernandez-Martınez and EduardoAranda-Bricaire, “Convergence and collisionavoidance in formation control: A survey of theartificial potential functions approach,” Multi-Agent Systems-Modeling, Control, Programming,Simulations and Applications, Intech Open AccessPublisher, Croatia, pp. 103–126, 2011.


safe autonomous agent formation operations via obstacle

Documents