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

TL;DR

This paper proposes two cooperative localization schemes for multi-agent systems to determine their global frame transformations using local measurements, achieving almost global convergence with stability guarantees.

Contribution

It introduces two novel frame localization algorithms with asymptotic and finite-time stability for multi-agent systems, ensuring convergence up to an unknown bias.

Findings

Estimates converge almost globally to true transformations.

Algorithms achieve asymptotic and finite-time stability.

Simulation validates the effectiveness of the proposed methods.

Abstract

This paper studies the problem of multi-agent cooperative localization of a common reference coordinate frame in $\mathbb{R}^3$. Each agent in a system maintains a body-fixed coordinate frame and its actual \textit{frame transformation} (translation and rotation) from the global coordinate system is unknown. The mobile agents aim to determine their \textit{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.

Equations

$${\mathbf{\omega }}^{\wedge }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right].$$

$$SE(3)=\left\{\mathbf{T}=\begin{bmatrix}\mathbf{Q}&\mathbf{p}\\ \mathbf{0}&1\end{bmatrix}\in\mathbb{R}^{4\times 4}\bigg{|}\mathbf{Q}\in SO(3),\mathbf{p}\in\mathbb{R}^{3}\right\}.$$

$$\mathfrak{se}(3):=\left\{\bm{\xi}^{\wedge}=\begin{bmatrix}\bm{\omega}^{\wedge}&\mathbf{v}\\ \mathbf{0}&0\end{bmatrix}\in\mathbb{R}^{4\times 4}\bigg{|}~{}\omega^{\wedge}\in\mathfrak{so}(3),\mathbf{v}\in\mathbb{R}^{3}\right\}.$$

$${\left[\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right]}^{\wedge }:=\left[\begin{array}{cc}{\mathbf{\omega }}^{\wedge }& \mathbf{v}\\ 0& 0\end{array}\right]\text{ and }{\left[\begin{array}{cc}{\mathbf{\omega }}^{\wedge }& \mathbf{v}\\ 0& 0\end{array}\right]}^{\vee }:=\left[\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right],$$

$$\mathbf{T}=\exp ({\mathbf{\xi }}^{\wedge })=\sum _{k=0}^{\infty }\frac{1}{k!}({\mathbf{\xi }}^{\wedge }{)}^k.$$

$$\mathbf{\xi }=\text{ln}(\mathbf{T}{)}^{\vee }.$$

$${\mathbf{T}}_i^{-1}=\left[\begin{array}{cc}{\mathbf{R}}_i^{\top }& -{\mathbf{R}}_i^{\top }{\mathbf{p}}_i\\ 0& 1\end{array}\right]\in SE(3).$$

$${\mathbf{T}}_{ij}={\mathbf{T}}_i^{-1}{\mathbf{T}}_j=\left[\begin{array}{cc}{\mathbf{R}}_i^{\top }{\mathbf{R}}_j& {\mathbf{R}}_i^{\top }({\mathbf{p}}_j-{\mathbf{p}}_i)\\ 0& 1\end{array}\right].$$

$${\dot{\mathbf{T}}}_i={\mathbf{T}}_i{\left[\begin{array}{c}{\mathbf{v}}_i^i\\ {\mathbf{\omega }}_i^i\end{array}\right]}^{\wedge }=\left[\begin{array}{cc}{\mathbf{R}}_i& {\mathbf{p}}_i\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{{\mathbf{\omega }}_i^i}^{\wedge }& {\mathbf{v}}_i^i\\ 0& 0\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}_i{{\mathbf{\omega }}_i^i}^{\wedge }& {\mathbf{R}}_i{\mathbf{v}}_i^i\\ 0& 0\end{array}\right]$$

$${\mathbf{P}}_i:=\left[\begin{array}{cc}{\mathbf{Q}}_i& {\mathbf{q}}_i\\ 0& 1\end{array}\right],$$

$$\dot{\mathbf{P}}_{i}(t)=-\begin{bmatrix}\mathbf{v}^{i}_{i}\\ \bm{\omega}^{i}_{i}\end{bmatrix}^{\wedge}\mathbf{P}_{i}(t)+\sum_{j\in\mathcal{N}_{i}}\Big{(}\mathbf{T}_{ij}(t)\mathbf{P}_{j}(t)-\mathbf{P}_{i}(t)\Big{)}$$

$$\mathbf{T}_{i}\dot{\mathbf{P}}_{i}=-\mathbf{T}_{i}\begin{bmatrix}\mathbf{v}^{i}_{i}\\ \bm{\omega}^{i}_{i}\end{bmatrix}^{\wedge}\mathbf{P}_{i}+\sum_{j\in\mathcal{N}_{i}}\Big{(}\mathbf{T}_{j}\mathbf{P}_{j}-\mathbf{T}_{i}\mathbf{P}_{i}\Big{)}.$$

$${\dot{\mathbf{S}}}_i=\sum _{j\in {\mathcal{N}}_i}({\mathbf{S}}_j-{\mathbf{S}}_i).$$

$$\dot{\mathbf{S}}=-(\mathbf{L}\otimes {\mathbf{I}}_4)\mathbf{S}.$$

$$(1_n\otimes {\mathbf{I}}_4)({\mathbf{w}}_1\times {\mathbf{I}}_4)\mathbf{S}(0),$$

$$\mathbf{S}(t)={\exp }^{-(\mathbf{L}\otimes {\mathbf{I}}_4)t}\mathbf{S}(0).$$

$${\mathbf{S}}_c$$

$$=\sum _{i=1}^n{w}_{1i}\left[\begin{array}{cc}{\mathbf{R}}_i(0){\mathbf{Q}}_i(0)& {\mathbf{R}}_i(0){\mathbf{q}}_i(0)+{\mathbf{p}}_i(0)\\ 0& 1\end{array}\right]$$

$$=\left[\begin{array}{cc}{\mathbf{Q}}_c& {\mathbf{q}}_c\\ 0& 1\end{array}\right],$$

$${\mathbf{P}}_i={\mathbf{T}}_i^{-1}{\mathbf{S}}_i=\left[\begin{array}{cc}{\mathbf{R}}_i^{\top }{\mathbf{Q}}_{S_i}& {\mathbf{R}}_i^{\top }({\mathbf{q}}_{S_i}-{\mathbf{p}}_i)\\ 0& 1\end{array}\right].$$

