Formation Tracking of Heterogeneous Mobile Agents Using Distance and Area Constraints

This paper presents two formation tracking control strategies for a combined set of single and double integrator agents with an arbitrary undirected communication topology.The first approach is based on the design of distance-based potential functions with interagent collision avoidance using local information about the distance and orientation between agents and the desired trajectory. The second approach adds signed area constraints to the desired formation specification and a control strategy that uses distance as well as area terms is designed to achieve tracking convergence. Numerical simulations show the performance from both control laws.


Introduction
The consensus problem of multiagent systems has gained considerable interest in the research community recently. For instance, Shang and Ye [1] proposed a leader-follower nonlinear distributed control algorithm for single integrator agents, based on local information such that the following agents track their corresponding leaders. Higher order multiagent systems have been studied by Wang et al. [2] using multiple Lyapunov functions, showing that consensus can be reached under a switching topology within a finite set of digraphs with an average delay time between changes.
An extension to the consensus problem, called formation control, studies strategies to distribute agents in geometric patterns avoiding interagent collisions [3]. In distance-based formation control (DFC) the agents achieve a formation pattern defined by interagent distances according to a predefined communication graph, for example, in Ramazani et al. [4]. The DFC allows a more decentralized and scalable setup than position based control strategies because the control laws can be implemented using local sensors on the agents. Representative works are of Dimarogonas and Johansson [5] for the case of undirected communication graphs or Oh and Ahn [6] that used an Euclidean distance matrix. The DFC has been addressed for the case of single or double integrators, separately in Zavlanos et al. [7] and leader-follower schemes in Anderson et al. [8]. Control of formations specified only by interagent angles has been studied by Basiri et al. [9]. Formation and communication changes have also been studied to find suitable control laws that maintain rigidity in Zelazo et al. [10]. Experimental work is found in Fidan et al. [11] using local video cameras and Antonelli et al. [12] with laser range finder devices. Our previous contributions in DFC are in Lopez-Gonzalez et al. [13] for a distance-orientation scheme, Ferreira-Vazquez et al. [14] adding desired internal angles in the formation pattern to compensate for the flip ambiguity problem, and Ferreira-Vazquez et al. [15] using planar as well as volume constraints for DFC in 3D. A recent paper by Anderson et al. [16] makes a detailed convergence analysis of DFC with signed area constraints using standard 2 Complexity potential functions. However, their results are restricted to formations of three and four agents without any collision avoidance techniques.
In some applications, the agents must follow a prescribed trajectory maintaining the DFC simultaneously. This problem is known as formation tracking or marching control. The models for the agents depending on the approximation used, kinematic or dynamical, could be of first or second order which involves different physical robots with an appropriate linearization. Therefore, the combination of these simplified models is the reason to consider this setup as a heterogeneous multiagent system (e.g., Zheng et al. [17], Hernandez-Martinez et al. [18]).
For first-order agents, the leader-follower schemes add the reference velocity for the leader, whereas the follower agents estimate this velocity using an adaptive method in Kang et al. [19]. In Rozenheck et al. [20], a subset of leader agents follows the marching path and the followers combine a proportional-integral control with the standard gradient method. The velocity and position of a target are communicated to the leader agent in Cai and De Queiroz [21]. The orientation of the formations and the enclosing and tracking of a moving target is studied in Garcia De Marina et al. [22] cancelling the phenomena of distance mismatches due to the local measurement of distance. Alternative works are of Xiao et al. [23] for a leader-follower distance maintenance and obstacle avoidance for unicycle-type agents using nonlinear model predictive control with neurodynamic optimization.
For the case of double integrator models, in Dimarogonas and Johansson [24] formation tracking is achieved transmitting a common velocity. The case of unmanned aerial vehicles (UAVs) is studied in Zhang et al. [25], where a linearized elastic distance vector adapts to the changes of the follower velocities. A set of underwater mobile agents in a vertical plane formation is studied in Do [26], where an optimal assignment algorithm selects the appropriated reference trajectories to the agents. Switched routines of tracking and encircling of a moving target in a V-shape DFC are addressed in Dang and Horn [27] using attractive or repulsive and rotational force fields.
The analysis of a combined set of first-second-order agents becomes a useful result in the sense that heterogeneous agents can be formed within the same setup. Our previous work in Hernandez-Martinez et al. [18] studies the DFC for terrestrial (first-order) and aerial (second-order) agents. Also, the potential of collaborative work of aerial and ground agents is shown in Harik et al. [28], where an aerial agent provides way points to the ground agents to carry objects in unsafe industrial areas using a predictive vision to keep a distance and bearing to the leader. This paper extends our previous work in Hernandez-Martinez et al. [18], focusing on static DFC, for the case of formation tracking of heterogeneous agents communicated by an arbitrary undirected graph. The main contributions and originality of this work are given by the next points: (i) The approach applies to a combined set of first-and second-order agents, where a second-order leader is chosen to follow a desired marching path whilst all the agents maintain a DFC.
(ii) Two control laws are designed using Lyapunov techniques. The first strategy depends on distance and orientation measurements only applied to rigid undirected formation topologies of distance. Convergence to the formation setup is shown for an arbitrary number of agents.
(iii) In order to avoid symmetric solutions, a second strategy is designed to add desired area constraints related to the standard triple product of a subset of robots. It defines a new area based or planar topology based on triplets of robots separately of the previous distance formation topology. The combination of distance and area restrictions helps to avoid undesired final patterns of the agents and the convergence to the desired formation eliminating some distance links.
(iv) Both approaches are based on a general class of artificial potential functions with attractive and repulsive behavior designed to ensure convergence and collision avoidance. The performance of the strategies is shown by numerical simulations.
The paper is organized in the following sections. Section 2 presents the problem definition for the mixed group of agents and the distance-based formation topology. Section 3 addresses the first strategy of DFC. The definition of the planar topology related to the area constraints is given in Section 4 and the addition of this planar topology to the previous DFC is studied in Section 5, completed with the analysis of the appendix section. Numerical simulations for both strategies are shown in Section 6. Finally, some conclusion remarks are given in Section 7.

