Gaussian-Sum Filter for Range-based 3D Relative Pose Estimation in the Presence of Ambiguities

TL;DR

This paper presents a computationally-efficient estimator, in the form of a Gaussian-sum filter (GSF), to realize range-based relative pose estimation in an infrastructure-free, 3D, setup and is shown to avoid divergence to local minima induced by the ambiguous poses.

Abstract

Multi-robot systems must have the ability to accurately estimate relative states between robots in order to perform collaborative tasks, possibly with no external aiding. Three-dimensional relative pose estimation using range measurements oftentimes suffers from a finite number of non-unique solutions, or ambiguities. This paper: 1) identifies and accurately estimates all possible ambiguities in 2D; 2) treats them as components of a Gaussian mixture model; and 3) presents a computationally-efficient estimator, in the form of a Gaussian-sum filter (GSF), to realize range-based relative pose estimation in an infrastructure-free, 3D, setup. This estimator is evaluated in simulation and experiment and is shown to avoid divergence to local minima induced by the ambiguous poses. Furthermore, the proposed GSF outperforms an extended Kalman filter, demonstrates similar performance to the computationally-demanding particle filter, and is shown to be consistent.

Figures (9)

• Figure 1: (a) Problem setup for a two-tag multi-robot system, where each robot is equipped with Tags $\tau_i$ and $\tau_j$. Without loss of generality, the pink robot, defined as Robot $1$, is considered to be the reference robot. (b) Visualization of all the possible ambiguous relative poses between robots $1$ and $p$. The relative pose in mode $1$ is the "true" pose and modes $2,\,3,$ and $4$ are ambiguities. The range measurements are $y_{1i},\,y_{1j},\,y_{2i},$ and $y_{2j}$.
• Figure 2: (a) Visualization of the geometric relation between tags $\tau_1$, $\tau_2$ of Robot $1$ and $\tau_\mu$, $\mu \in \{i,j\}$ of Robot $p$ resolved in $\mathcal{F}_1$. The range measurements consist of $y_{1\mu}$ and $y_{2\mu}$, $\mu \in \{i,j\}$. The reference point in Robot $1$, $1$, and the frame $\mathcal{F}_1$ are arbitrarily defined. (b) Visualization of the relation between frames $\mathcal{F}_1$, $\mathcal{F}_p$, and $\mathcal{F}_r$. Tags $\tau_i$ and $\tau_j$ are mounted on Robot $p$. In both figures, the superscript $(\cdot)$ represents the mode number.
• Figure 3: Comparison between the true pose and the ambiguous GI-LS pose estimates in a system of three robots, each having two tags. The opaque drones denote the true poses. The lighter shaded drones with their respective covariance plots are the pose estimates and their corresponding uncertainties.
• Figure 4: The performance of the EKF, GSF and PF on simulated data for two-tag Robots $2$ and $3$, with Robot $1$ as reference robot. The GSF and PF are initialized with $8$ GI-LS estimates and $1500$ particles, respectively. The EKF is initialized in a wrong mode among the 8 GI-LS estimates. The shaded regions represent the $\pm 3\sigma$ bounds.
• Figure 5: GSF trajectory estimation plot for a single run in simulation, shown in 2D. Only some modes of the GSF and only the relative position between Robot$~1$ and Robot$~2$ are shown for clarity. The ground truth starts at the location the quadcopters are plotted, and Robot $1$ is the reference robot.