$${\mathbf{P}}_i(t)\to \left[\begin{array}{cc}{\mathbf{R}}_i^{\top }{\mathbf{Q}}_c& {\mathbf{R}}_i^{\top }({\mathbf{q}}_c-{\mathbf{p}}_i)\\ 0& 1\end{array}\right],\forall i\in \mathcal{V},$$

$${\hat{\mathbf{p}}}_i(t):=-{\hat{\mathbf{R}}}_i(t){\mathbf{q}}_i(t).$$

$${\hat{\mathbf{p}}}_i^i(\infty ):=-{\mathbf{q}}_i(\infty )={\mathbf{R}}_i^{\top }({\mathbf{p}}_i-{\mathbf{q}}_c)={\mathbf{p}}_i^i-{\mathbf{R}}_i^{\top }{\mathbf{q}}_c$$

$${\mathbf{T}}_c:=\left[\begin{array}{cc}{\mathbf{R}}_c& {\mathbf{q}}_c\\ 0& 1\end{array}\right]\in SE(3)$$

$${\hat{\mathbf{p}}}_i\to -{\hat{\mathbf{R}}}_i{\mathbf{R}}_i^{\top }({\mathbf{q}}_c-{\mathbf{p}}_i)={\mathbf{R}}_c^{\top }({\mathbf{p}}_i-{\mathbf{q}}_c),\forall i\in \mathcal{V},$$

$$\underset{t\to \infty }{\lim }{\hat{\mathbf{T}}}_i(t)=\left[\begin{array}{cc}{\mathbf{R}}_c^{\top }{\mathbf{R}}_i& {\mathbf{R}}_c^{\top }({\mathbf{p}}_i-{\mathbf{q}}_c)\\ 0& 1\end{array}\right]={\mathbf{T}}_c^{-1}{\mathbf{T}}_i(t),$$

$${\mathbf{Q}}_c$$

$$\displaystyle=\Big{[}(\mathbf{w}_{1}\otimes\mathbf{I}_{3})[\mathbf{Z}_{0}]_{1},(\mathbf{w}_{1}\otimes\mathbf{I}_{3})[\mathbf{Z}_{0}]_{2},(\mathbf{w}_{1}\otimes\mathbf{I}_{3})[\mathbf{Z}_{0}]_{3}\Big{]}$$

$${\dot{\mathbf{P}}}_i(t)=-{\left[\begin{array}{c}{\mathbf{v}}_i^i\\ {\omega }_i^i\end{array}\right]}^{\wedge }{\mathbf{P}}_i(t)+\sum _{j\in {\mathcal{N}}_i}\frac{{\mathbf{T}}_{ij}(t){\mathbf{P}}_j(t)-{\mathbf{P}}_i(t)}{||{\mathbf{T}}_{ij}(t){\mathbf{P}}_j(t)-{\mathbf{P}}_i(t)|{|}_F^{\alpha }},$$

$$||{\mathbf{T}}_{ij}{\mathbf{P}}_j-{\mathbf{P}}_i|{|}_F^{\alpha }=||{\mathbf{T}}_j{\mathbf{P}}_j-{\mathbf{T}}_i{\mathbf{P}}_i|{|}_F^{\alpha }$$

$${\mathbf{T}}_{ij}{\mathbf{P}}_j-{\mathbf{P}}_i=$$

$$\left[\begin{array}{cc}{\mathbf{R}}_i^{\top }{\mathbf{R}}_j& {\mathbf{R}}_i^{\top }({\mathbf{p}}_j-{\mathbf{p}}_i)\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\mathbf{Q}}_j& {\mathbf{q}}_j\\ 0& 1\end{array}\right]-\left[\begin{array}{cc}{\mathbf{Q}}_i& {\mathbf{q}}_i\\ 0& 1\end{array}\right]$$

Full text

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

1 Introduction

Consider a network of $n$ autonomous agents in $3$-dimensional space. Associated with each agent $i$, there are a position vector, i.e., $\mathbf{p}_{i}\in\mathbb{R}^{3}$, (normally correpsonding to its centroid) and a matrix in $SO(3)$, i.e., $\mathbf{R}_{i}\in SO(3)$, its orientation, representing the orientation of its body-fixed coordinate frame, ${}^{i}\Sigma$, relative to the global coordinate frame $\Sigma$ (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 $SE(3)$) or the time-varying poses of the agents which are characterized by the pairs $(\mathbf{R}_{i},\mathbf{p}_{i})\in SO(3)\times\mathbb{R}^{3}$. 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 $SO(3)$ Tron and Vidal (2014); Tron et al. (2016); Lee and Ahn (2018); Tran et al. (2018, 2019). 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 $SO(3)$ 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 $SO(3)$ 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 (2018) and formation control Lee and Ahn (2018); Tran et al. (2018, 2019). 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 (2018); Thunberg et al. (2017); Tran et al. (2018, 2019). 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 (2018); Thunberg et al. (2017); Tran et al. (2018, 2019) 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 (2018); Tran et al. (2018, 2019).

The consensus problem on $SE(3)$ 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 $\mathbb{R}^{4\times 4}$ 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 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 frame localization protocols (on $SE(3)$) in this work are extended from the orientation estimation laws proposed in our previous works Tran et al. (2018, 2019). 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 2. Section 3 proposes a frame localization law and establishes almost global exponential convergence of the estimated frame transformations. A frame localization scheme with finite-time stability property is introduced in Section 4. We provide simulation results in Section 5. Finally, Section 6 concludes this paper.

2 Preliminaries and Problem Formulation

In this paper we use the following notations. Given two vectors $\mathbf{x},\mathbf{y}\in\mathbb{R}^{3}$, their dot product is denoted by $\mathbf{x}\cdot\mathbf{y}$. The symbol $\Sigma$ represents a global coordinate frame and the symbol ${}^{k}\Sigma$ with the superscript index $k$ denotes the $k$-th local coordinate frame. Let $\mathbf{1}_{n}=[1,\ldots,1]^{\top}\in\mathbb{R}^{n}$ be the vector of all ones, and $\mathbf{I}_{3}$ denotes the $3\times 3$ identity matrix. For two matrices $\mathbf{A}$ and $\mathbf{B}$, $\mathbf{A}\otimes\mathbf{B}$ denotes the Kronecker product between $\mathbf{A}$ and $\mathbf{B}$. The trace of a matrix is denoted by $\text{tr}(\cdot)$. For $\mathbf{A},\mathbf{B}\in\mathbb{R}^{d\times l}$ the Frobenius metric is given by $\lVert{\mathbf{A}-\mathbf{B}}\rVert_{F}=\sqrt{\text{tr}\left\{(\mathbf{A}-\mathbf{B})^{\top}(\mathbf{A}-\mathbf{B})\right\}},$ which is the Euclidean distance in $\mathbb{R}^{d\times l}$.

