Collaborative multiple micro air vehicles’ localization and target tracking in GPS-denied environment from range

Abstract

We treat the problem of simultaneous collaborative multiple micro air vehicles’ localization and target tracking using time-varying range and (relative and absolute) velocity measurements. The proposed solution combines robustly local nonlinear observers that estimate the relative positions between agents and their neighbors, and cooperative filters that fuse each agent’s local estimates to globally localize them with respect to the target (and therefore to each other). These estimates are then introduced in a dynamic consensus-type control law that ensures the global collective target tracking while simultaneously estimating the target’s velocity, without needing any external reference which makes it applicable in GPS-denied environments. Finally, a simulation scenario is studied in order to show the efficiency of the proposed solution.

Keywords

Collaborative tracking noncooperative target localization multiple micro air vehicles Global Positioning System-denied environment

Introduction

Over the last decades we have witnessed the explosion of applications incorporating networks of robotic vehicles. Inspired by the behavior of animals in nature and motivated by the fact that a variety of objectives can be more efficiently, rapidly, and robustly accomplished collaboratively rather than independently, multiagent systems have been in the core of attention from both theoreticians and practitioners. Of particular interest have been applications involving multiple (aerial, ground, marine) vehicles that need to collaborate to achieve a common goal such as to ensure the exploration of unknown environments, to follow targets, to seek dangerous emitting sources, or to ensure high-precision photography.¹ Note that in order to attain the corresponding desired objective the location of the vehicles is an information of paramount importance. It is exploited in the guidance, control, and estimation algorithms that ensure the successful undertaking of the mission scenario. However, such global information, as obtained for example by GPS receivers, is not available in indoor environments² and in general, due to hardware malfunction or unavailability of the minimum number of GPS satellites. Instead local, low-cost sensors (cameras, infrared sensors, sonars) are usually incorporated to provide a sufficient localization.

This work focuses on the design of a distributed control law which in an ideal (perfect measurements, relative positions available) scenario ensures that a number of multiple micro air vehicles (MAVs) track the unknown motion of a target. Our precise objective is to propose a robust law based on (local) relative distance measurements and noisy velocity measurements to combine localization, target velocity estimation, and target tracking algorithms to obtain a globally exponential solution for the simultaneous collective agent localization and target tracking problem. The target can either be static, in which case this boils down to a consensus problem, or dynamic, yielding a setting similar to the classical leader–follower problem. In both cases, only a set of agents (at least one) usually has access to a relative information about the motion of the target while each agent has only access to local information. However, depending on the measurements available and the interagent communication characteristics, ensuring exact target tracking can be impossible and the mission objective is relaxed into following the target at a certain distance. Such a scenario arises exactly when relative distance measurements are available, instead of relative positions. These are possibly complemented by noisy relative or absolute velocity measurements of the agents. The reason why exact tracking cannot be achieved lies on the fact that distributed controllers frequently require relative position information. For that to be obtained the observability analysis reveals that a persistent relative motion between any pair of agents has to be present.^3,4 The distributed control law has to account for this additional motion.

We propose a solution to the problem of collective target tracking for a target with unknown constant velocity (or unknown varying velocity but known acceleration) based on the agents’ relative distance, relative velocity, and absolute velocity measurements. The communication topology between agents is considered as undirected and connected, while the communication topology between the target and the connected followers (at least one) is directed, which yields a strongly connected digraph. Our contribution consists of: (a) designing a localization algorithm that provides for every agent an estimation of the relative positions with respect to its neighbors and the target, (b) designing an estimator for each agent that provides an estimate of the target’s velocity, (c) designing an estimator that filters the noisy relative velocity measurements and explicitly take it into account in the global stability analysis, and (d) designing a distributed control law that allows for the followers to collaboratively track the target and ensures a persistent relative motion.

Concerning the localization, in the literature we distinguish two large types of scenarios^5,6: (a) Mutual localization, referring to the scenario where each agent needs to find its own (static) position in a reference frame common to the entire network and (b) collaborative localization referring to the localization of a (dynamic) target using an already mutually localized network. Our problem is of the second type. Depending on the community (control, robotics, sensors) and the mission objective, we can have 2D or 3D models, centralized or distributed algorithms, a variety of available measurements, for example absolute position (GPS), relative positions, distances, bearings or IMU measurements, and additional known points (anchors, markers). Additionally, the localization solutions can be signal/information based or model based which are essentially divided into optimization based and observer based. Observer-based, distributed estimation algorithms have recently been shown to present some rather interesting robust characteristics. In particular, it was established that observers which are distributed can enhance the quality of estimation by eliminating noise, see Tabarea et al.⁷ and Li and Sanfelice,⁸ which is of great interest in all applications.

Hence motivated by these recent developments and unlike the probabilistic and Kalman filter-based approaches,^9–11 which cannot in general guarantee analytical global convergence, following Sarras et al.¹² we adopt an observer-based approach to treat the problem of multivehicle collaborative localization using time-varying range and relative velocity measurements without requiring any global positioning information. The range measurements can be obtained using a variety of sensors such as stereo-vision systems that typically equip robotic vehicles¹³ or by combining monocular cameras from different vehicles.¹⁴ This measurement scenario renders our obtained algorithm applicable to GPS-denied environments. We show that if each agent can obtain a good estimate of the target’s velocity then it successfully localizes itself with respect to the target by the combination of local estimates of its neighbors’ relative positions and the fusion with the neighbors’ own estimates.

As opposed to other works, for example Bahr et al.,¹⁵ Dandach et al.,¹⁶ Deghat et al.¹⁷ our algorithm does not require global information (absolute position) but rather local measurements. Compared to the relevant work in Chai et al.⁶ that treats the collaborative localization problem with respect to a static target, instead of single integrators we consider double integrator dynamics to model the agents’ translational dynamics and require no knowledge on the rate of change of the distances. Furthermore, we extend these results to the scenario of a dynamic target and show that by adopting an approach inspired by the recent developments on dynamically scaled Lyapunov functions^18,19 we are able to show relative localization with a uniform global exponential convergence using a strict Lyapunov function.

Concerning the distributed control design, we follow the control paradigm laid in Hong et al.^20,21 and followed by many others, for example Ren and Beard,¹ Cai and Huang,²² Liu and Huang,²³ and references therein. We show that in the case of known relative positions the interconnection of a consensus-type tracking law and a consensus-type target velocity estimator provides a globally exponential tracking solution. Compared to the landmark work,²¹ that more generally treats directed and switching graph topologies, we provide an alternative Lyapunov function that does not require knowledge of the global graph topology properties and thus, contributes in rendering it a truly distributed solution. While Ren and Beard,¹ Cai and Huang,²² Liu and Huang²³ consider also cases of nonlinear models for the agents’ dynamics and more general graph topologies, they also assume that the relative positions are readily available and that all measurements are not corrupted by measurements. Works using mainly range measurements are focused on formation control, see for example Tron et al.,⁵ Montijano et al.,¹³ Oh and Ahn,²⁴ and Oh et al.²⁵

Although our results focus on target tracking, by straighforwardly modifying the control terms to incorporate desired distances between neighbor agents we can obtain a solution to target tracking with a prescribed formation of the followers and interagent collision avoidance, for the latter see the recent work of Franchi et al.²⁶ and references therein. These features are illustrated in the simulation scenario.

Model and problem formulation

Network topology

We consider that the interconnection graph describing the communication between the N + 1 agents forming the multiagent system, target included, is formed by an undirected graph describing the network of the N followers and a directed graph connecting the target to some followers. The complete (directed, strongly connected) network topology can be modeled using the Laplacian matrix $L : = [l_{i j}] \in ℝ^{(N + 1) \times (N + 1)}, i, j \in {0, \dots, N}$ , whose elements are defined as

