# Robust Distributed Planar Formation Control for Higher-Order Holonomic and Nonholonomic Agents

We present a distributed formation control strategy for agents with a variety of dynamics to achieve a desired planar formation. The proposed strategy is fully distributed, does not require inter-agent communication or a common sense of orientation, and can be implemented using relative position measurements acquired by agents in their local coordinate frames. We show how the control designed for agents with the simplest dynamical model, i.e., the single-integrator dynamics, can be extended to holonomic agents with higher-order dynamics such as quadrotors, and nonholonomic agents such as unicycles and cars. We prove that the proposed strategy is robust to saturations in the input, unmodeled dynamics, and switches in the sensing topology. We further show that the control is relaxed in the sense that agents can move along a rotated and scaled control direction without affecting the convergence to the desired formation. This observation is used to design a distributed collision avoidance strategy. We demonstrate the proposed approach in simulations and further present a distributed robotic platform to test the strategy experimentally. Our experimental platform consists of off-the-shelf equipment that can be used to test and validate other multi-agent algorithms. The code and implementation instructions for this platform are available online and free.

## Authors

• 9 publications
• 3 publications
• 3 publications
• 4 publications
• ### Robust 3D Distributed Formation Control with Application to Quadrotors

We present a distributed control strategy for a team of quadrotors to au...
09/01/2018 ∙ by Kaveh Fathian, et al. ∙ 0

• ### Prescribed Performance Distance-Based Formation Control of Multi-Agent Systems (Extended Version)

This paper presents a novel control protocol for robust distance-based f...
11/17/2019 ∙ by Farhad Mehdifar, et al. ∙ 0

• ### Formation Control and Network Localization via Distributed Global Orientation Estimation in 3-D

In this paper, we propose a novel distributed formation control strategy...
08/10/2017 ∙ by Byung-Hun Lee, et al. ∙ 0

• ### Distributed Kalman Filters for Relative Formation Control of Mobile Agents

Formation control (FC) of multi-agent plays a critical role in a wide va...
10/12/2021 ∙ by Martijn van der Marel, et al. ∙ 0

• ### Influencing Flock Formation in Low-Density Settings

Flocking is a coordinated collective behavior that results from local se...
04/23/2018 ∙ by Daniel Y. Fu, et al. ∙ 0

• ### Nonlinear Model Predictive Control for 3D Formation of Multirotor Micro Aerial Vehicles with Relative Sensing in Local Coordinates

The complex tasks such as surveillance, construction, search and rescue ...
04/07/2019 ∙ by I. Kagan Erunsal, et al. ∙ 0

• ### Distributed formation control for manipulator end-effectors

We present three classes of distributed formation controllers for achiev...
07/08/2021 ∙ by Haiwen Wu, et al. ∙ 0

##### This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

## Supplementary Material

Video of paper summary, simulations, and experiments is available at . Code for simulations and the distributed multi-robot platform can be download from .

## I Introduction

Technological advances in recent years has made it increasingly possible to deploy a large number of agents to cooperatively execute tasks such as environmental mapping and monitoring [1, 2], delivery of goods [3], and object manipulation [4, 5, 6]. In these applications, the ability to bring the agents to a desired geometric shape is a fundamental building block upon which more sophisticated maneuvering and navigation policies are constructed. By assigning local control laws to individual agents, distributed formation control strategies ensure that a desired geometric shape emerge from the collective behavior of agents. Compared to the centralized methods, distributed strategies have better scalability, naturally parallelized computation, resilience to communication loss and hardware failure, and robustness to uncertainty and lack of global measurements.

There exists a large body of work on formation control of multi-agent systems [7, 8, 9], however, depending on the restrictions and assumptions considered in the problem, existing literature can be divided into smaller categories. Examples are classes of methods that require position measurements in a global coordinate frame [10, 11, 12], a common heading direction [13, 14], inter-agent communication [15, 16], or a complete inter-agent sensing graph [17]. Unlike the aforementioned methods, a certain class of formation control strategies do not require these assumptions, and a desired formation can be achieved without global measurements or communication. Distance-based [18, 19, 20], bearing-based [21, 22, 14, 23], and barycentric coordinate-based [24, 25, 26, 27, 28, 29, 30] formation control strategies are among the methods that fall in the latter class. In a barycentric coordinate-based formation control strategy, in contrast to distance- and bearing-based formations, the desired formation is defined in terms of both distances and bearing angles that are subtended from agents to their neighbors. Since common sensors such as laser scanners, radars, sonars, and stereo cameras can provide both angle and distance measurements, the focus of this work is such desired formations.