2.1 Special Euclidean Groups

The set of rotation matrices in $\mathbb{R}^{3}$ is denoted by $SO(3)=\{\mathbf{Q}\in\mathbb{R}^{3\times 3}~{}|~{}\mathbf{Q}\mathbf{Q}^{\top}=\mathbf{I}_{3},\text{det}(\mathbf{Q})=1\}$. The space of $3\times 3$ skew-symmetric matrices is denoted by $\mathfrak{so}(3):=\{\mathbf{A}\in\mathbb{R}^{3\times 3}|\mathbf{A}^{\top}=-\mathbf{A}\}$. For any $\bm{\omega}=[\omega_{x},\omega_{y},\omega_{z}]^{\top}\in\mathbb{R}^{3}$, the hat map $(\cdot)^{\wedge}:~{}\mathbb{R}^{3}\rightarrow\mathfrak{so}(3)$ is defined such that $\bm{\omega}\times\mathbf{x}=\bm{\omega}^{\wedge}\mathbf{x},\forall\mathbf{x}\in\mathbb{R}^{3}$, where

[TABLE]

The vee map is the inverse of the hat map and defined as $(\cdot)^{\vee}:~{}\mathfrak{so}(3)\rightarrow\mathbb{R}^{3}$ 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:

[TABLE]

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

[TABLE]

The the hat map $(\cdot)^{\wedge}:~{}\mathbb{R}^{6}\rightarrow\mathfrak{se}(3)$ and the vee map $(\cdot)^{\vee}:~{}\mathfrak{se}(3)\rightarrow\mathbb{R}^{6}$ associated with $\mathfrak{se}(3)$ are given as

[TABLE]

respectively. The exponential map relates the $\mathbf{T}\in SE(3)$ and $\bm{\xi}^{\wedge}\in\mathfrak{se}(3)$ as

[TABLE]

The inverse operator of the exponential map is given as

[TABLE]

2.2 Graph theory

An interaction graph characterizing an interaction topology of a multi-agent network is denoted by $\mathcal{G}=(\mathcal{V},\mathcal{E})$, where, $\mathcal{V}=\{1,\ldots,n\}$ denotes the vertex set and $\mathcal{E}\subseteq\mathcal{V}\times\mathcal{V}$ denotes the set of edges of $\mathcal{G}$. An edge is defined by the ordered pair $e_{k}=(i,j),k=1,\ldots,m,m=|\mathcal{E}|$. The graph $\mathcal{G}$ is said to be undirected if $(i,j)\in\mathcal{E}$ implies $(j,i)\in\mathcal{E}$, i.e. if $j$ is a neighbor of $i$, then $i$ is also a neighbor of $j$. If the graph $\mathcal{G}$ is directed, $(i,j)\in\mathcal{E}$ does not necessarily imply $(j,i)\in\mathcal{E}$. The set of neighboring agents of $i$ is denoted by $\mathcal{N}_{i}=\{j\in\mathcal{V}:(i,j)\in\mathcal{E}\}$. The Laplacian matrix $\mathbf{L}=[l_{ij}]$ associated with $\mathcal{G}$ is defined as $l_{ij}=-1$ for $(i,j)\in\mathcal{E},~{}i\neq j$, $l_{ii}=-\sum_{j\in\mathcal{N}_{i}}l_{ij},~{}\forall i=1,\ldots,n$, and $l_{ij}=0$ otherwise.

2.3 Problem formulation

Consider a network of $n$ mobile agents in $3$-dimensional space. Let $\mathbf{p}_{i}$ and $\mathbf{p}_{i}^{i}\in\mathbb{R}^{3}$ be the position of agent $i$ expressed in the global frame $\Sigma$ and its body-fixed coordinate frame ${}^{i}\Sigma$, respectively. The pair $(\mathbf{R}_{i},\mathbf{p}_{i})\in SO(3)\times\mathbb{R}^{3}$ characterizes the pose of each agent $i$ in the Cartesian ambient space. The rigid body motion of agent $i$ (or the $i$-th frame transformation) is given as $\mathbf{T}_{i}(t)=\begin{bmatrix}\mathbf{R}_{i}(t)&\mathbf{p}_{i}(t)\\ \mathbf{0}&1\end{bmatrix}\in SE(3)$ and its inverse transformation can be computed as

[TABLE]

The relative transformation between the two corresponding body-fixed coordinate frames of agents $i$ and $j$, which is denoted as $\mathbf{T}_{ij}\in SE(3)$, is given as

[TABLE]

Let $\mathbf{R}_{ij}:=\mathbf{R}_{i}^{\top}\mathbf{R}_{j}$ be the relative orientation between two local coordinate frames ${}^{i}\Sigma$ and ${}^{j}\Sigma$ and $\mathbf{p}_{ij}^{i}:=\mathbf{R}_{i}^{\top}\mathbf{p}_{ij}=\mathbf{R}_{i}^{\top}(\mathbf{p}_{j}-\mathbf{p}_{i})$ the relative position between agent $i$ and $j$ which are measured locally in the local frame of agent $i$. Then, the relative transformation can be expressed as $\mathbf{T}_{ij}=\begin{bmatrix}\mathbf{R}_{ij}&\mathbf{p}_{ij}^{i}\\ \mathbf{0}&1\end{bmatrix}$.

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

[TABLE]

where $\mathbf{v}^{i}_{i}\in\mathbb{R}^{3}$ and $\bm{\omega}^{i}_{i}\in\mathbb{R}^{3}$ denote the linear velocity and the angular velocity of agent $i$ measured in ${}^{i}\Sigma$. We assume that each agent $i$ is able to measure $\mathbf{v}^{i}_{i}$ and $\bm{\omega}^{i}_{i}$ and the relative transformations (translation and rotation) to its neighboring agents without noise. If an edge $(i,j)\in\mathcal{E}$, then agent $i$ can measure $\mathbf{T}_{ij}\in SE(3)$ and it also can receive information communicated from agent $j$. For this, we make the following assumptions.

Assumption 1

Each agent $i$ in the system locally measures its body velocity, i.e., $[\mathbf{v}_{i}^{i},\bm{\omega}_{i}^{i}]\in\mathbb{R}^{3}\times\mathbb{R}^{3}$, and the relative transformation $\mathbf{T}_{ij}\in SE(3)$ defined in (1) with regard to its neighbors $j\in\mathcal{N}_{i}$.