l_{i j} = {\begin{matrix} \sum_{j \in N_{i}} w_{i j} & i = j \\ - w_{i j} & i \neq j \end{matrix}

(1)

where w_ij = 0 if i = j, w_ij > 0 if

j \in N_{i}

and w_ij = 0 otherwise. In this case,

N_{i}

stands for the set of agents transmitting information to the ith agent. Note that, by construction,

L

has zero row sum, i.e.

L 1_{N + 1} = 0

, where

1_{N + 1}

is a column vector of size N + 1 filled with ones or, equivalently,

l_{i i} - \sum_{j \in N_{i}} l_{i j} = 0

. Following this definition, we note that the undirected graph topology between the N followers with the N × N (symmetric) is described by the Laplacian matrix

L_{u} : = [l_{i j}]

, for

i, j \in {1, \dots, N}

. Further, define as

B

an N × N diagonal matrix whose ith element is either

b_{i} > 0

or 0 based on whether the ith agent receives information from the target, i.e. belongs to the set

N_{0}

. For more details on network topologies and their properties refer, for example to Ren and Beard.¹

Dynamic model

We consider that the dynamics of each of the N + 1 identical agents composing the multivehicle system of interest can be described by the double integrator model

{\dot{x}}_{i} = v_{i}

(2)

{\dot{v}}_{i} = u_{i}, i = {0, \dots, N}

(3)

with

x_{i}, v_{i} \in R^{3}

denoting the position and velocity vectors of the ith vehicle in the inertial frame, while

u_{i} \in R^{3}

is the applied acceleration.

By the index i = 0 we denote the (static or dynamic) target with respect to which the localization will be referred. As is evident, the static scenario corresponds to a target’s dynamics

{\dot{x}}_{0} = 0

(4)

{\dot{v}}_{0} = 0

(5)

In the dynamic case, we assume that the target’s acceleration u₀ is known or zero, with the latter corresponding to a scenario of a noncooperative target moving in straight line at maximal velocity.

Now, we naturally define the relative position, velocity, and acceleration between two agents as

x_{i j} = x_{i} - x_{j}

(6)

v_{i j} = v_{i} - v_{j}

(7)

u_{i j} = u_{i} - u_{j}

(8)

that yield the required relative dynamics

{\dot{x}}_{i j} = v_{i j}

(9)

{\dot{v}}_{i j} = u_{i j}

(10)

For our localization problem, we consider that the available measurements consist of the relative velocities and distances^a

y_{i} = col (v_{i j}^{T}, d_{i j}^{T})

(11)

with the distance d_ij between agent i and its neighbor j defined as

d_{i j} : = | x_{i} - x_{j} | = | x_{i j} |

(12)

A simple derivation provides

{\dot{d}}_{i j} = \frac{x_{i j}^{T} v_{i j}}{d_{i j}} = \frac{v_{i j}^{T} x_{i j}}{d_{i j}}

(13)

In conclusion, the complete model on which our design will be based is summarized as

{\dot{x}}_{i j} = v_{i j}

(14)

{\dot{v}}_{i j} = u_{i j}

(15)

{\dot{d}}_{i j} = \frac{v_{i j}^{T} x_{i j}}{d_{i j}}

(16)

Cooperative localization

Before presenting our main results, we define some additional notation and then remind the definition of a persistently exciting (PE) function. The notation for a matrix A being positive (semi-)definite is expressed by $A ≻ 0 (≽ 0)$ , while for the case of a positive scalar a we write instead a > 0. We note $λ_{m} (A)$ the minimal eigenvalue of a square matrix A. The notation $| \cdot |$ will refer, depending on its argument, either to the absolute value of a scalar function, to the Euclidean norm of a vector, or to the induced two-norm of a matrix.

Definition 1. Let the function $v_{i j} : R_{\geq 0} \to R^{3}$ be continuous. It is PE if there exist some T > 0 and $μ > 0$ such that

\int_{t}^{t + T} v_{i j} (τ) v_{i j}^{T} (τ) d τ ≽ μ I ≻ 0, \forall t

(17)

For the distance-based localization scenario we have at hand, we require that certain relative velocities are PE which means that in order for an agent to be able to reconstruct a relative position with respect to a neighboring agent, it is necessary to move out of the line of sight (straight line connecting two agents) for some time which in fact is required for the relative position to be observable.^3,4 In practice, this condition imposes a requirement on the applied accelerations (control inputs) which can always be ensured for each agent by including an excitation term but however, might complicate the stability analysis.

Single vehicle localization from direct local measurements: Static target

In this subsection we will consider the problem of localization of each agent with respect to its neighbors by incorporating local, noiseless measurements, and considering a static target. This will be achieved by means of a designed nonlinear observer based on the invariant-manifold observer methodology, see Astolfi et al.²⁷ and Karagiannis and Astolfi¹⁸ for the general setting and Martin and Sarras,²⁸ Martin and Sarras,²⁹ and Sarras et al.¹² for recent applications on MAVs.

Proposition 1. Consider the dynamical system defined in equations (14) to (16) and assume that v_ij is PE. Then, the dynamical system

{\dot{ξ}}_{i} : = - \frac{K_{i j} d_{i j}^{2}}{2} u_{i j} - K_{i j} v_{i j} v_{i j}^{T} {\hat{x}}_{i j} + v_{i j}

(18)

{\hat{x}}_{i j} : = ξ_{i} + \frac{d_{i j}^{2}}{2} K_{i j} v_{i j}

(19)

is a globally exponential observer with gain K_ij > 0.

Proof. First, let us define the relative position estimation error

z_{i j} : = ξ_{i} + β_{i} (y_{i}, {\hat{y}}_{i}) - x_{i j} = : {\hat{x}}_{i j} - x_{i j}

(20)

for a certain mapping β_i, that generally can also depend on a filtered y_i denoted

{\hat{y}}_{i}

, that will be properly selected. At this point we examine the case where β_i is a function only of y_i. Then, the general form of the z_i dynamics gives

\begin{matrix} {\dot{z}}_{i j} : = {\dot{ξ}}_{i} + \partial_{y_{i}} β_{i} {\dot{y}}_{i} - {\dot{x}}_{i j} = {\dot{ξ}}_{i} + \partial_{d_{i j}} β_{i} {\dot{d}}_{i j} + \partial_{v_{i j}} β_{i} {\dot{v}}_{i j} - {\dot{x}}_{i j} \\ = {\dot{ξ}}_{i} + \partial_{d_{i j}} β_{i} \frac{v_{i j}^{T} x_{i j}}{d_{i j}} + \partial_{v_{i j}} β_{i} u_{i j} - v_{i j} \end{matrix}

With the choice

{\dot{ξ}}_{i} : = - \partial_{d_{i j}} β_{i} \frac{v_{i j}^{T} {\hat{x}}_{i j}}{d_{i j}} - \partial_{v_{i j}} β_{i} u_{i j} + v_{i j}

and the β_i mapping as

β_{i} (y_{i}) : = \frac{d_{i j}^{2}}{2} K_{i j} v_{i j}

(21)

the z_i dynamics obtains the more explicit form

{\dot{z}}_{i j} = - K_{i j} v_{i j} v_{i j}^{T} z_{i j}

(22)

From Lemma 5 of Loría and Panteley³⁰ we know that the nominal system (22) has a uniformly global exponentially stable (UGES) equilibrium at the origin for a PE and uniformly bounded v_ij.

Remark 1. From the converse Lyapunov lemma (Lemma 1 of Loría and Panteley³⁰) we know that there exists a quadratic Lyapunov function

V_{z_{i}} : = \frac{1}{2} z_{i j}^{T} P (t) z_{i j}

(23)

with P(t) such that

0 ≺ {c_{1}}^{I} ⪯ P (t) = P^{T} {(t)}^{⪯} c_{2} I

, the unique solution of the equation

\dot{P} - P K_{i j} v_{i j} v_{i j}^{T} - v_{i j} v_{i j}^{T} K_{i j} P = - Q

(24)

with

Q (t) = Q^{T} (t)

such that

0 ≺ c_{3} I ⪯ Q (t) ⪯ c_{4} I

This lemma will be exploited in the construction of a strict, dynamically scaled Lyapunov function of the more general solution that follows in the next subsection.

Remark 2. Notice that in our algorithm, we further require that the relative acceleration between neighboring agents be either available or can be reconstructed. As is common in the literature for example, the agents might transmit their respective control actions (accelerations or resulting predicted positions) to their neighbors. ³¹ If these signals are imperfectly known, due for example to transmission perturbations, we can explicitly provide a robustness analysis by treating the imperfections as additive disturbances and using our Lyapunov function in an input-to-state stable (ISS) analysis. Alternatively, and under the assumption that relative motion varies slowly, we can consider that the relative acceleration is reconstructed by numerical differentiation of the available relative velocities.

Remark 3. Let us mention that in the case where relative orientations (rotation matrices) are available, by means of bearing measurements, and assuming that each agent is equipped with a gyro, we can adapt the obtained algorithms to such scenario. In such case the agents do not need to be already mutually localized and the transmitted signals are communicated in the proper local frame of each agent.

Single vehicle localization from filtered local measurements: Dynamic target

In continuation of the previous scenario, we proceed to extend the localization algorithm to the case of a dynamic target in the presence of noisy velocity measurements, without assuming any particular noise characteristics.

Proposition 2. Consider the dynamical system defined in equations (14) to (16) and assume that v_ij is PE. Then, the dynamical system

\begin{matrix} {\dot{ξ}}_{i} := - \frac{K_{i j} d_{i j}^{2}}{2} (u_{i j} - K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) ({\hat{v}}_{i j} - v_{i j})) \\ - K_{i j} {\hat{v}}_{i j} {\hat{v}}_{i j}^{T} {\hat{x}}_{i j} + {\hat{v}}_{i j} \end{matrix}