In this work, we present a unified distributed control strategy for planar formations of agents with a variety of dynamics. In particular, we consider agents with linear or linearizable holonomic dynamics, such as quadrotors, and further extend the control to agents with nonholonomic dynamics such as unicycles and cars. We start by formulating a semidefinite programming (SDP) problem to determine control gains for agents with the single-integrator model. We show that this design strategy enjoys several robustness properties such as robustness to saturations in the input, switching in the sensing topology, and disturbances in the control direction. We show that if agents move along a control direction that is scaled by an arbitrary positive value, and rotated by an arbitrary amount up to , convergence to the desired formation is still guaranteed. This observation is exploited later to design a fully distributed collision avoidance strategy. The control for single-integrator agents is extended subsequently to agents with higher-order holonomic dynamics, where we show the set of control gains computed from the SDP problem can be used directly to achieve the formation without having to redesign the control. As an example, we use the gains designed for single-integrator agents to achieve a planar formation of quadrotors. Following the same philosophy, we show that the control gains can be used directly for agents with nonholonomic dynamics such as unicycles and cars. Furthermore, the proposed nonholonomic control remains robust to input saturations and unmodeled/unknown dynamics. To vet the theoretical results, several simulations are presented for quadrotors, differential drive robots with unicycle dynamics, and cars, where it is shown that agents achieve a desired formation without collision. To typify the results further, the proposed control strategy is tested experimentally on a distributed differential-drive wheeled robotic platform with different numbers of robots and desired formations.

### I-a Contributions and Related Work

The formation control strategy used in this work is inspired by Lin et al. [31, 32], who presented the general theory for agents with single-integrator dynamics with directed and undirected sensing topologies. In this paper, we focus on a subclass of sensing topologies that are undirected and universally rigid. Under this assumption, the contributions of this paper include a novel SDP approach to design necessary control gains. The particular gain matrices found from SDP lead to robustness of the formation to saturated inputs, disturbances, and unmodeled dynamics, which previous work did not explore. Furthermore, we explore the extension of the control to agents with higher-order dynamics such as quadrotors, and nonholonomic dynamics such as unicycles and cars.

The approach used for extending the control strategy to agents with unicycle model is inspired by Zhao et al. [33, 34], where application of gradient-descent control strategies for agents with nonholonomic dynamics are studied, and an obstacle avoidance strategy for a distance-based formation control is proposed. Contributions of this paper over aforementioned work include incorporating a barycentric coordinate-based formation control strategy, extension to agents with car dynamics, and proving robustness of the proposed control to unmodeled dynamics.

Another contribution above previous works is a distributed, robust, collision avoidance strategy that unlike [33] that considers only static obstacles deals with moving obstacles (i.e., adjacent agents). Many distributed formation control literature do not consider collision avoidance (e.g., [31, 32]), and existing collision avoidance approaches are often not fully distributed. Collision avoidance, together with extension of the control to agents with unicycle and car dynamics and thorough simulation and experimental studies further distinguish this paper from our previous work [35, 36].

We further present a portable and low-cost distributed robotic platform that consists of off-the-shelf components (see Fig. 1). This platform is used to validate our proposed formation control experimentally and can be used to test other multi-agent control strategies. Since the platform is distributed, the number of robots used for an experiment is only limited by the available resources. The code and technical implementation details related to this platform are made available online and free, and are accessible in the Supplementary Material section.

In summary, the main contributions of this paper are

• A distributed planar formation control for vehicles with a large variety of holonomic and nonholonomic dynamics.

• Eliminating the need for global position measurements, common heading direction, inter-agent communication, or complete sensing graph.

• Guaranteed global convergence to the desired formation with provable robustness to saturated input, unmodeled dynamics, and disturbances.

• A fully distributed and heuristic collision avoidance algorithm incorporated in the formation control strategy.

• A low-cost distributed robotic platform with off-the-shelf components for validation of distributed control algorithms.

The paper is organized as follows. The notation and assumptions used throughout the paper are introduced in Section II. In Section III, the control strategy for agents with single-integrator dynamics is introduced, the SDP gain design algorithm is presented, and robustness of the proposed approach to perturbations and saturated input is proven. Gains designed for single-integrator agents are used in Section IV to extend the control to agents with higher order linear or linearizable holonomic dynamics such as quadrotors. In Sections V and VI, the control is further extended to agents with nonholonomic unicycle and car dynamics, where robustness to saturations in the input and unmodeled dynamics is shown. Additional topics such as collision avoidance, time-varying sensing topologies, and scale of the formation are discussed in Section VII. Lastly, in Sections VIII and IX simulation and experimental results are presented to typify the proposed strategy.