Assumption 2

The underlying interaction graph $\mathcal{G}(\mathcal{V},\mathcal{E})$ contains a spanning tree.

The agents in the system aim to estimate for their actual rigid body motions $\mathbf{T}_{i}(t)\in SE(3),i=1,\ldots,n,$, a process is called frame localization, and each agent $i$ holds an estimate of the body transformation $\hat{\mathbf{T}}_{i}=\hat{\mathbf{T}}_{i}(\hat{\mathbf{R}}_{i},\hat{\mathbf{p}}_{i})\in SE(3),\forall i\in\mathcal{V}$. By using the local measurements in Assumption 1, the objective is for the agents to cooperatively localize the global coordinate frame, e.g., $\Sigma$, up to a transformation, $\mathbf{T}_{c}(\mathbf{R}_{c},\mathbf{p}_{c})\in SE(3)$, which is unknown but deterministic and common to all agents.

Problem 2.1** **(Asymptotic Frame Localization)

Consider a system of $n$ mobile agents in $\mathbb{R}^{3}$. Under the Assumptions 1 and 2, design a cooperative localization scheme for each agent $i$ to estimate its transformation $\mathbf{T}_{i}$ up to an unknown constant transformation $\mathbf{T}_{c}$.

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

Consider a system of $n$ mobile agents in $\mathbb{R}^{3}$. Under the Assumptions 1 and assume that $\mathcal{G}$ is connected and undirected, design a cooperative localization scheme for each agent $i$ to estimate its transformation $\mathbf{T}_{i}$ up to an unknown constant transformation $\mathbf{T}_{c}$ in a finite time.

3 Distributed Esimation of A Common Reference Frame

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.

3.1 Propose Estimation Law

For each agent $i$ we introduce an auxiliary matrix $\mathbf{P}_{i}\in\mathbb{R}^{4\times 3}$ as follows

[TABLE]

where $\mathbf{Q}_{i}\in\mathbb{R}^{3\times 3}$ is a nonsingular matrix and $\mathbf{q}_{i}\in\mathbb{R}^{3}$. Note that $\mathbf{P}_{i}$ is defined in the Cartesian ambient space and $\mathbf{Q}_{i}(0)$ has full-column rank and initialized randomly. Note that the set of nonsingular matrices in $\mathbb{R}^{3\times 3}$ is a dense set of the set of $3\times 3$ matrices, i.e., if $\mathbf{Q}_{i}$ is initialized randomly from a continuous uniform distribution on its entries, then $\mathbf{Q}_{i}$ will be almost surely nonsingular. Each agent $i$ implements the following localization law

[TABLE]

where $\mathbf{P}_{j}\in\mathbb{R}^{4\times 4}$ is the auxiliary matrix associated with agent $j$ and it is communicated from agent $j\in\mathcal{N}_{i}$. 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, $\hat{\mathbf{T}}_{i}$, is derived from the corresponding auxiliary matrix $\mathbf{P}_{i}$. The way, which constructs $\hat{\mathbf{T}}_{i}$ from $\mathbf{P}_{i}$, will be described latter in this section.

3.2 Analysis

The localization law (4) can be rewritten as

[TABLE]

By introducing the transformation $\mathbf{P}_{i}=\mathbf{T}_{i}^{-1}\mathbf{S}_{i}$ and noticing that $\dot{\mathbf{S}}_{i}=\mathbf{T}_{i}\dot{\mathbf{P}}_{i}+\dot{\mathbf{T}}_{i}\mathbf{P}_{i}=\mathbf{T}_{i}\dot{\mathbf{P}}_{i}+\mathbf{T}_{i}\begin{bmatrix}\mathbf{v}^{i}_{i}\\ \bm{\omega}^{i}_{i}\end{bmatrix}^{\wedge}\mathbf{P}_{i}$. Therefore, the above equation can be expressed as

[TABLE]

Let $\mathbf{S}:=[\mathbf{S}_{1}^{\top},\ldots,\mathbf{S}_{n}^{\top}]^{\top}\in\mathbb{R}^{4n\times 4}$ be the stack matrix of all $\mathbf{S}_{i}\in\mathbb{R}^{4\times 4},i=1,\ldots,n$. By combining the above frame localization dynamics for all agents we obtain a compact form

[TABLE]

Theorem 3.1

Assume that Assumptions 1 and 2 hold. Under the frame localization law (4), $\mathbf{S}(t)$ in (6) globally exponentially converge to

[TABLE]

where $\mathbf{w}_{1}=[w_{11},\ldots,w_{1n}],\mathbf{w}_{1}\mathbf{1}_{n}=1,$ is the left eigenvector of the Laplacian $\mathbf{L}$ corresponding to the zero eigenvalue.

{pf}

Since $\mathcal{G}$ has a spanning tree the associated Laplacian $\mathbf{L}\in\mathbb{R}^{n\times n}$ has a simple zero eigenvalue and the other eigenvalues have positive real parts. The right and left eigenvectors corresponding to the zero eigenvalue are $\mathbf{1}_{n}$ and $\mathbf{w}_{1}=[w_{11},\ldots,w_{1n}],w_{1i}>0,\mathbf{w}_{1}\mathbf{1}_{n}=1,$ respectively (Ren et al., 2004, Lemma 1). Further, there exists $\mathbf{S}_{c}\in\mathbb{R}^{4\times 4}$ such that $\mathbf{S}(t)$ globally exponentially converges to $(\mathbf{1}_{n}\otimes\mathbf{I}_{4})\mathbf{S}_{c}$.

Consider the solution to (6) as

[TABLE]

Let the Jordan form of $\mathbf{L}$ be $\mathbf{L}=\mathbf{UDV}^{-1}$ where $\mathbf{D}=\text{diag}\{0,\lambda_{2},\ldots,\lambda_{n}\}$ whose diagonal terms are eigenvalues of $\mathbf{L}$, $\mathbf{U}=[\mathbf{1}_{n},\mathbf{r}_{2},\ldots,\mathbf{r}_{n}]$ and $\mathbf{V}^{-1}=[\mathbf{w}_{1}^{\top},\ldots,\mathbf{w}_{n}^{\top}]^{\top}\in\mathbb{R}^{n\times n}$. Then the steady-state solution $\lim_{t\rightarrow\infty}\mathbf{S}(t)=(\mathbf{1}_{n}\otimes\mathbf{I}_{4})(\mathbf{w}_{1}\times\mathbf{I}_{4})\mathbf{S}(0)$. Thus, $\mathbf{S}_{i},i\in\mathcal{V},$ converge to $\mathbf{S}_{c}:=(\mathbf{w}_{1}\times\mathbf{I}_{4})\mathbf{S}(0)$, i.e., a convex combination of the initial matrices $\{\mathbf{S}_{i}(0)\}_{i\in\mathcal{V}}$. $\blacksquare$ The steady-state matrix $\mathbf{S}_{c}\in\mathbb{R}^{4\times 4}$ is given as