(25)

{\hat{x}}_{i j} : = ξ_{i} + \frac{d_{i j}^{2}}{2} K_{i j} {\hat{v}}_{i j}

(26)

\dot{r} : = - c_{7} (r - 1) + \frac{c_{2}^{2} K_{i j}^{2}}{c_{1} c_{5}} | v_{i j} |^{2} | {\hat{v}}_{i j} - v_{i j} |^{2}

(27)

{\dot{\hat{v}}}_{i j} : = u_{i j} - K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) ({\hat{v}}_{i j} - v_{i j})

(28)

is a globally exponential observer, for some

c_{i} > 0

, with

r (0) \geq 1

and gains

\begin{matrix} K_{i j} : = c_{8} + \frac{c_{5} + c_{6} + c_{7} c_{2}}{c_{3}} \\ K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) : = c_{9} I + (r - 1) \frac{c_{2}^{2}}{c_{1} c_{5}} K_{i j}^{2} | v_{i j} |^{2} I \\ + \frac{c_{2}^{2}}{c_{6} r} (K_{i j}^{2} | {\hat{v}}_{i j} |^{2} | {\hat{x}}_{i j} |^{2} + 1) I \end{matrix}

Proof. First, let us define the relative position estimation error as in equation (20). Then, the general form of the z_i dynamics gives

\begin{matrix} {\dot{z}}_{i j} : = {\dot{ξ}}_{i} + \partial_{{\hat{y}}_{i}} β_{i} {\dot{\hat{y}}}_{i} + \partial_{y_{i}} β_{i} {\dot{y}}_{i} - {\dot{x}}_{i j} \\ = {\dot{ξ}}_{i} + \partial_{{\hat{d}}_{i j}} β_{i} {\dot{\hat{d}}}_{i j} + \partial_{{\hat{v}}_{i j}} β_{i} {\dot{\hat{v}}}_{i j} + \partial_{d_{i j}} β_{i} {\dot{d}}_{i j} & + \partial_{v_{i j}} β_{i} {\dot{v}}_{i j} - {\dot{x}}_{i j} \\ = {\dot{ξ}}_{i} + \partial_{{\hat{d}}_{i j}} β_{i} {\dot{\hat{d}}}_{i j} + \partial_{{\hat{v}}_{i j}} β_{i} {\dot{\hat{v}}}_{i j} + \partial_{d_{i j}} β_{i} \frac{v_{i j}^{T} x_{i j}}{d_{i j}} \\ + \partial_{v_{i j}} β_{i} u_{i j} - v_{i j} \end{matrix}

which with the choice

\begin{matrix} {\dot{ξ}}_{i} : = - \partial_{{\hat{d}}_{i j}} β_{i} {\dot{\hat{d}}}_{i j} - \partial_{{\hat{v}}_{i j}} β_{i} {\dot{\hat{v}}}_{i j} - \partial_{d_{i j}} β_{i} \frac{{\hat{v}}_{i j}^{T} {\hat{x}}_{i j}}{d_{i j}} \\ - \partial_{v_{i j}} β_{i} u_{i j} + {\hat{v}}_{i j} \end{matrix}

reduces, after defining

e_{v_{i j}} : = {\hat{v}}_{i j} - v_{i j}

, to

\begin{matrix} {\dot{z}}_{i j} = - \partial_{d_{i j}} β_{i} (\frac{{\hat{v}}_{i j}^{T} {\hat{x}}_{i j}}{d_{i j}} - \frac{v_{i j}^{T} x_{i j}}{d_{i j}}) + {\hat{v}}_{i j} - v_{i j} \\ = - \partial_{d_{i j}} β_{i} \frac{v_{i j}^{T}}{d_{i j}} z_{i j} - \partial_{d_{i j}} β \frac{{\hat{x}}_{i j}^{T}}{d_{i j}} e_{v_{i j}} + e_{v_{i j}} \end{matrix}

Selecting further the β_i mapping as

\begin{matrix} β_{i} (y_{i}, {\hat{y}}_{i}) : = \frac{d_{i j}^{2}}{2} K_{i j} {\hat{v}}_{i j} = \frac{d_{i j}^{2}}{2} K_{i j} (v_{i j} + e_{v_{i j}}) \\ \partial_{d_{i j}} β_{i} = d_{i j} K_{i j} (v_{i j} + e_{v_{i j}}) \end{matrix}

the z_i dynamics obtains the more explicit form

\begin{matrix} {\dot{z}}_{i j} = - K_{i j} v_{i j} v_{i j}^{T} z_{i j} - K_{i j} e_{v_{i j}} v_{i j}^{T} z_{i j} - K_{i j} (v_{i j} + e_{v_{i j}}) {\hat{x}}_{i j}^{T} e_{v_{i j}} + e_{v_{i j}} \\ = - K_{i j} v_{i j} v_{i j}^{T} z_{i j} - K_{i j} e_{v_{i j}} v_{i j}^{T} z_{i j} - (K_{i j} {\hat{v}}_{i j} {\hat{x}}_{i j}^{T} - I) e_{v_{i j}} \end{matrix}

Taking the function $V_{z_{i}}$ defined in equation (23) and computing its time derivative along trajectories of the z_i dynamics yields

\begin{matrix} {\dot{V}}_{z_{i}} := \frac{1}{2} z_{i j}^{T} (\dot{P} (t) - P (t) K_{i j} v_{i j} v_{i j}^{T} - v_{i j} v_{i j}^{T} K_{i j} P (t)) z_{i j} \\ - z_{i j}^{T} P (t) K_{i j} e_{v_{i j}} v_{i j}^{T} z_{i j} - z_{i j}^{T} P (t) (K_{i j} {\hat{v}}_{i j} {\hat{x}}_{i j}^{T} - I) e_{v_{i j}} \\ \leq - \frac{c_{3}}{2} | z_{i j} |^{2} + c_{2} | z_{i j} |^{2} K_{i j} | e_{v_{i j}} | | v_{i j} | \\ + c_{2} | z_{i j} | (K_{i j} | {\hat{v}}_{i j} | | {\hat{x}}_{i j} | + 1) | e_{v_{i j}} | \\ \leq - (\frac{c_{3}}{2} - \frac{c_{5} + c_{6}}{2}) | z_{i j} |^{2} + \frac{c_{2}^{2}}{2 c_{5}} K_{i j}^{2} | v_{i j} |^{2} | e_{v_{i j}} |^{2} | z_{i j} |^{2} \\ + \frac{c_{2}^{2}}{c_{6}} (K_{i j}^{2} | {\hat{v}}_{i j} |^{2} | {\hat{x}}_{i j} |^{2} + 1) | e_{v_{i j}} |^{2} \end{matrix}