Problem Definition
where ∈ R 2 is the input vector of the agent , and is the velocity of the second-order agents. As shown below, the single and double integrators are related to kinematic or dynamic posture models of mobile agents, respectively. This allows a feasible combination of distinct modeling complexities into the same motion coordination scheme. Consider the last agent to be the leader agent.
Assume that each agent is communicated to agents that belong to its adjacent subset ⊂ (note that = card( )). Thus, the possible interagent communication defines a distance-based formation graph (DFG) given by Reference axis where = is the set of vertices related to the agents; = {( , ) ∈ × } is the set of edges that represent the possible interagent communications; therefore ( , ) ∈ if ∈ . The set = { }, ∀( , ) ∈ , contains the desired distances between and ; that is, ‖ − ‖ = ∈ R, ∀ ̸ = , ∈ within a desired formation pattern [13].
A well-defined DFG must be connected, that is, there are no isolated nodes ( ̸ = ⌀), and rigid, where at least 2 − 3 communication edges are defined for the agents. It is assumed that = for all ( , ) ∈ . This paper addresses the case of undirected graphs, that is, bidirectional communication between agents, where if ∈ , then ∈ , for all ̸ = . On the other hand, define the distance between and as = = ‖r ‖ = ‖ − ‖ and as the angle of r with respect to a reference axis (e.g., the magnetic pole of the Earth), as shown in Figure 1. It is assumed that the values of distance and orientation can be measured by the combination of local sensors as laser range finders or lidar and magnetometers.

Distance-Based Control Strategy
Let = = − be the distance error between any pair of agents and .
Definition 1 (see Lopez-Gonzalez et al. [13]). Define an Attractive-Repulsive Distance-based Potential Function (ARD-PF) : R → R for and as a function satisfying the following conditions: is a smooth function of .
For example, in the simulation example and experimental work below, we consider the ARD-PF given in Hernandez-Martinez et al. [18]: The time derivative of can be expressed aṡ Note that the time derivative of can be written aṡ From Figure 1 it is clear that (polar coordinates), and (6) can be reduced tȯ Substituting (7) in (5), theṅ Note also from Figure 1 that Ω = −Ω . Since = + , then cos = − cos and sin = − sin . This property is useful in the following result.

