Asynchronous and Parallel Distributed Pose Graph Optimization

A recent paper by members of the DCIST alliance has received a 2020 honorable mention from IEEE Robotics and Automation Letters.

The paper presents Asynchronous Stochastic Parallel Pose Graph Optimization (ASAPP), the first asynchronous algorithm for distributed pose graph optimization (PGO) in multi-robot simultaneous localization and mapping. By enabling robots to optimize their local trajectory estimates without synchronization, ASAPP offers resiliency against communication delays and alleviates the need to wait for stragglers in the network. Furthermore, ASAPP can be applied on the rank-restricted relaxations of PGO, a crucial class of non-convex Riemannian optimization problems that underlies recent breakthroughs on globally optimal PGO. Under bounded delay, the authors establish the global first-order convergence of ASAPP using a sufficiently small stepsize. The derived stepsize depends on the worst-case delay and inherent problem sparsity, and furthermore matches known result for synchronous algorithms when there is no delay. Numerical evaluations on simulated and real-world datasets demonstrate favorable performance compared to state-of-the-art synchronous approach, and show ASAPP’s resilience against a wide range of delays in practice.

Source: Yulun Tian, Alec Koppel, Amrit Singh Bedi, and Jonathan P. How, “Asynchronous and Parallel Distributed Pose Graph Optimization,” in IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 5819-5826, Oct. 2020.

More information:

Non-Monotone Energy-Aware Information Gathering for Heterogeneous Robot Teams

A recent paper by members of the DCIST alliance considers the problem of planning trajectories for a team of sensor-equipped robots to reduce uncertainty about a dynamical process. Optimizing the trade-off between information gain and energy cost (e.g., control effort, energy expenditure, distance travelled) is desirable but leads to a non-monotone objective function in the set of robot trajectories. Therefore, common multi-robot planning algorithms based on techniques such as coordinate descent lose their performance guarantees. Methods based on local search provide performance guarantees for optimizing a non-monotone submodular function, but require access to all robots’ trajectories, making it not suitable for distributed execution. This work proposes a distributed planning approach based on local search, and shows how to reduce its computation and communication requirements without sacrificing algorithm performance. The team demonstrates the efficacy of their proposed method by coordinating robot teams composed of both ground and aerial vehicles with different sensing and control profiles, and evaluate the algorithm’s performance in two target tracking scenarios. Results show up to 60% communication reduction and 80-92% computation reduction on average when coordinating up to 10 robots, while outperforming the coordinate descent based algorithm in achieving a desirable trade-off between sensing and energy expenditure.

Source: X. Cai, B. Schlotfeldt, K. Khosoussi, N. Atanasov, G.J. Pappas, J.P. How “Non-Monotone Energy-Aware Information Gathering for Heterogeneous Robot Teams”, IEEE Int. Conf. Robot. Autom. (ICRA), ArXiv preprint:, 2021.


Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping

A recent paper by members of the DCIST alliance develops an open-source C++ library for real-time metric- semantic visual-inertial Simultaneous Localization And Mapping (SLAM). The library goes beyond existing visual and visual-inertial SLAM libraries (e.g., ORB-SLAM, VINSMono, OKVIS, ROVIO) by enabling mesh reconstruction and semantic labeling in 3D. Kimera is designed with modularity in mind and has four key components: a visual-inertial odometry (VIO) module for fast and accurate state estimation, a robust pose graph optimizer for global trajectory estimation, a lightweight 3D mesher module for fast mesh reconstruction, and a dense 3D metric-semantic reconstruction module. The modules can be run in isolation or in combination, hence Kimera can easily fall back to a state-of-the-art VIO or a full SLAM system. Kimera runs in real-time on a CPU and produces a 3D metric-semantic mesh from semantically labeled images, which can be obtained by modern deep learning methods.

Source: A. Rosinol, M. Abate, Y. Chang, L. Carlone “Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping”, IEEE Int. Conf. Robot. Autom. (ICRA), ArXiv preprint:, 2020.

Open-source Code:

Task: RA1.A1 The Swarm’s Knowledge Base: Contextual Perceptual Representations

Points of Contact: Luca Carlone (PI), Antoni Rosinol.


Asymptotically Optimal Planning for Non-myopic Multi-Robot Information Gathering

