Multi-agent Localization of A Common Reference Coordinate Frame: An Extrinsic Approach

# Multi-agent Localization of A Common Reference Coordinate Frame: An Extrinsic Approach

[    [
###### Abstract:

This paper studies the problem of multi-agent cooperative localization of a common reference coordinate frame in . Each agent in a system maintains a body-fixed coordinate frame and its actual frame transformation (translation and rotation) from the global coordinate system is unknown. The mobile agents aim to determine their trajectories of rigid-body motions (or the frame transformations, i.e., rotations and translations) with respect to the global coordinate frame up to a common frame transformation by using local measurements and information exchanged with neighbors. We present two frame localization schemes which compute the rigid-body motions of the agents with asymptotic stability and finite-time stability properties, respectively. Under both localization laws, the estimates of the frame transformations of the agents converge to the actual frame transformations almost globally and up to an unknown constant transformation bias. Finally, simulation results are provided.

First]Quoc Van Tran First,Second]Hyo-Sung Ahn

School of Mechanical Engineering, Gwangju Institute of Science and Technology, Gwangju, Republic of Korea (e-mail: tranvanquoc, hyosung@gist.ac.kr).

Department of Electrical Engineering, Colorado School of Mines, Golden, CO, USA (e-mail: hahn@mines.edu.

Keywords: Multi-agent systems, Frame Localization, Distributed computation, Estimation algorithms, Sensor networks, Measurement and instrumentation.

11footnotetext: The work of this paper has been supported by the National Research Foundation (NRF) of Korea under the grant NRF-2017R1A2B3007034.

\@xsect

Consider a network of autonomous agents in -dimensional space. Associated with each agent , there are a position vector, i.e., , (normally correpsonding to its centroid) and a matrix in , i.e., , its orientation, representing the orientation of its body-fixed coordinate frame, , relative to the global coordinate frame (See also Fig. 1). This paper addresses the problem of estimating the trajectories of the rigid-body motions (as elements of the group of Euclidean transformations ) or the time-varying poses of the agents which are characterized by the pairs . The global coordinate frame is unknown to all agents and they have only relative measurements and information communicated from their neighboring agents. To solve the problem, the agents in the system cooperatively localize a common reference coordinate frame and estimate their frame transformations with regard to the common frame.

In the literature, there has been recently a large number of works on consensus Igarashi et al. (2009); Sarlette et al. (2009); Thunberg et al. (2017); Markdahl et al. (2018); Gui and de Ruiter (2018); Zong and Shao (2016); Wei et al. (2018) and estimation on Tron and Vidal (2014); Tron et al. (2016); Lee and Ahn (2017); Tran et al. (2018a, b). The consensus protocols have a wide range of applications in the orientation synchronization of rigid bodies in the Cartesian space. While the consensus algorithms designed directly on guarantee only local and asymptotic convergence property Igarashi et al. (2009); Sarlette et al. (2009), those using local representations of orientations can provide almost global consensus and finite-time stability property Gui and de Ruiter (2018); Zong and Shao (2016); Wei et al. (2018). However, the local representations of orientations suffer from singularities, e.g., the angle-axis representation or the modified Rodriguez parameters, or the ambiguity in the orientation representations, e.g., the unit quaternions. For this reason, we only focus on the orientation control and estimation protocols which use directly the group to represent orientations. Orientation estimation approaches have been proposed recently and are widely used in network localization Tron and Vidal (2014); Tron et al. (2016); Lee and Ahn (2017) and formation control Lee and Ahn (2017); Tran et al. (2018a, b); Tran and Ahn (2018). The orientation estimation algorithms can guarantee almost global convergence of the computed orientations.

The above-mentioned orientation estimation and control schemes can also be classified into intrinsic algorithms Igarashi et al. (2009); Tron and Vidal (2014); Markdahl et al. (2018) and extrinsic algorithms Sarlette et al. (2009); Lee and Ahn (2017); Thunberg et al. (2017); Tran et al. (2018a, b). In particular, the intrinsic algorithms design the orientation estimation and consensus laws directly on the Riemannian manifold, i.e., the sphere Markdahl et al. (2018) or the special orthogonal groups Igarashi et al. (2009); Tron and Vidal (2014). By reshaping the cost function used in the estimation protocol, the convexity of the problem is guaranteed and the orientations of the agents (whose interaction graph is connected and undirected) can be estimated almost globally Tron and Vidal (2014); Markdahl et al. (2018). Whereas, in the extrinsic approaches, rotation matrices are embedded into auxiliary matrices which are defined and evolve in the Euclidean ambient space through a typical consensus protocol. The auxiliary matrices are then exploited to control Sarlette et al. (2009) or estimate Lee and Ahn (2017); Thunberg et al. (2017); Tran et al. (2018a, b) the orientation matrices. In contrast to the intrinsic algorithms, the extrinsic algorithms can guarantee almost global convergence of the rotation matrices for systems with general graph topologies which contain a spanning tree Lee and Ahn (2017); Tran et al. (2018a, b).

The consensus problem on was studied in Thunberg et al. (2016) with only local region of attraction. As the first contribution of this work, we formulate the frame localization problem and propose an extrinsic-based algorithm to estimate the trajectories of the rigid-body motions of the agents in the system. In particular, the estimation law is designed by implementing consensus protocol on the auxiliary matrices in and derived the poses of the agents from the auxiliary matrices. We show that the poses of the agents can be estimated almost globally and exponential fast up to an unknown constant frame transformation under the assumption of the existence of a spanning tree in the interaction graph. Secondly, a finite-time frame localization law is then proposed for systems with undirected and connected graph topologies. We establish an almost global stability and the finite-time convergence of the estimated frame transformations of the agents to the actual frame transformations up to a common unknown transformation. The proposed proposed frame localization protocols (on ) in this work are extended from the orientation estimation laws proposed in our previous works Tran et al. (2018a, b). Finally, simulation results are provided to show the effectiveness of the proposed frame localization schemes.

The rest of this paper is outlined as follows. Preliminaries on the special Euclidean groups and graph theory, and the problem formulation are presented in Section Multi-agent Localization of A Common Reference Coordinate Frame: An Extrinsic Approach. Section Multi-agent Localization of A Common Reference Coordinate Frame: An Extrinsic Approach proposes a frame localization law and establish the almost global exponential convergence of the estimated frame transformations. A frame localization scheme with finite-time stability property is introduced in Section Multi-agent Localization of A Common Reference Coordinate Frame: An Extrinsic Approach. We provide simulation results in Section Multi-agent Localization of A Common Reference Coordinate Frame: An Extrinsic Approach. Finally, Section Multi-agent Localization of A Common Reference Coordinate Frame: An Extrinsic Approach concludes this paper.

\@xsect

In this paper we use the following notations. Given two vectors , their dot product is denoted by . The symbol represents a global coordinate frame and the symbol with the superscript index denotes the -th local coordinate frame. Let be the vector of all ones, and denotes the identity matrix. For two matrices and , denotes the Kronecker product between and . The trace of a matrix is denoted by . For the Frobenius metric is given by which is the Euclidean distance in .

\@xsect

The set of rotation matrices in is denoted by . The space of skew-symmetric matrices is denoted by . For any , the hat map is defined such that , where

 ω∧=⎡⎢⎣0−ωzωyωz0−ωx−ωyωx0⎤⎥⎦.

The vee map is the inverse of the hat map and defined as Bullo and Lewis (2005).

The special Euclidean group, representing a trajectory of the motion (or poses, i.e., translation and rotation) of a rigid body agent, is given by a set of transformation matrices:

 SE(3)={T=[Qp01]∈R4×4∣∣∣Q∈SO(3),p∈R3}.

Note that the sets and are not vectorspaces, but they are matrix Lie groups Barfoot (2018). Let the set

 se(3):={ξ∧=[ω∧v00]∈R4×4∣∣∣ ω∧∈so(3),v∈R3}.

The the hat map and the vee map associated with are given as

respectively. The exponential map relates the and as

 T=exp(ξ∧)=∞∑k=01k!(ξ∧)k.

The inverse operator of the exponential map is given as

 ξ=ln(T)∨.
\@xsect

An interaction graph characterizing an interaction topology of a multi-agent network is denoted by , where, denotes the vertex set and denotes the set of edges of . An edge is defined by the ordered pair . The graph is said to be undirected if implies , i.e. if is a neighbor of , then is also a neighbor of . If the graph is directed, does not necessarily imply . The set of neighboring agents of is denoted by . The Laplacian matrix associated with is defined as for , , and otherwise.

\@xsect

Consider a network of mobile agents in -dimensional space. Let and be the position of agent expressed in the global frame and its body-fixed coordinate frame , respectively. The pair characterizes the pose of each agent in the Cartesian ambient space. The rigid body motion of agent (or the -th frame transformation) is given as and its inverse transformation can be computed as

 T−1i=[R⊤i−R⊤ipi01]∈SE(3).

The relative transformation between the two corresponding body-fixed coordinate frames of agents and , which is denoted as , is given as

 Tij=T−1iTj=[R⊤iRjR⊤i(pj−pi)01]. (1)

Let be the relative orientation between two local coordinate frames and and the relative position between agent and which are measured locally in the local frame of agent . Then, the relative transformation can be expressed as .

The kinematic of the rigid body motion of agent is given as

 (2)

where and denote the linear velocity and the angular velocity of agent measured in . We assume that each agent is able to measure and and the relative transformations (translation and rotation) to its neighboring agents without noise. If an edge , then agent can measure and it also can receive information communicated from agent . For this, we make the following assumptions.

###### Assumption 1

Each agent in the system locally measures its body velocity, i.e., , and the relative transformation defined in (1) with regard to its neighbors .

###### Assumption 2

The underlying interaction graph contains a spanning tree.

The agents in the system aim to estimate for their actual rigid body motions , a process is called frame localization, and each agent holds an estimate of the body transformation . By using the local measurements in Assumption 1, the objective is for the agents to cooperatively localize the global coordinate frame, e.g., , up to a transformation, , which is unknown but deterministic and common to all agents.

###### Problem 2.1 (Asymptotic Frame Localization)

Consider a system of mobile agents in . Under the Assumptions 1 and 2, design a cooperative localization scheme for each agent to estimate its transformation up to an unknown constant transformation .

###### Problem 2.2 (Finite-Time Frame Localization)

Consider a system of mobile agents in . Under the Assumptions 1 and 2, design a cooperative localization scheme for each agent to estimate its transformation up to an unknown constant transformation in a finite time.

\@xsect

This section presents a distributed estimation protocol and establishes the almost global asymptotic convergence of the estimated poses to the actual poses of the agents up to a common reference transformation by using the relative pose measurements.

\@xsect

For each agent we introduce an auxiliary matrix as follows

 Pi:=[Qiqi01], (3)

where is a nonsingular matrix and . Note that is defined in the Cartesian ambient space and has full-column rank and initialized randomly. Note that the set of nonsingular matrices in is a dense set of the set of matrices, i.e., if is initialized randomly from a continuous uniform distribution on its entries, then will be almost surely nonsingular. Each agent implements the following localization law

 ˙Pi(t)=−[viiωii]∧Pi(t)+∑j∈Ni(Tij(t)Pj(t)−Pi(t)) (4)

where is the auxiliary matrix associated with agent and it is communicated from agent . In contrast to the intrinsic algorithms in the literature , the frame localization law (4) evolves in the Cartesian ambient space and the frame transformation estimate of each agent, , is derived from the corresponding auxiliary matrix . The way, which constructs from , will be described latter in this section.

\@xsect

The localization law (4) can be rewritten as

 Ti˙Pi=−Ti[viiωii]∧Pi+∑j∈Ni(TjPj−TiPi).

By introducing the transformation and noticing that . Therefore, the above equation can be expressed as

 ˙Si=∑j∈Ni(Sj−Si). (5)

Let be the stack matrix of all . By combining the above frame localization dynamics for all agents we obtain a compact form

 ˙S=−(L⊗I4)S. (6)
###### Theorem 3.1

Assume that Assumptions 1 and 2 hold. Under the frame localization law (4), in (6) globally exponentially converge to

 (1n⊗I4)(w1×I4)S(0),

where is the left eigenvector of the Laplacian corresponding to the zero eigenvalue.

Proof. Since has a spanning tree the associated Laplacian has a simple zero eigenvalue and the other eigenvalues have positive real parts. The right and left eigenvectors corresponding to the zero eigenvalue are and respectively (Ren et al., 2004, Lemma 1). Further, there exists such that globally exponentially converges to .

Consider the solution to (6) as

 S(t)=exp−(L⊗I4)tS(0).

Let the Jordan form of be where whose diagonal terms are eigenvalues of , and . Then the steady-state solution . Thus, converge to , i.e., a convex combination of the initial matrices .

The steady-state matrix is given as

 Sc =(w1×I4)S(0)=n∑i=1w1iTi(0)Pi(0) =n∑i=1w1i[Ri(0)Qi(0)Ri(0)qi(0)+pi(0)01] =[Qcqc01], (7)

where , , and denotes the -th entry of the left eigenvector .

At a time instant , let then the auxiliary matrix is computed as

 Pi=T−1iSi=[R⊤iQSiR⊤i(qSi−pi)01]. (8)

Since globally exponentially as (Theorem 3.1) we have the following lemma.

###### Lemma 3.1

Assume that Assumptions 1 and 2 hold. Under the frame localization law (4),

 Pi(t)→[R⊤iQcR⊤i(qc−pi)01],∀i∈V,

globally exponentially as , i.e., and , where and are defined in (7).

\@xsect

We now assume that estimates of orientation, , and position, , of agent are derived from and as follows ( and are defined in (3)). The orientation estimate is constructed from by the Gram-Schmidt procedure (GSOP, see Appendix References) and

 ^pi(t):=−^Ri(t)qi(t). (9)

It is noticed from Lemma 3.1 that

 ^pii(∞):=−qi(∞)=R⊤i(pi−qc)=pii−R⊤iqc (10)

specifies the estimate of position of agent expressed locally in body frame . It follows that the position of agent expressed in , i.e., , is estimated up to a common constant translation .

Let . Then, we have the following result.

###### Corollary 1

Assume that Assumptions 1 and 2 hold. Under the frame localization law (4), if is constructed from by the Gram-Schmidt procedure (GSOP) and is computed by (9), then there exist an unknown constant transformation

 Tc:=[Rcqc01]∈SE(3) (11)

such that as , for all , if .

Proof. Let and be derived from and by the GSOP, respectively. It follows from Lemma A.1 in Appendix that for all . Since (Lemma 3.1), as , where the unknown constant orientation . As a result, from (9) and Lemma 3.1, we have

 ^pi→−^RiR⊤i(qc−pi)=R⊤c(pi−qc),∀i∈V,

as . Consequently, one has

 limt→∞^Ti(t)=[R⊤cRiR⊤c(pi−qc)01]=T−1cTi(t),

where is defined in (11).

For the validity of the estimated frame transformations, , the singularity of the steady-state matrix defined in (7) is undesired. For this, we now show that is nonsingular if the initial matrices satisfy the condition . From (7), is explicitly computed as

 Qc =n∑i=1w1iRi(0)Qi(0)=(w1⊗I3)Z0 =[(w1⊗I3)[Z0]1,(w1⊗I3)[Z0]2,(w1⊗I3)[Z0]3]

where denotes the -th column of . It follows that contains linearly independent columns if and only if and column vectors of are linearly independent. The second condition follows from the nonsingularity of (for almost all random initializations of the entries of ) and the first condition implies .

###### Corollary 2

Since the dimension of is which is a lower-dimensional subspace of and hence its Lebesgue measure is zero. Thus, the steady-state estimates of the frame transformations are well-defined for almost all initial matrices . Further, the frame transformations of the agents are computed almost globally exponentially up to a common constant transformation . In other words, the frame transformations of the agents are computed relative to a reference frame whose frame transformation is .

###### Remark 1

Though the steady-state estimates of the frame transformations of the agents are proper they might not be well-defined at some time instants (See also Tran and Ahn (2018)). Indeed, if in (3) are initialized randomly in then some of have negative and some of those have positive determinants. Since all converge to , at least one of whose determinant changes sign. Thus, its determinant becomes zero at some time instants. Consequently, is nonsingular at those points.

###### Remark 2

If we instead construct the first two columns of from the first two column vectors of by the Gram-Schmidt orthonormalization process and the third column vector of is the cross product of the first two column vectors, then it can be shown that is well-defined for all for almost all initial matrices . Indeed, it is equivalent to show that the first two column vectors of in (6) are linearly independent for all initial matrices but a set of zero measure. The proof follows similar lines as the proof of (Thunberg et al., 2018, Lemma 4(4)) and is omitted.

The frame localization scheme with asymptotic convergence property is illustrated in Algorithm 1

\@xsect

In this section, a finite-time frame localization law is proposed for systems with undirected and connected graph topologies. We establish an almost global stability and the finite-time convergence of the estimated frame transformations of the agents to the actual frame transformations up to an unknown common transformation.

\@xsect

Each agent holds an auxiliary matrix as defined in (3) and the estimate of the frame transformation of agent is constructed from by following the same computations in Corollary 1. For each agent , we propose the following frame localization law

 ˙Pi(t)=−[viiωii]∧Pi(t)+∑j∈NiTij(t)Pj(t)−Pi(t)||Tij(t)Pj(t)−Pi(t)||αF, (12)

where the positive scalar . The above frame localization law is continuous due to the Remark 3.

To analyze the above localization law, the following Lemma is useful.

###### Lemma 4.1

The denominator of the second term in the right hand side of (12) can be equivalently computed as

 ||TijPj−Pi||αF=||TjPj−TiPi||αF (13)

Proof. First, we have

 TijPj−Pi= [R⊤iRjR⊤i(pj−pi)01][Qjqj01]−[Qiqi01] =[R⊤iRjQjR⊤iRjqj+R⊤i(pj−pi)01]−Pi =[R⊤i001][RjQjRjqj+(pj−pi)01]−Pi =[R⊤i001]TjPj−[R⊤i001][RiQiRiqi+pi01] =Ki(TjPj−TiPi),

where . By using this relation, one has

 ||TijPj−Pi||αF=tr[(TijPj−Pi)⊤(TijPj−Pi)] =tr[(TjPj−TiPi)⊤K⊤iKi(TjPj−TiPi)] =tr[(TjPj−TiPi)⊤(TjPj−TiPi)] =||TjPj−TiPi||αF,

which competes the proof.

\@xsect

By using the transformation and the above Lemma, the frame localization law (12) can be written as

 ˙Si=∑j∈NiSj−Si||Sj−Si||αF. (14)
###### Remark 3

The above system is time-continuous as will be shown in the following. Let be the -th column vector of and be the stacked vector of all column vectors of , for all . Furthermore, from the definition of the Frobenius norm, we have

 ∥Si−Sj∥F =√tr{(Si−Sj)⊤(Si−Sj)} =√(si−sj)⊤(si−sj) =∥si−sj∥,

where denotes the Euclidean norm. By using the above equation, we can rewrite (14) into a vector form as

 (15)

with the right hand side of the above equation is continuous Trinh et al. (2017). If , it is discontinuous Cortés (2006).

Let be the stack matrix of all . By combining the above frame localization dynamics for all agents we obtain a compact form

 ˙S(t)=−(¯L⊗I4)S(t). (16)

where the matrix is defined as

 ¯lij=⎧⎪ ⎪⎨⎪ ⎪⎩0, (i,j)∈E, i≠j, Si=Sj or (i,j)∉E, i≠j−1/∥Si−Sj∥αF,(i,j)∈E, i≠j, Si≠Sj∑k∈Ni¯lik,i=j,i∈V,

which is a weighted Laplacian for the graph .

Assume that is undirected and connected. Then, . Thus, is invariant under (16). Let , , and let . Since is time-invariant, it follows that . Note that .

###### Theorem 4.1

Under the estimation law (12) and assume that is a connected undirected graph, globally asymptotically converges to in a finite time with settling time bounded by

 Tc≤V(0)α/2κα

where , and with being the smallest nonzero eigenvalue of the Laplacian associated with .

Proof. Consider a Lyapunov candidate function

 V(t)=(1/2)n∑i=1∥δi∥2F=(1/2)n∑i=1tr(δ⊤iδi). (17)

Note that is radially unbounded, positive definite, continuously differentiable, and in . The time derivative of along the trajectory of (16) is given as

 ˙V(t) =∑ni=1tr(δ⊤i˙δi)=tr{∑ni=1δ⊤i˙δi} =−tr{n∑i=1δ⊤i∑j∈Ni(δi−δj)/∥δi−δj∥αF} =−tr{∑(i,j)∈E(δ⊤i−δ⊤j)(δi−δj)/∥δi−δj∥αF} =−∑(i,j)∈E∥δi−δj∥2F/∥δi−δj∥αF =−∑(i,j)∈E(∥δi−δj∥2F)(2−α)/2 ≤−(∑(i,j)∈E∥δi−δj∥2F)(2−α)/2 (18) ≤−[tr{∑(i,j)∈E(δ⊤i−δ⊤j)(δi−δj)}](2−α)/2 ≤−[tr{δ⊤(L⊗I4)δ}](2−α)/2 ≤−[4∑k=1[δ]