Exploration, Mapping and Scalar Field Estimation using a Swarm of Resource-Constrained Robots

Ragesh Kumar Ramachandran

Advisor : Dr Spring Berman

Autonomous Collective Systems Laboratory

Motivation

• Development of robot platforms that can be deployed in swarms to perform tasks autonomously over large spatial and temporal scales.

• Many potential applications for robotic swarms, including exploration, environmental monitoring, disaster response, search-and-rescue, mining, and intelligence-surveillance-reconnaissance, will require to operate in dynamic, uncertain and GPS denied environments

• Despite these limitations, it may be necessary for the swarm to characterize its surroundings, for instance to map obstacles, payloads for transport, or hazardous areas to avoid.

• Our current research progress and future work focus on novel estimation and mapping strategies using robotic swarms in these types of applications.

Overview

• An Advection-Diffusion-Reaction Based Approach to Mapping an Environmental Feature.

• A Probabilistic Topological Approach to Quantifying Environmental Features.

• A Probabilistic Approach to Constructing Topological Maps of an Environment.

• Metric Map Construction using a Stochastic Robotic Swarm Leveraging Received Signal Strength.

• Distributed mapping and information theoretic exploration.

• Scalar Field Estimation by Large Networks with Partially Accessible Measurements.

• Ongoing work

Problem Statement

Mapping a single feature of interest using a robotic swarm without communication or localization capabilities.

• Robotic swarms with stochastic behaviors.

• Macroscopic abstraction using partial differential equations

• Mapping formulated as an optimal control problem.

• The most common approach in robotics is SLAM and probabilistic mapping.

• Mapping an environment using a robotic swarm is a relatively new area of research in the robotics.

• In Dirafzoon et al.(2013)a robotic swarm is used to identify the topological features of an environment from information about the times at which robots encounter other robots and environmental features.

Robot State Changes (Chemical Reactions)

No-flux Boundary Conditions : $\vec{n} \cdot (D\nabla u - \mathbf{v}(t)u) = 0 ~~~ on ~~\partial \Omega \times [0,T] $

Initial Condition : Gaussian density centered at a point $\mathbf{x}_0 \in \Omega$

u(x,t) := Active robot density, D := Diffusion coefficient, $\mathbf{v}(t)$ := Velocity field

k := Reaction rate constant, $K(\mathbf{x})$ := Feature Indicator Function (Map)

Objective Function

Admissible control inputs

With the macroscopic model as Constraint equations

The optimization problem is solved using a gradient descent approach

$\Psi_i(u_i,K)$ is the $i^{th}$ constraint and $p_i$ is the adjoint variable.

Gradient Equation

$$\mathbf{J}' = \sum_{i = 1}^{N} (\int_0^T(ku_ip_i)(\mathbf{x})dt) + \lambda K(\mathbf{x})$$

$p_i$ is computed by solving the adjoint equation.

The equations are solved backwards

Related Work

• Our previous work, Ramachandran et al.(2015) uses a PDE based model of the swarm to solve the problem. But requires large number of robots.

• Ramaithitima et al.(2016) propose an algorithm that covers the free space of the environment with robots and then constructs an approximate generalized Voronoi graph. But requires communication.

• In Dirafzoon et al.(2015) persistent homology is applied on a point cloud of the domain’s free region based on encounter time with robots. But each robot requires identification label.

Current focus

As a first step to solve this problem, we quantify the topological features (holes or obstacles) in the domain.

Types of Scenarios

Type I: Robots receive accurate estimates of their global positions when they are close to the boundary of the domain.

Type II: Robots do not receive global position updates anywhere in the domain.

Robot Motion Model

The displacement of a robot over a time step $\Delta t$ is given by: $$\mathbf{X}(t + \Delta t) = \mathbf{X}(t) + \mathbf{V}(t)\Delta t + \mathbf{W}(t)$$

Where,

$$\mathbf{V}(t) = [v_x(t),\ v_y(t)]^T = [v\ \cos(\theta(t)), \ v\ \sin(\theta(t))]^T$$ $$\mathbf{X}(t) = [x(t),\ y(t)]^T$$ $\mathbf{W}(t) \in \mathbb{R}^2$ is a zero mean Gaussian random vector

