3-D Relative Localization for Multi-Robot Systems with Angle and Self-Displacement Measurements

Abstract

Realizing relative localization by leveraging inter-robot local measurements is a challenging problem, especially in the presence of measurement noise. Motivated by this challenge, in this paper we propose a novel and systematic 3-D relative localization framework based on inter-robot interior angle and self-displacement measurements. Initially, we propose a linear relative localization theory comprising a distributed linear relative localization algorithm and sufficient conditions for localizability. According to this theory, robots can determine their neighbors' relative positions and orientations in a purely linear manner. Subsequently, in order to deal with measurement noise, we present an advanced Maximum a Posterior (MAP) estimator by addressing three primary challenges existing in the MAP estimator. Firstly, it is common to formulate the MAP problem as an optimization problem, whose inherent non-convexity can result in local optima. To address this issue, we reformulate the linear computation process of the linear relative localization algorithm as a Weighted Total Least Squares (WTLS) optimization problem on manifolds. The optimal solution of the WTLS problem is more accurate, which can then be used as initial values when solving the optimization problem associated with the MAP problem, thereby reducing the risk of falling into local optima. The second challenge is the lack of knowledge of the prior probability density of the robots' relative positions and orientations at the initial time, which is required as an input for the MAP estimator. To deal with it, we combine the WTLS with a Neural Density Estimator (NDE). Thirdly, to prevent the increasing size of the relative positions and orientations to be estimated as the robots continuously move when solving the MAP problem, a marginalization mechanism is designed, which ensures that the computational cost remains constant.

Figures (28)

• Figure 1: Impacts of prior densities on the relative localization accuracy. In this example, the truth of the initial value $\mu$ is set to zero and all prior densities are modeled as Gaussian distributions. An ideal prior density has a mean close to the truth and a small covariance. Specifically, we define $\mathcal{N}_1 = \mathcal{N}(\mu_0, 500 \cdot I_n)$, $\mathcal{N}_2 = \mathcal{N}(\mu_{0.2}, 25 \cdot I_n)$, $\mathcal{N}_3 = \mathcal{N}(\mu_{0.4}, 25 \cdot I_n)$, $\mathcal{N}_4 = \mathcal{N}(\mu_0, 5 \cdot I_n)$, $\mathcal{N}_5 = \mathcal{N}(\mu_0, 0.2 \cdot I_n)$, where $\mu_{z}$ satisfies $\frac{\left\| \mu_{z} - \mu \right\|}{\left\| \mu \right\|} = z$, and $z$ denotes the offset of the mean $\mu_{z}$ with respect to $\mu$. Figures (b)-(f) illustrate one-dimensional examples corresponding to $\mathcal{N}_1$ to $\mathcal{N}_5$.
• Figure 2: Overview of the proposed relative localization framework. Algorithm \ref{['al general']} formulates the relative localization problem at a single time instant as a linear equation based on angle and self-displacement measurements. Algorithm \ref{['al wtls']} enhances robustness by reformulating this linear equation as a WTLS optimization problem on manifolds. For improved accuracy over multiple time instants, a MAP estimator is employed in Algorithm \ref{['al total']}, where Algorithm \ref{['al nde']} approximates the required prior probability density and the output of Algorithm \ref{['al wtls']} serves as the initial values for the MAP optimization problem.
• Figure 3: Illustration of measurements
• Figure 4: The scenario with four robots
• Figure 5: $ ijms$ and geometric relation between $i,j,m,s$ and $j',m',s'$

Theorems & Definitions (16)

• Definition 1
• Definition 2
• Lemma 1
• Theorem 1
• Theorem 2
• Remark 1
• Theorem 3
• Definition 3
• Proposition 1
• Remark 2