Multirobot Cooperative Localization Algorithm with Explicit Communication and Its Topology Analysis

Abstract

This paper proposes a new cooperative localization algorithm that separates communication and observation into independent mechanisms. While existing algorithms acknowledge observations between robots are crucial in cooperative localization schemes, communication is considered only an auxiliary role in observation update but not explicitly stated. However, such algorithms require the communication to be available whenever needed, and it is difficult to consider the effect of communication imperfection, which is unavoidable in real systems. We propose the Global State–Covariance Intersection (GS-CI) multirobot cooperative localization algorithm that can independently update localization estimates through both observation and communication steps. We also provide a theoretical upper bound of the resulting estimation uncertainty based on observation and communication topologies. Simulations using generated data validates the theoretical analysis, and shows the comparable performance to the centralized equivalent approach with less communication together with real-world data.

Appendix: Auxiliary Lemmas

Lemma 1

For $P > 0$ and the index set I, we have $[P^{-1}]_{I} > {[P]_I}^{-1}$.

Proof

For convenience, we partition P into blocks where the upper-left submatrix is exactly $[P]_{I}$. We denote $A=[P]_{I}$ for simplicity, which gives $P = \begin{bmatrix} A &{} B\\ B^{\mathsf {T}} &{} C \end{bmatrix}$. By matrix inversion lemma, we have

$$ P^{-1} = \begin{bmatrix} A^{-1} + A^{-1}B(C-B^{\mathsf {T}}A^{-1}B)^{-1}B^{\mathsf {T}}A^{-1} &{} -A^{-1}B(C-B^{\mathsf {T}}A^{-1}B)^{-1} \\ -(C-B^{\mathsf {T}}A^{-1}B)^{-1}B^{\mathsf {T}}A^{-1} &{} (C-B^{\mathsf {T}}A^{-1}B)^{-1} \end{bmatrix}. $$

By assumption, $P^{-1} > 0$, and thus the diagonal submatrix $(C-B^{\mathsf {T}}A^{-1}B)^{-1} > 0$. In conclusion, we arrive $[P^{-1}]_{I}= A^{-1} + A^{-1}B(C-B^{\mathsf {T}}A^{-1}B)^{-1}B^{\mathsf {T}}A^{-1} > A^{-1} = [P]_I^{-1}$.

Lemma 2

Suppose that $P > 0$ and $P = \begin{bmatrix} A_{m \times m} &{} B\\ B^{\mathsf {T}} &{} C_{n \times n} \end{bmatrix}$, then $P < P'= \begin{bmatrix} \frac{1}{c}A &{} 0\\ 0 &{} \frac{1}{1-c}C \end{bmatrix}$ for $c\in (0,1)$.

Proof

Consider a nonzero vector $v^{\mathsf {T}} =[(1-c) x^{\mathsf {T}}, \, c y^{\mathsf {T}} ]$ where $x \in \mathbb {R}^m$ and $y \in \mathbb {R}^n$. Since $P > 0$, $v^{\mathsf {T}}P v > 0$, or

$$ (1-c)^2 x^{\mathsf {T}} A x + c^2 y^{\mathsf {T}} C y + c(1-c) y^{\mathsf {T}} B^{\mathsf {T}} x + c(1-c) x^{\mathsf {T}} B y > 0. $$

For arbitrary $u^{\mathsf {T}} =[ x^{\mathsf {T}}, \, y^{\mathsf {T}} ]$, we then know that

$$ Q= \begin{bmatrix} (1-c)^2 A &{} c(1-c)B\\ c(1-c)B^{\mathsf {T}} &{} c^2 C \end{bmatrix} > 0. $$

The result follows by $P' - P = \frac{1}{c(1-c)}J Q J > 0$, where $J = \mathsf {Diag}(I_{m}, -I_{n})$.