SEAL: Simultaneous Exploration and Localization in Multi-Robot Systems

GP regression is a method that is frequently used to model spatial phenomena. Simulating the hidden mapping from training data to the target phenomenon is possible while considering the uncertainty provided by the natural non-parametric generalization of linear regression. Assume that the target phenomenon, a wireless source, satisfies a multivariate joint Gaussian distribution in this case. The Gaussian probability distribution of the phenomena $\omega(g)$ as given by the mean function $\mu(g)=\varepsilon[(g)]$ and covariance function $\delta^{2}(g,g^{\prime})=\varepsilon[(\omega(g)-\mu(g))^{T}\times(\omega(g)-\mu(g)]$ is the output of the trained GP model using training data.

Formally, let $GP^{[i]}=[g^{[i]}_{1},...,g^{[i]}_{k}]^{T}$ be the grid positions at which $r_{i}$ observed $k$ noisy RSSI readings [26] from the source $S^{[i]}=[s^{[i]}_{1},...,s^{[i]}_{k}]^{T}$. Since the mean function is assumed to be zero without losing generality, each observation is noisy, with the formula $y=\omega(g)+\epsilon$. To achieve this, given a sampling location $g_{k}\in GP$, we have the conditional posterior means $\mu_{g_{k}|GP_{j},y_{j}}$ and variance $\delta^{2}_{g_{k}|GP_{j},y_{j}}$ as follows from the learned GP model defining the Gaussian distribution of $\omega(g_{k})\sim N(\mu_{g_{k}|GP_{j},y_{j}},\delta^{2}_{g_{k}|GP_{j},y_{j}})$ for sample position $k$.

Once each robot learns its GP model for its current position, they share GPs with the connected robots for global exploration. Since each local GP is assumed to be learned by each robot, we assume that the underlying data distribution to be explored can be characterized by a combination of $n$ fused GPs, $GP=\{GP_{1},...,GP_{n}\}$ received from connected robots. We define $p(g\mid s_{1:n},X_{1:n})=b_{n}^{g}$ as the likelihood that, for each point in the environment, $g\in GP$ is best characterized by the GP that the $r_{i}$ has learned. Then, we produce a combination of GP models weighted by the $b_{n}^{g}$ at any grid $g\in GP$ as a linear combination of ${GP_{1},...,GP_{n}}$. The means $\mu^{\prime}_{g|GP,Y}$ and variance $\delta^{\prime 2}_{g|GP,Y}$ for robot $i$ are the parameters that define the model as:

In this problem, the algorithm computes the weight distribution in the expectation step (E-Step). Then, the maximization step updates the parameters of the local fused GP with the estimated weight distribution (M-Step), the same as discussed in GPMix [15]. Finally, the weight distribution is set to the following values before the first iteration for each random wireless sample $s$ at grid position $g$:

Where $\theta$ is the threshold for exploration and $O_{b^{g}}$ is the value of grid position in the exploration grid map.

Assume at a given time $t$, a team of robots considered as vertices of the graph contains $n\in\mathbb{N}$ connected robots can form a weighted undirected graph, denoted by $G=(V,E,A)$, of order $n$ consists of a vertex set $V=\{v_{1},...,v_{n}\}$, an undirected edge set $E\in V\times V$ is a range between connected robots and an adjacency matrix $A=\{a_{ij}\}_{n\times n}$ with non-negative element $a_{ij}>0$, if $(v_{i},v_{j})\in E$ and $a_{ij}=0$ otherwise. An undirected edge $e_{ij}$ in the graph $G$ is denoted by the unordered pair of robots $(v_{i},v_{j})$, which means that robots $v_{i}$ and $v_{j}$ can exchange information with each other.

Here, we only consider the undirected graphs, indicating that the robots’ communications are all bidirectional. Then, the connection weight between robots $v_{i}$ and $v_{j}$ in graph $G$ satisfies $a_{ij}=a_{ji}>0$ if they are connected; otherwise, $a_{ij}=a_{ji}=0$. Without loss of generality, it is noted that $a_{ii}=0$ indicates no self-connection in the graph. The degree of robot $v_{i}$ is defined by $d(v_{i})=\sum_{j=1}^{n}a_{ij}$ where $j\neq i$ and $i=1,2,...,n$. The Laplacian matrix of the graph $G$ is defined as $L_{n}=D-A$, where $D$ is the diagonal with $D=diag\{d(v_{1}),d(v_{2}),...,d(v_{n})\}$. If the graph $G$ is undirected, $L_{n}$ is symmetric and positive semi-definite. A path between robots $v_{i}$ and $v_{j}$ in a graph $G$ is a sequence of edges $\{(v_{i},v_{i1}),(v_{i1},v_{i2}),...,(v_{in-1},v_{in}),(v_{in},v_{j})\}$ in the graph with distinct robots $v_{in}\in V$. An undirected graph $G$ is connected if a path exists between any pair of distinct robots $v_{i}$ and $v_{j}$ where $(i,j=1,...,n)$.

We begin by creating the Estimated Relative Position Measurement Graph (ERPMG): $G_{E}=(V_{E},E_{E},A_{E})$, based on the Relative Position Estimation Graph (RPMG): $G=(V,E,A)$, which can be constructed using range information received from connected robots of an n-robot system and describes the relative position measurements among robots. Based on robotic motion constraints, we extend RPMG to accommodate all possible robot positions in the succeeding time step.

Since each robot also gets RSSI from other robots, we may map the predicted relative positions for each robot using the model and identify the $k$ soft maximum out of them by locating the intersection region as an area of interest. Once we have $k$ possible positions of each robot, we can generate $<n^{k}$ solvable graphs for optimization.

We consider a network with $n$ robots for optimization of expanded graphs, labeled by $V=\{1,2,...,n\}$ and $k$ possible connections to other robots. Every robot $i$ has a local convex objective function and a global constraint set. The network cost function is given by:

Here, $\mathbf{x}\in R^{k}$ is a global decision vector; $f_{i}:R^{k}\to R$ is the convex objective function of robot $i$ known only by robot $i$; $D$ is a bounded convex domain, which is (without loss of generality) characterized by an inequality constraint, i.e., $c(\mathbf{x})\leq 0$, where $c:R^{k}\to R$ is a convex constraint function known by all robots. In addition, to estimate the relative positioning of connected robots, we also calculate the probability of estimation $bel_{j}(\tilde{X})=p(\tilde{X}_{j}|\tilde{X}_{i},s_{i,j})$ for each relative position, where $\tilde{X}$ corresponds to the position estimation by $\mathbf{x}$, which will be used in the improvisation of the exploration grid. Technical and theoretical details of the proposed relative localization technique are explained in our previous work DGORL [2].

In the Rao-Blackwellization update procedure, according to [12], the proposal distribution is suboptimal, especially when the proprioceptive odometer measurements are less accurate than the position estimates through the relative location as mentioned in Section IV-B. Instead, we can use the persistent exploration grid map belief in Gaussian Process as discussed in Section IV-A to improve the localization accuracy and vice versa. Furthermore, in a Bayesian framework, we explicitly incorporate the exploration grid map belief into the measurement likelihood function:

According to the definition of posterior map belief, $b_{j-1}^{g^{[i]}}$ is a sufficient statistic for all previous positions $\tilde{X}_{1:j-1}$ and RSSI $s_{1:j-1}$, and $\psi$ is the belief update constant, the following expression holds:

Similarly, the belief $b_{j}^{g^{[i]}}$ is also a sufficient statistic for the current estimated position $\tilde{X}_{j}$ and the previous map belief $b_{j-1}^{g^{[i]}}$ , so we can get:

Thus Eq. 9 can be written as:

Now the weight of sample $j$ possesses the sample position for robot $i$ can be defined as:

Furthermore, for a sample approximating the position belief, a straightforward method to estimate pose uncertainty is to use all normalized weights in a discretized way as:

The primary goal of linearizing convex hull optimization is to maximize the coverage of the explorable space and distribute robots to unknown areas.

The method, as shown in Alg. 2, is a heuristic approach that starts by figuring out the convex hull of the observed map. Observing wall cells in the exploration map on the convex hull is used to forecast potential unobserved areas of the area. The probabilistic Hough line transformation, which uses a maximum likelihood estimation of a line via sparsely connected points, is used to find these unobserved sections of the region. It identifies unseen connections between observed wall segments. The map’s unseen areas are then expanded along the identified wall lines.

Each junction of these wall-line projections adds a projected corner (represented by a single cell, $c_{i,j}\in I_{o}$) to the exploration map. Limiting the distance of predicted corners from the closest observations is necessary to prevent nearly parallel lines from predicting corner locations that are unreasonably far from the observed space. The observed areas of the map and the corner points are then combined to form a new convex hull. This new convex hull provides an initial estimation of the boundaries of the exploration space. The cells inside the hull are initially anticipated to be free, while the cells along the hull are predicted to be occupied in the exploration map.

It is possible that the anticipated boundary, which is convex, overestimates the limits of the explorable space. However, this upbeat strategy boosts the payout in hazy places, increasing the motivation for their exploration. Although extending lines suggests that most buildings have a rectangular shape, this is consistent with many areas humans have created. While external borders are considered straight, the projected boundaries are not constrained to any orientation or polygons class, allowing the prediction to adapt to non-rectangular situations.

In the case of a non-linearize convex hull, projecting robots to specific unexplored regions would be non-optimal. To linearize a non-convex hull, we first need to find a convex hull that approximates the non-convex function. This can be done using techniques such as convex relaxation or piecewise linearization. Once we have a convex hull approximation, we can use the following linearization equation:

where $f(C)$ is the non-convex function we want to optimize, $\nabla f_{j}(C)$ are the convex functions that approximate $f(C)$, $a_{j}$ are non-negative coefficients, and $b$ is a constant that ensures the approximation is valid.

Upkeep on the projected map is done before moving on to the whole exploration. To account for unseen depth in barriers, occupied cells, $O_{o}\cup I_{o}$, are inflated into $I_{f}$; for example, when an obstacle is only visible from one side, it has no observable depth. User-specified inflation depth is determined by prior operating environment knowledge. Then, the map’s unreachable regions inside the expected perimeter are labeled as inferred obstacles.

We examine the temporal complexity for each observation using Algs. 1 and 2. The map resolution $\alpha_{M}$, the map size $n$, the maximum size of a convex hull $C$, the number of beams per scan $n_{z}$, the number of measurement $\alpha_{z}$, and the one of continuous map occupancy $\alpha_{m}$ for a map of size $m$ is used to define the complexity. We assume that the squared grid’s size is fixed, the query time is constant, and the data structure containing the OG map is identical to the GP data structure.

The proposed SEAL has two parallel processes: Gaussian process fusion for exploration belief estimation and Graph optimization for belief estimation of relative localization. Rao-Blackwellization will use both beliefs to update overall localization and map beliefs. The resulting time complexity of two parallel processes are $O(n^{2})$ and $O(\alpha^{-2}_{M}n_{z}\alpha^{-1}_{z}\alpha^{-1}_{m})$. It is worth noting that the time complexities of standard Log-Odds for generating OG mapping from GP are $O(n_{z}logn)$. Note that $n_{z}<n$ normally because of the limited sensing range and narrow beam angle. The time complexity of the proposed SEAL is, at worst, quadratic about the cone size and map resolution for belief update and GP computation, respectively. Importantly, the time cost of map belief update is quite similar to Log-Odds approach in a large scene because the independence of map size $n$ and the actual cone size is much smaller in cluttered environments. The time efficiency of SEAL calculation also depends on the linearization of convex hull optimization, which is $O(nlogn)$. Hence, the overall upper bound on the time complexity per robot for the proposed SEAL will be $O(n\times m)$, where m is the number of immediate (connected) neighbor robots.

This time complexity is significantly better than other methods, such as SLAM + RRT [9] and DRL [10], which have complexities $O\left(n\times\log(n)\right)$ and $O(n\times k)$, respectively, where $k$ is the number of temporal states, and $m<<k$.

Exploration: The exploration approach and the method put forward by RRT [9] and DRL [10] are compared with that of SEAL in the bookstore and house worlds, respectively, since they use the same environments so that we can obtain a fair comparison. Below are the used metrics for evaluation:

1. 1.

   Mapping Time: The amount of time spent in the mapping (a measure of how effective is the multi-robot exploration);

2. 2.

   Total Distance Traveled: This term refers to the cumulative path length of all robot’s trajectory, which is necessary for a multi-robot system to explore the entire map. The entire trajectory length gives an idea of the robot’s energy usage while subtly describing its investigation’s effectiveness;

3. 3.

   Exploration Percentage: The percentage of the generated map with time elapsed;

4. 4.

   Map SSIM: Structural similarity index measure of generated maps compared with ground truth map;

5. 5.

   CPU Utilization: The maximum percentage consumption of the processor of a robot throughout the robot’s trajectory;

6. 6.

   Memory Consumption (RAM): The maximum occupied memory by the robot throughout the trajectory;

7. 7.

   Communication payload (Data Size): The maximum size of shared data by a robot.

Localization: The localization approach used RRT-Exploration [9] (which used GMapping for SLAM and Map merging to merge maps and find relative localization between robots) and DGORL [2] (which only does relative localization) are compared in our experiments. We use the below metrics for evaluation:

1. 1.

   ATE (m): The absolute trajectory error computed for the whole navigated trajectory by each approach, which measured the deviation from the ground truth;

2. 2.

   ALE (m): The absolute localization error for the predicted location of each approach.

Comprehensive listing of all results are provided in Table I. To reduce measurement noise, we examined the exploration performance of each strategy after several trials. There are several simulations run in a three-robot system. Fig. 3 displays the mapping results of each approach made by the three robots in the simulated area. Fig. 3 also delineates the belief maps generated by SEAL, which are not provided by typical SLAM (or map merging) algorithms or other exploration approaches.

Note that the results consider the typical mapping time, the distance traveled, and the effectiveness of the mapping. Comparison with the original map is used to gauge mapping effectiveness and reported percentages are adjusted using gazebo world dimensions. Fig. 4 delineates the reduced traveled distance and achieves a high exploration percentage. It can be seen that the proposed SEAL traveled approximately half the distance and achieved a comparable exploration percentage. Furthermore, The exploration capabilities of RRT, DRL, and SEAL are thoroughly compared in Table I. These results suggest that SEAL is a more effective method for exploration activities, producing superior outcomes.

We also have analyzed the localization performance of SOTA RRT and our previous approach DGORL for localization and compared results with the proposed SEAL. Fig. 5 has shown the trajectory by each approach and localization error in terms of RMSE. Results show that our SEAL algorithm outperformed the RRT Exploration and DGORL, as it incorporates the map belief for location correction and achieved 25% higher localization accuracy than DGORL and 10% higher than RRT. To analyze the localization performance over iterations, Fig. 4 provides evidence for a gradual reduction in RMSE over robotic progression towards the maximum exploration. Additionally, Table I provided exact data showing the significant improvement of the absolute trajectory and localization errors by SEAL compared to the state-of-the-art approaches. Specifically, the proposed SEAL has 25% higher localization accuracy than RRT and DGORL.