Theorem 2. Consider the group of agents given by (1)-(2) communicated by a well-defined DFG given in (3) and a specific ARD-PF. Define the control law as
= Ω ⊤ − ( −̇) +̈, where , , ∈ R with , , > 0 are constant gains, and where = 1 if ( , ) ∈ and = 0 otherwise, and is defined in (2). Note that the addition of eliminates the 4 Complexity components of the distance errors between agents that do not have communication in the DFG. In the assumption that the agents are not collinear and do not collide at = 0 and formation graph is infinitesimally rigid, the closed-loop system given by (1), (2), and (9)- (11) will converge locally to the desired formation; that is, lim →∞ = 0, ∀ ∈ avoiding interagent collisions; that is, > 0, ∀ ̸ = , ∀ > 0, and the leader agent converges to ( ).
Remark 3. The control law (11) implies that is the only agent with complete information about the reference trajectory. The rest of the agents need the first or second time derivative of ( ) which are assumed to be communicated between agents. In order to design a completely decentralized control strategy,̇( ) can be obtained by observers or other similar approaches, for example, in Hernandez-Martinez et al. [29], Ren and Sorensen [30], and Haghighi and Cheah [31]. Note that the rest of the terms of the control law are defined to be dependent on the distance and orientation data.
Note that the velocity of the first-order agents also converges tȯ. Therefore all the agents converge to the same velocity.
On the other hand, since =̇, = ( 1 + 1), . . . , , theṅ =̈=̈, = ( 1 + 1), . . . , . Then, (27) and (28) Consequently, by using the fact that ∑ Ω ⊤ = 0 the multiagent system will converge to 0 = Ω ⊤ , = 1, . . . , , Reference axis r jk =      r jk      with ⊗ the Kronecker product and being the rigidity matrix of formation graph (3), Trinh et al. [32]. By the rigidity assumption, the rank of is 2 − 3 and using also the properties of the ARD-PF, this implies the solution to (33) requires = 0 implying the convergence in distance between agents. Coupled with the convergence of the velocities and the leader agent's tracking system, it is concluded that the agents converge to the desired formation tracking.

Remark 4.
It should be noted that the convergence result is local since there may still exists undesired equilibrium satisfying Ω ⊤ = 0, = 1, . . . , 1 , with ̸ = 0. This situation may occur when some agents are collinear, decreasing the rank of the rigidity matrix . A detailed analysis of the equilibria points for these systems is quite complex, exceeds the scope of the present paper, and will be studied in future works.
Remark 5. Finally, the Lyapunov function tends to infinity when any → 0. Because the agents do not collide at = 0 by the initial assumption anḋ≤ 0, it is clear that the communicated agents can be near to each other but ∀ , ∈ will never be equal to zero, and therefore the agents do not collide. Adding a repulsive function with similar properties to an ARD-PF may extend the collision avoidance to all agents.

Planar Topology
It is also possible to define a planar topology for a formation. For any 3-tuple ( , , ) the oriented planar area defined by agents , , and can be expressed by where k = [0, 0, 1] ⊤ is the unitary vector orthogonal to the planar work space, as shown in Figure 2. This planar information provides relative angular information between agents in any given configuration. Let 2 be the number of planar 3-tuples specified for a desired configuration. Let 2 be the set of all 3-tuples ( , , ), = 1, . . . , 2 , for which 6 Complexity planar communication is established and the set of desired area values ⋆ = { ⋆ } is defined. It is assumed that the planar communication is undirected; that is, ( , , ) ∈ 2 ⇒ ( , , ) ∈ 2 and ( , , ) ∈ 2 . Besides, ⋆ = ⋆ = − ⋆ , ∀ , , . The planar topology is then defined as Additionally, let be defined such as = 1 if ( , , ) ∈ 2 and 0 otherwise. Observe that for a rigid graph the agent configurations verifying the distance constraints are invariant to group translations and rotations. Besides, there may also be other configurations of agents that verify the distance constraints but are symmetric to each other and will have different area values for some 3-tuples. Therefore, the planar topology may help to specify the desired formation better.
Observe also that which will be useful in the proofs.