Robot Data

Data that robot $j \in \{1,...,N\}$ obtains at time $t_k \in [0, T]$, $k \in \{1,...,K\}$, consists of the element $d^j_k = \{\mu^j_k, \Sigma^j_k\}$.

$\mu^j_k \in \mathbb{R}^{2}$ is the mean of the robot's estimate of its $(x,y)$ position.

$\Sigma^j_k \in \mathbb{R}^{2\times2}$ is the covariance matrix of its position estimate.

Both at time $t_k$

Processing the Data

• Discretize the domain into a high-resolution uniform grid. $m_i$ denotes a grid cell.

• Use the robots' data to assign each grid cell $m_i$ a probability $p^f_i$ of being free.

• Integrate the Gaussian distribution with mean $\mu^j_k$ and covariance matrix $\Sigma^j_k$ over the cell region to obtain $p^f_i$.

Processing the Data

• Assign a score $s_i \in [0, \infty)$ to each grid cell $m_i$. $s_i = \frac{1}{\left | P_i \right |}\sum _{p \in P_i} \log\left ( \frac{1}{1 - p} \right )$. Where $P_i = \{p_{ijk} ~|~ p_{ijk} > \rho, \rho > 0\}$ and $p_{ijk}$ is computed using the $k^{th}$ data element of the $j^{th}$ robot.

• Finally, $p^f_i$ of each grid cell being free is computed as $p_{i}^{f} ~=~ 1 - (\exp(s_i))^{-1}$.

Finding the Number of Features or Obstacles

• Point cloud is obtained by sampling a dense, uniformly random set of points from the domain based on a probability map of the domain and rejecting those points that are inside a grid cell whose $p^f_i$ is below a given threshold.

• Landmark points are selected from the point cloud using the sequential max-min algorithm.

• Finally, a Rips complex based filtration is constructed from landmark points and barcode diagrams are extracted.

• The procedure was tested for Type I & Type II scenarios.

Rips complex based filtration

Background (Algebraic Topology)

• vectors $v_0, v_1, ..., v_k \in \mathbb{R}^n$ are affinely independent if the vectors $ v_1 - v_0, ..., v_k - v_0 $ are linearly independent.

• A $k$-simplex $\sigma \subset \mathbb{R}^n$ can be defined as the convex hull of $k+1$ affinely independent vectors $\{v_0, v_1, ..., v_k\}$, called vertices. $\sigma = [v_0, v_1, ..., v_k]$

• A face $\tau$ is the convex hull of a subset of $\{v_0, v_1, ..., v_k\}$

• A simplicial complex $\kappa$ is defined as a finite collection of simplices $\sigma$.

Illustration

Background continued...

• Rips complex is formed by choosing a parameter $\delta > 0$ and adding a $k$-simplex to the simplicial complex if every vertex in the simplex is within a distance $\delta$ from every other

• If $f: \kappa \to \mathbb{R} $ is a function such that $\tau \leq \sigma$ implies that $f(\tau) \leq f(\sigma)$, then $f^{-1}((-\infty, \delta])$ is a simplicial complex denoted by $\kappa_\delta$ and $\delta_1 \leq \delta_2$ implies that $\kappa_{\delta_1} \subseteq \kappa_{\delta_2}$, yielding a filtration of simplicial complexes with $\delta$ as the filtration parameter.

• A barcode diagram is a graphical representation used to identify the persistent topological features

Rips complex based filtration with barcode

Effect of varying time of deployment on estimation

Effect of varying number of robots on estimation

Experiments on TYPE II environment

Experimental Setups

An Experimental Illustration

Barcode

Experimental Validation of Estimation

Extension of the Previous work

What more can be done?

• How to find an automated procedure to threshold the density function in order to isolate the obstacle region?

• Can we build a topological map of the domain?

Topological Map Illustration

¹Taken from Dr.Markus Haid blog

Compute Threshold And Identify Number Of Obstacles

• A filtration is generated by the thresholding the probability map and constructing a simplicial complex at each level and choosing $\delta$ = 1 − threshold ($\alpha$) as the filtration parameter