where we applied Young’s inequality to the two cross-terms of the first inequality. In order to handle the last two cross-terms in the above right-hand side we employ a dynamic scaling of the form

W_{z_{i}} : = \frac{V_{z_{i}}}{r}

(29)

with r dynamics given, with

r (0) \geq 1

, as

\dot{r} : = - c_{7} (r - 1) + \frac{c_{2}^{2}}{c_{1} c_{5}} K_{i j}^{2} | v_{i j} |^{2} | e_{v_{i j}} |^{2}

Then, the time derivative of $W_{z_{i}}$ can be shown to be

\begin{matrix} {\dot{W}}_{z_{i}} = \frac{{\dot{V}}_{z_{i}}}{r} - W_{z_{i}} \frac{\dot{r}}{r} \\ \leq \frac{{\dot{V}}_{z_{i}}}{r} + c_{2} | z_{i j} |^{2} c_{7} \frac{(r - 1)}{r} \\ - c_{1} \frac{| z_{i j} |^{2}}{r} \frac{c_{2}^{2}}{c_{1} c_{5}} K_{i j}^{2} | v_{i j} |^{2} | e_{v_{i j}} |^{2} \\ \leq - (\frac{c_{3}}{2} - \frac{c_{5} + c_{6} + c_{7} c_{2}}{2}) \frac{| z_{i j} |^{2}}{r} \\ + \frac{c_{2}^{2}}{c_{6}} (K_{i j}^{2} | {\hat{v}}_{i j} |^{2} | {\hat{x}}_{i j} |^{2} + 1) \frac{| e_{v_{i j}} |^{2}}{r} \end{matrix}

with the last right-hand side term depending on the error between the filtered

{\hat{v}}_{i j}

and the true measurements v_ij.

Choosing

{\dot{\hat{v}}}_{i j} : = u_{i j} - K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) e_{v_{i j}}

(30)

with

K_{v_{i}}

a (free) positive gain function of

{\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r

, yields the dynamics of the filtering error

e_{v_{i j}} : = {\hat{v}}_{i j} - v_{i j}

{\dot{e}}_{v_{i j}} : = - K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) e_{v_{i j}}

(31)

By simple derivations one can show that the following function

V_{e_{v}} : = \frac{1}{2} | e_{v_{i j}} |^{2}

(32)

is a Lyapunov function for the

e_{v_{i j}}

dynamics since it satisfies

{\dot{V}}_{e_{v}} = - e_{v_{i j}}^{T} K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) e_{v_{i j}}

and hence, ensuring global exponential convergence of the estimate

{\hat{v}}_{i j}

to v_ij. Similarly, for the r dynamics we take the function

V_{r} : = \frac{1}{2} {(r - 1)}^{2}

(33)

that gives

{\dot{V}}_{r} = - c_{7} {(r - 1)}^{2} + (r - 1) \frac{c_{2}^{2}}{c_{1} c_{5}} K_{i j}^{2} | v_{i j} |^{2} | e_{v_{i j}} |^{2}

Selecting then the functions

K_{i j} : = c_{8} I, c_{8} > c_{3} - c_{5} + c_{6} + c_{7} c_{2}

and

\begin{matrix} K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) : = c_{9} I + (r - 1) \frac{c_{2}^{2}}{c_{1} c_{5}} K_{i j}^{2} | v_{i j} |^{2} I \\ + \frac{c_{2}^{2}}{c_{6} r} (K_{i j}^{2} | {\hat{v}}_{i j} |^{2} | {\hat{x}}_{i j} |^{2} + 1) I \end{matrix}

with

c_{9} > 0

, we can finally establish that the composite function

W_{z_{i}} + V_{e_{v}} + V_{r}

serves as a Lyapunov function for the complete dynamics with

\dot{\overset{\overset{︷}{W_{z_{i}} + V_{e_{v}} + V_{r}}}{}} \leq - c_{8} \frac{| z_{i j} |^{2}}{r} - c_{7} {(r - 1)}^{2} - c_{9} | e_{v_{i j}} |^{2}

which establishes UGES of the origin.

Remark 4. Notice that in the case where the mapping β_i is simply defined as

β_{i} (y_{i}) : = \frac{d_{i j}^{2}}{2} K_{i j} v_{i j}

then the resulting error dynamics is described as

{\dot{z}}_{i j} = - K_{i j} v_{i j} v_{i j}^{T} z_{i j} - e_{v_{i j}}

Then, using the PE condition, UGES of the nominal z_i system with respect to the origin, and UGES of the origin for the $e_{v_{i j}}$ system we can immediately conclude, for example from cascaded systems ³² or ISS arguments, ⁶ UGES of the interconnected system.

Collaborative localization from fusion of local estimates and measurements

In this subsection, we take advantage of the collaborative setting between the agents, which share information with their local neighbors, in order to enhance the localization capabilities of the agents, in particular, that do not have direct relative measurements with respect to the target. Of course for a static target the measurement v_i of each agent suffices.

To this end, define the fused estimate of the relative coordinates between agent j and the target as

{\hat{x}}_{i 0}^{j} : = ρ_{j} - {\hat{x}}_{i j}

(34)

ρ_{0} : = 0

(35)

Then, the proposed consensus-based estimation mechanism for agent i, that exploits the fusion of its own estimate with the ones of its neighbors to produce a more accurate fused estimate, is given by

{\dot{ρ}}_{i} : = v_{i} - {\hat{v}}_{0}^{i} + λ_{0} \sum_{j \in N_{i}} ({\hat{x}}_{i 0}^{j} - ρ_{i}), λ_{0} > 0

(36)

with

{\hat{v}}_{0}^{i}

an estimation of the target’s velocity v₀ by the ith agent to be defined in the following section.

We now state the following result.

Proposition 3. Consider the dynamical system defined in equations (14) to (16) and assume that v_ij is PE. Then, the dynamical system

\begin{matrix} {\dot{ξ}}_{i} := - \frac{K_{i j} d_{i j}^{2}}{2} (u_{i j} - K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) ({\hat{v}}_{i j} - v_{i j})) \\ - K_{i j} {\hat{v}}_{i j} {\hat{v}}_{i j}^{T} {\hat{x}}_{i j} + {\hat{v}}_{i j} \end{matrix}

(37)

{\hat{x}}_{i j} : = ξ_{i} + \frac{d_{i j}^{2}}{2} K_{i j} {\hat{v}}_{i j}

(38)

\dot{r} : = - c_{7} (r - 1) + \frac{c_{2}^{2}}{c_{1} c_{5}} K_{i j}^{2} | v_{i j} |^{2} | {\hat{v}}_{i j} - v_{i j} |^{2}

(39)

{\dot{\hat{v}}}_{i j} : = u_{i j} - K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) ({\hat{v}}_{i j} - v_{i j})

(40)

{\dot{ρ}}_{i} : = v_{i} - {\hat{v}}_{0}^{i} + λ_{0} \sum_{j \in N_{i}} ({\hat{x}}_{i 0}^{j} - ρ_{i})

(41)

with

r (0) \geq 1

, ensures that when

{\hat{v}}_{0}^{i}

is such that

v_{0} - {\hat{v}}_{0}^{i} \to 0

every agent is globally exponentially localized with respect to the target, for some

c_{i} > 0

and with gains

\begin{matrix} K_{i j} : = c_{8} + \frac{c_{5} + c_{6} + c_{7} c_{2}}{c_{3}} \\ K_{v_{i}} ({\hat{x}}_{i j}, {\hat{v}}_{i j}, v_{i j}, r) : = c_{9} I + (r - 1) \frac{c_{2}^{2}}{c_{1} c_{5}} K_{i j}^{2} | v_{i j} |^{2} I \\ + \frac{c_{2}^{2}}{c_{6} r} (K_{i j}^{2} | {\hat{v}}_{i j} |^{2} | {\hat{x}}_{i j} |^{2} + 1) I \end{matrix}