## Ii Notation and Assumptions

We consider a team of agents with the inter-agent sensing topology described by an undirected graph , where is the set of vertices, and is the set of edges. Each vertex of the graph represents an agent. An edge from vertex to indicates that agents and can measure the relative position of each other in their local coordinate frames. In such a case, agents and are called neighbors. The set of neighbors of agent is denoted by . We denote by

the set of eigenvalues of matrix

.

Throughout this paper we assume that the desired formation and the sensing topology are such that achieving the formation is physically feasible. In particular, we assume that the sensing topology is undirected and universally rigid. This assumption is both necessary and sufficient [37, 32] for guaranteeing the existence of control gains that are computed from the proposed SDP approach. We further point out that by “formation” we imply a desired geometric shape up to a positive scale factor. To fix the scale of the formation to a desired value an augmented control is presented in Section VII.

## Iii Formation Control for Single-integrator Dynamics

In this section, we present the distributed formation control strategy introduced in [31] for agents with single-integrator dynamics. We then propose a novel design approach for finding stabilizing control gains by formulating a convex optimization problem. The results of this section are a cornerstone for formation control of agents with more complicated dynamic models that are discussed in the subsequent sections.

### Iii-a Control Strategy

The single-integrator dynamics can be described as

 ˙qi=ui, (1)

where is the coordinate of agent in a common global coordinate frame (unknown to the agent), and is the control law. To bring the agents to a desired formation, the control law for each agent can be chosen as

 ui:=∑j∈NiAij(qj−qi), (2)

where are constant control gain matrices that will be designed later, and each has the form

 Aij:=[aijbij−bijaij],aij,bij∈R. (3)

Thanks to the commutativity property of the matrices, the closed-loop dynamics with coordinates and expressed in agents’ local coordinate frames is identical to the case that coordinates are expressed in a global coordinate frame (for more details see [35]). The geometric intuition behind the control strategy (2) is explained in the following example.

###### Example 1.

Consider three agents in Fig. 2, where agents 2 and 3 are neighbors of agent 1. Let and denote the position of neighbors in agent 1’s local coordinate frame, and assume that control gains for agent 1 are given as

 A12=[2−112],A13=[−13−3−1]. (4)

From (2

), the control vector for agent 1 is computed as

 u1=A12q2+A13q3=[1−2], (5)

which is shown in the figure and can be interpreted geometrically as follows. At any instance of time, agent 1 moves along the control vector with the speed equal to the vector’s magnitude. Note that due to the special structure of gain matrices , they can be interpreted as scaled rotation matrices that rotate and scale vectors connecting agent 1 to its neighbors. One can see that this action is independent of agent 1’s local coordinate frame position and orientation, hence, and can replaced by their coordinates in a global coordinate frame for analysis.

Let and denote the aggregate state and control vectors of all agents, respectively. Using this notation, the closed-loop dynamics under the control strategy (2) can be expressed as

 ˙q=Aq, (6)
 A=⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣−∑nj=2A1jA12⋯A1nA21−∑nj=1j≠2A2j⋯A2n⋮⋱⋮An1An2⋯−∑n−1j=1Anj⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦∈R2n×2n,

where for the

block is defined as a zero matrix. Note that the

diagonal blocks of are the negative sum of the rest of the blocks on the same row. Hence, has block Laplacian structure, and it follows that vectors

 1:=[1,0,1,0,…,1,0]⊤∈R2n¯1:=[0,1,0,1,…,0,1]⊤∈R2n (7)

are in the kernel111If , the kernel or null space of is defined as . of .

Let denote the coordinates of agents at the desired formation (the orientation, translation, and scale of the desired formation can be chosen arbitrarily). Further, let denote the coordinates of agents when the desired formation is rotated by degrees about the origin. The following theorem states the conditions that guarantee the convergence of agents to the desired formation.

###### Theorem 1.

Consider agents with single-integrator dynamics (1) and control (2). If the ’s are chosen such that in (6)

1. [label=()]

2. has null vectors and ,

3. Other than the four zero eigenvalues associated with these null vectors, all eigenvalues of have negative real parts,

then, agents globally converge to the desired formation.

###### Proof.

The formal proof can be found in our previous work [35, Thm. 1], and is based on the observation that if nonzero eigenvalues of matrix have negative real parts, all trajectories of the linear system exponentially converge to the kernel of . The kernel of is nothing but all rotations, translations, and non-negative scale factors of the desired formation. ∎

