Angle-Constrained Formation Control for Circular Mobile Robots

05/10/2020 ∙ by Nelson P. K. Chan, et al. ∙ University of Groningen Universidad Complutense de Madrid 0

In this letter, we investigate the formation control problem of mobile robots moving in the plane where, instead of assuming robots to be simple points, each robot is assumed to have the form of a disk with equal radius. Based on interior angle measurements of the neighboring robots' disk, which can be obtained from low-cost vision sensors, we propose a gradient-based distributed control law and show the exponential convergence property of the associated error system. By construction, the proposed control law has the appealing property of ensuring collision avoidance between neighboring robots. We also present simulation results for a team of four circular mobile robots forming a rectangular shape.



There are no comments yet.


page 1

page 2

page 3

page 4

This week in AI

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

I Introduction

Formation control studies the problem of controlling the spatial deployment of teams of mobile robots in order to achieve a specific geometric shape. By maintaining a certain geometric shape, the teams can subsequently be deployed to perform complex missions. Recent advances in this field focus on the design of distributed algorithms such that the formation control problem can be solved by only exploiting local information.

Over the years, different approaches for formation control have been studied, and these can be classified according to the sensing and control variables that can be related to a geometrical property of the desired deployment for the robots

[1]. One class of formation control strategies is the rigidity-based control strategies. In this class, rigidity theory plays a key role in characterizing a (at least locally) unique target deployment, which can be achieved by a systematic design of distributed control laws. Utilizing the distance [1, 2] (or bearing [3, 4]) rigidity theory, we can define a specific deployment or target formation shape in terms of a set of inter-robot distance (or bearing) constraints. The robots use available relative position or distance (or relative bearing) measurements in the design and execution of the distributed control laws. Recently, new rigidity theories, such as angle rigidity [5], ratio-of-distance-rigidity [6], and bearing-ratio-of-distance-rigidity theory [7] have also been developed for characterizing a (at least locally) unique target formation shape using a set of angle,  ratio-of-distance, and  bearing-ratio-of-distance constraints, respectively. These theories focus on providing more flexibility to the target deployment by allowing scaling or rotational motions.

In addition, several works deal with practical aspects when implementing the proposed rigidity-based control strategies in real world settings. Among others, [8] considers robust distance-based formation control with prescribed performance, taking into account collision avoidance and connectivity maintenance between neighboring agents while they are subjected to unknown external disturbances; [9] considers the bearing-only formation control problem with limited visual sensing while [10]

introduces estimators for controlling distance rigid formations under inconsistent measurements.

One common aspect in the above-mentioned rigidity-based formation control theories is that the mobile robots are assumed to be simple points. As each robot is represented by a point in the plane, there can be only one relative position, distance, or bearing measurement between a pair of neighboring mobile robots. Instead of treating each robot as a point, we treat robots in this work as objects with area so that multiple features in the area can be measured by its neighbors. In particular, we assume each mobile robot to have a circular shape and move with single-integrator dynamics in the plane. Furthermore, each mobile robot can observe two distinctive features from its designated neighboring robots. These are the outermost points of the neighboring robots’ disk that can be seen from its centroid. In other words, we have the internal angle information of the neighboring robots. The desired formation shape can then be described in terms of feasible internal angle constraints, which have a close relationship to the distance constraints that are used in distance-based formation control. This approach enables us to make the following novel contribution in the field of formation control:

We provide an angle-constrained formation control algorithm, which resembles distance-based formation control. The main feature of our algorithm is that it requires only direction/bearing/unit vectors as measurements instead of a vector (that requires range and direction). Furthermore, our algorithm provides collision avoidance guarantees where the clearance distance (which is twice the radius) between neighboring robots is not breached by design.

This letter is organized as follows. Section II provides preliminaries on graph and distance rigidity theory. Next, in Section III, the problem setting and problem formulation are presented. Section IV provides details concerning the control design and the local exponential convergence of the associated error dynamics. A numerical example is included in Section V and Section VI concludes our work.

Notation. The cardinality of a given set is denoted by . For a vector , is the transpose, is the perpendicular vector satisfying , and is the -norm of . The vector (or ) denotes the vector with entries being all s (or

s). The set of all combinations of linearly independent vectors

is denoted by . For a matrix , , , and denotes its null space, its column space, and its rank, respectively. The identity matrix is denoted by while (or ) is the diagonal (or block diagonal) matrix with entries of vector (or matrices ) on the main diagonal (or block). Finally, given matrices and , is the Kronecker product of and , and we denote .