Furthermore, when $v_{0} - {\hat{v}}_{0}^{i}$ is bounded the localization error is bounded.

Proof. For $i = 1, \dots N$ , we define

σ_{i} : = ρ_{i} - x_{i 0}

(42)

σ_{0} : = 0, {\dot{σ}}_{0} = 0

(43)

Then we obtain the consensus system

\begin{matrix} {\dot{σ}}_{i} = - λ_{0} \sum_{j \in N_{i}} (σ_{i} - σ_{j}) + λ_{0} \sum_{j \in N_{i}} ({\hat{x}}_{i j} - x_{i j}) + v_{0} - {\hat{v}}_{0}^{i} \\ = - λ_{0} \sum_{j \in N_{i}} (σ_{i} - σ_{j}) + λ_{0} \sum_{j \in N_{i}} z_{i j} + v_{0} - {\hat{v}}_{0}^{i} \end{matrix}

with σ_i seen as the individual states of the N agents and σ₀ the state of a leader, while the last two terms are seen as external signals. Defining the stacked variables

\begin{matrix} σ : = col (σ_{0}, \dots, σ_{N}) \\ τ_{i} : = \sum_{j \in N_{i}} z_{i j} \\ τ : = col (τ_{0}, \dots, τ_{N}) \\ ψ : = col (v_{0}, \dots, v_{N}) - col ({\hat{v}}_{0}^{0}, \dots, {\hat{v}}_{0}^{N}), {\hat{v}}_{0}^{0} : = 0 \end{matrix}

we obtain the dynamics

\dot{σ} : = - λ_{0} (L \otimes I_{3}) σ + λ_{0} τ + ψ

(44)

As is well known, from the properties of the assumed underlying graph topology, we have that the nominal system $\dot{σ} = - λ_{0} (L \otimes I_{3}) σ$ has a UGES equilibrium at the origin. Furthermore, we know there exists a quadratic Lyapunov function, defined here as

V_{σ} : = σ^{T} (Ξ \otimes I_{3}) σ, Ξ = Ξ^{T} ≻ 0

(45)

that establishes the claim of the nominal system. The first part of the proof is concluded by standard arguments on cascaded systems (see e.g. Lemma 2.1 or Proposition 2.3 of Loría and Panteley³²) since the complete error system consists of two nominal UGES subsystems interconnected through the terms τ, ψ that satisfy a linear growth condition. The second part of the proof follows immediately from the observation that the σ system is ISS with

v_{0} - {\hat{v}}_{0}^{i}

as an input.

Remark 5. Although not presented here, notice that our results are also applicable for switched communication graphs (due e.g. to loss of communication link or measurements) under the additional assumption of uniform connectivity as is done, for example for the single-landmark multiagent localization in the recent work of Chai et al. ⁶ We stress again that in our setting, however, the derivative of the relative distances is not required and furthermore, measurement noise is explicitly treated by means of additional filters.

Remark 6. Notice that in an all-to-all communication scenario it is not mandatory to have different scaling dynamics $\dot{r}$ for every pair of neighboring agents. A single one is sufficient by modifying the right-hand side of equation (39) to include the sum of all terms $\frac{c_{2}^{2}}{c_{1} c_{5}} K_{i j}^{2} | v_{i j} |^{2} | {\hat{v}}_{i j} - v_{i j} |^{2}$ for all neighbors i and j.

Collaborative tracking control with unknown target velocity

We now examine a distributed control law that ensures the tracking of a target with unknown velocity. Furthermore, the target’s velocity needs to be estimated by means of a (globally) converging observer. Finally, we will modify the proposed control law by adding an additional term in order to impose a motion to each agent such that the persistence-of-excitation condition (17), required by the localization algorithm, is readily satisfied.

For ease of reference we remind the main working assumptions:

The acceleration u₀ of the target is known or zero.

The interagent communication is defined by a static undirected, connected graph topology modeled by its Laplacian $L_{u} : = [l_{i j}]$ as in (1) but for $i, j \in {1, \dots, N}$ .

The topology between target and agents (at least one) is described by a directed path (at least one) and as such the complete topology is strongly connected.

Case: Known relative positions

In this subsection we consider first the ideal scenario where the relative positions x_ij are measured. This is summarized in the following assumption.

Assumption 1. The relative positions x_ij between agents are available and at least one agent has access to its relative position with respect to the target.

This assumption will naturally be removed when we consider the interconnection between the localization algorithm and the control law.

Let us first define by ${\hat{v}}_{0}^{i}$ the estimate of the target’s velocity v₀ by agent i. Now, similarly to Hong et al.,²¹ we define the control law as

u_{i} = u_{0} - k_{v} (v_{i} - {\hat{v}}_{0}^{i}) - k_{x} (\sum_{j \in N_{i}} l_{i j} x_{i j} + \sum_{k \in N_{0}} b_{k} x_{k 0})

(46)

with positive constants k_x, k_v, while the observer that provides the target’s velocity v₀ for agent i is selected as

{\dot{\hat{v}}}_{0}^{i} : = u_{0} - \frac{k_{v}}{k_{x}} (\sum_{j \in N_{i}} l_{i j} x_{i j} + \sum_{k \in N_{0}} b_{k} x_{k 0})

(47)

Furthermore, define the error variables^b

X : = col (x_{1}, \dots, x_{N}) - 1_{N} \otimes x_{0}

(48)

V : = col (v_{1}, \dots, v_{N}) - 1_{N} \otimes v_{0}

(49)

S : = - col ({\hat{v}}_{0}^{1}, \dots, {\hat{v}}_{0}^{N}) + 1_{N} \otimes v_{0}

(50)

The error dynamics can then be shown to take the following form

\dot{S} = \frac{k_{x}}{k_{v}} ((L_{u} + B) \otimes I_{3}) X

(51)

\dot{X} = V

(52)

\dot{V} = - k_{x} ((L_{u} + B) \otimes I_{3}) X - k_{v} V - k_{v} S

(53)

Proposition 4. Consider the error dynamics given in equations (51) to (53). Then, the origin is uniformly globally exponentially stable for $k_{x}, k_{v} > 0$ .

Proof. In order to establish the convergence properties we consider the following Lyapunov function, that is composed of four parts

V_{1} : = \frac{1}{2} (S^{T} P_{1} S + X^{T} P_{2} X + V^{T} P_{3} V)

(54)

V_{2} : = ϵ_{2} X^{T} V

(55)

V_{3} : = ϵ_{3} S^{T} V

(56)

V_{4} : = ϵ_{4} X^{T} S

(57)

V : = V_{1} + V_{2} + V_{3} + V_{4}

(58)

Its time derivative along trajectories of the error dynamics gives

\begin{matrix} \dot{V} := - k_{v} ϵ_{3} | S |^{2} - \frac{k_{x}}{k_{v}} (ϵ_{2} k_{v} - ϵ_{4}) X^{T} ((L_{u} + B) \otimes I_{3}) X \\ - V^{T} (P_{3} - ϵ_{2} I) V \end{matrix}

\begin{matrix} + S^{T} (k_{x} (\frac{1}{k_{v}} P_{1} - ϵ_{3} I) ((L_{u} + B) \otimes I_{3}) - ϵ_{2} k_{v} I) X \\ - V^{T} (P_{3} + ϵ_{3} k_{v} I - ϵ_{4} I) S \\ + V^{T} (P_{2} - ϵ_{2} k_{v} I - k_{x} (P_{3} - \frac{ϵ_{3}}{k_{v}} I) ((L_{u} + B) \otimes I_{3})) X \end{matrix}

Then with the selection

P_{1} : = k_{v} ϵ_{3} I + \frac{k_{v} ϵ_{2}}{k_{x}} ({(L_{u} + B)}^{- 1} \otimes I_{3})