[TABLE]

where $\mathbf{Q}_{c}:=\sum_{i=1}^{n}{w}_{1i}\mathbf{R}_{i}(0)\mathbf{Q}_{i}(0)\in\mathbb{R}^{3\times 3}$, \mathbf{q}_{c}:=\sum_{i=1}^{n}{w}_{1i}\Big{(}\mathbf{R}_{i}(0)\mathbf{q}_{i}(0)+\mathbf{p}_{i}(0)\Big{)}\in\mathbb{R}^{3}, and ${w}_{1i}$ denotes the $i$-th entry of the left eigenvector $\mathbf{w}_{1}$.

At a time instant $t$, let $\mathbf{S}_{i}(t)=\begin{bmatrix}\mathbf{Q}_{S_{i}}(t)\in\mathbb{R}^{3\times 3}&\mathbf{q}_{S_{i}}(t)\in\mathbb{R}^{3}\\ \mathbf{0}&1\end{bmatrix}$ then the auxiliary matrix $\mathbf{P}_{i}$ is computed as

[TABLE]

Since $\mathbf{S}_{i}\rightarrow\mathbf{S}_{c},\forall i\in\mathcal{V},$ globally exponentially as $t\rightarrow\infty$ (Theorem 3.1) we have the following lemma.

Lemma 3.1

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

[TABLE]

globally exponentially as $t\rightarrow\infty$, i.e., $\mathbf{Q}_{i}\rightarrow\mathbf{R}_{i}^{\top}\mathbf{Q}_{c}$ and $\mathbf{q}_{i}\rightarrow\mathbf{R}_{i}^{\top}(\mathbf{q}_{c}-\mathbf{p}_{i})$, where $\mathbf{Q}_{c}$ and $\mathbf{q}_{c}$ are defined in (7).

3.3 Construction of Frame Transformations

We now assume that estimates of orientation, $\hat{\mathbf{R}}_{i}$, and position, $\hat{\mathbf{p}}_{i}$, of agent $i$ are derived from $\mathbf{Q}_{i}$ and $\mathbf{p}_{i}$ as follows ($\mathbf{Q}_{i}$ and $\mathbf{p}_{i}$ are defined in (3)). The orientation estimate $\hat{\mathbf{R}}_{i}^{T}(t)$ is constructed from $\mathbf{Q}_{i}(t)$ by the Gram-Schmidt procedure (GSOP, see Appendix A) and

[TABLE]

It is noticed from Lemma 3.1 that

[TABLE]

specifies the estimate of position of agent $i$ expressed locally in body frame ${}^{i}\Sigma$. It follows that the position of agent $i$ expressed in ${}^{i}\Sigma$, i.e., ${\mathbf{p}}_{i}^{i}$, is estimated up to a common constant translation $\mathbf{q}_{c}\in\mathbb{R}^{3}$.

Let \mathbf{Z}_{0}:=\left[\big{(}\mathbf{R}_{1}(0)\mathbf{Q}_{1}(0)\big{)}^{\top},\ldots,\big{(}\mathbf{R}_{n}(0)\mathbf{Q}_{n}(0)\big{)}^{\top}\right]^{\top}\in\mathbb{R}^{3n\times 3}. Then, we have the following result.

Corollary 1

Assume that Assumptions 1 and 2 hold. Under the frame localization law (4), if $\hat{\mathbf{R}}_{i}^{T}(t)$ is constructed from $\mathbf{Q}_{i}(t)$ by the Gram-Schmidt procedure (GSOP) and $\hat{\mathbf{p}}_{i}$ is computed by (9), then there exist an unknown constant transformation

[TABLE]

such that $\hat{\mathbf{T}}_{i}(t)\rightarrow\mathbf{T}_{c}^{-1}\mathbf{T}_{i}(t)$ as $t\rightarrow\infty$, for all $i\in\mathcal{V}$, if $\text{Range}(\mathbf{Z}_{0})\cap\text{null}(\mathbf{w}_{1}\otimes\mathbf{I}_{3})=\emptyset$.

{pf}

Let $\mathbf{\hat{R}}_{i}^{T}$ and $\mathbf{R}_{Si}$ be derived from $\mathbf{Q}_{i}$ and $\mathbf{Q}_{Si}$ by the GSOP, respectively. It follows from Lemma A.1 in Appendix that $\mathbf{\hat{R}}_{i}^{T}=\mathbf{R}_{i}^{T}\mathbf{R}_{Si}$ for all $t$. Since $\mathbf{Q}_{i}\rightarrow\mathbf{R}_{i}^{\top}\mathbf{Q}_{c}$ (Lemma 3.1), $\mathbf{\hat{R}}_{i}^{\top}\rightarrow\mathbf{R}_{i}^{\top}\mathbf{R}_{c},\forall i\in\mathcal{V},$ as $t\rightarrow\infty$, where the unknown constant orientation $\mathbf{R}_{c}:=\text{GSOP}(\mathbf{Q}_{c})$. As a result, from (9) and Lemma 3.1, we have

[TABLE]

as $t\rightarrow\infty$. Consequently, one has

[TABLE]

where $\mathbf{T}_{c}\in SE(3)$ is defined in (11).

For the validity of the estimated frame transformations, $\hat{\mathbf{T}}_{i}(\mathbf{\hat{R}}_{i},\hat{\mathbf{p}}_{i})$, the singularity of the steady-state matrix $\mathbf{Q}_{c}$ defined in (7) is undesired. For this, we now show that $\mathbf{Q}_{c}$ is nonsingular if the initial matrices satisfy the condition $\text{Range}(\mathbf{Z}_{0})\cap\text{null}(\mathbf{w}_{1}\otimes\mathbf{I}_{3})=\emptyset$. From (7), $\mathbf{Q}_{c}$ is explicitly computed as

[TABLE]