Ii Preliminaries

This section provides the necessary concepts in graph theory and distance rigidity theory. For a more detailed exposure of the material, we refer to for instance, [11] on graph theory, and [12, 13] on distance rigidity theory.

Ii-a Graph theory

An undirected graph is defined as a pair , where and denote the finite set of vertices and the set of unordered pairs of the vertices, called edges. We assume the graph does not have self-loops, i.e., , and . The edge indicates vertices and are neighbors of each other. The set of neighbors of vertex is denoted by . By assigning an arbitrary orientation to each edge of , we obtain an oriented graph . The incidence matrix associated to has rows encoding the oriented edges and columns encoding the vertices. if vertex is the (tail) head of edge and otherwise. For a connected and undirected graph, we have and .

Ii-B Distance rigidity theory

Let be a point in the plane and a collection of points, called a configuration, be given by the stacked vector . We can embed the graph into the plane by assigning to each vertex , a point . The pair denotes a framework in . We assume if , i.e., no two vertices are mapped to the same position. Related to , we define the distance rigidity function as


with each entry of the vector being half the squared distance between two points. Given the distance rigidity function (1), we say a framework is distance rigid, if there exists a neighborhood of such that, if and , then is congruent to . Let be the relative position vector associated to , and be the stacked vector of s. Using the incidence matrix , we obtain . Besides, let . Using these expressions, (1) can be written in compact form as . By taking the Jacobian of (1), we obtain the distance rigidity matrix as


Let be an infinitesimal variation of . A motion is said to be trivial if corresponds to a translation and or a rotation of the entire framework. Trivial motions in the plane are a translation in the - and in the -direction, a rotation, and the combination thereof, all applied to the entire framework. We say a framework is infinitesimally distance rigid if and only if the set of infinitesimally distance motions consists of only the trivial motions. This can be translated to the following condition on the distance rigidity matrix: . Furthermore, an infinitesimally distance rigid framework must have at least edges. If the number of edges is exactly , then the framework is said to be minimally and infinitesimally distance rigid.


Fig. 1: Sensing setup with robot being the ‘observer’ and robot the ‘observed’ robot. On the left panel, robot detects the points and of robot and the internal angle can be obtained from the bearing measurements and . In the middle panel, we use geometrical arguments to relate to the inter-center distance and the radius . On the right panel, we have a geometrical view supporting Proposition 2.

Iii Problem Setup

We consider a group of mobile robots moving in the plane. Let be the index set of the robots. Each robot has a circular shape with center specified by and radius by . For simplicity, we assume the radii of the robots have the same value and let represent this common value. We assume the robots are moving with single-integrator dynamics, i.e.,


where is the controlled velocity to be designed. The group dynamics is given by with the stacked vectors and .

Each robot is equipped with a sensory system mounted at the center of the robot. With the equipped sensory system, we assume the robots are able to detect two points on the surface of each of its designated neighbors. To illustrate this, let us consider without loss of generality a pair of robots labeled and within the group of robots, see Fig. 1. We assume robot has the role of ‘observer’ and robot is the ‘observed’ robot. Since robot is the observer, it is able to detect two points on the surface of robot . We denote the position of the detected points as and to indicate these are the positions of robot as detected by robot . The measurements from robot that are available to robot are the relative bearing measurements and , with and being the relative position from the detected points to the center of robot . The two bearing vectors form an angle centered at , as can be seen in Fig. 1. By the inner product rule, we obtain

Remark 1

It should be noted the lines in the direction of the unit vectors and are both tangent lines from the point to robot . Hence, these lines are perpendicular to the radius of the circle, i.e, and . Furthermore, the triangle with vertices , , and and the triangle with vertices , , and are reflections of each other with the line connecting and as the line of reflection. Hence, the angle .

By considering the geometry, we can obtain an alternative expression for , which is related to the radii of and the inter-center distance between the robots. To this end, we first define some auxiliary relative state variables. For robots and , let denotes the relative position, the distance, and the relative bearing between the centers of the robots. Also, is the perpendicular vector obtained by rotating counterclockwise by . We have with being the rotation matrix.

Proposition 1

The internal angle is related to the inter-center distance between the robots and and the radii of the robots as