(59)

P_{2} : = k_{v} ϵ_{2} I + k_{x} (ϵ_{1} - \frac{ϵ_{3}}{k_{v}}) (ϵ_{0} \otimes I_{3})

(60)

P_{3} : = ϵ_{1} I

(61)

ϵ_{0} : = - (L_{u} + B) \otimes I_{3}

(62)

all cross-terms disappear apart from the one in

S, V

. In order to establish our claim we need to ensure that the Lyapunov function V in equation (58) is positive definite with respect to the state (

S, X, V

) and that the negative terms in

\dot{V}

dominate the remaining cross-term. The former is ensured if the matrix

[\begin{matrix} P_{1} & ϵ_{4} I & ϵ_{3} I \\ ϵ_{4} I & P_{2} & ϵ_{2} I \\ ϵ_{3} I & ϵ_{2} I & P_{3} \end{matrix}] ≻ 0

(63)

while the latter if the matrix

[\begin{matrix} ϵ_{1} - ϵ_{2} & \frac{1}{2} (k_{v} (ϵ_{1} + ϵ_{3}) - ϵ_{4}) \\ \frac{1}{2} (k_{v} (ϵ_{1} + ϵ_{3}) - ϵ_{4}) & ϵ_{3} k_{v} \end{matrix}] ≽ ϵ_{5} I

(64)

for some (gain adjustable)

ϵ_{5} > 0

. By applying Schur’s complement to the above matrices we obtain the sufficient conditions

ϵ_{1} > ϵ_{2}

(65)

ϵ_{2} ≪ 1, ϵ_{3} ≪ 1 ϵ_{4} ≪ 1

(66)

k_{v} > \frac{\min (ϵ_{3}, ϵ_{4})}{ϵ_{1}}

(67)

We finally obtain

\begin{matrix} \dot{V} \leq - ϵ_{5} (| S |^{2} | + V |^{2}) - \frac{k_{x}}{k_{v}} (ϵ_{2} k_{v} - ϵ_{4}) \\ \times λ_{m} (L_{u} + B) | X |^{2} < 0, \forall (S, X, V) \neq (0, 0, 0) \end{matrix}

which concludes the proof of global exponential stability of the origin.□

Remark 7. Let us stress the fact that the proposed strict Lyapunov function is derived with gain conditions independent of the network topology characteristics, apart of course from the fact that the multiagent system is connected. Notice instead that in Chai et al. ⁶ a strict Lyapunov was obtained under conditions on the gains k_x, k_v (denoted k, l in that reference) that depend on the minimum and maximum eigenvalues of the matrix $L_{u} + B$ , which signifies that knowledge of the entire network topology is a priori required.

Case: Estimated relative positions

We now couple the proposed control law with the observer for relative positions. The control law (46) and the target velocity estimator become

u_{i} = u_{0} - k_{v} (v_{i} - {\hat{v}}_{0}^{i}) - k_{x} (\sum_{j \in N_{i}} l_{i j} {\hat{x}}_{i j} + \sum_{k \in N_{0}} b_{k} ρ_{k})

(68)

{\dot{\hat{v}}}_{0}^{i} : = u_{0} - \frac{k_{v}}{k_{x}} (\sum_{j \in N_{i}} l_{i j} {\hat{x}}_{i j} + \sum_{k \in N_{0}} b_{k} ρ_{k})

(69)

To this end, and in order to simplify notation, we define the column stack of the relative position estimates as

Z : = col [z_{i j}]

(70)

Then, we can write compactly the complete closed-loop system as

\begin{matrix} \dot{σ} : = - λ_{0} (L \otimes I_{3}) σ + λ_{0} τ + S \\ \dot{S} = \frac{k_{x}}{k_{v}} ((L_{u} + B) \otimes I_{3}) X + \frac{k_{x}}{k_{v}} B σ + \frac{k_{x}}{k_{v}} A Z \\ \dot{X} = V \\ \dot{V} = - k_{x} ((L_{u} + B) \otimes I_{3}) X - k_{v} V - k_{v} S - k_{x} B σ \\ - k_{x} A Z \end{matrix}

with A, B constant matrices of appropriate dimensions while the

Z

terms are seen as exponentially decaying perturbations. The global exponential stability of the composite system is concluded either by a spectral analysis or by a straightforward direct Lyapunov analysis based on the sum of the Lyapunov functions for each subsystem (local estimator for

{\hat{x}}_{i j}

, fusion for

{\hat{x}}_{i 0}

, target velocity v₀ estimator and controlled system).

Imposing the PE condition through the control

Now we consider an additive term to our control law that should be defined in a way to enforce the PE condition, i.e. produce a sufficiently rich motion for every pair of neighboring agents, but conserve the network’s stability properties.

The modified control law for each agent now takes the form

u_{i} = u_{P E}^{i} + u_{0} - k_{v} (v_{i} - {\hat{v}}_{0}^{i}) - k_{x} (\sum_{j \in N_{i}} l_{i j} {\hat{x}}_{i j} + \sum_{k \in N_{0}} b_{k} ρ_{k})

(71)

{\dot{\hat{v}}}_{0}^{i} : = u_{0} - \frac{k_{v}}{k_{x}} (\sum_{j \in N_{i}} l_{i j} {\hat{x}}_{i j} + \sum_{k \in N_{0}} b_{k} ρ_{k})

(72)

For the stability analysis we define the stack of all

u_{P E} : = col ([u_{P E}^{i j}]) : = col ([u_{P E}^{i} - u_{P E}^{j}]), \forall j \in N_{i}

Then we write the complete closed-loop system as

\begin{matrix} \dot{σ} : = - λ_{0} (L \otimes I_{3}) σ + λ_{0} τ + S \\ \dot{S} = \frac{k_{x}}{k_{v}} ((L_{u} + B) \otimes I_{3}) X + \frac{k_{x}}{k_{v}} B σ + \frac{k_{x}}{k_{v}} A Z \\ \dot{X} = V \\ \dot{V} = - k_{x} ((L_{u} + B) \otimes I_{3}) X - k_{v} V - k_{v} S - k_{x} B σ \\ - k_{x} A Z + u_{P E} \end{matrix}

Based on the analysis of the previous subsection and by treating the input u_PE as a disturbance, we can show that our closed-loop system is ISS with respect to u_PE from which we can conclude practically global exponential stability since convergence is ensured to a neighborhood of the desired equilibrium trajectory that can be made (by assignment of the free function u_PE) very small but not identically zero.

Remark 8. We remind that the main results in Chai et al. ⁶ and Hong et al. ²¹ on which we are based hold also for switched graphs, under of course a condition of uniform connectivity, and as such our algorithm is also applicable to the case of switching communication graphs.

Simulation results

In this section we study the efficiency of the obtained algorithms by means of numerical simulations that serve as proof of concept. We will consider a two-dimensional scenario with three agents pursuing a target with constant linear motion. This motion is selected so that the target does not contribute to the satisfaction of the PE condition but rather is a task to be ensured by the agents. Additionally, we consider that the target is (and stays) in the field of view of only the first agent which is thus the only agent having available information about the target. We consider that all (relative, absolute) velocity measurements and accelerations are corrupted by Gaussian white noise.

The initial positions (in m) and velocities (in m/s) of the agents are, respectively, given as $x_{0} (0) = {[20, 0]}^{T}, x_{1} (0) = {[2, 0]}^{T}, x_{2} (0) = {[10, - 5]}^{T}, x_{3} (0) = {[3 \sin (π / 8), 5 \cos (π / 8)]}^{T}, v_{0} (0) = {[3]}^{T}, v_{1} (0) = {[0, 2]}^{T}, v_{2} (0) = {[1, 1]}^{T}, v_{3} (0) = {[2 \cos (π / 8), - 2 \sin (π / 8)]}^{T}$ . The parameters related to the observer are chosen as $c_{1} = c_{3} = 0.9, c_{2} = c_{4} = c_{9} = 1, c_{5} = c_{6} = c_{8} = 0.01, c_{7} = 0.005$ and the observer gains as $K_{10} = c_{8} + (c_{5} + c_{6} + c_{7} c_{2}) / c_{3}, K_{12} = K_{13} = K_{21} = K_{23} = K_{31} = K_{32} = 0.03$ . The gains were given small values to reduce the effect of noise and avoid unwanted phenomena such as overshooting but high enough to ensure an acceptably fast convergence. This was ensured by properly selecting the eigenvalues of the linearized error system to have negative real part.

