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.
The consensus problem of multiagent systems has gained considerable interest in the research community recently. For instance, Shang and Ye  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.  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 . 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. . 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  for the case of undirected communication graphs or Oh and Ahn  that used an Euclidean distance matrix. The DFC has been addressed for the case of single or double integrators, separately in Zavlanos et al.  and leader-follower schemes in Anderson et al. . Control of formations specified only by interagent angles has been studied by Basiri et al. . Formation and communication changes have also been studied to find suitable control laws that maintain rigidity in Zelazo et al. . Experimental work is found in Fidan et al.  using local video cameras and Antonelli et al.  with laser range finder devices. Our previous contributions in DFC are in Lopez-Gonzalez et al.  for a distance-orientation scheme, Ferreira-Vazquez et al.  adding desired internal angles in the formation pattern to compensate for the flip ambiguity problem, and Ferreira-Vazquez et al.  using planar as well as volume constraints for DFC in 3D. A recent paper by Anderson et al.  makes a detailed convergence analysis of DFC with signed area constraints using standard 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. , Hernandez-Martinez et al. ).
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. . In Rozenheck et al. , 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 . The orientation of the formations and the enclosing and tracking of a moving target is studied in Garcia De Marina et al.  cancelling the phenomena of distance mismatches due to the local measurement of distance. Alternative works are of Xiao et al.  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  formation tracking is achieved transmitting a common velocity. The case of unmanned aerial vehicles (UAVs) is studied in Zhang et al. , 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 , 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  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.  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. , 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. , 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.
2. Problem Definition
Let be the set of mobile agents with positions in the plane . Consider the first agents as single-integrators and the rest as double integrators; that is,where 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 ). Thus, the possible interagent communication defines a distance-based formation graph (DFG) given bywhere 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, , , within a desired formation pattern .
A well-defined DFG must be connected, that is, there are no isolated nodes (), and rigid, where at least 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 and as the angle of 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.
Problem Statement 1. The control objective is to design control laws , that satisfy simultaneously(i), , that is, convergence of the formation errors,(ii); that is, the leader agent converges to a desired reference trajectory .
3. Distance-Based Control Strategy
Let be the distance error between any pair of agents and .
Definition 1 (see Lopez-Gonzalez et al. ). Define an Attractive-Repulsive Distance-based Potential Function (ARD-PF) for and as a function satisfying the following conditions: (a) is a smooth function of .(b) and only if .(c) when .(d) with 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. :The time derivative of can be expressed asNote that the time derivative of can be written asFrom Figure 1 it is clear that (polar coordinates), and (6) can be reduced toSubstituting (7) in (5), thenNote also from Figure 1 that . Since , then and . 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 with are constant gains, andwhere if and otherwise, and is defined in (2). Note that the addition of eliminates the 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 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, , avoiding interagent collisions; that is, , , , 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. , Ren and Sorensen , and Haghighi and Cheah . Note that the rest of the terms of the control law are defined to be dependent on the distance and orientation data.
Proof. Consider the Lyapunov candidate functionwhere is a positive definite ARD-PF. Note that is always positive and vanishes for the desired formation tracking, that is, only when , , , , and . The time derivative of , substituting (1) and (2) is given bySubstituting (8) in the first term of (14), it becomesBecause the DFG is undirected, and , the previous equation can be reduced toTherefore, substituting (16) in (14), but changing the subscript by , the time derivative of can be reduced toThe summation in the first term of (17) can be divided according to the order of agents given in (1) and (2). Thus,The control laws (9)–(11) can be replaced in (18), obtainingNote that some terms are mutually cancelled. Therefore, it reduces toGrouping some terms in the previous equation, it becomesNote that produces the same real value, similarly to the case of . Then the last two summations of (21) are cancelled andObserve that the last term vanishes since for each column of the matrix it is possible to writewith and . Observing that and and using the properties of the trace operator implied thatFinally,Note that is negative semidefinite. Using LaSalle’s Invariance Theorem the system will converge to the largest invariant verifying , (first-order agents) and , (second-order agents), simultaneously. Substituting these conditions in closed-loop system (1), (2), and (9)–(11), thenNote that the velocity of the first-order agents also converges to . Therefore all the agents converge to the same velocity. On the other hand, since , , then , . Then, (27) and (28) satisfyConsequently, by using the fact that the multiagent system will converge toStacking all nonzero elements of , , without repeating the symmetric elements, (30) can be rewritten aswith the Kronecker product and being the rigidity matrix of formation graph (3), Trinh et al. . By the rigidity assumption, the rank of is and using also the properties of the ARD-PF, this implies the solution to (33) requires 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 , , with . 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 . Because the agents do not collide at by the initial assumption and , 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.
4. 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 bywhere 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 be the number of planar 3-tuples specified for a desired configuration. Let be the set of all 3-tuples , for which planar communication is established and the set of desired area values is defined. It is assumed that the planar communication is undirected; that is, and . Besides, . The planar topology is then defined asAdditionally, let be defined such as if 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 thatwhich will be useful in the proofs.
Problem Statement 2. The control objective is to design control laws , , that satisfy simultaneously(i), , that is, convergence of the formation errors,(ii), , that is, convergence of the specified areas,(iii); that is, the leader agent converges to a desired reference trajectory .
5. Distance and Area Based Control
Theorem 6. Consider the group of agents given by (1)-(2) communicated by a well-defined DFG given in (3) and planar topology (35). Define the control law aswhere are positive real constant gains, and are defined in (12), andSuppose that (a) the agents are not collinear and do not collide at and (b) the equilibria occur only when , . Then, in the closed-loop system (1), (2), and (37)–(39) the agents converge locally to the desired formation; that is, , , avoiding interagent collisions; that is, , , , and the leader agent converges to .
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 functionwhere 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 byUsing a similar derivation as in Theorem 2, (42) is reduced toUsing the matrix expression for given by (36) the second term on the left hand side can be written asSince and using the undirected assumption for the topology, the equation above simplifies toSubstituting (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), obtainingDefining the derivative (47) becomesFinally, grouping terms, defining to cancel two terms, the expression for is given byIt was shown in Theorem 2 that the term vanishes. It is shown in the appendix that to reach the following final expression for Note that is negative semidefinite. Using LaSalle’s Invariance Theorem, the system will converge to the largest invariant set satisfying . From (50) it follows thatSubstituting these equilibrium conditions into the closed-loop system (1), (2), and (37)–(39), thenObserve that all the agents converge to the same velocity . Besides, since , , then , . Then, (53) and (54) reduce toAdding (55) and (56) for all and using the fact that and Proposition A.1, (56) can be simplified to , which shows that the leader agent trajectory converges to . Thus, it is concluded that agents converge locally to the desired formation tracking.
Finally, note that the Lyapunov function is radially unbounded, that is, tends to infinity when any . Since the agents do not collide at by the initial assumption and , 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.
6. Numerical Simulations
6.1. Distance-Based Formation Control
In this section, a numerical simulation of the control strategy analyzed in Section 3 is presented with and ( and are first-order agents; and are second-order agents), with the complete DFG shown in the Figure 3 where the desired interagent distances are given by and (an equilateral triangle with side equal to formed by agents , , and and the leader agent placed in its center).
The trajectories of the agents in the plane are shown in Figure 4(a), where the symbols are the initial positions , , , . Some postures of the agents are depicted in the time instants , , , , and , respectively. Note that the agents converge to the desired formation, and the leader follows the trajectory as is shown in Figures 4(b) and 4(c), where the formation errors and the error of the trajectory tracking of , that is, , converge to zero, respectively.
(a) Trajectories of the agents in the plane
(b) Distance formation errors
(c) Trajectory tracking error of the leader agent
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).
(a) Distances between agents
(b) Control inputs
6.2. Distance and Area Based Formation Control
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 and have been eliminated. Now, a planar topology is set as in (35) with as shown in Figure 6, with the desired areas being . The control parameters for the areas are set to .
Some postures of the agents are depicted in the time instants , , , , and , 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).
(a) Trajectories of the agents
(b) Formation errors
(c) Area errors between 3-tuples
(a) Marching error of the leader agent
(b) Distances between agents
(c) Control inputs
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.
Proposition A.1. For a set of n agents with planar topology given by (35) the area errors in a given configuration satisfied the equation
Proof. The proof is shown by induction. For the first step let , write the expression for explicitly, and use the properties of , , and .Since is invariant for a circular shift of indexes, the right hand side becomes Assuming now that it is true for , in the next step , (A.1) is given byThe 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:Since by the undirected property of the topology and , further simplifications lead to 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.
This work was supported by Universidad Iberoamericana, CONACyT Mexico, Universidad Católica del Uruguay, and ANII, Uruguay.
S. Ramazani, R. R. Selmic, and M. de Queiroz, “Multiagent layered formation control based on rigid graph theory,” in Vamvoudakis Sarangapani Jagannathan Control of Complex Systems, G. V. Kyriakos and S. Jagannathan, Eds., pp. 397–419, Butterworth-Heinemann, Oxford, UK, 2016.View at: Google Scholar
B. D. O. Anderson, Z. Lin, and M. Deghat, “Combining distance-based formation shape control with formation translation,” in Developments in Control Theory Towards Glocal Control, L. Qindadn, J. Chen, T. Iwasaki, and H. Fujioka, Eds., vol. 1, pp. 121–130, Institution of Engineering and Technology, Stevenage, UK, 1st edition, 2012.View at: Publisher Site | Google Scholar
E. D. Ferreira-Vazquez, E. G. Hernandez-Martinez, J. J. Flores-Godoy, G. Fernandez-Anaya, and P. Paniagua-Contro, “Distance-based formation control using angular information between robots,” Journal of Intelligent and Robotic Systems, vol. 83, no. 3-4, pp. 543–560, 2016.View at: Publisher Site | Google Scholar
E. Ferreira-Vazquez, J. Flores-Godoy, E. Hernandez-Martinez, and G. Fernandez-Anaya, “Adaptive control of distance-based spatial formations with planar and volume restrictions,” in Proceedings of the 2016 IEEE Conference on Control Applications (CCA), pp. 905–910, Buenos Aires, Argentina, September 2016.View at: Publisher Site | Google Scholar
E. G. Hernandez-Martinez, E. D. Ferreira-Vazquez, A. Lopez-Gonzalez, J. J. Flores-Godoy, G. Fernandez-Anaya, and P. Paniagua-Contro, “Formation control of heterogeneous robots using distance and orientation,” in Proceedings of the IEEE Conference on Control Applications (CCA '16), pp. 507–512, Buenos Aires, Argentina, September 2016.View at: Publisher Site | Google Scholar
X. Cai and M. De Queiroz, “Multi-agent formation maintenance and target tracking,” in Proceedings of the 1st American Control Conference (ACC '13), pp. 2521–2526, Washington, DC, USA, June 2013.View at: Google Scholar
E. H. C. Harik, F. Guerin, F. Guinand, J.-F. Brethe, and H. Pelvillain, “UAV-UGV cooperation for objects transportation in an industrial area,” in Proceedings of the IEEE International Conference on Industrial Technology (ICIT '15), pp. 547–552, Seville, Spain, March 2015.View at: Publisher Site | Google Scholar