Note that in Theorem 1 convergence to the desired formation implies that the formation is achieved up to a rotation and translation in the global coordinate frame, and a non-negative scale factor. As we will discuss in Section VII, in applications where the scale is important, the control can be augmented to attain the desired scale. We should point out that null vectors correspond to the case where all agents coincide, which can be interpreted as the desired formation achieved with the zero scale. It can be shown that the set of initial conditions that converge to this coinciding equilibrium is measure zero. Notice that in practice, trajectories of agents cannot remain on a measure zero set (due to noise, disturbances, etc.), thus, coinciding agents are not of practical concern.

###### Remark 1.

The topological conditions that guarantee the existence of a symmetric matrix satisfying the conditions of Theorem 1 are studied in [24, Thm. 3.2], which presents the necessary and sufficient condition222To be specific, the necessary and sufficient condition is for a generic desired formation. For certain desired formations matrix exists even when the graph is not universally rigid. that the sensing graph is undirected and universally rigid. Throughout this paper, we assume that this condition is met.

### Iii-B Control Gain Design

Given a desired formation for agents with a universally rigid sensing topology, we present a novel algorithm to find control gain matrices that meet the conditions of Theorem 1. Let be the set of bases for the kernel of , where are given in (7), is the coordinates of agents at the desired formation, and is the rotated coordinates about the origin. Let

be the (full) singular value decomposition (SVD) of

, where

 U=[¯Q, Q] ∈R2n×2n, (8)

with defined as the last columns of .

###### Lemma 1.

Using in (8), define

 ¯A:=Q⊤AQ ∈R(2n−4)×(2n−4). (9)

Matrices and have the same set of nonzero eigenvalues.

Proof of Lemma 1 follows by observing that

is an orthogonal matrix, and

. Therefore is the projection of onto the orthogonal complement of . Effectively, the projection operation in (9) removes the zero eigenvalues of .

For an undirected sensing topology, by imposing the constraints , in (3) matrix can be designed to be symmetric. Note that from Remark 1 existence of such matrix is guaranteed. In this case, is symmetric, and its eigenvalues are real and can be ordered. Hence, can be computed by solving the optimization problem

 A = argmaxaij,bij λ1(−¯A) (10) subject to AN=0

where denote the smallest eigenvalue of a matrix. Note that (10) is a concave maximization problem [38], and can be formulated as the SDP problem

 A = argmaxaij,bij,γ γ (11) subject to ¯A+γI⪯0 AN=0

where the first constraint is a linear matrix inequality. In recent years, effective algorithms for numerically solving SDPs have been developed and are now available [39]. For our simulations, we used CVX [40], which is available free online, to solve problem (10). The proposed approach for finding stabilizing gain matrix is summarized in Algorithm 1.

We point out that the optimization approach used here relies on a centralized paradigm and knowledge of the sensing topology. Once gains are computed, they can be transmitted to agents before the mission. Hence, agents can use the prescribed gains during the mission to achieve the desired formation without a need for communication. If agents can communicate, distributed optimization techniques can be used to solve (11) without relying on the complete knowledge of the sensing topology. An example of such distributed design can be found in [24].

### Iii-C Robustness to Perturbations

An important characteristic of the proposed design approach is that the gains found via (10) lead to significant robustness to perturbations. For instance, noise and disturbances can cause an agent to move in a direction that is different from the desired control vector. The following theorem shows that by using the gains computed from (10) positive scaling and rotation of the control vectors (up to ) does not affect the convergence.

###### Theorem 2.

Given control gain matrix designed from (10), let denote a rotation matrix of radians, and be a scalar. If and , under the perturbed control

 ui:=ciRi∑j∈NiAij(qj−qi) (12)

single-integrator agents achieve the desired formation.

###### Proof.

We will use Definition 1 and Lemmas 2, 3, 4 that are given in the Appendix. Under the perturbed control (12), the aggregate dynamics can be represented by , where is a block diagonal matrix that contains the perturbation terms. Due to the special block structure of and , they can equivalently be represented in complex notation by denoting the blocks as a complex number . In this notation, diagonal entries of are , and since , have positive real parts. This, together with Lemma 3, implies that is contained in the right hand plane (RHP). By design, the complex representation of is Hermitian and negative semidefinite. Thus, is positive semidefinite, and from Lemma 4 we conclude that is contained in the union of the RHP and the imaginary axis. Thus, is a stable matrix, and trajectories of converge to the kernel of . Since is full-rank, null space of and are identical, which shows that the desired formation is achieved. ∎

### Iii-D Robustness to Saturated Input

