# Cohesive Motion Control Algorithm for Formation of Multiple Autonomous Agents

Authors: Debabrata Atta; Bidyadhar Subudhi; Madan M. Gupta
Journal: Journal of Robotics
 (2010)

---

## Abstract

This paper presents a motion control strategy for a rigid and constraint consistent formation that can be modeled by a directed graph whose each vertex represents individual agent kinematics and each of directed edges represents distance constraints maintained by an agent, called follower, to its neighbouring agent. A rigid and constraint consistent graph is called persistent graph. A persistent graph is minimally persistent if it is persistent, and no edge can be removed without losing its persistence. An acyclic (free of cycles in its sensing pattern) minimally persistent graph of Leader-Follower structure has been considered here which can be constructed from an initial Leader-Follower seed (initial graph with two vertices, one is Leader and another one is First Follower and one edge in between them is directed towards Leader) by Henneberg sequence (a procedure of growing a graph) containing only vertex additions. A set of nonlinear optimization-based decentralized control laws for mobile autonomous point agents in two dimensional plane have been proposed. An infinitesimal deviation in formation shape created continuous motion of Leader is compensated by corresponding continuous motion of other agents fulfilling the shortest path criteria.


---

## Body

## 1. Introduction

There are several examples of coordinated team work of mobile agents in nature like food foraging by a group of ants, flocking birds, fish schooling for protection from enemies, and so forth. These examples give us a lesson that a particular task accomplished by a single mobile autonomous agent (like fish and birds) may be done more efficiently by a group of agents if they perform in a collaborative manner.  During last thirty years or more, this fact has motivated many researchers in the field of control and automation significantly to contribute in several similar applications in industry. In some particular applications during their motion as whole, autonomous agents (e.g., robots, vehicles, etc.) need to maintain a particular geometrical shape for cohesive motion, called formation which satisfies some constraints like desired distance between two agents, desired angle between two lines joining two agents each. Examples of these types of formations are found in collective attack by a group of combat aircraft, search/rescue/surveillance/cooperative transportation by a troop of robots, underwater exploration/underwater inspection (like pipeline inspection) by a group of Autonomous Underwater Vehicles, attitude alignment of clusters of satellites, air traffic management system, automated highway system, and so forth.In the area of formation control of autonomous agents like robots, the motion control strategies may be either a centralized one or it could be a decentralized control. In centralized mode of control [1, 2], the command for all agents of the group are assigned by the central command control board or a designated group leader for monitoring and control of all agents to guide them be placed at desired position. The centralized formation control could be a good scheme for a small group of robots, when it is implemented with a single computer and a single sensor to monitor and control the entire group. Control of large number of robots in a formation requires greater computational capacity and large amount communication. Due to these problems, decentralized control is recommended when formation involves a large number of agents.  In the decentralized mode of control [3–5], one agent of the group can be a leader and others are followers (or each agent of the group can be a leader and follower except a designated group leader and the two outmost agents) and  as a follower, each agent generates its own commands autonomously (i.e., control law for each agent is provided separately such that each agent works autonomously) based on the relative measurement only from its neighbours without the need of an external supervisor, and the whole purpose of the formation motion is achieved.Although two types of basic control strategies are discussed in the last paragraph, motion control scheme for formation may be modified depending upon some factors like agent dynamics, interagent information exchange structure, control goals in different applications, and so forth.Formations are modeled usingformation graph whose each vertex represents individual agent kinematics and each edge represents interagent constraint (e.g., desired distance) that must be maintained during motion of formation. Specifically, graph is used to represent coordinated behaviour among agents. Depending upon the pattern of information exchange in that coordination, two types of graphs are possible, that is, directed and undirected.In undirected formation graph both of any pair of agents,  constrained by an edge have equal responsibility to satisfy the  constraint. For example, distance between any pair of agents is sensed by both the agents; that is, sensing is distributed. Therefore, structure of undirected graph suffers from various disadvantages. One of those disadvantages is information-based instability [6], which happens due to possibility of difference in distances measured by noisy sensors of any pair of agents. Communication requirement among agents is  more. Therefore, external observer-based centralized control strategy is best suited for undirected formation graph. Control law is mainly focused on rigidity property of formation.Formation graph isrigid [7] if distance between any pair of agents remains constant during any continuous motion of formation. A graph is said to be minimally rigid if it is rigid and if there is no rigid graph having the same vertices but fewer edges.In directed formation graph, only one (calledfollower) of any pair of agents, constrained by an edge has responsibility to satisfy the constraint. Therefore, decentralized control strategy is best suited for directed formation graph.A graph isconstraint consistent [8] if every agent is able to satisfy all the constraints on it provided all others are trying to do so. A formation that both rigid and constraint consistent is termed as persistentgraph.Persistence is a generalization to directed graphs of the undirected notion of rigidity. A persistent graph is minimally persistent if it is persistent and if no edge can be removed without losing persistence.However, focus in this paper is in development of control strategy for directed graph-based formation. There are several papers in which basics of directed formation graph-related issues are discussed [7–10]. Definitions and theorems [7–10] with regard to undirected and directed graphs included in Section 2.Digraph is called acyclic when no cycle is present in its sensing pattern [6, 8]. Control scheme for cyclic formation is more complicated than acyclic formation. Minimally persistent formation of autonomous agents may be formed in two ways. First one is leader-follower graph architecture constructed from an initial leader-follower seed by Henneberg Sequence with standard vertex additions or edge splitting [11]. Leader-follower type minimally persistent graph is always acyclic.  Another type of construction by sequence of specific operation elaborated in [9] such that every intermediate construction is also persistent. Minimally persistent graph constructed by this method may have cycles. In this paper, control strategies for only leader-follower type formation constructed from sequence of vertex addition is described.Although a number of research works have been directed in the area of cyclic formation graph, but still there remains scope of further work. In [12], Anderson et al. have proposed a distance preservation-based control law when cycles contain in the formation graph.In most recent works [13, 14] or some previous works [6], distributed control is provided with exploiting gradient control law for each autonomous agent in a formation separately. In [5], formation control strategy of leader-follower and three coleader structures is set up based on discrete-time motion equations considering decentralized approach. In [15], Anderson et al. analyzed control of leader-follower structure in continuous domain assuming linearized system for small motion and stability aspects are also discussed. However, we proceed for the control of leader-follower formation in a different way; that is, our approach is based on optimization of some distances.In the present paper, we consider the motion control scheme of theleader-follower structure type persistent formation in continuous domain that is based on optimization of a set of nonlinear objective functions under a set of equality constraints where each objective function (corresponding to each agent) corresponds to a specific constraints separately, as control scheme considers a decentralized approach. With advent of high speed computational platforms the solution associated with optimization procedure in the control generation is possible.This paper is organized as follows. In Section2, application of graph theory in formation control has been discussed. Problem formulation for formation control of multiple autonomous agents has been included in Section 3 followed by the development of control law, simulation, and conclusion in Sections 4, 5, and 6, respectively.


## 2. Application of Graph Theory in Formation Control

As briefly described about the graph theory in Section1, it is observed that graph is the best way for proper understanding of information flow among agents in a formation. This makes groundwork for control engineers to design an efficient control scheme for the motion control of formation. In this paper, formation of leader-follower structure has been considered and it may be extended to any number of agents. Therefore, it is worthwhile to understand how a leader-follower type formation graph is built up from an arbitrarily chosen number of agents. In this section, at the outset the pertinent definitions and theorems given in [7–10] about the rigidity and persistence are reviewed and subsequently the procedure of creating a leader-follower formation for a case of four numbers of autonomous agents is described.


### 2.1. Definitions Associated with Rigid Graph

#### 2.1.1. Infinitesimally Rigid Graph

A representationp of  an undirected graph G(V,E) with vertex  set V and edge set E is a function p:V→ℜd, where d(2,3,…) is the dimension of Euclidean space. Representation p is rigid if there exists ε>0 such that for all realizations due to continuous deformations p′ of distance set induced by p and satisfying, d(p,p′)<ε (where, d(p,p′)=max∥p(i)-p′(i)∥, where, i∈V), there holds ∥p′(i)-p′(j)∥=∥p(i)-p(j)∥ for all i,j∈V. We simply say this phenomenon as congruence relationship between p and  p′.


#### 2.1.2. Generically Rigid Graph

A graph is said to be generically rigid if almost all realizations due to continuous deformations are rigid. This definition of rigidity is to exclude some undesirable situations like certain collections of vertices are collinear during continuous deformations.


#### 2.1.3. Minimally Rigid Graph

A rigid graph is minimally rigid when no single edge can be removed without losing its rigidity.


#### 2.1.4. Laman’s Criteria (see [10])