• The barcode generated out of this filtration can be used to compute optimal threshold and identify number of obstacle.

Obstacle Segmentation

• The grid cells with $p^f_i < \alpha_{opt}$ belong to an obstacle.

• Classify the grid cells in each obstacle and identify its boundary.

• A graph is constructed with obstacle grid cell centers as vertices and the cells are classified by performing a BFS on this graph

• The boundaries of each obstacle are identified as the elements in graph that belong to the same obstacle and have fewer than four neighbors.

Obstacle Segmentation Illustration

Topological Map Generation

• A wave propagation algorithm is used to generate the approximate Generalized Voronoi Diagram(AGVD).

• A graph is constructed in the same manner as the obstacle graph.

• The algorithm is a modified form of Dijkstra's algorithm.

• The algorithm outputs the set of vertices which constitutes the topological map of the domain in the form of a discrete AGVD.

Topological Map Results

Topological Map Results

Experimental Illustration

¹Sean Wilson et al., IEEE R-AL, July 2016

Computational Complexity

• Probability map generation: $\mathcal{O}(NKM)$, where $N$ robots each collect $K$ items of data over a domain that is discretized into $M$ grid cells.

• Simplicial complex construction and barcode generation : $\mathcal{O}(M^{2.372})$, but for most practical applications it is close to $\mathcal{O}(M)$.

• Obstacle segmentation : $\mathcal{O}(M)$.

• AGVD extraction using the wave propagation algorithm :$\mathcal{O}(M\log{M})$.

• Worst case complexity of the approach : $\mathcal{O}(M^{2.372})$.

Problem Statement

We consider the problem of generating a metric map of a bounded, unknown, GPS-denied 2D environment using data collected by a swarm of N robots.

Extension of the Previous work

• A robots are equipped with receivers to receive broad casted signals(wifi, bluetooth).

• A mapping procedure is similar to the previous one, which is computing $p^f_i$ for each grid cell.

• We prove the completeness of the mapping approach.

Updated Measurement Model

$\mathbf{S}(\mathbf{X}(t)) = [S_1(\mathbf{X}(t)), ~...,~ S_l(\mathbf{X}(t))]^T$, $l$ number of transmitters(here $l = 2$).

$S_i(\mathbf{X}(t)) = K_i Pow_i ||\mathbf{X}(t) - \mathbf{X}_i||^{-\alpha}$, where $\alpha \in [0.1, 2]$, $Pow_i$ is the transmitted signal voltage of transmitter $i$

$\mathbf{N}_S(t) \in \mathbb{R}^l$ and $\mathbf{N}_V(t) \in \mathbb{R}^2$ as vectors of independent, zero-mean normal random variables.

Extended Kalman filter used.

Completeness of the Approach

Finally, we prove the existence of a threshold value of $p_{i}^{f}$ that distinguishes the free grid cells from the occupied cells.

Large Complex domain

Complex domain of size $20 m \times 20 m$, in which $N = 200$ robots were deployed for $T = 1200 s$

Problem Statement

We consider the problem of distributed mapping of a bounded, unknown, 2D environment using a swarm of N robots with localization capabilities.

We also formulate an information theoretic approach for exploring an unknown environment using the robotic swarm.

Information correlated Lévy walk exploration

• An exploration strategy which combines information theoretic ideas with Lévy walks

• Rather than moving in random directions as in Lévy walk direct the robots in direction with maximum information gain.

• We use Mutual information between the distance measurement and estimated map as the metric to measure information gain.

• The robots explore the unknown domain using our exploration strategy.

• Simultaneously, each robot updates its occupancy grid map based on its laser range sensor measurements.

• The robots' also update their map based on the map information from their neighbors.

Environments

Coverage

We consider the problem of reconstructing a two-dimensional scalar field using measurements from a subset of a network with local communication between nodes.

The main focus of the research is a derivation of bounds on the trace of the observability Gramian of the system, which can be used to quantify and compare the field estimation capabilities of chain and grid networks.

A comparison based on a performance measure related to the $H^2$ norm of the system is also used to study the robustness of the network topologies.

• Solution to estimation problem is associated with the observability of the system.