where $[\mathbf{Z}_{0}]_{i}$ denotes the $i$-th column of $\mathbf{Z}_{0}$. It follows that $\mathbf{Q}_{c}$ contains linearly independent columns if and only if $(\mathbf{w}_{1}\otimes\mathbf{I}_{3})[\mathbf{Z}_{0}]_{i}\neq\mathbf{0},\forall i=1,2,3,$ and column vectors of $\mathbf{Z}_{0}$ are linearly independent. The second condition follows from the nonsingularity of $\mathbf{Q}_{i}(0)$ (for almost all random initializations of the entries of $\mathbf{Q}_{i}(0)$) and the first condition implies $\text{Range}(\mathbf{Z}_{0})\cap\text{null}(\mathbf{w}_{1}\otimes\mathbf{I}_{3})=\emptyset$. $\blacksquare$

Corollary 2

Since the dimension of $\text{null}(\mathbf{w}_{1}\otimes\mathbf{I}_{3})$ is $3(n-1)$ which is a lower-dimensional subspace of $\mathbb{R}^{3n}$ and hence its Lebesgue measure is zero. Thus, the steady-state estimates of the frame transformations $\hat{\mathbf{T}}_{i},i\in\mathcal{V},$ are well-defined for almost all initial matrices $\{\mathbf{P}_{i}(0)\}_{i\in\mathcal{V}}$. Further, the frame transformations of the agents are computed almost globally exponentially up to a common constant transformation $\mathbf{T}_{c}(\mathbf{R}_{c},\mathbf{q}_{c})$. In other words, the frame transformations of the agents are computed relative to a reference frame whose frame transformation is $\mathbf{T}_{c}(\mathbf{R}_{c},\mathbf{q}_{c})$.

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 (2019)). Indeed, if $\{\mathbf{Q}_{i}\}_{i\in\mathcal{V}}$ in (3) are initialized randomly in $\mathbf{R}^{3\times 3}$ then some of $\{\mathbf{R}_{i}(0)\mathbf{Q}_{i}(0)\}_{i\in\mathcal{V}},$ have negative and some of those have positive determinants. Since all $\{\mathbf{R}_{i}(t)\mathbf{Q}_{i}(t)\}_{i\in\mathcal{V}},$ converge to $\mathbf{Q}_{c}$, at least one of $\{\mathbf{R}_{i}(t)\mathbf{Q}_{i}(t)\}_{i\in\mathcal{V}},$ whose determinant changes sign. Thus, its determinant becomes zero at some time instants. Consequently, $\mathbf{Q}_{i}$ is nonsingular at those points.

Remark 2

If we instead construct the first two columns of $\hat{\mathbf{R}}_{i}^{\top}$ from the first two column vectors of $\mathbf{Q}_{i}$ by the Gram-Schmidt orthonormalization process and the third column vector of $\hat{\mathbf{R}}_{i}^{\top}$ is the cross product of the first two column vectors, then it can be shown that $\hat{\mathbf{R}}_{i}$ is well-defined for all $t>0$ for almost all initial matrices $\{\mathbf{Q}_{i}(0)\}_{i\in\mathcal{V}}$. Indeed, it is equivalent to show that the first two column vectors of $\mathbf{S}_{i}(t)$ in (6) are linearly independent for all initial matrices $\{\mathbf{S}_{i}(0)\}_{i\in\mathcal{V}}$ but a set of zero measure Tran and Ahn (2019).

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

4 Finite-time Frame Localization

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.

4.1 Proposed Finite-time Frame Localization Law

Each agent $i$ holds an auxiliary matrix $\mathbf{P}_{i}\in\mathbb{R}^{4\times 4}$ as defined in (3) and the estimate of the frame transformation of agent $i$ is constructed from $\mathbf{P}_{i}$ by following the same computations in Corollary 1. For each agent $i$, we propose the following frame localization law

[TABLE]

where the positive scalar $0<\alpha<1$. 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

[TABLE]

{pf}

First, we have

[TABLE]

where $\mathbf{K}_{i}:=\begin{bmatrix}\mathbf{R}_{i}^{\top}&\mathbf{0}\\ \mathbf{0}&1\end{bmatrix}$. By using this relation, one has

[TABLE]

which competes the proof. $\blacksquare$

4.2 Analysis

By using the transformation $\mathbf{P}_{i}=\mathbf{T}_{i}^{-1}\mathbf{S}_{i},~{}\mathbf{S}_{i}\in\mathbb{R}^{4\times 4},$ and the above Lemma, the frame localization law (12) can be written as

[TABLE]

Remark 3

The above system is time-continuous as will be shown in the following. Let $[\mathbf{S}_{i}]_{k}\in\mathbb{R}^{4}$ be the $k$-th column vector of $\mathbf{S}_{i}$ and $\mathbf{s}_{i}:=[[\mathbf{S}_{i}]_{1}^{\top},[\mathbf{S}_{i}]_{2}^{\top},\ldots,[\mathbf{S}_{i}]_{4}^{\top}]\in\mathbb{R}^{16}$ be the stacked vector of all column vectors of $\mathbf{S}_{i}$, for all $i\in\{1,\ldots,n\}$. Furthermore, from the definition of the Frobenius norm, we have

[TABLE]

where $\lVert{\cdot}\rVert$ denotes the Euclidean norm. By using the above equation, we can rewrite (14) into a vector form as

[TABLE]

with $\alpha\in(0,1)$ the right hand side of the above equation is continuous Trinh et al. (2017). If $\alpha\geq 1$, it is discontinuous Cortés (2006). $\blacksquare$

Let $\mathbf{S}:=[\mathbf{S}_{1}^{\top},\ldots,\mathbf{S}_{n}^{\top}]^{\top}\in\mathbb{R}^{4n\times 4}$ be the stack matrix of all $\mathbf{S}_{i}\in\mathbb{R}^{4\times 4},i=1,\ldots,n$. By combining the above frame localization dynamics for all agents we obtain a compact form

[TABLE]

where the matrix $\mathbf{\bar{L}}=[\bar{l}_{ij}]\in\mathbb{R}^{n\times n}$ is defined as

[TABLE]

which is a weighted Laplacian for the graph $\mathcal{G}$.