If  an  undirected  graphG=(V,E) in ℜ2 with at least two vertices is rigid, then there exists a subset E′ of edges such that |E′|= 2|V|-3 and  any  subgraph G′′=(V′′,E′′) of G′ with at least two vertices satisfies |E′′|≤2|V(E′′)|-3, where |V(E′′)|is number  of vertices that are end-vertices of the edges in E′′.Lemma 1 (see [7]).
LetG=(V,E) be a minimally rigid graph in ℜ2 and G′=(V′,E′) a subgraph of G such that |E′|=2|V′|-3. Then, G′ is minimally rigid.In directed graph, each agent is only aware of its own distance constraints and can move freely as long as these distance constraints are satisfied. Persistence is a directed notion and rigidity is an undirected notion.  The properties of directed graphs are described below.


### 2.2. Definitions Associated with Persistent Graph

Suppose, for a directed graphG, desired distances dij>0 for all (i,j)∈E, edge set where i,j∈V, vertex set, and a realization p, then the following definitions are described.


#### 2.2.1. Active Edge

The edge(i,j)∈E is active if ∥p(i)-p(j)∥=dij, that is, if the corresponding distance constraint is satisfied.


#### 2.2.2. Fitting Position of a Vertex

The position of a vertexi∈V is fitting for any desired distance set {d} of G, if it is not possible to increase the set of active edges leaving i by changing the position of i while maintaining the positions of other vertices unchanged. Specifically, the position of vertex i, for a given realization p, is fitting if there is no p′∈ℜ2 for which the condition elaborated below is strictly satisfied:
(1){(i,j)∈E:∥p(i)-p(j)∥=dij}⊂{(i,j)∈E:∥p′-p(j)∥=dij}.


#### 2.2.3. Fitting Realization of a Graph

A realization of a graph is a fitting realization for a certain distance set{d} if all the vertices are at fitting positions for {d}.


#### 2.2.4. Constraint Consistent Graph

A realizationp of digraph G  is constraint consistent if there exists ε>0 such that any realization  p′ fitting for the distance set {d} induced by p  and satisfying d(p,p′)<ε is a realization of {d}. A graph is generically constraint consistent if almost all realizations are constraint consistent.


#### 2.2.5. Persistent Graph

Realizationp of the digraph G having desired distances dij>0 for all {i,j} is persistent if there exists ε>0 such that every realization due to continuous deformation, p′ fitting for the distance set induced by p and satisfying d(p,p′)<ε is congruent to p.


#### 2.2.6. Generically Persistent Graph

A graph is generically persistent if almost all possible realizations are persistent (same as elaborated in case of generically rigid).Theorem 1 (see [8]).
A realization is persistent if and only if it is rigid and constraint consistent.
A graph is generically persistent if and only if it is generically rigid and constraint consistent.


#### 2.2.7. Minimally Persistent Graph

A persistent graph is minimally persistent if it is persistent and if no edge can be removed without losing persistence.Theorem 2 (see [8]).
A rigid graph is minimally persistent if and only if either of the two conditions elaborated below is satisfied.(i)
Out of all vertices, each of three vertices has one outgoing edge and each of rest vertices has two outgoing edges(ii)
Out of all vertices, one vertex has no outgoing edge; another one vertex has one outgoing edge and each of   rest vertices has two outgoing edges.Theorem 3 (see [8]).
An acyclic digraph is persistent if all the conditions elaborated below are satisfied.
Out of all vertices,(i)
one vertex has one outgoing edge. This vertex represents Leader,(ii)
another vertex has one outgoing edge which must be incident to the Leader. This vertex represents first follower,(iii)
each of rest vertices must have two or more number of outgoing edges.


### 2.3. Construction of Acyclic Minimally Persistent Graph

Acyclic minimally persistent graph is always constructed starting from a combination of two vertices, one is called leader and the other one is first follower, and an edge directed towards leader using Henneberg Sequence with only vertex additions.


### 2.4. Henneberg Construction (Directed Case) Containing Vertex Addition

It describes the sequence of graphsG2,G3,…G|V|, such that each graph Gi+1(i≥2) can be obtained by a vertex addition starting from Gi, where i is number of vertices and |V| is cardinality of vertex set of desired graph. Therefore, the procedure of drawing the graph using Henneberg sequence is described as(i)
start with a directed edge between two vertices.The vertex towards which edge is directed is called leader and remaining vertex is called first follower. The combination of these two a vertex with a directed edge is called initial leader-follower seed,(ii)
at each step of growing graph, add a new vertex,(iii)
join the new vertex to two old vertices (corresponding to leader and first follower) via two new edges, directed towards old vertices.Figure1 shows the construction procedure of formation of four agents.Figure 1
Henneberg construction for making leader-follower type formation from four agents “a”, “b”, “c”, and “d” (i) “a”, “b” are initial vertices with  one edge directed towards “a” (leader) from “b” (first follower) (ii) “c” is new vertex added to (i) (iii) “d” is new vertex added to (b).


## 2.1. Definitions Associated with Rigid Graph

### 2.1.1. Infinitesimally Rigid Graph

A representationp of  an undirected graph G(V,E) with vertex  set V and edge set E is a function p:V→ℜd, where d(2,3,…) is the dimension of Euclidean space. Representation p is rigid if there exists ε>0 such that for all realizations due to continuous deformations p′ of distance set induced by p and satisfying, d(p,p′)<ε (where, d(p,p′)=max∥p(i)-p′(i)∥, where, i∈V), there holds ∥p′(i)-p′(j)∥=∥p(i)-p(j)∥ for all i,j∈V. We simply say this phenomenon as congruence relationship between p and  p′.


### 2.1.2. Generically Rigid Graph

A graph is said to be generically rigid if almost all realizations due to continuous deformations are rigid. This definition of rigidity is to exclude some undesirable situations like certain collections of vertices are collinear during continuous deformations.


### 2.1.3. Minimally Rigid Graph

A rigid graph is minimally rigid when no single edge can be removed without losing its rigidity.


### 2.1.4. Laman’s Criteria (see [10])

If  an  undirected  graphG=(V,E) in ℜ2 with at least two vertices is rigid, then there exists a subset E′ of edges such that |E′|= 2|V|-3 and  any  subgraph G′′=(V′′,E′′) of G′ with at least two vertices satisfies |E′′|≤2|V(E′′)|-3, where |V(E′′)|is number  of vertices that are end-vertices of the edges in E′′.Lemma 1 (see [7]).
LetG=(V,E) be a minimally rigid graph in ℜ2 and G′=(V′,E′) a subgraph of G such that |E′|=2|V′|-3. Then, G′ is minimally rigid.In directed graph, each agent is only aware of its own distance constraints and can move freely as long as these distance constraints are satisfied. Persistence is a directed notion and rigidity is an undirected notion.  The properties of directed graphs are described below.


## 2.1.1. Infinitesimally Rigid Graph

A representationp of  an undirected graph G(V,E) with vertex  set V and edge set E is a function p:V→ℜd, where d(2,3,…) is the dimension of Euclidean space. Representation p is rigid if there exists ε>0 such that for all realizations due to continuous deformations p′ of distance set induced by p and satisfying, d(p,p′)<ε (where, d(p,p′)=max∥p(i)-p′(i)∥, where, i∈V), there holds ∥p′(i)-p′(j)∥=∥p(i)-p(j)∥ for all i,j∈V. We simply say this phenomenon as congruence relationship between p and  p′.


## 2.1.2. Generically Rigid Graph

A graph is said to be generically rigid if almost all realizations due to continuous deformations are rigid. This definition of rigidity is to exclude some undesirable situations like certain collections of vertices are collinear during continuous deformations.


## 2.1.3. Minimally Rigid Graph

A rigid graph is minimally rigid when no single edge can be removed without losing its rigidity.


## 2.1.4. Laman’s Criteria (see [10])

If  an  undirected  graphG=(V,E) in ℜ2 with at least two vertices is rigid, then there exists a subset E′ of edges such that |E′|= 2|V|-3 and  any  subgraph G′′=(V′′,E′′) of G′ with at least two vertices satisfies |E′′|≤2|V(E′′)|-3, where |V(E′′)|is number  of vertices that are end-vertices of the edges in E′′.Lemma 1 (see [7]).
LetG=(V,E) be a minimally rigid graph in ℜ2 and G′=(V′,E′) a subgraph of G such that |E′|=2|V′|-3. Then, G′ is minimally rigid.In directed graph, each agent is only aware of its own distance constraints and can move freely as long as these distance constraints are satisfied. Persistence is a directed notion and rigidity is an undirected notion.  The properties of directed graphs are described below.


## 2.2. Definitions Associated with Persistent Graph

Suppose, for a directed graphG, desired distances dij>0 for all (i,j)∈E, edge set where i,j∈V, vertex set, and a realization p, then the following definitions are described.


### 2.2.1. Active Edge

The edge(i,j)∈E is active if ∥p(i)-p(j)∥=dij, that is, if the corresponding distance constraint is satisfied.


### 2.2.2. Fitting Position of a Vertex