• Little work on estimation of initial conditions other than through the inversion of the observability Gramian

• Checking the rank condition for large interconnected systems is computationally ill-conditioned due to the high dimensionality of the observability Gramian.

• A graph-theoretic characterization of observability has been more widely used than a matrix-theoretic characterization for large complex networked systems.

• The observability of complex networks is studied in Yang-Yu Liu et al.(2013) using structural observability, which presents a general result that holds true for most of the chosen network parameters (the edge weights).

• In Meng Ji et al.(2007), a graph-theoretic approach based on equitable partitions of graphs is used to derive necessary conditions for observability of networks

• Alternately, Zhengzhong Yuan et al.(2013) uses a matrix-theoretic approach to develop a maximum multiplicity theory to characterize the exact controllability of a network based on the network spectrum.

• Consider a set of N nodes with local communication ranges and local sensing capabilities.

• Each node is capable of measuring the value of a scalar field at its location and communicating this value to its neighbors

• The nodes take measurements at some initial time and transmit this information using a nearest-neighbor averaging rule

• The problem is to reconstruct the initial measurements taken by all the nodes from the measurements of a subset of accessible nodes.

We define the information flow dynamics of node $i$ as: $$\frac{dx_i}{dt} = \sum_{(i, j) \in \mathcal{N}_i } (x_j-x_i)$$ $\mathcal{N}_i$ denote the set of neighbors of node $i$.

The information flow dynamics over the entire network becomes : $$\dot{\mathbf{X}}(t) = - \mathbf{L}(\mathcal{G}) \mathbf{X}(t), \mathbf{Y}(t) = \mathbf{C} \mathbf{X}(t)$$ $$\mathbf{X}(0) = \mathbf{X}_0.$$ $\mathbf{L}(\mathcal{G})$ denote the Laplacian of the graph. $\mathbf{Y}(t)$ is the measurement vector at time $t$.

We solve the scalar field reconstruction problem by posing it as an optimization problem. We frame our optimization objective as the computation of $ \mathbf{X}_0$ that minimizes the functional $J(\mathbf{X}_0)$, defined as :

subject to the constraint of the system dynamics. $\hat{\mathbf{Y}}(t)$ is the observed data.

The convexity of $J(\mathbf{X}_0)$ ensures the convergence of gradient descent methods to its global minima.

The gradient can be computed as :

We find $P(T)$ by solving following equation forward :

Where $\hat{u}(\tau) \equiv \mathbf{Y}(T - \tau) - \hat{\mathbf{Y}}(T - \tau)$

$\mathbf{L}^*(\mathcal{G})$ and $\mathbf{C}^*$ are transposes of $\mathbf{L}(\mathcal{G})$ and $\mathbf{C}$ respectively

Ragesh K. Ramachandran, Karthik Elamvazhuthi, and Spring Berman. An optimal control approach to mapping GPS-denied environments using a stochastic robotic swarm. In Int’l. Symp. on Robotics Research (ISRR), 2015.

Ragesh K. Ramachandran, Sean Wilson, and Spring Berman. A probabilistic topological approach to feature identification using a stochastic robotic swarm. In To appear in the Int’l. Symposium on Distributed Autonomous Robotic Systems (DARS), 2016.

Ragesh K. Ramachandran, Sean. Wilson, and Spring. Berman. A probabilistic approach to automated construction of topological maps using a stochastic robotic swarm. IEEE Robotics and Automation Letters, 2(2):616–623, April 2017.

Ragesh K. Ramachandran and Spring Berman. The effect of communication topology on scalar field estimation by large networks with partially accessible measurements. In Proceedings of the 2017 American Control Conference (ACC), Seattle, WA, USA, May 24–26, 2017.

Ragesh K. Ramachandran and Spring Berman. Automated Construction of Metric Maps using a Stochastic Robotic Swarm Leveraging Received Signal Strength. In revision for IEEE Transactions on Robotics, 2018.

Ragesh K. Ramachandran, Zahi Kakish, and Spring Berman. Information correlated lévy walk exploration and distributed mapping using a swarm of robots. In preparation for IEEE Transactions on Robotics, 2018.