The desired result can be obtained by employing the cosine double-angle identity and noting from Remark 1 that is a right triangle with . Fig. 1 provides the geometric illustration.

Note that (4) and (5) are equivalent for obtaining the internal angle ; the former is based on the available bearing measurements while the latter is based on geometry.

Remark 2

As robots and are of circular shape, the feasible interval for the inter-center distance is . This also poses restrictions on the value for and . From (5), it follows that implies and . Correspondingly, an increase in the value of results in an increase of and a decrease of .

We can rewrite (5) as . By obtaining from (4) and knowing , we can infer the inter-center distance . With this observation, we define an internal angle rigidity function given by


for describing a framework . By Remark 2, there is a one-to-one relationship between the newly defined rigidity function (6) and the distance rigidity function (1). The Jacobian of (6) is


with being the stacked vector of distances s, , and . The matrix is positive definite as each . Thus, we have .

Now we can define the desired target formation shape by a framework where the vector satisfies a set of desired internal angle constraints . One way to obtain the internal angle constraints is to employ (5) when the desired distance constraints are given. Moreover, the formation is minimally and infinitesimally rigid in the distance rigidity sense. The formation control problem that is considered in this work can be formulated as follows:

Angle-constrained Formation Control Problem with Collision Avoidance: Given a set of feasible internal angle constraints111We give a formal definition of such a set in Section IV-E. obtained using (5) from a minimally and infinitesimally rigid framework and an initial configuration with . Design a control law utilizing only the neighboring measurements obtained as in (4) such that

  • Collision avoidance: , ;

  • Convergence: as .

Iv Gradient-Based Control Design

In this section, we pursue a gradient-based control design approach utilizing angle-based potential functions for solving the formation control problem. To each edge , we define the error signal . By Remark 2, we deduce the feasible region for the error signal is , with and . Both and are strictly positive.

Iv-a Proposed angle-based potential function

For a robot pair , we take as potential function


The denominator term ensures collision avoidance between the neighboring robots and , i.e., given that . The function is non-negative in . Furthermore, if and only if and if approaches the lower bound from above, i.e., when the mobile robots are approaching each other.

The first derivative can be obtained as . The value of equals zero if and only if and the sign of depends on the sign of .

The second derivative is given as  . is positive when . Recall ; therefore, we need to determine when . By some algebraic computations, we obtain if and only if . When , we have the region for which is positive is a subset of , whereas when , we have is positive over the entire domain .

The properties of (8) will be used later for deriving the exponential convergence of the error dynamics.

Iv-B Gradient-based control law for each robot

The local potential function for each robot is with being the stacked vector of error signals s. The control input is then


Utilizing (5), the term can be evaluated as


Note that (10) requires relative state variables , , and the knowledge of . However, robot has access to only the relative bearing measurements and for each . Nonetheless, we show that the gradient-control law (9) can be implemented using these available measurements.

Proposition 2

The gradient-based control law (9) can be implemented by each robot using the set of available measurements .

To implement (9), we need to rewrite (10) in terms of the available measurements and . To this end, first, we seek expressions for the positions and . Let us consider again Fig. 1. Denote the intersection between the lines connecting the center of the robots and the two intersection points as (marked with the -symbol in the right panel of Fig. 1). Let , , and satisfying . can also be written as a fraction of the inter-center distance , i.e., with . We can now express the positions and as , and . Recall is the unit vector between the centers of the robots. Subsequently, the relative position and can be obtained as , and , while their sum equals Due to the reflection observation in Remark 1, we have . Using the previous computations, we obtain for the sum of the relative bearing measurements In addition, Since , we can rewrite as by using , , and the sine double-angle identity . Substituting the obtained expressions in (10) and utilizing (5) yield


where , i.e, using (11), we can implement (9) without knowledge of the range information and the radii of the robots.

Iv-C Gradient-based control law for the group of robots

The overall potential function can be expressed as the sum of all the individual potential functions , i.e., . The control law in (9) is then . By noting , we obtain the following compact form for the closed-loop formation control system:


with the vector denoting the gradients of (8) for each robot pair .

Lemma 1

The closed-loop formation control system (12) has the following properties:

  1. The formation centroid is stationary, i.e., ;

  2. Each mobile robot can have its own local coordinate system for obtaining the required relative state measurements and implementing the desired control action.

The proof is similar to Lemma 4 in [14], and thus not provided here.

Iv-D Internal angle error system