A recent paper by members of the DCIST alliance develops a novel highly scalable sampling-based planning algorithm for multi-robot active information acquisition tasks in complex environments. Active information gathering scenarios include target localization and tracking, active Simultaneous Localization and Mapping (SLAM), surveillance, environmental monitoring and others. The goal is to compute control policies for mobile robot sensors which minimize the accumulated uncertainty of a dynamic hidden state over an a priori unknown horizon. To design optimal sensor policies, we propose a novel nonmyopic sampling-based approach that simultaneously explores both the robot motion space and the information space reachable by the sensors. We show that the proposed algorithm is probabilistically complete, asymptotically optimal, and convergences exponentially fast to the optimal solution. Moreover, we demonstrate that by biasing the sampling process towards regions that are expected to be informative, the proposed method can quickly compute sensor policies that achieve user-specified levels of uncertainty in large-scale estimation tasks that may involve large multi-robot teams, workspaces, and dimensions of the hidden state. We provide extensive simulation results that corroborate the theoretical analysis and show that the proposed algorithm can address large-scale estimation tasks.
Target localization and tracking scenario: Two robots with limited field-of-view (blue ellipses) navigate an environment with obstacles to localize and track six targets of interest. Target uncertainty is illustrated in red.
Source: Yiannis Kantaros, Brent Schlotfeldt, Nikolay Atanasov, and George J. Pappas: ‘Asymptotically Optimal Planning for Non-myopic Multi-Robot Information Gathering’ In Proceedings of the 2019 Robotics: Science and Systems (RSS), Freiburg, Germany, June 2019.
Points of Contact: George J. Pappas

Active Exploration in Signed Distance Fields

When performing tasks in unknown environments it is useful for a team of robots to have a good map of the area to assist in efficient, collision-free planning and navigation. A recent paper by members of the DCIST alliance tackles the problem of autonomous mapping of unknown environments using information theoretic metrics and signed distance field maps. Signed distance fields are discrete representations of environmental occupancy in which each cell of the environment stores a distance to the nearest obstacle surface, with negative distances indicating that the cell is within an obstacle. Such a representation has many benefits over the more traditional occupancy grid map including trivial collision checking, and easy extraction of mesh representations of the obstacle surfaces. The researchers use a truncated signed distance field, which only keeps track of cells near obstacle surfaces, and model each cell as a Gaussian random variable with an expected distance and a variance determined incrementally using a realistic RGB-D sensor noise model. The use of Gaussian random variables enables the closed form computation of Shannon mutual information between a Gaussian sensor measurement and the Gaussian cells it intersects. This allows for efficient evaluations of expected information when planning and evaluating possible future trajectories. Using these tools, a robot is able to efficiently evaluate a large number of trajectories before choosing the best next step to increase its information about the environment. The researchers show the resulting active exploration algorithm running on several simulated 2D environments of varying complexity. The figure shows a snapshot of the robot exploring the most complex of the three environments. These simulations can be viewed in more detail in the video linked below.

Points of Contact: Vijay Kumar (PI), Kelsey Saulnier.

Citation:  K. Saulnier., N. Atanasov, G. J.Pappas, & V. Kumar, “Information Theoretic Active Exploration in Signed Distance Fields,” IEEE International Conference on Robotics and Automation (ICRA), Paris, France, June 2020. (Accepted)

Learning Multi-Agent Policies from Observations

A recent paper from the DCIST team introduces a framework for learning to perform multi-robot missions by observing an expert system executing the same
mission. The expert system is a team of robots equipped with a library of controllers, each designed to solve a specific task. The expert system’s policy selects the controller necessary to successfully execute the mission at each time step, based on the states of the robots and the environment. The objective of the learning framework is to enable an un-trained team of robots (i.e., imitator system) — equipped with the same library of controllers but not the expert policy — to learn to execute the mission with performance comparable to that of the expert system. Based on un-annotated and noisy observations of the expert system, a multi-hypothesis filtering technique estimates the series of individual controllers executed by the expert policy. Then, the history of estimated controllers and environmental states provide supervision to train a neural network policy for the imitator system. When evaluated on a perimeter protection scenario, experimental results suggest that the learned policy endows the imitator system with performance comparable to that of the expert system.
Source: P. Pierpaoli, H. Ravichandar, N. Waytowich, A. Li, D. Asher, M. Egerstedt.  “Inferring and Learning Multi-Robot Policies from Observations”, International Conference on Intelligent Robots and Systems (IROS), 2020 – under review
Points of Contact: Pietro Pierpaoli; Harish Ravichandar {pietro.pierpaoli, harish.ravichandar}

Sim-to-(Multi)-Real: Transfer of Low-Level Robust Control Policies to Multiple Quadrotors

A recent paper by members of the DCIST alliance develops the use of reinforcement learning techniques to train policies in simulation that transfer remarkably well to multiple different physical quadrotors. Quadrotor stabilizing controllers often require careful, model-specific tuning for safe operation. The policies developed are low-level, i.e., they map the rotorcrafts’ state directly to the motor outputs. The trained control policies are very robust to external disturbances and can withstand harsh initial conditions such as throws. The work shows how different training methodologies (change of the cost function, modeling of noise, use of domain randomization) might affect flight performance. The is the first work that demonstrates that a simple neural network can learn a robust stabilizing low-level quadrotor controller (without the use of a stabilizing PD controller) that is shown to generalize to multiple quadrotors.

Project page:

Sim-to-(Multi)-Real: Transfer of Low-Level Robust Control Policies to Multiple Quadrotors
A.Molchanov, T. Chen, W. Hönig, J. A.Preiss, N. Ayanian, G. S. Sukhatme
IEEE/RSJ International Conference on Robots and Systems (IROS) 2019

DCIST Task: RA3.A1 Robust Adaptive Machine Learning
Contact: Gaurav Sukhatme

Planning with Uncertain Specifications (PUnS)

Consider the task of setting a dinner table. It involves placing the appropriate serving utensils and silverware according to the dishes being served. Some of the objects need to be placed in a particular order as they might be stacked on top of each other or due to cultural traditions. Many real-world tasks demonstrate such temporal characteristics that can succinctly modeled with linear temporal logic (LTL).

However defining sound and complete task specifications in LTL is non-trivial, and it remains desirable to allow robots to infer the task specifications through intuitive modalities like demonstrations, spoken instructions or corrections. Thus specifications for a task can at best be stated as a belief over candidate LTL formulas. An ideal learner would try to simultaneously satisfy as many of these candidate properties as possible while performing the task.

We tackle this problem in this work, where we introduces a novel problem formulation for planning with uncertain specifications (PUnS). To do that we first identify four evaluation criteria than capture the semantics of satisfying a belief over LTL formulas. These include:

  • Most likely: Here only the formula with the highest probability in the belief distribution is satisfied, the other formulas are ignored.
  • Maximum coverage: The largest set of unique formulas in the belief distribution is satisfied.
  • Minimum regret: The hypothesis averaged satisfaction of the belief distribution is maximised.
  • Chance constrained: The maximum probability of failure is determined by the user and the distribution is modified to only capture a set of formulas that have a cumulative probability larger than the failure probability.

We demonstrate that the choice of evaluation criterion results in a trade-off between fliexibility in task execution and risk-aversion. Finally we also demonstrate that every instance of a PUnS problem can be compiled into an equivalent reward machine. This is an equivalent Markov decision process (MDP) for the original non-Markov PUnS problem.

We demonstrate our technique on the table-setting task where the task specifications were inferred from 30 demonstrations, and the policy computed using the PUnS formulation sets the table with a very low error rate (0.001%).

Source: A. Shah, S. Li, J. Shah, ”Planning With Uncertain Specifications (PUnS),” IEEE International Conference on Robotics and Automation (ICRA), Paris, France, June 2020 (accepted).

Points of Contact: Julie Shah (PI) and Ankit Shah

Synthesis of a Time-Varying Communication Network by Robot Teams with Information Propagation Guarantees

A recent paper by Xi Yu and M. Ani Hsieh from the University of Pennsylvania presents a distributed control and coordination strategy that allows a swarm of mobile robots to form an intermittently connected communication network as they monitor a given environment. The approach assumes robots are tasked to patrol a set of perimeters in a region of interest and are only able to communicate with one another when they move into each other’s communication range as they monitor their assigned perimeters. The work shows how intermittent connectivity can be achieved as each robot synchronizes its speed with other robots moving along neighboring perimeters. As robots synchronize with one another, they effectively ensure future pair-wise rendezvous with robots on neighboring perimeters.  This enables the team to form a time-varying communication network where information can be successfully transmitted between any pair of robots within some finite time period.  Additionally, the approach provides bounds on the time needed to propagate information throughout the network. The feasibility of the strategy and the validity of the approach is validated in simulation.

Fig. 1.  An overhead view of an urban environment (left) where robots are tasked to patrol perimeters around different buildings.  As robots move into each other’s communication range (robots on perimeters 7 and 8 and robots on perimeters 11 and 12 in the right figure), information can be exchanged.  In both figures, blue robots are robots who have obtained a new piece of information from a robot on a neighboring perimeter while red robots are robots who have not received the information.  As robots rendezvous with each other, the information will eventually propagate throughout the entire network.

Learning Decentralized Controllers with Graph Neural Networks

A recent paper by members of the DCIST alliance develops a method for distributed control of large networks of mobile robots with interacting dynamics and sparsely available communications. Their approach is to learn local controllers that require only local information and communications at test time by imitating the policy of centralized controllers using global information at training time. By extending aggregation graph neural networks to time varying signals and time varying network support, they learn a single common local controller which exploits information from distant teammates using only local communication interchanges. The researchers apply this approach to the problem of flocking to demonstrate performance on communication graphs that change as the robots move. They examine how a decreasing communication radius and faster velocities increase the value of multi-hop information.

Task: RA1.C1 Joint Resource Allocation in Perception-Action-Communication Loops
Points of Contact: Alejandro Ribeiro (PI) and Ekaterina Tolstaya



Citation: E. Tolstaya, F. Gama, J. Paulos, G. Pappas, V. Kumar, A. Ribeiro “Learning Decentralized Controllers for Robot Swarms with Graph Neural Networks.” Conference on Robot Learning, October 2019.