The position of a vertexi∈V is fitting for any desired distance set {d} of G, if it is not possible to increase the set of active edges leaving i by changing the position of i while maintaining the positions of other vertices unchanged. Specifically, the position of vertex i, for a given realization p, is fitting if there is no p′∈ℜ2 for which the condition elaborated below is strictly satisfied:
(1){(i,j)∈E:∥p(i)-p(j)∥=dij}⊂{(i,j)∈E:∥p′-p(j)∥=dij}.


### 2.2.3. Fitting Realization of a Graph

A realization of a graph is a fitting realization for a certain distance set{d} if all the vertices are at fitting positions for {d}.


### 2.2.4. Constraint Consistent Graph

A realizationp of digraph G  is constraint consistent if there exists ε>0 such that any realization  p′ fitting for the distance set {d} induced by p  and satisfying d(p,p′)<ε is a realization of {d}. A graph is generically constraint consistent if almost all realizations are constraint consistent.


### 2.2.5. Persistent Graph

Realizationp of the digraph G having desired distances dij>0 for all {i,j} is persistent if there exists ε>0 such that every realization due to continuous deformation, p′ fitting for the distance set induced by p and satisfying d(p,p′)<ε is congruent to p.


### 2.2.6. Generically Persistent Graph

A graph is generically persistent if almost all possible realizations are persistent (same as elaborated in case of generically rigid).Theorem 1 (see [8]).
A realization is persistent if and only if it is rigid and constraint consistent.
A graph is generically persistent if and only if it is generically rigid and constraint consistent.


### 2.2.7. Minimally Persistent Graph

A persistent graph is minimally persistent if it is persistent and if no edge can be removed without losing persistence.Theorem 2 (see [8]).
A rigid graph is minimally persistent if and only if either of the two conditions elaborated below is satisfied.(i)
Out of all vertices, each of three vertices has one outgoing edge and each of rest vertices has two outgoing edges(ii)
Out of all vertices, one vertex has no outgoing edge; another one vertex has one outgoing edge and each of   rest vertices has two outgoing edges.Theorem 3 (see [8]).
An acyclic digraph is persistent if all the conditions elaborated below are satisfied.
Out of all vertices,(i)
one vertex has one outgoing edge. This vertex represents Leader,(ii)
another vertex has one outgoing edge which must be incident to the Leader. This vertex represents first follower,(iii)
each of rest vertices must have two or more number of outgoing edges.


## 2.2.1. Active Edge

The edge(i,j)∈E is active if ∥p(i)-p(j)∥=dij, that is, if the corresponding distance constraint is satisfied.


## 2.2.2. Fitting Position of a Vertex

The position of a vertexi∈V is fitting for any desired distance set {d} of G, if it is not possible to increase the set of active edges leaving i by changing the position of i while maintaining the positions of other vertices unchanged. Specifically, the position of vertex i, for a given realization p, is fitting if there is no p′∈ℜ2 for which the condition elaborated below is strictly satisfied:
(1){(i,j)∈E:∥p(i)-p(j)∥=dij}⊂{(i,j)∈E:∥p′-p(j)∥=dij}.


## 2.2.3. Fitting Realization of a Graph

A realization of a graph is a fitting realization for a certain distance set{d} if all the vertices are at fitting positions for {d}.


## 2.2.4. Constraint Consistent Graph

A realizationp of digraph G  is constraint consistent if there exists ε>0 such that any realization  p′ fitting for the distance set {d} induced by p  and satisfying d(p,p′)<ε is a realization of {d}. A graph is generically constraint consistent if almost all realizations are constraint consistent.


## 2.2.5. Persistent Graph

Realizationp of the digraph G having desired distances dij>0 for all {i,j} is persistent if there exists ε>0 such that every realization due to continuous deformation, p′ fitting for the distance set induced by p and satisfying d(p,p′)<ε is congruent to p.


## 2.2.6. Generically Persistent Graph

A graph is generically persistent if almost all possible realizations are persistent (same as elaborated in case of generically rigid).Theorem 1 (see [8]).
A realization is persistent if and only if it is rigid and constraint consistent.
A graph is generically persistent if and only if it is generically rigid and constraint consistent.


## 2.2.7. Minimally Persistent Graph

A persistent graph is minimally persistent if it is persistent and if no edge can be removed without losing persistence.Theorem 2 (see [8]).
A rigid graph is minimally persistent if and only if either of the two conditions elaborated below is satisfied.(i)
Out of all vertices, each of three vertices has one outgoing edge and each of rest vertices has two outgoing edges(ii)
Out of all vertices, one vertex has no outgoing edge; another one vertex has one outgoing edge and each of   rest vertices has two outgoing edges.Theorem 3 (see [8]).
An acyclic digraph is persistent if all the conditions elaborated below are satisfied.
Out of all vertices,(i)
one vertex has one outgoing edge. This vertex represents Leader,(ii)
another vertex has one outgoing edge which must be incident to the Leader. This vertex represents first follower,(iii)
each of rest vertices must have two or more number of outgoing edges.


## 2.3. Construction of Acyclic Minimally Persistent Graph

Acyclic minimally persistent graph is always constructed starting from a combination of two vertices, one is called leader and the other one is first follower, and an edge directed towards leader using Henneberg Sequence with only vertex additions.


## 2.4. Henneberg Construction (Directed Case) Containing Vertex Addition

It describes the sequence of graphsG2,G3,…G|V|, such that each graph Gi+1(i≥2) can be obtained by a vertex addition starting from Gi, where i is number of vertices and |V| is cardinality of vertex set of desired graph. Therefore, the procedure of drawing the graph using Henneberg sequence is described as(i)
start with a directed edge between two vertices.The vertex towards which edge is directed is called leader and remaining vertex is called first follower. The combination of these two a vertex with a directed edge is called initial leader-follower seed,(ii)
at each step of growing graph, add a new vertex,(iii)
join the new vertex to two old vertices (corresponding to leader and first follower) via two new edges, directed towards old vertices.Figure1 shows the construction procedure of formation of four agents.Figure 1
Henneberg construction for making leader-follower type formation from four agents “a”, “b”, “c”, and “d” (i) “a”, “b” are initial vertices with  one edge directed towards “a” (leader) from “b” (first follower) (ii) “c” is new vertex added to (i) (iii) “d” is new vertex added to (b).


## 3. Problem Formulation

For simplicity, we restrict our analysis only in quadrilateral formation of leader-follower structure taking into account four mobile autonomous point agents, in plane.  This  formation is shown in Figure2. One among them is leader which has no outgoing edge; that is, it is free to move along a specified path and does not have any responsibility to maintain any distance constraint from other agents, the second one as the first follower which has one outgoing edge; that is, it requires to maintain only one desired distance constraint from the leader, the third one as ordinary follower-1 which has two outgoing edges; that is, it requires to maintain two distance constraints, one of which is directed towards leader and other one towards first follower, the fourth one as ordinary follower-2 which has two outgoing edges; that is, it requires to maintain two distance constraints, one of which is directed towards first follower and other one towards ordinary follower-1.Figure 2
Quadrilateral formation of leader-follower structure. In this figure, the notations are as follows. R-1 denotes the leader and R-2 denotes the first follower, R-3 is for the ordinary follower-1 and R-4 is for ordinary follower-2.The different distance constraints among agents assumed are as follows:d1 is the desired distance maintained by first follower from leader, d2 is the desired distance maintained by ordinary follower-1 from first follower, d3 is the desired distance maintained by ordinary follower-1 from leader, d4 is the desired distance maintained by ordinary follower-2 from ordinary follower-1, and d5 is the desired distance maintained by ordinary follower-2 from first follower.We assume the desired distances among the agents satisfy noncollinear condition such that at least three point agents do not form a straight line.Assumptions.
(i) For simplicity, for each agent “i”, the kinematic model [5] of unicycle nonholonomic point agent is considered as follows:
(2)(ẋi,ẏi)=(vicosθi,visinθi),θ̇i=ωi,
where pi(t)=(xi(t),yi(t)) with i=1,2,3,4   denotes, the position of ith agent, θi(t), and vi(t) denote the orientation and translational velocity of the ith agent at each instant of time t, respectively. (xi(t),yi(t)) is with respect to an earth-fixed coordinate coordinate system.
(ii) Each agent can measure its position with respect to earth-fixed coordinate system by proper sensor arrangement.
(iii) First follower has position information of leader only with respect to its own point body system by using an active sensor (e.g., sonar).
(iv) Ordinary follower-1 has position information of leader and first follower only with respect to its own point body system by using an active sensor (e.g., sonar).
(v) Ordinary follower-2 has position information of first follower and ordinary follower-1 only with respect   to its own point body system by using an active sensor (e.g., sonar).
(vi) Each agent (except leader) can achieve the positions of neighbours w.r.t earth-fixed coordinate system from the relative informations of its neighbours and its own position w.r.t. earth-fixed coordinate   system.Now, the objective is to provide a control scheme for this triangular formation (starting from a non-collinear arrangement) such that during the motion of three robots for any mission, desired interagent distances are preserved.