Using the definition of the error vector , and expressions (5) and (12), we can obtain the error dynamics


The matrix is symmetric and at least positive semidefinite. Moreover, for any infinitesimally and minimally distance rigid framework , can be shown to be a function of the error vector around the origin by employing the law of cosines. By this observation, we conclude the error dynamics given by (13) constitute an autonomous system.

The main result will be the local exponential stability of the error dynamics (13). To this end, we first construct a compact and invariant sub-level set for the overall potential function .  
Previously, we have holds if and only if . Let . We define the ‘hypercube’ as


with being the Cartesian product and being the ordered edge index set. Choose such that


Let . As , we have and also . Choose and define


By definition, the sub-level set is closed and as , it is also bounded. Thus, is a compact set. The time-derivative of can be obtained as


This implies . Whenever , we have by (17) that ; therefore, the set is also positive invariant. As and , the overall potential function can serve as a candidate Lyapunov function. We are ready to state and prove the main result.

Theorem 1

Consider a group of circular shaped robots modeled with single-integrator dynamics (3) and having a graph topology such that the desired formation is minimally and infinitesimally rigid in the distance rigidity sense. Let be such that it is in the compact and invariant set (16). Then is a locally exponential stable equilibrium point of the error dynamics (13).

The proof can be divided into three main stages. First, we consider the asymptotic stability of the origin . The set has the property of being compact and positive invariant. In addition, the value for can be chosen such that for every vector , the formation is minimally and infinitesimally rigid in the distance rigidity sense, and close to the target formation. Due to our choice of , we have that has full row rank. Since and positive definite, also has full row rank. This in turn implies is positive definite. Let

be the minimal eigenvalue of the matrix

in , i.e., . It follows from (17) that


holds. The value is negative definite for all ; therefore, local asymptotic stability of the origin is attained.

Next, we aim to show the following two inequalities as is done in [2]:


with , , and being positive constants that we need to determine. These inequalities facilitate the proof to exponential stability of the origin. To this end, recall the overall potential function


Within the set , we can find a value for such that


By Lemma 3.2 in [15], we have the function is Lipschitz continuous in . In addition, the function is positive within the set , and thus also in the subset . The remainder of the proof for obtaining the positive constants , , and of (19) follows closely [2] and for this reason, it is omitted.

Finally, we can show exponential stability of the origin as a result of the previous two steps. Substituting (19) in (18), we obtain


By Theorem 4.10 in [15], we can conclude that the origin is exponentially stable in . The error norm can be shown to be bounded by an exponential decreasing function as


with . This concludes the proof.


(a) Robot trajectories


(b) Inter-center distances


(c) Internal angle errors
Fig. 2: Simulation with a team of circular mobile robots having radii . On the left panel, we have the robot trajectories; dashed circles represent initial configuration while solid circles are final robot positions. The solid lines are the robot center trajectories. In the center panel, the convergence of the distances (dashed) to their desired values (solid) is depicted. The black solid line represents between the robots. The right panel shows the convergence of the internal angle errors. The black solid line depicts the value for the hypercube .

Iv-E Equilibrium Sets

Theorem 1 concerns the local exponential convergence of the formation control system to the desired formation shape. In general, the set of equilibrium points of the mobile robots can be given by . The set of correct formation shapes can be given by while the set of incorrect formation shapes is . Considering the target formation shape is minimally and infinitesimally rigid, we can conclude that the formation shapes in are not infinitesimally rigid, since the null space of also consists of a non-trivial vector . As in distance-based control, the set includes configurations where all the robots’ center are collinear. Moreover, we can obtain the following on the equilibrium set of the -dynamics and the -dynamics:

Lemma 2

The equilibrium sets of the error system (13) is the same as the equilibrium sets of the closed-loop formation control system (12).

Since , obviously . It remains to show . Assume such that holds. From (12), we also have . Since, , we obtain , contradicting the assumption . This concludes the proof.

V Numerical Example

V-a Simulation Setup

We apply the proposed control law to a team of circular robots with radii . The collective goal is to form a rectangular shape with the inter-center distances given as , , and . Using (5), we obtain , , and . The initial configuration, depicted as dashed circles in Fig. 2(a), has center positions , , , and . Using this initial configuration, we can illustrate the collision avoidance feature of the proposed control law and the convergence to the desired formation shape, even though . We can obtain , and set the gain for speeding up the convergence.