We assume that we do not have any prior knowledge on the relative positions and thus, choose the estimates as ${\hat{x}}_{i j} (0) = 0$ which translates to initial observer states given by $ξ_{i} (0) = - \frac{d_{i j}^{2} (0)}{2} K_{i j} v_{i j} (0)$ . In addition, the initial conditions for the fused estimates are again taken as $ρ_{1} (0) = {[0, 0]}^{T}, ρ_{2} (0) = {[0, 0]}^{T}$ while the initial condition for the dynamic scaling r(t) is selected as $r (0) = 1$ . We also select the fusion gain $λ_{0} = 2$ and the initial estimations of the target’s velocity as ${\hat{v}}_{0}^{i} (0) = 0$ .

Furthermore, we consider the standard scenario where velocity measurements are corrupted by band-limited Gaussian white noise n_ij (although any type of noise can be considered) with noise power intensity $σ_{m} = 10^{- 4} / 5 {(m / s)}^{2} / H z$ and a sampling period of $T_{s} = 10^{- 3} (s)$ .

On the other hand, the control gains for the distributed law are chosen, respectively, as k_x = 1 and $k_{v} = 0.5$ . Finally, the persistent terms in the controllers are chosen as $u_{P E}^{1} = {[- 2 \cos (t), - 2 \sin (t)]}^{T}, u_{P E}^{2} = {[- \frac{1}{5} \sin (\frac{t}{5}) + \sin (t) \sin (\frac{t}{5}) - \frac{1}{5} \cos (t) \cos (\frac{t}{5}), \frac{1}{5} \cos (\frac{t}{5}) - \sin (t) \cos (\frac{t}{5}) - \frac{1}{5} \cos (t) \sin (\frac{t}{5})]}^{T}$ , $u_{P E}^{3} = {[4 \sin (t + \frac{π}{2}), - 4 \sin (2 t)]}^{T}$ . These were selected with different frequencies and amplitudes in order to illustrate the effect in both estimation and tracking as will be shown in the figures below. Of course the richer (larger, faster) the motion of each agent the faster the convergence of the local estimates and consequently, of the localization error and the tracking error.

The resulting positions of the target and the agents are depicted in Figure 1. In the ideal scenario where the relative positions would be available, and hence not requiring a persistent motion of the agents, and with no measurement noise, the positions of all agents would exactly converge to the target’s position. However, since a persisting motion is required to successfully estimate the relative positions, the positions of all agents converge in neighborhoods around the target’s position with their size depending on the amplitude of the corresponding persistent input. Of course, the smaller the amplitude of the persistent input the slower the convergence of the estimated relative positions.

Figure 1.

Positions of the target and the agents.

From Figure 2 we see that the velocities are PE (and linearly independent) and thus, we can obtain converging local estimates of the relative positions (see Figures 3 to 5). Figure 6 depicts the noisy, estimated and true values of the relative velocity v₁₀ in order to illustrate the persistent excitation and the effect of the applied filtering. In particular, by zooming on a specific time interval we observe the effect of the noise as well as the result of the filtering. Of course, the former can be further adjusted by proper selection of the filter gains. Similar effects are observed for the other relative and absolute velocities. In addition, notice in both components the effect of the persistent motion required in order to be able to estimate the relative position. As such, this persistent motion (and its magnitude) is imposed by the persistent part of the ith agent’s (agent 1 for this figure) control law ( $u_{P E}^{1}$ ) that leads to the persistent relative velocity of the figure. Finally, we can visualize the fused estimates for the three agents in Figure 7. We observe that all agents are successfully localized with respect to the target and furthermore, that the effect of the noisy measurements has been significantly removed (although some slight oscillations do appear). Hence, the transient behavior is quite smooth and the convergence is exponential as was proposed by the theoretical analysis.

Figure 2.

Agents’ true velocities.

Figure 3.

Estimation error for x₁₀.

Figure 4.

Estimation error for x₂₃.

Figure 5.

Estimation error for x₁₃.

Figure 6.

Relative (noisy, estimated, true) velocity v₁₀ (upper: first component, lower: second component).

Figure 7.

Error between fused estimates ρ_i and true relative positions $x_{i 0}$ (left column: first component, right column: second component).

Now, in order to show the robustness of the proposed approach and how it can be adapted to other scenarios of interest, we consider the same scenario, with same initial conditions, noises, and gains, but now taking into account a desired formation geometry and the interagent collision avoidance. For the former requirement we need only to modify the control law and the target velocity estimator in order to include the desired distances (relative positions), which we simply select as $x^{d} = {[3, 4]}^{T}, x_{12}^{d} = x^{d}, x_{21}^{d} = - x_{12}^{d}, x_{13}^{d} = x^{d}, x_{31}^{d} = - x_{13}^{d}, x_{23}^{d} = x^{d}, x_{32}^{d} = - x_{23}^{d}$ . For the latter requirement to be satisfied, we add in the control law an additional term inspired by the avoidance strategy in Kahn et al.,³³ call it $u_{c}^{i}$ for the ith agent, which for our scenario gives

\begin{matrix} u_{c}^{1} = \frac{k_{c}}{q} (\exp (- {\hat{x}}_{10}^{T} {\hat{x}}_{10} / q) {\hat{x}}_{10} \\ + \exp (- {\hat{x}}_{12}^{T} {\hat{x}}_{12} / q) {\hat{x}}_{12} + \exp (- {\hat{x}}_{13}^{T} {\hat{x}}_{13} / q) {\hat{x}}_{13}) \\ u_{c}^{2} = \frac{k_{c}}{q} (\exp (- {\hat{x}}_{21}^{T} {\hat{x}}_{21} / q) {\hat{x}}_{21} \\ + \exp (- {\hat{x}}_{23}^{T} {\hat{x}}_{23} / q) {\hat{x}}_{23}) \\ u_{c}^{3} = \frac{k_{c}}{q} (\exp (- {\hat{x}}_{31}^{T} {\hat{x}}_{31} / q) {\hat{x}}_{31} \\ + \exp (- {\hat{x}}_{32}^{T} {\hat{x}}_{32} / q) {\hat{x}}_{32}) \end{matrix}

with gain k_c = 10 and the parameter q = 5 that defines the repulsion distance. Finally we slightly decrease the amplitude of the persistent terms u_PE as,

u_{P E}^{1} = {[- \cos (t), - \sin (t)]}^{T}

u_{P E}^{2} = {[- \frac{1}{5} \sin (\frac{t}{5}) + \sin (t) \sin (\frac{t}{5}) - \frac{1}{5} \cos (t) \cos (\frac{t}{5}), \frac{1}{5} \cos (\frac{t}{5}) - \sin (t) \cos (\frac{t}{5}) - \frac{1}{5} \cos (t) \sin (\frac{t}{5})]}^{T}

u_{P E}^{3} = {[\frac{2}{3} s i n (2 t + \frac{π}{2}), - \frac{2}{3} s i n (2 t)]}^{T}

, to show its impact on the convergence of the estimated relative positions, that will be larger.

The evolution of the agents’ positions, the geometric formation of the followers in different time instances, and the relative distances are depicted in Figures 8 to 10, respectively. We observe that the additional requirements (formation and interagent collision avoidance) are readily satisfied and that the relative distances among followers converge around the desired nominal value. The discrepancies observed are due to the contribution of the persistent terms, that are required to ensure the observability, and the desired formation geometry.

Figure 8.

Formation scenario: positions of the target and the agents.