## 4. Development of Control Law

Here, we intend to develop set of decentralized control laws for overall formation. Therefore, for each agent, we provide separate control law for continuous movement in autonomous manner based on local knowledge only of direction of its neighbours. Control Law for each agent is derived by optimizing corresponding objective function with given constraints involving desired distance constraints of formation. The unknown variables to be solved arex and y coordinates (which are continuous function of time) of position (with respect to earth-fixed coordinate system) of corresponding agent for which objective function is derived.Assumptions.
(i) Interagent distances are sufficiently large so that initial collision among robots can be avoided.
(ii) Initially positions of robots are not collinear (already stated).
(iii) During motion of formation, failure of sensors do not occur.
(iv) There is no time delay in sensing.
(v) Control input in the form of translational velocity and angular velocity (discussed later in this section) calculated from final and initial position of any agent should be necessarily generated by controller of each agent.When all the agents of given formation completes movement to a new set of position coordinates from an old set of position coordinates during certain period of time such that desired distances among agents are preserved for both set of positions and not any other distance preserving position set is available in between these two position sets during motion, then the movement of formation from the old set to new set of positions is called one complete displacement of formation.Before proceeding to develop the control laws for all agents, it is assumed that at any timeteach agent is maintaining its own distance constraints. Then how these agents move to their new positions for a complete displacement is discussed below.


### 4.1. Control Law for Leader