Assume that $\mathcal{G}$ is undirected and connected. Then, $(\mathbf{1}_{n}\otimes\mathbf{I}_{4})^{T}\dot{\mathbf{S}}(t)=-(\mathbf{1}_{n}\otimes\mathbf{I}_{d})^{T}(\mathbf{\bar{L}}\otimes\mathbf{I}_{4})\mathbf{S}(t)=\mathbf{0}$. Thus, $(\mathbf{1}_{n}\otimes\mathbf{I}_{4})^{T}\mathbf{S}(t)$ is invariant under (16). Let $\mathbf{S}_{c}:=(1/n)(\mathbf{1}_{n}\otimes\mathbf{I}_{4})^{\top}\mathbf{S}(t)\in\mathbb{R}^{4\times 4}$, $\mathbf{S}_{i}(t)=\mathbf{S}_{c}+\bm{\delta}_{i}(t)$, and let $\bm{\delta}(t):=[\bm{\delta}_{1}^{T},\ldots,\bm{\delta}_{n}^{T}]^{T}\in\mathbb{R}^{4n\times 4}$. Since $\mathbf{S}_{c}$ is time-invariant, it follows that $\dot{\bm{\delta}}_{i}(t)=\dot{\mathbf{S}}_{i}(t)$. Note that $\bm{\delta}_{i}-\bm{\delta}_{j}=\mathbf{S}_{i}-\mathbf{S}_{j}$.

Theorem 4.1

Under the estimation law (12) and assume that $\mathcal{G}_{o}$ is a connected undirected graph, $\mathbf{S}(t)$ globally asymptotically converges to $(\mathbf{1}_{n}\otimes\mathbf{I}_{4})\text{Ave}\{\mathbf{S}_{i}(0)\}_{i\in\mathcal{V}}$ in a finite time with settling time $T_{c}>0$ bounded by

[TABLE]

where $V(0)=\frac{1}{2}\sum_{1}^{n}||\bm{\delta_{i}}(0)||_{F}^{2}$, and $\kappa=(2\lambda_{2})^{\frac{2-\alpha}{2}}$ with $\lambda_{2}$ being the smallest nonzero eigenvalue of the Laplacian $\mathbf{L}(\mathcal{G})$ associated with $\mathcal{G}$.

{pf}

Consider a Lyapunov candidate function

[TABLE]

Note that $V$ is radially unbounded, positive definite, continuously differentiable, and $V=0$ in \mathcal{S}_{o}:=\big{\{}\{\mathbf{S}_{i}\}_{i\in\mathcal{V}}~{}|~{}\mathbf{S}_{i}=\mathbf{S}^{c},~{}\forall i\in\mathcal{V}\big{\}}. The time derivative of $V$ along the trajectory of (16) is given as

[TABLE]

where $[\bm{\delta}]_{k}\in\mathbb{R}^{4n}$ denotes the $k$-th(1$\leq k\leq 4$) column vector of the matrix $\bm{\delta}\in\mathbb{R}^{4n\times 4}$ and the inequality (18) is derived by applying Lemma B.1 with $\frac{1}{2}<\frac{2-\alpha}{2}<1$. Under the assumption that $\mathcal{G}$ is connected undirected, $\mathbf{L}\otimes\mathbf{I}_{d}$ has $4$ zero eigenvalues and $\text{null}(\mathbf{L}\otimes\mathbf{I}_{d})=\text{span}\{\text{Range}(\mathbf{1}_{n}\otimes\mathbf{I}_{4})\}$. Since $[\bm{\delta}]_{k}\perp\text{Range}(\mathbf{1}_{n}\otimes\mathbf{I}_{4})$, $\forall k=1,\ldots,4$, one has

[TABLE]

where $\lambda_{2}$ is the smallest nonzero eigenvalue of $\mathbf{L}(\mathcal{G})$. Substituting the above inequalities into (19) yields

[TABLE]

where $\kappa=(2\lambda_{2})^{(2-\alpha)/{2}}$. It follows from Lemma B.2 and (20) that $V(t)$ converges to [math] in finite time. In other words, $\{\mathbf{S}_{i}\}_{i\in\mathcal{V}}$ converges to the invariant set $\mathcal{S}_{o}$. As a result, it follows that $\mathbf{S}_{i}(t),~{}\forall i\in\mathcal{V}$, globally converges to $\mathbf{S}^{c}=\text{Ave}\{\mathbf{S}_{i}(0)\}_{i\in\mathcal{V}}$ with settling time $T_{c}\leq V(0)^{1-\frac{2-\alpha}{2}}/(\kappa(1-\frac{2-\alpha}{2}))=2V(0)^{\frac{\alpha}{2}}/(\kappa\alpha)$. $\blacksquare$

Corollary 3

Assume that Assumption 1 holds and the interaction graph $\mathcal{G}$ is undirected and connected. Under the frame localization law (12), if $\hat{\mathbf{R}}_{i}^{\top}(t)$ is constructed from $\mathbf{Q}_{i}(t)$ by the Gram-Schmidt procedure (GSOP) and $\hat{\mathbf{p}}_{i}$ is computed by (9), then there exist an unknown constant transformation $\mathbf{T}_{c}\in SE(3)$ such that $\hat{\mathbf{T}}_{i}(t)\rightarrow\mathbf{T}_{c}^{-1}\mathbf{T}_{i}(t)$ in a finite time, for all $i\in\mathcal{V}$, for almost all initializations of $\{\mathbf{P}_{i}(0)\}_{i\in\mathcal{V}}$.

{pf}

The proof follows from the fact that $\mathbf{S}_{i}(t)\rightarrow(\mathbf{1}_{n}^{\top}\otimes\mathbf{I}_{4})\mathbf{S}(0)=\text{Ave}\{\mathbf{S}_{i}(0)\}_{i\in\mathcal{V}},\forall i\in\mathcal{V},$ globally asymptotically as $t\rightarrow T_{c}$ (Theorem 4.1) and by following same lines as in the proof of Corollary 1. The estimated frame transformations of the agents, $\hat{\mathbf{T}}_{i}$, converge almost globally asymptotically to the actual frame transformations in a finite-time, up to a common constant transformation $\mathbf{T}_{c}(\mathbf{R}_{c},\mathbf{q}_{c})$, if the column vectors of \mathbf{Z}_{0}:=\left[\big{(}\mathbf{R}_{1}(0)\mathbf{Q}_{1}(0)\big{)}^{\top},\ldots,\big{(}\mathbf{R}_{n}(0)\mathbf{Q}_{n}(0)\big{)}^{\top}\right]^{\top}\in\mathbb{R}^{3n\times 3} are not orthogonal to $\text{Range}(\mathbf{1}_{n}\otimes\mathbf{I}_{3})$. $\blacksquare$

4.3 Relation between Actual Frame Transformations and Estimated Frame Transformations