In practice, the velocity of an agent cannot take arbitrary large values. Thus, any large control input will be saturated by a maximum feasible/allowed speed. This, however, does not affect convergence of agents to the desired formation.

###### Theorem 3.

Consider single-integrator agents with dynamics (1) and assume that is a real positive scalar. If is saturated such that , then under the control (2) the desired formation is achieved globally.

###### Proof.

To model the input saturation we can define the diagonal matrix with diagonal elements

 (S)ii={1if |ui|≤umaxumax|ui|if |ui|>umax. (13)

As illustrated in Fig. 3, diagonal elements of can be considered as functions that saturate any large input to the maximum value . The closed-loop dynamics under the saturated input can be expressed in the vector form via

 ˙q=SAq (14)

System (14) should be understood as a family of switched dynamical systems, for which the solution is well-defined in the Filippov sense (see Chapter 2 in [41] for more details). To show that this system is uniformly stable, we consider

 V:=−12q⊤Aq≥0 (15)

as a common Lyapunov function candidate for all systems. Note that since is negative semidefinite, is a positive semidefinite scalar valued function. Time derivative of along the trajectory of (14) is

 ˙V =−q⊤A˙q =−q⊤ASAq =−(S12Aq)⊤(S12Aq)=−∥S12Aq∥2 ≤0, (16)

where is the diagonal matrix with elements given by the square root of diagonal entries of . Note that all diagonal elements of are strictly positive, hence is well-defined. Since is a positive semidefinite, continuously differentiable, and radially unbounded function, from Lemma 6, Corollary 1 (found in the Appendix), and LaSalle’s invariance principle it follows that all trajectories of (14) converge to the zero set of , which is the kernel of . Thus, the desired formation is achieved. ∎

###### Remark 2.

To reject steady state errors, the control law (2) can be augmented by an integrator term as

 ui:=k0∑j∈NiAij(qj−qi)+k1∫t0∑j∈NiAij(qj−qi)dτ, (17)

where are scalar control gains. It can be shown that if , this augmented control rejects constant input/output disturbances (see [36, Sec. III-D] for more details).

## Iv Formation Control for Agents with Higher-order Dynamics

In this section, we extend the single-integrator control strategy to agents with higher-order dynamics. We show how the control gains designed for single-integrator agents in Section III-B can be used directly to control higher-order agents without having to find a new control strategy or redesign the gains by solving a new optimization problem. We assume that the aggregate higher-order dynamics of all agents can be expressed in the controllable canonical form

 ⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣˙q˙q(1)⋮˙q(m−1)˙q(m)⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦=⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣0I0⋯000I0⋮⋱⋮000I000⋯0⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣qq(1)⋮q(m−1)q(m)⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦+⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣00⋮0I⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦u, (18)

where is the aggregate position vector of all agents, denotes the ’th derivative of , and

is the identity matrix. Although at first sight (

18) may seem restrictive, in fact, it encompasses a large class of agents. This is because by coordinate transformation techniques such as feedback linearization, or approximation techniques such as linearization and gain scheduling, dynamics of many systems can be expressed as (18).

Given the gain matrix designed for agents with the single-integrator model, the control for agents with dynamics (18) can be chosen as

 u=k0Aq+k1Aq(1)+⋯+kmAq(m), (19)

where are scalar control gains. Note that (19) can be implemented locally using only the relative measurements (due to the special structure of ). Under this control, the closed-loop dynamics is given by

 ⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣˙q˙q(1)⋮˙q(m-1)˙q(m)⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦=⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣0I0⋯000I0⋮⋱⋮000Ik0Ak1Ak2A⋯kmA⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦¯A⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣qq(1)⋮q(m-1)q(m)⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦. (20)
###### Theorem 4.

If for all nonzero roots of the polynomial equation

 λm+1−kmμλm−⋯−k1μλ−k0μ=0 (21)

have negative real parts, then under control (19), agents with dynamics (18) globally converge to the desired formation.

###### Proof.

The closed-loop state matrix given in (20) is in the (block) controllable canonical form. From this observation and Lemma 5 (found in the Appendix), the characteristic equation of is given by

 det(λm+1I−kmλmA−⋯−k1λA−k0A) =∏μ∈eig(A)(λm+1I−kmλmμ−⋯−k1λμ−k0μ)=0, (22)

which from the assumption of the theorem implies that the nonzero eigenvalues of have negative real parts. ∎

To find gains that satisfy the condition of Theorem 4 the Routh-Hurwitz criterion can be used.

###### Remark 3.

In above analysis, the control can alternatively be chosen as

 u=k0Aq+k1q(1)+⋯+kmq(m). (23)

In this case, agents do not need measurements of states for their neighbors. Note that (23) can also be implemented using only the local relative measurements.

###### Example 2.

Quadrotor dynamics can be described as [42]

 ⎡⎢⎣¨x¨y¨z⎤⎥⎦ =R⎡⎢⎣00ua⎤⎥⎦−⎡⎢⎣00g⎤⎥⎦, (24a) ⎡⎢ ⎢⎣˙φ˙θ˙ψ⎤⎥ ⎥⎦ =T⎡⎢⎣ωxωyωz⎤⎥⎦, (24b) ⎡⎢⎣˙ωx˙ωy˙ωz⎤⎥⎦ =J−1⎡⎢⎣uxuyuz⎤⎥⎦−J−1⎛⎜⎝⎡⎢⎣ωxωyωz⎤⎥⎦×J⎡⎢⎣ωxωyωz⎤⎥⎦⎞⎟⎠, (24c)

where, as illustrated in Fig. 4, are coordinates of the quadrotor’s center of mass in the world frame, are roll, pitch, yaw angles that describe the orientation of the quadrotor body frame in the world frame, are the angular body rates about associated body axes, is the gravitational constant, is a mass-normalized thrust input, and

are moment inputs applied to the airframe about corresponding body axes. Further,

is the mass moment of inertia matrix, is the rotation matrix parameterized in terms of -- Euler angles as

 R:=⎡⎢ ⎢⎣cψcθ−sφsψsθ−cφsψcψsθ+cθsφsψcθsψ+cψsφsθcφcψsψsθ−cψcθsφ−cφsθsφcφcθ⎤⎥ ⎥⎦, (25)

where are respectively shorthand notations for functions, and

 T:=1cφ⎡⎢⎣cφcθ0−cθsφsθcφ−cθsφ−sθ0cθ⎤⎥⎦∈R3×3 (26)

is the transformation matrix that relates the roll, pitch, yaw derivatives to the angular velocities in the body frame.

Linearizing dynamics (24) about the hover point , , , and gives the quadrotor linearized dynamics

 δ¨x= gδθδ¨θ=uyδ¨y=−gδφδ¨φ=uxδ¨z= uaδ¨ψ=uz (27)

where represents a small displacement about the equilibrium/linearization point. Since we are interested in 2D formations, we only consider the lateral dynamics along the - axes, and separately control the quadrotor’s altitude by setting to stabilize it at a constant altitude.

To represent the dynamics in the canonical form (18), we define

 δ¯θi:=gδθi, δ¯φi:=−gδφi, ¯uyi:=guyi, ¯uxi:=−guxi,

where subscript is used to distinguish agents. Using this notation, (27) can be described in the vector form as

 ˙pi=⎡⎢ ⎢ ⎢⎣0I0000I0000I0000⎤⎥ ⎥ ⎥⎦pi+⎡⎢ ⎢ ⎢⎣000I⎤⎥ ⎥ ⎥⎦ui (28)

where

 pi:=[δxi,δyi,δ˙xi,δ˙yi,δ¯θi,δ¯φi,δ˙¯θi,δ˙¯φi]⊤,ui:=[¯uyi,¯uxi]⊤, (29)

are respectively the state and control vectors, and is the identity matrix. Note that by defining the aggregate position vector as , dynamics of agents can be expressed in the form (18). This model will be used in the Simulations section to achieve a desired formation.

## V Formation Control for Agents with Unicycle Dynamics

Motion profile of many vehicles, e.g., differential drive robots or fixed-wing aerial vehicles, can be described via the unicycle model. In this section, we introduce the unicycle model and propose a formation control strategy to achieve the desired formation using the control gains that were designed for single-integrator agents. We then show that the desired formation is achieved even if the input is saturated, and the control strategy is robust to unknown dynamics that are not considered in the kinematic unicycle model. We assume henceforth that a symmetric negative semidefinite gain matrix is designed for the desired formation by solving the optimization problem (10).

### V-a Unicycle Dynamics

The unicycle model can be described by

 ˙xi=vicos(θi)˙yi=visin(θi)˙θi=ωi (30)

where are coordinates of of agent in a global coordinate frame (unknown to agent), is the heading angle with respect to the -axis of the global coordinate frame, and scalars are respectively the linear and angular velocities of the agent, as illustrated in Fig. 5. In the kinematic model it is assumed that and can be directly controlled.

To derive an alternative formulation for (30) that is more suitable for the formation control design, we define the heading vector and its perpendicular vector as

 hi:=[cos(θi)sin(θi)],h⊥i:=[−sin(θi)cos(θi)]. (31)

Seeing that , we can describe the dynamics (30) equivalently by

 ˙qi=hivi˙hi=h⊥iωi. (32)

Let be the aggregate position vector of all agents, and similarly let , , be the aggregate heading, linear velocity, and angular velocity vectors, respectively. Using this notation, the motion of all agents can be collectively expressed as

 ˙q=Hv˙h=H⊥ω. (33)

where matrices are defined as

 H:=⎡⎢ ⎢ ⎢ ⎢ ⎢⎣h10⋯00h20⋮⋱⋮00⋯hn⎤⎥ ⎥ ⎥ ⎥ ⎥⎦,H⊥:=⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣h⊥10⋯00h⊥20⋮⋱⋮00⋯h⊥n⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦. (34)

### V-B Control Strategy

Consider a team of unicycle agents with dynamics (32). We seek to assign controls and such that agents autonomously achieve a desired formation. Let be a symmetric gain matrix designed in Section III-B for agents with single-integrator model to achieve the desired formation. Further, let given in (2) be the desired holonomic control direction for agent . The proposed control strategy is as follows. Each agent computes the control vector and its projections along the heading direction and its perpendicular vector . These projections are then used as the linear and angular velocity commands. That is, the linear and angular velocity control are given by

 vi:=h⊤iuiωi:=h⊥⊤iui, (35)

as illustrated in Fig. 5. Note that (35) can be implemented using only the local relative position measurements acquired by agents.

###### Theorem 5.

Let be a symmetric gain matrix designed for single-integrator agents. Under the control (35), unicycle agents globally converge to the desired formation.

###### Proof.

By replacing the control (35) in (32), the closed-loop dynamics can be expressed in the vector form by

 ˙q=HH⊤Aq˙h=H⊥H⊥⊤Aq. (36)

Since is symmetric and negative semidefinite, we can consider

 V:=−12q⊤Aq≥0 (37)

as a Lyapunov function candidate. Time derivative of along the trajectory of (36) is

 ˙V =−q⊤A˙q =−q⊤AHH⊤Aq =−(H⊤Aq)⊤(H⊤Aq)=−∥H⊤Aq∥2 ≤0, (38)

which implies that the system is stable. To show convergence to the desired formation we use the LaSalle’s invariance principle and show that converges to the kernel of . Since implies that , by LaSalle’s invariance principle converges to the largest invariant set in . Thus, one of the following cases must hold:

1. [label=()]

2. ,

Case (i) implies that the desired formation is achieved. In case (ii), implies that there exists constants , with at least one , such that

 Aq=⎡⎢ ⎢ ⎢ ⎢ ⎢⎣c1h⊥1c2h⊥2⋮cnh⊥n⎤⎥ ⎥ ⎥ ⎥ ⎥⎦≠0. (39)

Since , from (36) we get . Thus, and are constant, and from (39) we conclude that (and thus ) is constant for all nonzero . From the definition of in (34), one can see that has full column rank. Therefore, it does not have a right null vector, and from (39) we have . This shows , and consequently from (36) we get . This implies that the heading vectors are not fixed and rotating, which is a contradiction and shows that case (ii) cannot happen. ∎

###### Remark 4.

From the closed-loop dynamics (36) one can see that when agents are at the desired formation, i.e., , we have and hence the heading directions do not vary. This implies that the controller drives agents to the desired formation, however their heading at the desired formation is not controlled and can take an arbitrary value. If desired, a supplementary control can be added to regulate heading angles after convergence.

###### Remark 5.

It is worth pointing out that the control (35) can drive unicycle agents with a cart attached to the desired formation. In this case the position and orientation of the attached carts are not controlled. The dynamics of a unicycle agent with cart attached is similar to the dynamics of a car, which is studied in the next section.

### V-C Robustness to Saturated Input

In practice, the linear and angular velocities that an agent can execute are often limited to a certain range. We show that under a saturated input, convergence of agents to the desired formation is not affected.

###### Theorem 6.

Consider the unicycle model (32) and assume that are two real positive scalars. If and are saturated such that , , then under the control (35) the desired formation is achieved globally.

###### Proof.

To model the input saturation we can define the diagonal matrices with diagonal elements

 (S)ii={1if |vi|≤vmaxvmax|vi|if |vi|>vmax (40)

and

 (41)

Elements of can be considered as functions that saturate any large input to the maximum allowed values (cf. Fig. 3 for saturated single-integrator control). The closed-loop dynamics under the saturated input can be expressed in the vector form via

 ˙q=HSH⊤Aq˙h=H⊥EH⊥⊤Aq. (42)

System (42) should be understood as a family of switched dynamical systems, for which we choose as a common Lyapunov function candidate. Time derivative of along the trajectory of (42) is

 ˙V =−q⊤A˙q =−q⊤AHSH⊤Aq =−(S12H⊤Aq)⊤(S12H⊤Aq)=−∥S12H⊤Aq∥2 ≤0. (43)

Thus, satisfies conditions of Lemma 6 and Corollary 1, and from LaSalle’s invariance principle it follows that all trajectories of (42) converge to the zero set of , which is the set of all desired formations. ∎

### V-D Robustness to Unmodeled Dynamics

In practice, the linear and angular velocities of a vehicle cannot change instantaneously. The dynamic behavior of these velocities, which is not accounted for in the unicycle model (30), can be modeled by

 ˙xi=vicos(θi)˙yi=visin(θi)˙vi=−avi+bsi˙θi=ωi˙ωi=−cωi+dri (44)

where are controls to adjust the linear and angular velocities, and are strictly positive scalars, which depend on the vehicle’s inertia, motor dynamics, friction, etc., and are in general unknown. We show that unmodeled velocity dynamics does not affect the convergence of the unicycle control strategy (35). That is, applying the control

 si:=h⊤iuiri:=h⊥⊤iui (45)

in (44) results in the desired formation.

###### Theorem 7.

Let be a symmetric gain matrix designed for single-integrator agents. Under the control (45) agents with dynamics (44) globally converge to the desired formation.

###### Proof.

Substituting (45) in (44) gives the closed-loop dynamics in the vector form as

 ˙q=Hv˙v=bH⊤Aq−av˙h=H⊥ω˙ω=dH⊥⊤Aq−cω (46)

where and are aggregate linear and angular velocity vectors, respectively. Consider the Lyapunov function candidate

 V:=−b2q⊤Aq+12v⊤v ≥ 0. (47)

Time derivative of along the trajectory of (46) is

 ˙V =−bq⊤A˙q+˙v⊤v =−bq⊤AHv+bq⊤AHv−av⊤v =−av⊤v ≤ 0. (48)

Similar to the proof of Theorem 5, we use LaSalle’s invariance principle and show that the largest invariant set consists of the desired formations. By setting to find the invariant sets, from (V-D) we get , which implies that . Consequently, from (46) we should have that , which implies one of the following two cases:

1. [label=()]

2. , .

Case (i) implies that the desired formation is achieved, where by replacing in (46) the dynamics reduces to

 ˙h=H⊥ω˙ω=−cω. (49)

This shows , and therefore converges to zero and converges to a constant value. Thus, the set , which consists of the desired formations, is an invariant set.

We now show that case (ii) cannot be an invariant set. Using a similar reasoning to the proof of Theorem 5, from , , and dynamics (46) one can conclude that in this case , , and are all constant and nonidentical to zero. Further, , which from (46) implies that and hence . This, together with having full column rank implies that , which is a contradiction to being constant. This shows that case (ii) is not an invariant set, which concludes the proof. ∎

###### Remark 6.

In (44), we assumed that have the same value for all agents. This assumption was made to simplify the notation and does not affect the generality of the results. One can assign a different value to these parameters for each agent and use the same analysis to prove the convergence.

###### Remark 7.

In (44), the assumption implies that agents are zero-input stable, which often holds in practice. However, for the control can be modified using the velocity feedback as

 si:=−ks(vi−h⊤iui)ri:=h⊥⊤iui, (50)

where is a positive control gain. Using similar analysis to the proof of Theorem 7, one can show that if is chosen such that , the agents converge to the desired formation. Lastly, with multiplying by the sign of , respectively, the assumption can be relaxed to only knowing the sign of these parameters.

## Vi Formation Control for Agents with Car Dynamics

Cars are another common platform for which attaining a desired formation is often of interest (e.g., in intelligent transportation systems). In this section, we present a control strategy for agents with both front and rear-wheel drive car model. We then show that the convergence is not affected when the input is saturated, and the control is robust to unmodeled dynamics. Similar to previous section, henceforth we assume that a symmetric negative semi-definite control gain matrix is designed by solving the optimization problem (10).

### Vi-a Control Strategy for Front-Wheel Drive Car

Consider an agent with the front-wheel drive car model as illustrated in Fig. 6. The motion of this agent can be described by the dynamics

 ˙xi=vicos(θi+φi)˙yi=visin(θi+φi)˙θi