Leader does not need to maintain any distance constraint from any other agent in formation. A specified control action is provided for its dynamics such that it moves along a specified path (trajectory); that is, each position (at each instant of timet) coordinate is known (preprogrammed (known)/unknown) to computational system of the leader. Suppose at time t initial position coordinate for leader is assumed as ((x1In(t),y1In(t)). Let the leader move to a new position (x1f(t),y1f(t)), that is, the final position (rest point), in very small period of time dt  such that continuity preserves between(x1In(t),y1In(t)) & (x1f(t),y1f(t)); that is, the distance between these two positions is sufficiently small.This motion of the leader and corresponding movement of the first follower is shown in Figure 3. According to Figure 2,  ds⃗1=dx1î+dy1ĵ, where s⃗1(t), a vector field along the trajectory curve of the leader, î and ĵ are unit vectors along x and y direction of the global coordinate system, and dx1=x1f-x1In and dy1=y1f-y1In. It should be noted that (x1In(t),y1In(t)) and (x1f(t),y1f(t)) are always on s⃗1(t). Therefore, control input to reach its final position is(3)v⃗1(t)=ds⃗1(t)dt=dx1dtî+dy1dtĵ=v1x(t)î+v1y(t)ĵ,
where, dx1/dt=v1x(t) and dy1/dt=v1y(t).Figure 3
Motion of leader and first follower for a very small duration of time.The translational velocity control input duringdt=∥v⃗1∥=(v1x(t))2+(v1y(t))2 meter/second.The angular velocity control input (rad./sec) during same period of time is(4)ω1(t)=tan-1|dy1dx1|whendx1is+ve,dy1is+ve=(π-tan-1|dy1dx1|)whendx1is-ve,dy1is+ve=-(π-tan-1|dy1dx1|)whendx1is-ve,dy1is-ve=-tan-1|dy1dx1|whendx1is+ve,dy1is-ve=-π2orπ2whendx1=0anddy1is+veor-ve.


### 4.2. Control Law for First Follower

It may be noted that first follower has one outgoing edge; that is, it has to maintain only one distance constraint (desired distanced1) and that is to leader. Initial and final position coordinates for the leader are (x1In(t),y1In(t)), (x1f(t),y1f(t)), respectively. Then, first follower senses the disturbance in position of leader; that is, it senses error in desired distance constraint (d1) by sensing the final position of the leader staying at its initial position (x2In(t),y2In(t)). It tries to satisfy this distance constraint to leader. Therefore, suppose it moves to a rest point (final position) (x2f(t),y2f(t)) at the next instant of time dt after the instant during which the leader moves to its final position. During this movement of first follower, leader is assumed to be stationary at the position (x1f(t),y1f(t)). A condition is given to first follower such that only due to  disturbance in position of leader, first follower changes its position.To maintain the cohesive motion with the leader, distance of the first follower from the leader must be d1 at final position of both the agents. It can be formulated as follows:(5)(x1f(t)-x2f(t))2+(y1f(t)-y2f(t))2-d12=0,
where the values of x1f,y1f, and d1  are known to computational system of first follower. But it is clear from (5) that locus of the position of first follower is a circle. Hence, its rest position may be at anywhere on this circle. First follower may take its rest position for which it crosses over the leader and may collide with leader. Undesirable consequence of this phenomenon is that ordinary follower may collide with leader and first follower both for maintaining the distant constraints from both of them. Hence, to provide a control avoiding this unsafe situation, a restriction to the motion of first follower must be imposed, such that it reaches to a.In Figure3, ∥ds⃗2∥ is defined as the distance between the final and initial position of first follower. Here, s⃗2(t) a vector field along the trajectory curve of first follower. It should be noted that (x2In(t),y2In(t)), and (x2f(t),y2f(t)) are always on s⃗2(t). Then, we have(6)∥ds⃗2∥2=(x2f(t),x2In(t))2+(y2f(t),y2In(t))2.
Therefore, ∥ds⃗2∥ must be minimum such that first follower moves along the shortest path to its final position. Hence, the first follower follows the leader maintaining safe motion. Now, we intend to propose a control law for motion of first follower satisfying the aforesaid conditions.  Actually, the whole problem may be treated as an optimization problem, where minimization of objective function (7) under equality constraint (6) should be performed. And a control law based on this optimization is presented as
(7)v⃗2(t)=ds⃗2dt=dx2dtî+dy2dtĵ=v2x(t)î+v2y(t)ĵ,
where dx2/dt=v2x(t) and dy2/dt=v2y(t) and î and ĵ are unit vectors along x and y direction of the global coordinate system.The translational velocity control input =∥v⃗2∥=(v2x(t))2+(v2y(t))2 meter/second.The angular velocity (rad/sec) control input is(8)ω2(t)=tan-1|dy2dx2|whendx2is+ve,dy2is+ve=(π-tan-1|dy2dx2|)whendx2is-ve,dy2is+ve=-(π-tan-1|dy2dx2|)whendx2is-ve,dy2is-ve=-tan-1|dy2dx2|whendx2is+ve,dy2is-ve=-π2orπ2whendx2=0anddy2is+veor-ve.


### 4.3. Control Law for Ordinary Follower-1

Ordinary follower-1 tries to satisfy two distance constraints; that is, it has two outgoing edges, the first one(d2) is directed towards first follower and the second one (d3) is directed towards leader. Let the leader and first follower be placed at their corresponding final positions. The ordinary follower senses the disturbances in position of leader and first follower; that is, it senses error in desired distance constraints d2 and d3 by sensing the final position of the first follower and leader, respectively. It tries to satisfy these distance constraints to first follower and leader (as shown in Figure 4). Therefore, suppose the ordinary follower moves to its final position (rest point) at the next time instant of the time (dt) after the instant during which first follower moves to its final position. During this movement of ordinary follower, leader and first follower are assumed to be stationary at the position (x1f(t),y1f(t)) and (x2f(t),y2f(t)), respectively. At the final position, ordinary follower satisfies its distance constraints. Here, a condition is given to the ordinary follower such that only when disturbances in positions of both leader as well as first follower (not merely leader) occur; ordinary follower changes its position to final one. This final position is assumed (x3f(t),y3f(t)). The assumed initial position of the ordinary follower is (x3In(t),y3In(t)). Now, according to the desired distance constraints for it, two conditions are to be satisfied as given in(9)(x1f(t)-x3f(t))2+(y1f(t)-y3f(t))2-d32=0,(x2f(t)-x3f(t))2+(y2f(t)-y3f(t))2-d22=0,
where x1f,y1f,x2f,y2f,d2, and d3 are known to the computational system of ordinary follower. Hence, ∥ds⃗3∥ is defined as the distance between the final and initial position of ordinary follower. Here, s⃗3(t) a vector field along the trajectory curve of ordinary follower. It should be noted that (x3In(t),y3In(t)) and (x3f(t),y3f(t)) are always on s⃗3(t). Then, we define
(10)∥ds⃗3∥2=(x3f(t),x3In(t))2+(y3f(t),y3In(t))2.
Actually, (9) are equations of two circles. They meet at two different points. The ordinary follower will follow the leader and first follower maintaining safe motion and moves to any one meeting point such that ∥ds⃗3∥ is minimum. By maintaining ∥ds⃗3∥ minimum, ordinary follower moves along shortest path to its final position. Now, it is the need to propose a control law for motion of first follower satisfying the aforesaid conditions. Actually, the whole problem may be treated as an optimization problem, where minimization of objective function (10) under equality constraint (9) should be performed. And a control law based on this optimization is presented as                                                                                                      (11)v⃗3(t)=ds⃗3dt=dx3dtî+dy3dtĵ=v3x(t)î+v3y(t)ĵ,
where, dx3/dt=v3x(t),  dy3/dt=v3y(t), î and ĵ are unit vectors along x, and y direction of the global coordinate system.Figure 4
Motion of leader, first follower, and ordinary follower-1 for a very small duration of time.The translational velocity control input∥v⃗3∥=(v3x(t))2+(v3y(t))2 m/second.Angular velocity (rad./sec) control input is(12)ω3(t)=tan-1|dy3dx3|whendx3is+ve,dy3is+ve=(π-tan-1|dy3dx3|)whendx3is-ve,dy3is+ve=-(π-tan-1|dy3dx3|)whendx3is-ve,dy3is-ve=-tan-1|dy3dx3|whendx3is+ve,dy3is-ve=-π2orπ2whendx3=0anddy3is+veor-ve.


### 4.4. Control Law for Ordinary Follower-2

Ordinary follower-2 tries to satisfy two distance constraints; that is, it has two outgoing edges, first one(d4) is directed towards ordinary follower-1 and second one (d5) is directed towards first follower. Let the first follower and ordinary follower-1 be placed at their corresponding final positions. The ordinary follower senses the disturbances in position of first follower and ordinary follower-1; that is, it senses error in desired distance constraints d5 and d4 by sensing the final position of the first follower and ordinary follower-1. It tries to satisfy these distance constraints to first follower and ordinary follower-1. Therefore, suppose the ordinary follower-2 moves to its final position (rest point) at the next instant of the time (dt) after the instant during which ordinary follower-1 moves to its final position. During this movement of ordinary follower-2– ordinary follower-1, first follower and leader are assumed to be stationary at the position (x3f(t),y3f(t)),(x2f(t),y2f(t)), and (x1f(t),y1f(t)), respectively. At the final position, the ordinary follower satisfies its distance constraints. Here, a condition is given to the ordinary follower such that only when disturbances in positions of both leader as well as first follower (not merely leader) occur, ordinary follower changes its position to final one. This final position is assumed (x4f(t),y4f(t)). The assumed initial position of the ordinary follower is (x4In(t),y4In(t)). Now according to the desired distance constraints for it, two conditions are to be satisfied as given in(13)(x2f(t)-x4f(t))2+(y2f(t)-y4f(t))2-d52=0,(x3f(t)-x4f(t))2+(y3f(t)-y4f(t))2-d42=0,
where x2f,y2f,x3f,y3f,d4, and d5 are known to the computational system of ordinary follower. Hence, ∥ds⃗4∥ is defined as distance between the final and initial position of ordinary follower. Here, s⃗4(t) is a vector field along the trajectory curve of ordinary follower. It should be noted that (x4In(t),y4In(t)) and (x4f(t),y4f(t)) are always on s⃗4(t). Then we define (14)∥ds⃗4∥2=(x4f(t),x4In(t))2+(y4f(t),y4In(t))2.
Actually, (13) are equations of two circles. They meet at two different points. The ordinary follower will follow the leader and first follower maintaining safe motion and moving to any one meeting point such that ∥ds⃗4∥ is minimum. By maintaining ∥ds⃗4∥ minimum, ordinary follower moves along shortest path to its final position. Now it is the need to propose a control law for motion of first follower satisfying aforesaid conditions. Actually, the whole problem may be treated as an optimization problem where minimization of objective function (14) under equality constraint (13) should be performed. And a control law based on this optimization is presented as                                                                                                      (15)v⃗4(t)=ds⃗4dt=dx4dtî+dy4dtĵ=v4x(t)î+v4y(t)ĵ,
where dx4/dt=v4x(t) and dy4/dt=v4y(t) and î and ĵ are unit vectors along x and y directions of the global coordinate system.The translational velocity control input∥v⃗4∥=(v4x(t))2+(v4y(t))2 m/second.Angular velocity (rad./sec) control input is(16)ω4(t)=tan-1|dy4dx4|,whendx4is+ve,dy4is+ve=(π-tan-1|dy4dx4|),whendx4is-ve,dy4is+ve=-(π-tan-1|dy4dx4|),whendx4is-ve,dy4is-ve=-tan-1|dy3dx3|,whendx3is+ve,dy3is-ve=-π2orπ2,whendx3=0anddy3is+veor-ve.Remarks 1.
Hence, from the previous discussion it is concluded that for each complete displacement of considered quadrilateral formation, at the end of first instant of timedt the leader moves to its desired final position. Then, at the end of next instant dt (which is the second one) the first follower moves to its final desired position to maintain distance constraints to the leader, during which leader is kept stationary. At end of another instant dt (which is third one) the ordinary follower-1 reaches to its final position to maintain distance constraints to both leader and first follower. At end of another instant dt (which is fourth one) the ordinary follower-2 reaches to its final position to maintain distance constraints to both leader and first follower. Therefore, the agents are not reaching their corresponding final position exactly at the same time. Consequently, during the period from “after the starting of first instant” and “before the end of fourth  instant” desired distances are not preserved among the agents, rather the first follower, ordinary follower-1 and ordinary follower-2 try to form up. So it may be concluded that after every 4*dt time, the desired formation is obtained. Therefore, each agent start to move to its new position (to be in a new position set) after every 3*dt time. That is there is a discontinuous motion that occurs for every agent. Therefore, for formation of n number of agents, after every n*dt time the desired formation is maintained. Each agent start to move to its new position (to be in a new position set) after every (n-1)*dt time. However, if the dt is chosen as very small we may assume that all the agents reach their corresponding final (new) positions during first instant of time dt (almost same time taken by leader to reach its desired final position) and during each complete displacement of formation, and continuous motion of formation is maintained. Consequently we may also assume all the agents move with continuous motion. Simulation results in the next section are also done based on this assumption.


## 4.1. Control Law for Leader

Leader does not need to maintain any distance constraint from any other agent in formation. A specified control action is provided for its dynamics such that it moves along a specified path (trajectory); that is, each position (at each instant of timet) coordinate is known (preprogrammed (known)/unknown) to computational system of the leader. Suppose at time t initial position coordinate for leader is assumed as ((x1In(t),y1In(t)). Let the leader move to a new position (x1f(t),y1f(t)), that is, the final position (rest point), in very small period of time dt  such that continuity preserves between(x1In(t),y1In(t)) & (x1f(t),y1f(t)); that is, the distance between these two positions is sufficiently small.This motion of the leader and corresponding movement of the first follower is shown in Figure 3. According to Figure 2,  ds⃗1=dx1î+dy1ĵ, where s⃗1(t), a vector field along the trajectory curve of the leader, î and ĵ are unit vectors along x and y direction of the global coordinate system, and dx1=x1f-x1In and dy1=y1f-y1In. It should be noted that (x1In(t),y1In(t)) and (x1f(t),y1f(t)) are always on s⃗1(t). Therefore, control input to reach its final position is(3)v⃗1(t)=ds⃗1(t)dt=dx1dtî+dy1dtĵ=v1x(t)î+v1y(t)ĵ,
where, dx1/dt=v1x(t) and dy1/dt=v1y(t).Figure 3
Motion of leader and first follower for a very small duration of time.The translational velocity control input duringdt=∥v⃗1∥=(v1x(t))2+(v1y(t))2 meter/second.The angular velocity control input (rad./sec) during same period of time is(4)ω1(t)=tan-1|dy1dx1|whendx1is+ve,dy1is+ve=(π-tan-1|dy1dx1|)whendx1is-ve,dy1is+ve=-(π-tan-1|dy1dx1|)whendx1is-ve,dy1is-ve=-tan-1|dy1dx1|whendx1is+ve,dy1is-ve=-π2orπ2whendx1=0anddy1is+veor-ve.


## 4.2. Control Law for First Follower

It may be noted that first follower has one outgoing edge; that is, it has to maintain only one distance constraint (desired distanced1) and that is to leader. Initial and final position coordinates for the leader are (x1In(t),y1In(t)), (x1f(t),y1f(t)), respectively. Then, first follower senses the disturbance in position of leader; that is, it senses error in desired distance constraint (d1) by sensing the final position of the leader staying at its initial position (x2In(t),y2In(t)). It tries to satisfy this distance constraint to leader. Therefore, suppose it moves to a rest point (final position) (x2f(t),y2f(t)) at the next instant of time dt after the instant during which the leader moves to its final position. During this movement of first follower, leader is assumed to be stationary at the position (x1f(t),y1f(t)). A condition is given to first follower such that only due to  disturbance in position of leader, first follower changes its position.To maintain the cohesive motion with the leader, distance of the first follower from the leader must be d1 at final position of both the agents. It can be formulated as follows:(5)(x1f(t)-x2f(t))2+(y1f(t)-y2f(t))2-d12=0,
where the values of x1f,y1f, and d1  are known to computational system of first follower. But it is clear from (5) that locus of the position of first follower is a circle. Hence, its rest position may be at anywhere on this circle. First follower may take its rest position for which it crosses over the leader and may collide with leader. Undesirable consequence of this phenomenon is that ordinary follower may collide with leader and first follower both for maintaining the distant constraints from both of them. Hence, to provide a control avoiding this unsafe situation, a restriction to the motion of first follower must be imposed, such that it reaches to a.In Figure3, ∥ds⃗2∥ is defined as the distance between the final and initial position of first follower. Here, s⃗2(t) a vector field along the trajectory curve of first follower. It should be noted that (x2In(t),y2In(t)), and (x2f(t),y2f(t)) are always on s⃗2(t). Then, we have(6)∥ds⃗2∥2=(x2f(t),x2In(t))2+(y2f(t),y2In(t))2.
Therefore, ∥ds⃗2∥ must be minimum such that first follower moves along the shortest path to its final position. Hence, the first follower follows the leader maintaining safe motion. Now, we intend to propose a control law for motion of first follower satisfying the aforesaid conditions.  Actually, the whole problem may be treated as an optimization problem, where minimization of objective function (7) under equality constraint (6) should be performed. And a control law based on this optimization is presented as
(7)v⃗2(t)=ds⃗2dt=dx2dtî+dy2dtĵ=v2x(t)î+v2y(t)ĵ,
where dx2/dt=v2x(t) and dy2/dt=v2y(t) and î and ĵ are unit vectors along x and y direction of the global coordinate system.The translational velocity control input =∥v⃗2∥=(v2x(t))2+(v2y(t))2 meter/second.The angular velocity (rad/sec) control input is(8)ω2(t)=tan-1|dy2dx2|whendx2is+ve,dy2is+ve=(π-tan-1|dy2dx2|)whendx2is-ve,dy2is+ve=-(π-tan-1|dy2dx2|)whendx2is-ve,dy2is-ve=-tan-1|dy2dx2|whendx2is+ve,dy2is-ve=-π2orπ2whendx2=0anddy2is+veor-ve.


## 4.3. Control Law for Ordinary Follower-1

Ordinary follower-1 tries to satisfy two distance constraints; that is, it has two outgoing edges, the first one(d2) is directed towards first follower and the second one (d3) is directed towards leader. Let the leader and first follower be placed at their corresponding final positions. The ordinary follower senses the disturbances in position of leader and first follower; that is, it senses error in desired distance constraints d2 and d3 by sensing the final position of the first follower and leader, respectively. It tries to satisfy these distance constraints to first follower and leader (as shown in Figure 4). Therefore, suppose the ordinary follower moves to its final position (rest point) at the next time instant of the time (dt) after the instant during which first follower moves to its final position. During this movement of ordinary follower, leader and first follower are assumed to be stationary at the position (x1f(t),y1f(t)) and (x2f(t),y2f(t)), respectively. At the final position, ordinary follower satisfies its distance constraints. Here, a condition is given to the ordinary follower such that only when disturbances in positions of both leader as well as first follower (not merely leader) occur; ordinary follower changes its position to final one. This final position is assumed (x3f(t),y3f(t)). The assumed initial position of the ordinary follower is (x3In(t),y3In(t)). Now, according to the desired distance constraints for it, two conditions are to be satisfied as given in(9)(x1f(t)-x3f(t))2+(y1f(t)-y3f(t))2-d32=0,(x2f(t)-x3f(t))2+(y2f(t)-y3f(t))2-d22=0,
where x1f,y1f,x2f,y2f,d2, and d3 are known to the computational system of ordinary follower. Hence, ∥ds⃗3∥ is defined as the distance between the final and initial position of ordinary follower. Here, s⃗3(t) a vector field along the trajectory curve of ordinary follower. It should be noted that (x3In(t),y3In(t)) and (x3f(t),y3f(t)) are always on s⃗3(t). Then, we define
(10)∥ds⃗3∥2=(x3f(t),x3In(t))2+(y3f(t),y3In(t))2.
Actually, (9) are equations of two circles. They meet at two different points. The ordinary follower will follow the leader and first follower maintaining safe motion and moves to any one meeting point such that ∥ds⃗3∥ is minimum. By maintaining ∥ds⃗3∥ minimum, ordinary follower moves along shortest path to its final position. Now, it is the need to propose a control law for motion of first follower satisfying the aforesaid conditions. Actually, the whole problem may be treated as an optimization problem, where minimization of objective function (10) under equality constraint (9) should be performed. And a control law based on this optimization is presented as                                                                                                      (11)v⃗3(t)=ds⃗3dt=dx3dtî+dy3dtĵ=v3x(t)î+v3y(t)ĵ,
where, dx3/dt=v3x(t),  dy3/dt=v3y(t), î and ĵ are unit vectors along x, and y direction of the global coordinate system.Figure 4
Motion of leader, first follower, and ordinary follower-1 for a very small duration of time.The translational velocity control input∥v⃗3∥=(v3x(t))2+(v3y(t))2 m/second.Angular velocity (rad./sec) control input is(12)ω3(t)=tan-1|dy3dx3|whendx3is+ve,dy3is+ve=(π-tan-1|dy3dx3|)whendx3is-ve,dy3is+ve=-(π-tan-1|dy3dx3|)whendx3is-ve,dy3is-ve=-tan-1|dy3dx3|whendx3is+ve,dy3is-ve=-π2orπ2whendx3=0anddy3is+veor-ve.


## 4.4. Control Law for Ordinary Follower-2

Ordinary follower-2 tries to satisfy two distance constraints; that is, it has two outgoing edges, first one(d4) is directed towards ordinary follower-1 and second one (d5) is directed towards first follower. Let the first follower and ordinary follower-1 be placed at their corresponding final positions. The ordinary follower senses the disturbances in position of first follower and ordinary follower-1; that is, it senses error in desired distance constraints d5 and d4 by sensing the final position of the first follower and ordinary follower-1. It tries to satisfy these distance constraints to first follower and ordinary follower-1. Therefore, suppose the ordinary follower-2 moves to its final position (rest point) at the next instant of the time (dt) after the instant during which ordinary follower-1 moves to its final position. During this movement of ordinary follower-2– ordinary follower-1, first follower and leader are assumed to be stationary at the position (x3f(t),y3f(t)),(x2f(t),y2f(t)), and (x1f(t),y1f(t)), respectively. At the final position, the ordinary follower satisfies its distance constraints. Here, a condition is given to the ordinary follower such that only when disturbances in positions of both leader as well as first follower (not merely leader) occur, ordinary follower changes its position to final one. This final position is assumed (x4f(t),y4f(t)). The assumed initial position of the ordinary follower is (x4In(t),y4In(t)). Now according to the desired distance constraints for it, two conditions are to be satisfied as given in(13)(x2f(t)-x4f(t))2+(y2f(t)-y4f(t))2-d52=0,(x3f(t)-x4f(t))2+(y3f(t)-y4f(t))2-d42=0,
where x2f,y2f,x3f,y3f,d4, and d5 are known to the computational system of ordinary follower. Hence, ∥ds⃗4∥ is defined as distance between the final and initial position of ordinary follower. Here, s⃗4(t) is a vector field along the trajectory curve of ordinary follower. It should be noted that (x4In(t),y4In(t)) and (x4f(t),y4f(t)) are always on s⃗4(t). Then we define (14)∥ds⃗4∥2=(x4f(t),x4In(t))2+(y4f(t),y4In(t))2.
Actually, (13) are equations of two circles. They meet at two different points. The ordinary follower will follow the leader and first follower maintaining safe motion and moving to any one meeting point such that ∥ds⃗4∥ is minimum. By maintaining ∥ds⃗4∥ minimum, ordinary follower moves along shortest path to its final position. Now it is the need to propose a control law for motion of first follower satisfying aforesaid conditions. Actually, the whole problem may be treated as an optimization problem where minimization of objective function (14) under equality constraint (13) should be performed. And a control law based on this optimization is presented as                                                                                                      (15)v⃗4(t)=ds⃗4dt=dx4dtî+dy4dtĵ=v4x(t)î+v4y(t)ĵ,
where dx4/dt=v4x(t) and dy4/dt=v4y(t) and î and ĵ are unit vectors along x and y directions of the global coordinate system.The translational velocity control input∥v⃗4∥=(v4x(t))2+(v4y(t))2 m/second.Angular velocity (rad./sec) control input is(16)ω4(t)=tan-1|dy4dx4|,whendx4is+ve,dy4is+ve=(π-tan-1|dy4dx4|),whendx4is-ve,dy4is+ve=-(π-tan-1|dy4dx4|),whendx4is-ve,dy4is-ve=-tan-1|dy3dx3|,whendx3is+ve,dy3is-ve=-π2orπ2,whendx3=0anddy3is+veor-ve.Remarks 1.
Hence, from the previous discussion it is concluded that for each complete displacement of considered quadrilateral formation, at the end of first instant of timedt the leader moves to its desired final position. Then, at the end of next instant dt (which is the second one) the first follower moves to its final desired position to maintain distance constraints to the leader, during which leader is kept stationary. At end of another instant dt (which is third one) the ordinary follower-1 reaches to its final position to maintain distance constraints to both leader and first follower. At end of another instant dt (which is fourth one) the ordinary follower-2 reaches to its final position to maintain distance constraints to both leader and first follower. Therefore, the agents are not reaching their corresponding final position exactly at the same time. Consequently, during the period from “after the starting of first instant” and “before the end of fourth  instant” desired distances are not preserved among the agents, rather the first follower, ordinary follower-1 and ordinary follower-2 try to form up. So it may be concluded that after every 4*dt time, the desired formation is obtained. Therefore, each agent start to move to its new position (to be in a new position set) after every 3*dt time. That is there is a discontinuous motion that occurs for every agent. Therefore, for formation of n number of agents, after every n*dt time the desired formation is maintained. Each agent start to move to its new position (to be in a new position set) after every (n-1)*dt time. However, if the dt is chosen as very small we may assume that all the agents reach their corresponding final (new) positions during first instant of time dt (almost same time taken by leader to reach its desired final position) and during each complete displacement of formation, and continuous motion of formation is maintained. Consequently we may also assume all the agents move with continuous motion. Simulation results in the next section are also done based on this assumption.


## 5. Simulation Studies

The control laws (3), (7), (11), and (15) for different agents have been tested successfully via three cases of simulations for specified formation with consideration of d1=d2=d3=d4=d5=2 meters. Length of each time instant is considered as 0.01 second for simulation during optimization as described in what follows.


### 5.1. Specific Assumptions in Different Cases

#### 5.1.1. Case I and Case II

In CaseI and Case II, the assumptions are as follows:(i)
Initial position coordinates are (2,4), (2,2), (3.732,3), and (3.732,1) for leader, first follower, Ordinary Follower-1, and Ordinary Follower-2, respectively;(ii)
Translational velocity input to the leader is 1 meter/second;(iii)
Distance travelled by leader is 1.5 meter in each case;


#### 5.1.2. Case III

Here the considerations are as follows:(i)
Initial position coordinates are (2,4), (2,2), (3.732,3), and (3.732,1) for leader, First follower, Ordinary Follower-1, and Ordinary Follower-2, respectively;(ii)
Leader is assumed to move along a sinusoidal path. The equations that describe that sinusoidal path of the leader is considered as(17)x(t)=0.03tmeter,y(t)=sin(0.03t)meter;(iii)
Time travelled by the leader is 100 second;Control laws given in (3), (7), (11), and (15) require the final position of the corresponding agent at each instant of time during their motion. For the leader, the final position at each instant of time is available as the path is specified for it, but for other agents, these positions must be calculated. To find out the final position at each instant of time t, the controller in each case requires optimization of an quadratic objective function under one or two quadratic equality constraints as described in Section 4. Several optimization methods are available for this purpose. Our choice here is to exploit Sequential Quadratic Programming (SQP) as it is one of the most popular and robust algorithms for nonlinear continuous optimization. The method is based on solving a series of subproblems designed to minimize a quadratic model of the objective subject to a linearization of the constraints. In the proposed controller, the objective functions are chosen as quadratic whilst the constraints are taken as nonlinear quadratic which can be linearized during course of optimization procedure. At the beginning of each instant of time t (i.e., at the beginning of a complete displacement of the whole formation), the position of each agent is used as the initial position in control law of that particular agent. This position coordinate is also assumed as starting point of that agent’s complete iterative procedure (in optimization process using SQP) for finding out its final position.That iterative procedure follows the steps elaborated below:(i)
making a Quadratic Programming (QP) subproblem (based on a quadratic approximation of  the Lagrangian function) using nonlinear objective function and equality constraints,(ii)
solving that Quadratic Programming (QP) sub problem at each iteration,(iii)
during (ii) updating an estimate of the Hessian of the Lagrangian at each iteration using the BFGS (Broyden–Fletcher–Goldfarb–Shanno) formula [16, 17],(iv)
quadratic Programming solution at each iteration performing appropriate Line Search using Merit Function [16–18].In this work, MATLAB optimization toolbox has been used for solving the above said optimization problem using Sequential Quadratic Programming. However, alternatively other packages  such as NPSOL, NLPQL, OPSYC, and OPTIMA can be used.


### 5.2. Description of Simulation Result in Different Cases

#### 5.2.1. Case I

We provide straight line motion to the Leader such that it moves along globalx-axis. The paths followed by all agents during motion of formation are shown in Figure 5. Distances (d1,d2,d3,d4, and d5) are observed maintained at specified values.Figure 5
Straight line motion of formation of four agents with leader-follower structure.


#### 5.2.2. Case II

We provide 0.09 degree (approximately 0.00157 rad/sec) angular velocity input to the motion of leader.The translational velocity input is constant at 1 m/s. In this case complete path of the leader may be considered as part of a complete circle. For this case paths of  all agents during motion are shown in Figure6. From the simulation studies of Case II it is found that the distances among agents are maintained at the desired values during the motion of the formation as in case of Case-I.  Plots of d1 versus t, d2 versus t and d3 versus t, d4 versus t, d5 versus t are shown in Figures 7, 8, 9, 10, and 11, respectively. These plots are also observed in Case-I also.Figure 6
Motion of  formation  of four agents with  leader-follower structure, with 0.09 degree (approximately 0.00157 radian) orientation at each instant of time provided in the motion of leader.Figure 7
Plot of distance  between leader and first follower versus time.Figure 8
Plot of distance between first follower and ordinary follower 1 versus time.Figure 9
Plot of distance between leader and ordinary follower 1 versus time.Figure 10
Plot of distance between ordinary follower 1  and ordinary follower 2 versus time.Figure 11
Plot of distance between first follower and and ordinary follower 2 versus time.


#### 5.2.3. Case III

As the path of leader is sinusoidal the velocity and orientation (corresponding angular velocity) change at each instant. The changes in translational velocity and angular velocity (which are control inputs for leader) along sinusoidal path are shown in Figure12, Figure 13, and Figure 10. for travelling time 250 seconds. For this case path followed by first follower and ordinary follower along with leader are shown in Figure 11. Here also in this case similar observations are made on the maintaining of the distance constraints during the motion of the formation as in Case I and Case II.Figure 12
Plot of  angular velocity (rad/second) versus time (second) for sinusoidal motion of  leader.Figure 13
Plot of  velocity versus time for sinusoidal motion  of leader.Figure 14
Motion of triangular formation of leader-follower structure, with sinusoidal motion of leader. In this figure during motion of formation, paths of leader, first follower, and ordinary follower are shown. During motion interagent distances are maintained at desired value.


## 5.1. Specific Assumptions in Different Cases

### 5.1.1. Case I and Case II

In CaseI and Case II, the assumptions are as follows:(i)
Initial position coordinates are (2,4), (2,2), (3.732,3), and (3.732,1) for leader, first follower, Ordinary Follower-1, and Ordinary Follower-2, respectively;(ii)
Translational velocity input to the leader is 1 meter/second;(iii)
Distance travelled by leader is 1.5 meter in each case;


### 5.1.2. Case III

Here the considerations are as follows:(i)
Initial position coordinates are (2,4), (2,2), (3.732,3), and (3.732,1) for leader, First follower, Ordinary Follower-1, and Ordinary Follower-2, respectively;(ii)
Leader is assumed to move along a sinusoidal path. The equations that describe that sinusoidal path of the leader is considered as(17)x(t)=0.03tmeter,y(t)=sin(0.03t)meter;(iii)
Time travelled by the leader is 100 second;Control laws given in (3), (7), (11), and (15) require the final position of the corresponding agent at each instant of time during their motion. For the leader, the final position at each instant of time is available as the path is specified for it, but for other agents, these positions must be calculated. To find out the final position at each instant of time t, the controller in each case requires optimization of an quadratic objective function under one or two quadratic equality constraints as described in Section 4. Several optimization methods are available for this purpose. Our choice here is to exploit Sequential Quadratic Programming (SQP) as it is one of the most popular and robust algorithms for nonlinear continuous optimization. The method is based on solving a series of subproblems designed to minimize a quadratic model of the objective subject to a linearization of the constraints. In the proposed controller, the objective functions are chosen as quadratic whilst the constraints are taken as nonlinear quadratic which can be linearized during course of optimization procedure. At the beginning of each instant of time t (i.e., at the beginning of a complete displacement of the whole formation), the position of each agent is used as the initial position in control law of that particular agent. This position coordinate is also assumed as starting point of that agent’s complete iterative procedure (in optimization process using SQP) for finding out its final position.That iterative procedure follows the steps elaborated below:(i)
making a Quadratic Programming (QP) subproblem (based on a quadratic approximation of  the Lagrangian function) using nonlinear objective function and equality constraints,(ii)
solving that Quadratic Programming (QP) sub problem at each iteration,(iii)
during (ii) updating an estimate of the Hessian of the Lagrangian at each iteration using the BFGS (Broyden–Fletcher–Goldfarb–Shanno) formula [16, 17],(iv)
quadratic Programming solution at each iteration performing appropriate Line Search using Merit Function [16–18].In this work, MATLAB optimization toolbox has been used for solving the above said optimization problem using Sequential Quadratic Programming. However, alternatively other packages  such as NPSOL, NLPQL, OPSYC, and OPTIMA can be used.


## 5.1.1. Case I and Case II

In CaseI and Case II, the assumptions are as follows:(i)
Initial position coordinates are (2,4), (2,2), (3.732,3), and (3.732,1) for leader, first follower, Ordinary Follower-1, and Ordinary Follower-2, respectively;(ii)
Translational velocity input to the leader is 1 meter/second;(iii)
Distance travelled by leader is 1.5 meter in each case;


## 5.1.2. Case III

Here the considerations are as follows:(i)
Initial position coordinates are (2,4), (2,2), (3.732,3), and (3.732,1) for leader, First follower, Ordinary Follower-1, and Ordinary Follower-2, respectively;(ii)
Leader is assumed to move along a sinusoidal path. The equations that describe that sinusoidal path of the leader is considered as(17)x(t)=0.03tmeter,y(t)=sin(0.03t)meter;(iii)
Time travelled by the leader is 100 second;Control laws given in (3), (7), (11), and (15) require the final position of the corresponding agent at each instant of time during their motion. For the leader, the final position at each instant of time is available as the path is specified for it, but for other agents, these positions must be calculated. To find out the final position at each instant of time t, the controller in each case requires optimization of an quadratic objective function under one or two quadratic equality constraints as described in Section 4. Several optimization methods are available for this purpose. Our choice here is to exploit Sequential Quadratic Programming (SQP) as it is one of the most popular and robust algorithms for nonlinear continuous optimization. The method is based on solving a series of subproblems designed to minimize a quadratic model of the objective subject to a linearization of the constraints. In the proposed controller, the objective functions are chosen as quadratic whilst the constraints are taken as nonlinear quadratic which can be linearized during course of optimization procedure. At the beginning of each instant of time t (i.e., at the beginning of a complete displacement of the whole formation), the position of each agent is used as the initial position in control law of that particular agent. This position coordinate is also assumed as starting point of that agent’s complete iterative procedure (in optimization process using SQP) for finding out its final position.That iterative procedure follows the steps elaborated below:(i)
making a Quadratic Programming (QP) subproblem (based on a quadratic approximation of  the Lagrangian function) using nonlinear objective function and equality constraints,(ii)
solving that Quadratic Programming (QP) sub problem at each iteration,(iii)
during (ii) updating an estimate of the Hessian of the Lagrangian at each iteration using the BFGS (Broyden–Fletcher–Goldfarb–Shanno) formula [16, 17],(iv)
quadratic Programming solution at each iteration performing appropriate Line Search using Merit Function [16–18].In this work, MATLAB optimization toolbox has been used for solving the above said optimization problem using Sequential Quadratic Programming. However, alternatively other packages  such as NPSOL, NLPQL, OPSYC, and OPTIMA can be used.


## 5.2. Description of Simulation Result in Different Cases

### 5.2.1. Case I

We provide straight line motion to the Leader such that it moves along globalx-axis. The paths followed by all agents during motion of formation are shown in Figure 5. Distances (d1,d2,d3,d4, and d5) are observed maintained at specified values.Figure 5
Straight line motion of formation of four agents with leader-follower structure.


### 5.2.2. Case II

We provide 0.09 degree (approximately 0.00157 rad/sec) angular velocity input to the motion of leader.The translational velocity input is constant at 1 m/s. In this case complete path of the leader may be considered as part of a complete circle. For this case paths of  all agents during motion are shown in Figure6. From the simulation studies of Case II it is found that the distances among agents are maintained at the desired values during the motion of the formation as in case of Case-I.  Plots of d1 versus t, d2 versus t and d3 versus t, d4 versus t, d5 versus t are shown in Figures 7, 8, 9, 10, and 11, respectively. These plots are also observed in Case-I also.Figure 6
Motion of  formation  of four agents with  leader-follower structure, with 0.09 degree (approximately 0.00157 radian) orientation at each instant of time provided in the motion of leader.Figure 7
Plot of distance  between leader and first follower versus time.Figure 8
Plot of distance between first follower and ordinary follower 1 versus time.Figure 9
Plot of distance between leader and ordinary follower 1 versus time.Figure 10
Plot of distance between ordinary follower 1  and ordinary follower 2 versus time.Figure 11
Plot of distance between first follower and and ordinary follower 2 versus time.


### 5.2.3. Case III

As the path of leader is sinusoidal the velocity and orientation (corresponding angular velocity) change at each instant. The changes in translational velocity and angular velocity (which are control inputs for leader) along sinusoidal path are shown in Figure12, Figure 13, and Figure 10. for travelling time 250 seconds. For this case path followed by first follower and ordinary follower along with leader are shown in Figure 11. Here also in this case similar observations are made on the maintaining of the distance constraints during the motion of the formation as in Case I and Case II.Figure 12
Plot of  angular velocity (rad/second) versus time (second) for sinusoidal motion of  leader.Figure 13
Plot of  velocity versus time for sinusoidal motion  of leader.Figure 14
Motion of triangular formation of leader-follower structure, with sinusoidal motion of leader. In this figure during motion of formation, paths of leader, first follower, and ordinary follower are shown. During motion interagent distances are maintained at desired value.


## 5.2.1. Case I

We provide straight line motion to the Leader such that it moves along globalx-axis. The paths followed by all agents during motion of formation are shown in Figure 5. Distances (d1,d2,d3,d4, and d5) are observed maintained at specified values.Figure 5
Straight line motion of formation of four agents with leader-follower structure.


## 5.2.2. Case II

We provide 0.09 degree (approximately 0.00157 rad/sec) angular velocity input to the motion of leader.The translational velocity input is constant at 1 m/s. In this case complete path of the leader may be considered as part of a complete circle. For this case paths of  all agents during motion are shown in Figure6. From the simulation studies of Case II it is found that the distances among agents are maintained at the desired values during the motion of the formation as in case of Case-I.  Plots of d1 versus t, d2 versus t and d3 versus t, d4 versus t, d5 versus t are shown in Figures 7, 8, 9, 10, and 11, respectively. These plots are also observed in Case-I also.Figure 6
Motion of  formation  of four agents with  leader-follower structure, with 0.09 degree (approximately 0.00157 radian) orientation at each instant of time provided in the motion of leader.Figure 7
Plot of distance  between leader and first follower versus time.Figure 8
Plot of distance between first follower and ordinary follower 1 versus time.Figure 9
Plot of distance between leader and ordinary follower 1 versus time.Figure 10
Plot of distance between ordinary follower 1  and ordinary follower 2 versus time.Figure 11
Plot of distance between first follower and and ordinary follower 2 versus time.


## 5.2.3. Case III

As the path of leader is sinusoidal the velocity and orientation (corresponding angular velocity) change at each instant. The changes in translational velocity and angular velocity (which are control inputs for leader) along sinusoidal path are shown in Figure12, Figure 13, and Figure 10. for travelling time 250 seconds. For this case path followed by first follower and ordinary follower along with leader are shown in Figure 11. Here also in this case similar observations are made on the maintaining of the distance constraints during the motion of the formation as in Case I and Case II.Figure 12
Plot of  angular velocity (rad/second) versus time (second) for sinusoidal motion of  leader.Figure 13
Plot of  velocity versus time for sinusoidal motion  of leader.Figure 14
Motion of triangular formation of leader-follower structure, with sinusoidal motion of leader. In this figure during motion of formation, paths of leader, first follower, and ordinary follower are shown. During motion interagent distances are maintained at desired value.


## 6. Conclusions

In this paper, a new algorithm using a set of decentralized control laws based on optimization (using Sequential Quadratic Programming) of distance constraints has been proposed for the motion control of aleader-follower structure type formation of multiple mobile autonomous agents. The effectiveness of the proposed control schemes have been demonstrated through a number simulation studies. During the motion of formation, the interagent distances are maintained at desired values.The above described control design strategy may be extended to the formation of any number of autonomous agents in the form of leader-follower structure in which  each agent (except First Follower) observes the distances of only two neighboring agents to which it needs to maintain distance constraints. The control strategy considering kinematics (other than unicycle nonholonomic point model) of each agent and phenomenon of obstacle avoidance are currently in contemplation. The future work in this context may be to pursue the stability study of proposed control design.



---

*Source: 360925-2010-07-28.xml*