The relation between the estimates of the body motions, $\hat{\mathbf{T}}_{i}$, and the actual body motions of the agents, $\mathbf{T}_{i}$ is illustrated in Fig. 2. In particular, $\hat{\mathbf{T}}_{i},\forall i\in\mathcal{V},$ are obtained from $\mathbf{T}_{i}$ by first translating the agents by $-\mathbf{q}_{c}\in\mathbb{R}^{3}$ followed by a rotation $\mathbf{R}_{c}^{\top}\in SO(3)$ about the origin of the global coordinate frame $\Sigma$. Note that the frame transformation $\mathbf{T}_{c}(\mathbf{R}_{c},\mathbf{q}_{c})$ is an unknown constant and $\mathbf{R}_{c}=\text{GSOP}(\mathbf{Q}_{c})$ where $\mathbf{Q}_{c}$ and $\mathbf{q}_{c}$ are computed as (7).

5 Simulation

Consider a system of four mobile agents in $\mathbb{R}^{3}$. The initial orientations of the agents are chosen randomly and their initial positions are given as: $\mathbf{p}_{1}(0)=[0,0,0]^{\top},~{}\mathbf{p}_{2}(0)=[4,0,0]^{\top},~{}\mathbf{p}_{3}(0)=[0,0,4]^{\top},$ and $\mathbf{p}_{4}(0)=[0,4,0]^{\top}$. The agents travel with angular velocities: $\bm{\omega}_{1}^{1}=[0.3,0,0]^{\top},~{}\bm{\omega}_{2}^{2}=[0,0.3,0]^{\top},~{}\bm{\omega}_{3}^{3}=[0,0,0.3]^{\top},~{}\bm{\omega}_{4}^{4}=\mathbf{0}$, and linear velocities: $\mathbf{v}_{1}=[1,0,0]^{\top},~{}\mathbf{v}_{2}=[0,1,0]^{\top},~{}\mathbf{v}_{3}=[0,0,1]^{\top}$ and $\mathbf{v}_{4}=[1,1,0]^{\top}$, respectively.

We provide simulation results for frame localization of the system of four agents under the localization law (4) and the finite-time localization law (12) with the corresponding interaction graph topologies in Figs. 3 and 4, respectively. In both cases, it is observed that the estimated poses of the agents asymptotically converge to the actual poses as $t\rightarrow\infty$ since the induced norms of the orientation errors, $||\mathbf{R}_{i}\hat{\mathbf{R}}_{i}^{\top}-\mathbf{R}_{c}||_{F}\rightarrow 0$ (See Figs. 3(b) and 4(b)), and the norms of the position errors $||(\mathbf{p}_{j}-\mathbf{p}_{i})-\mathbf{R}_{c}(\hat{\mathbf{p}}_{j}-\hat{\mathbf{p}}_{i})||\rightarrow 0$ (See Figs. 3(c) and 4(c)).

To show the advantage of the finite-time localization scheme (12) over the localization protocol with asymptotic stability property (4) the zoomed plots of the orientation and position estimation errors in the time interval $t\in[3,4]$ are included. It is shown that the finite-time frame localization scheme (12) has fast convergence time (about $3.2$ seconds), whereas, the convergence time of the estimated poses under (4) is infinitely long.

6 Conclusion

In this work, we presented two frame localization schemes for estimating the trajectories of rigid-body motions of multi-agent systems in $\mathbb{R}^{3}$. Under the first localization law, the estimated frame transformations of the agents converge to the actual frame transformations almost globally and exponentially up to an unknown constant transformation bias. Whereas, under the second frame localization protocol, the estimated frame transformations of the agents converge to the actual frame transformations in a finite time up to an unknown constant transformation bias. Simulation results were provided to support the theoretical analysis. There are several possible applications of the proposed frame localization schemes in the network pose formation control and the multi-agent cooperative estimation and which are left as the future work.

Appendix A The Gram-Schmidt Orthonormalization Process (GSOP)

For a set of $d$ independent vectors $\mathcal{Z}=\{\mathbf{z}_{1},\mathbf{z}_{2},\ldots,\mathbf{z}_{d}\}$ in $\mathbb{R}^{d}$, the GSOP, which constructs $d$ orthonormal column vectors of $\mathbf{Q}=[\mathbf{q}_{1}~{}\ldots\mathbf{q}_{d}]\in\text{SO}(d)$ from $\mathcal{Z}$, is defined as follows,

[TABLE]

where $\langle\cdot,\cdot\rangle$ denotes the inner product, and the coefficient $\alpha$ is chosen such that $\text{det}(\mathbf{Q})=1$ as $\alpha:=\text{sign}\left(\text{det}\left([\mathbf{q}_{1},\ldots,\mathbf{q}_{d-1},{\mathbf{v}_{d}}/{\lVert{\mathbf{v}_{d}}\rVert}]\right)\right).$ If the set $\mathcal{Z}$ contains a linearly dependent vector (which linearly depends on one or more vectors in $\mathcal{Z}$), there exists $\mathbf{v}_{i}=\mathbf{0},~{}i\in\{1,\ldots,d\}$, and hence $\mathbf{q}_{i}=\mathbf{0}$.

Lemma A.1

(Tran et al., 2018, Lemma 2)* Consider the rotation transformation $\mathbf{Q}_{i}=\mathbf{R}_{i}^{\top}\mathbf{Q}_{Si},\mathbf{R}_{i}\in SO(3),\mathbf{Q}_{i}$ and $\mathbf{Q}_{Si}\in\mathbb{R}^{3\times 3}$. If $\mathbf{\hat{R}}_{i}^{\top}$ and $\mathbf{R}_{Si}$ are derived from $\mathbf{Q}_{i}$ and $\mathbf{Q}_{Si}$ by the above GSOP, respectively, then there holds*

[TABLE]

Appendix B Finite-Time Convergence Theory

Lemma B.1

Hardy et al. (1952)** If $\xi_{1},\ldots,\xi_{d}\geq 0$ and $0\leq p\leq 1$, then

[TABLE]

A condition for finite-time convergence of continuous-time systems is given by the following lemma.

Lemma B.2

(Bhat and Bernstein (2000)). Suppose that there exists a positive-definite and continuous function $V(x,t):\mathbb{R}^{n}\times[0,\infty)\rightarrow\mathbb{R}$. If there exists $\kappa>0,~{}\alpha\in(0,1)$, and open neighborhood $\mathcal{U}_{0}\in\mathbb{R}^{d}$ of the origin such that

[TABLE]

then $V=0$ for $t\geq T$, with the settling time $T\leq V^{1-\alpha}(0)/(\kappa(1-\alpha))$.