V-B Simulation Results

The trajectories of the robots are depicted in Fig. 2(a). In addition, the inter-center distances and the internal angle errors between the robots are given in Figs. 2(b) and 2(c), respectively. Let us focus on robot , the green robot in Fig. 2(a). It has the neighboring robots (red robot) and (magenta robot). From the figure, we observe that since robots and are close to each other initially, robot quickly moves away from robot , and almost attains the desired constraint with robot . However, due to this motion, its distance to the neighboring robot has increased to about . This can also be observed from Fig. 2(c), where we see an increase in the magenta colored signal representing error . Since robot is now sufficiently far from robot , it then tries to satisfy the internal angle constraint with robot as can be observed in both Figs. 2(b) and 2(c) . By zooming in on Fig. 2(c), we can observe exponential convergence of the error signals starting around s. All the error signals are then well below the threshold value of .

Vi Conclusions

In this letter, we have solved the formation control problem for circular mobile robots subjected to internal angle constraints. A gradient-descent control law requiring only relative bearing measurements for implementation has been proposed. This control law enjoys local exponential convergence for the error dynamics and ensures collision avoidance between neighboring robots.


  • [1] K.-K. Oh, M.-C. Park, and H.-S. Ahn, “A survey of multi-agent formation control,” Automatica, vol. 53, pp. 424–440, mar 2015.
  • [2] Z. Sun, S. Mou, B. D. Anderson, and M. Cao, “Exponential stability for formation control systems with generalized controllers: A unified approach,” Systems & Control Letters, vol. 93, pp. 50–57, jul 2016.
  • [3] S. Zhao and D. Zelazo, “Bearing rigidity and almost global bearing-only formation stabilization,” IEEE Transactions on Automatic Control, vol. 61, no. 5, pp. 1255–1268, may 2016.
  • [4] ——, “Bearing Rigidity Theory and Its Applications for Control and Estimation of Network Systems: Life Beyond Distance Rigidity,” IEEE Control Systems, vol. 39, no. 2, pp. 66–83, Apr. 2019.
  • [5] L. Chen, M. Cao, and C. Li, “Angle rigidity and its usage to stabilize planar formations.”
  • [6] K. Cao, Z. Han, X. Li, and L. Xie, “Ratio-of-Distance Rigidity in Distributed Formation Control,” in 2018 15th International Conference on Control, Automation, Robotics and Vision (ICARCV).   IEEE, Nov. 2018.
  • [7] K. Cao, D. Li, and L. Xie, “Bearing-ratio-of-distance rigidity theory with application to directly similar formation control,” Automatica, vol. 109, p. 108540, Nov. 2019.
  • [8] F. Mehdifar, C. P. Bechlioulis, F. Hashemzadeh, and M. Baradarannia, “Prescribed performance distance-based formation control of multi-agent systems (extended version).”
  • [9] D. Frank, D. Zelazo, and F. Allgöwer, “Bearing-only formation control with limited visual sensing: Two agent case,” IFAC-PapersOnLine, vol. 51, no. 23, pp. 28–33, 2018.
  • [10] H. G. de Marina, M. Cao, and B. Jayawardhana, “Controlling rigid formations of mobile agents under inconsistent measurements,” IEEE Transactions on Robotics, vol. 31, no. 1, pp. 31–39, feb 2015.
  • [11] F. Bullo, Lectures on Network Systems, 1st ed.   Kindle Direct Publishing, 2019, with contributions by J. Cortes, F. Dorfler, and S. Martinez. [Online]. Available:
  • [12] B. D. O. Anderson, C. Yu, B. Fidan, and J. M. Hendrickx, “Rigid graph control architectures for autonomous formations,” IEEE Control Systems Magazine, vol. 28, no. 6, pp. 48–63, Dec 2008.
  • [13] M. de Queiroz, X. Cai, and M. Feemster, Formation Control of Multi-Agent Systems.   John Wiley & Sons, Ltd, jan 2019.
  • [14] Z. Sun, S. Mou, M. Deghat, and B. D. O. Anderson, “Finite time distributed distance-constrained shape stabilization and flocking control ford-dimensional undirected rigid formations,” International Journal of Robust and Nonlinear Control, vol. 26, no. 13, pp. 2824–2844, nov 2015.
  • [15] H. K. Khalil, Nonlinear Systems (3rd Edition).   Pearson, 2001.