Remark 7. The control law (39) implies that
is the only agent with complete information about the reference trajectory. The rest of the agents need the first or second time derivative of ( ) which are assumed to be communicated or estimated between agents as discussed in Remark 3. Note that the rest of the terms of the control law are defined to be dependent on the relative distance, orientation, and area data.
Proof. Consider the Lyapunov candidate function where is a positive definite ARD-PF given by Definition 1 and as in (40). Note that is always positive and vanishes for the desired formation tracking. The time derivative of , substituting (1) and (2), is given bẏ Using a similar derivation as in Theorem 2, (42) is reduced tȯ Using the matrix expression for given by (36) the second term on the left hand side can be written as Since = − ⊤ and using the undirected assumption for the topology, the equation above simplifies to ∑ ( , , )∈ 2= ∑ , , , , Substituting (45) into (43) and dividing the first term according to the order of agents given in (1) and (2), thus, The control laws (37)-(39) can be replaced in (46), obtaininġ Defining = 3 the derivative (47) becomeṡ Finally, grouping terms, defining = 3 to cancel two terms, the expression foṙis given bẏ It was shown in Theorem 2 that the term ∑ =1 [ ⊤ Ω ]v anishes. It is shown in the appendix that ∑ 1 = 0 to reach the following final expression for= Note thaṫis negative semidefinite. Using LaSalle's Invariance Theorem, the system will converge to the largest invariant set satisfyinġ= 0. From (50) it follows that Ω ⊤ + = 0, = 1, . . . , 1 , =̇, = ( 1 + 1) , . . . , .
Finally, note that the Lyapunov function is radially unbounded, that is, tends to infinity when any → 0. Since the agents do not collide at = 0 by the initial assumption anḋ ≤ 0, it is clear that distance between agents ∀ , ∈ will never be equal to zero, and therefore the agents do not collide.

Remark 8.
A distance-based only approach allows for symmetric solutions to appear that may be undesired in some situations. The introduction of oriented area terms eliminates those solutions. However, other undesired equilibria may appear, similar to what was discussed in Remark 4.

Distance-Based Formation Control.
In this section, a numerical simulation of the control strategy analyzed in Section 3 is presented with = 4 and 1 = 2 ( 1 and 2 are first-order agents; 3 and 4 are second-order agents), with the complete DFG shown in the Figure 3 where the desired interagent distances are given by 12 = 23 = 31 = 5 and 14 = 24 = 34 = 5/2 cos( /6) (an equilateral triangle with side equal to 5 formed by agents 1 , 2 , and 3 and the leader agent 4 placed in its center).
The leader agent 4 follows a circled-shape trajectory given by    Note that the agents do not collide because the values of distance between agents, given by , ∀ , as shown in Figure 5(a), are strictly greater than zero. Finally, the control inputs are depicted in Figure 5(b). Figure 6 shows the second simulation scenario for the control law that uses distances and area. It uses the same desired formation, leader trajectory, initial conditions, and control parameters as in the previous simulation. However, the formation graph is not complete since the edges (2, 4) and (3, 4) have been eliminated. Now, a planar topology is set as in (35) with 2 = {(1, 2, 4), (1,3,4)} as shown in Figure 6, with the desired areas being ⋆ 124 = − ⋆ 134 = 5 2 √ 3/12. The control parameters for the areas are set to = = 0.5.

Distance and Area Based Formation Control.
Some postures of the agents are depicted in the time instants = 40, = 75, = 100, = 125, and = 150, in Figure 7(a). Observe that the agents converge to the desired formation, with distance and area errors converging to zero as shown in Figures 7(b) and 7(c). Besides the leader follows the desired trajectory ( ) as is shown in Figure 8(a). Similarly to the previous simulation the agents do not collide as shown in Figure 8(b). Finally, the control inputs are depicted in Figure 8(c).

Conclusions
This paper addresses the case of formation tracking of a combined set of first-and second-order agents in an undirected graph communication. The approach is based on the measures of distance and orientation between agents using a family of distance-based functions with interagent collision avoidance. The approach ensures the local convergence of the formation errors to zero. The control law is completed with signed area constraints related to triplets of robots. The subset of robots involved in these new restrictions constructs a planar topology. The combination of the two formation strategies results in a versatile control setup, where undesired final patterns and local minima can be eliminated. Simulation results show the performance of the control laws. Future work will include the analysis of directed and mixed communication graphs, further analysis on local minima and local sensor mismatch, the extension for the case of first-order kinematic models of omnidirectional agents moving on the plane together with second-order dynamical models of aerial agents moving in the space, and the experimentation of the distance and area measurements using local onboard sensors.
Proof. The proof is shown by induction. For the first step let = 3, write the expression for explicitly, and use the properties of , r , and .
Since is invariant for a circular shift of indexes, the right hand side becomes Assuming now that it is true for = − 1, in the next step = , (A.1) is given by (A.4) The first summation of the right hand side is zero because of the induction hypothesis. In order to group terms a change of indexes is performed to have all the summations in terms of and , giving the following: because the term inside the parenthesis is identically zero. Thus, the induction step is complete.

Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.