Figure 9.

Formation scenario: evolution of the formation of follower agents ((x−y) in (m)).

Figure 10.

Formation scenario: relative distances.

For completeness, we illustrate also the time evolution of the true velocities of all agents in Figure 11 as well as the fused estimates for the relative positions in Figure 12. As expected, the estimates have a slower convergence with respect to the previous scenario and present some slight oscillations due to noise around the true values.

Figure 11.

Formation scenario: agents’ true velocities.

Figure 12.

Formation scenario: error between fused estimates ρ_i and true relative positions $x_{i 0}$ (left column: first component, right column: second component).

Conclusions

We have proposed a robust algorithm for simultaneous collaborative localization and target tracking problem. The localization mechanism exploits (local) relative distances and noisy velocity measurements so that each agent first obtains an estimation of the relative positions with respect to its neighbors and then fuses this estimate with the ones communicated by the neighbors. These estimates are then fed to a consensus-type distributed control law that includes an estimation of the target’s velocity, to achieve exact target tracking. Our algorithm is designed to ensure the observability of the system, represented by a persistence-of-excitation condition on the relative motion of the agents, and the attenuation of noise. The stability properties induced by our algorithm are established through a thorough Lyapunov analysis. Finally, the performance of our scheme is analyzed by means of two simulation scenarios that show among others the robustness to unaccounted acceleration measurement noise and the possibility to consider a formation geometry and interagent collision avoidance.

In the near future, these theoretical results are expected to be tested experimentally on our fleet of quadrotors and under realistic environmental and communication scenarios.

Footnotes

Acknowledgements

The first author would like to acknowledge the fruitful discussions with prof. Emmanuel Nuño of the University of Guadalajara, Mexico.

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the PR-GUIMAUVE of ONERA.

Notes

References

Ren

and Beard

RW.

Distributed consensus in multi-vehicle cooperative control. London: Springer-Verlag, 2008.

Bachrach

and Roy

Autonomous flight in unknown indoor environments.

Int J Micro Air Veh 2009; 1: 217–228.

Bishop

Fidan

Anderson

BDO

et al . Optimality analysis of sensor-target localization geometries. Automatica 2010; 46: 479–492.

Jauffret

and Pillon

Observability in passive target motion analysis.

IEEE Trans Aerosp Electron Syst 1996; 32: 1290–1300.

Tron

Thomas

Loianno

et al . A distributed optimization framework for localization and formation control: applications to vision-based measurements. IEEE Control Syst Mag 2016; 36: 22–44.

Chai

Lin

et al . Single landmark based collaborative multi-agent localization with time-varying range measurements and information sharing. Syst Control Lett 2016; 87: 56–63.

Tabarea

Slotine

and Pham

QC.

How synchronization protects from noise.

PLoS Comput Biol 2010; 6: 1–9.

and Sanfelice

RG.

Interconnected observers for robust decentralized estimation with performance guarantees and optimized connectivity graph.

IEEE Trans Control Netw Syst 2016; 3: 1–11.

Fox

Burgard

Kruppa

et al . A probabilistic approach to collaborative multi-robot localization. Auton Robots 2000; 81: 325–344.

10.

Roumeliotis

and Bekey

GA.

Distributed multirobot localization.

IEEE Trans Robot 2002; 18: 781–795.

11.

Kia

Rounds

and Martinez

Cooperative localization for mobile agents: a recursive decentralized algorithm based on Kalman-filter decoupling.

IEEE Control Syst Mag 2016; 36: 86–101.

12.

Sarras

Marzat

Bertrand

et al . Collaborative multi-vehicle localization with respect to static/dynamic target from range and velocity measurements. In: International conference on unmanned aircraft systems (ICUAS), Miami, FL, USA, 13–16 June 2017, pp.850–859.

13.

Montijano

Cristofalo

Zhou

et al . Vision-based distributed formation control without an external positioning system. IEEE Trans Robot 2016; 32: 339–351.

14.

Piasco

Marzat

and Sanfourche

Collaborative localization and formation flying using distributed stereo-vision. In: IEEE international conference on robotics and automation, Stockholm, Sweden, 16–21 May 2016, pp.1202–1207.

15.

Bahr

Leonard

and Fallon

MF.

Cooperative localization for autonomous underwater vehicles.

Int J Robot Res 2009; 28: 714–728.

16.

Dandach

Fidan

Dasgupta

et al . A continuous time linear adaptive source localization algorithm, robust to pesristent drift. Syst Control Lett 2009; 58: 7–16.

17.

Deghat

Shames

Anderson

et al . Localization and circumnavigation of a slowly moving target using bearing measurements. IEEE Trans Autom Control 2014; 59: 2182–2188.

18.

Karagiannis

and Astolfi

Dynamic scaling and observer design with application to adaptive control.

Automatica 2009; 45: 2883–2889.

19.

Praly

Carnevale

and Astolfi

Dynamic versus static weighting of Lyapunov functions.

IEEE Trans Autom Control 2013; 58: 1557–1561.

20.

Hong

and Gao

Tracking control for multi-agent consensus with an active leader and variable topology.

Automatica 2006; 42: 1177–1182.

21.

Hong

Chen

and Bushnell

Distributed observers design for leader-following control of multi-agent networks.

Automatica 2008; 44: 846–850.

22.

Cai

and Huang

The leader-following consensus for multiple uncertain Euler-Lagrange systems with an adaptive observer.

IEEE Trans Autom Control 2016; 61: 3152–3157.

23.

Liu

and Huang

Adaptive leader-following consensus for a class of higher-order nonlinear multi-agent systems with directed switching networks.

Automatica 2017; 79: 84–92.

24.

and Ahn

HS.

Formation control of mobile agents based on inter-agent distance dynamics.

Automatica 2011; 47: 2306–2312.

25.

Park

and Ahn

HS.

A survey of multi-agent formation control.

Automatica 2015; 53: 424–440.

26.

Franchi

Stegagno

and Oriolo

Decentralized multi-robot encirclement of a 3D target with guaranteed collision avoidance.

Auton Robot 2016; 40: 245–265.

27.

Astolfi

Karagiannis

and Ortega

Nonlinear and adaptive control. London: Springer-Verlag, 2008.

28.

Martin

and Sarras

A simple model-based estimator for the quadrotor using only inertial measurements. In: IEEE 55th Conference on Decision and Control (CDC), Las Vegas, NV, USA, 12–14 December 2016, pp.7123–7128.

29.

Martin

and Sarras

A global observer for attitude and gyro biases from vector measurements. In: Twentieth IFAC world congress, Toulouse, France, 9–14 July, 2017, pp.15979–15986.

30.

Loría

and Panteley

Uniform exponential stability of linear time-varying systems: revisited.

Syst Control Lett 2002; 47: 13–24.

31.

Rochefort

Piet-Lahanier

Bertrand

et al . Model predictive control of cooperative vehicles using systematic search approach. Control Eng Pract 2014; 32: 204–217.

32.

Loría

and Panteley

Cascaded nonlinear time-varying systems: analysis and design. Lect Notes Control Inform Sci 2005; 311: 23–64.

33.

Kahn

Marzat

Piet-Lahanier

et al . Cooperative estimation and fleet reconfiguration for multi-agent systems. In: Proceedings of the IFAC workshop on multivehicle systems, Genova, Italy, 18 May 2015, pp.11–16.

Collaborative multiple micro air vehicles’ localization and target tracking in GPS-denied environment from range–velocity measurements

Abstract

Keywords

Introduction

Model and problem formulation

Network topology

Dynamic model

Cooperative localization

Single vehicle localization from direct local measurements: Static target

Single vehicle localization from filtered local measurements: Dynamic target

Collaborative localization from fusion of local estimates and measurements

Collaborative tracking control with unknown target velocity

Case: Known relative positions

Case: Estimated relative positions

Imposing the PE condition through the control

Simulation results

Conclusions

Footnotes

Acknowledgements

Declaration of conflicting interests

Funding

Notes

References