Distributed spatial awareness for robot swarms
Simon Jones, Sabine Hauert
TL;DR
This paper introduces a system for robot swarms to develop shared spatial awareness using local sensing and communication.
Contribution
A novel distributed spatial awareness system using Gaussian belief propagation and continuous movement for robot swarms.
Findings
The system builds a global reference frame with low bandwidth and computation.
Two example swarm algorithms were demonstrated in simulation.
The approach showed reliable performance on real robots with imperfect sensing.
Abstract
Building a distributed spatial awareness within a swarm of locally sensing and communicating robots enables new swarm algorithms. We use local observations by robots of each other and Gaussian belief propagation message passing combined with continuous swarm movement to build a global and distributed swarm-centric frame of reference. With low bandwidth and computation requirements, this shared reference frame allows new swarm algorithms. We characterise the system in simulation and demonstrate two example algorithms, then demonstrate reliable performance on real robots with imperfect sensing.
Genes, proteins, chemicals, diseases, species, mutations and cell lines named across the full text — each resolved to its canonical identifier and authoritative record.
Click any figure to enlarge with its caption.
Figure 10
Figure 11
Figure 1
Figure 2
Figure 3
Figure 4
Figure 5
Figure 6
Figure 7
Figure 8
Figure 9
Figure 12
Figure 13
Figure 14
Figure 15
Figure 16- —https://doi.org/10.13039/100014013UK Research and Innovation
- —https://doi.org/10.13039/100019180HORIZON EUROPE European Research Council
Peer Reviews
No public reviews on file for this paper yet. If you reviewed it on a platform where reviews are public (OpenReview, ICLR, NeurIPS, ICML), you can paste yours below so the community can read it here.
Videos
No videos yet. Explain this paper in a talk, walkthrough, or lecture? Add one.
Taxonomy
TopicsDistributed Control Multi-Agent Systems · Insect Pheromone Research and Control · Distributed Sensor Networks and Detection Algorithms
Introduction
Swarm robotics, inspired by swarms in nature, has the potential for resilient, robust, and redundant solutions to a wide range of problems such as mapping, logistics, search and rescue, disaster recovery, and environmental monitoring. Many relatively simple and cheap robots, each following simple rules, with local interactions between themselves and the environment are capable of producing a desired emergent swarm-level behaviour (Schranz et al., 2020; Jones et al., 2016; Crosscombe et al., 2017; Birattari et al., 2019; Tzoumas et al., 2023; Jones et al., 2019).
There are many areas of swarm algorithm design where access to global information would be useful, but unless that information is inferred or constructed through purely local interactions, it does not fit within the distributed swarm paradigm. One example is spatial awareness, by which we mean that an agent within the swarm is aware of its own location with respect to the swarm as a whole; the swarm shares a spatial reference frame. The availability of a completely distributed, completely local, low cost, shared reference frame would open up algorithmic approaches previously not possible. By using Gaussian belief propagation, we can construct this within a swarm of robots based only on local observation and messaging. Robots move around, constructing a distributed, size-limited factor graph of observations of other robots and odometry information. Message passing within and between neighbouring robots results in convergence on a shared frame of reference; each robot knows where it is in relation to it. To demonstrate the potential of this, we focus on two applications, shape formation and logistics, and follow up with application to a swarm of real robots.
Swarm shape formation is a proxy for a number of real-world problems such as search and rescue or emergency communication. Many problems rely on the swarm maintaining a coherent shape or coverage of particular areas. This has been tackled in swarm robotics in a number of ways which in their essence involve the construction of a frame of reference, or coordinate system. These systems often rely on robots transitioning to a static state to serve as anchors for further extensions to the shape and coordinate system, or unrealistic assumptions of position knowledge.
The use of swarms for intralogistics is an emerging area, where perhaps we can move beyond the lab into real-world applications. With our DOTS (Jones et al., 2022) robots we aim to demonstrate a simple but functional application. Specifically, we consider the potential for small scale out-of-the-box solutions for everyday environments; simply delimit an area of floor and add robots and small cargo carriers. Users would download an app to their phone and use this to call for a carrier to deposit an item. Via Bluetooth, any robot within range would respond and provide the carrier and take it and the item to be stored. To retrieve the item, the user would use the app again, robots would talk with their neighbours until a robot with recent knowledge of the item heard, which would pick up the carrier and take it to the user. Even simple random walk algorithms are capable of effective retrieval in logistics applications.
We describe the implementation of a system, which we call Distributed Spatial Awareness (DSA), to provide a completely local and distributed shared frame of reference. We characterise its performance in simulation, examine the trade-offs with computational and communication cost, and to demonstrate it, we show simple but effective shape formation, and enhanced knowledge awareness within an intralogistics application. We then implement DSA on a small swarm of real DOTS robots. This paper is organised as follows, in the next section, we look at the background, Sect. 3 covers methods, Sect. 4 analyses and discusses the results, and Sect. 5 concludes.
This paper was originally presented at DARS’24. It has been extended with new material describing the implementation of DSA on a swarm of real robots and the related results.
Background
Gaussian belief propagation
Gaussian belief propagation (GBP) is a method of performing distributed iterative probabilistic inference or state estimation on a graph of relations between Gaussian variables by means of message passing. It is not new (Pearl, 1982; Weiss & Freeman, 1999; Bickson et al., 2008) but has received recent attention, with convergence guarantees (Su & Wu, 2015; Du et al., 2018) and applicability to distributed systems (Davison & Ortiz, 2019; Ortiz et al., 2020; Murai et al., 2024; Patwardhan et al., 2023). A widely used way to represent this inference problem is a factor graph (Dellaert et al., 2017); a bipartite graph consisting of nodes which are either variables or factors. Variables are quantities we wish to estimate or infer and factors, which connect to one or more variables, represent constraints on those connected variables, for example derived from some measurement of the environment.
Performing inference on a factor graph is well studied and at the heart of many robotics estimation problems. Solver libraries such as Ceres (Agarwal et al., 2023) and GTSAM (Dellaert & Contributors, 2022) are highly performant, but rely on centralised representation and processing. Davison and Ortiz (2019) make the argument that the completely decentralised and incremental processing of GBP is a better fit for future systems as processing power becomes more distributed. This naturally fits with the swarm robotics paradigm, and allows the inference of global properties by using purely local interactions.
Related consensus-based schemes achieve agreement via weighted averaging over time-varying neighbourhood graphs, Olfati-Saber et al. (2007); Jadbabaie et al. (2003). These methods update agent state directly and typically either presuppose a shared coordinate frame so state averaging is meaningful, or operate purely on a local relative basis without constructing a global frame.Table 1. Notation used throughoutSymbolType/unitsMeaning \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {G}=(\mathcal {X},\mathcal {F},\mathcal {E})$$\end{document} Factor graphVariables \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {X}$$\end{document} , factors \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {F}$$\end{document} , edges \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {E}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {x}_i$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathbb {R}^2$$\end{document} State variable (pose) in the swarm-centric frame \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$b(\vec {x})$$\end{document} GaussianBelief at x; by default in canonical parameters \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$(\eta ,\Lambda )_G$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$m_{x\rightarrow f}$$\end{document} , \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$m_{f\rightarrow x}$$\end{document} GaussianVariable-to-factor and factor-to-variable GBP messages in \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$(\eta ,\Lambda )_G$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$z=(\eta ,\Lambda )_G$$\end{document} Gaussian paramsFactor parameters (prior or measurement) in canonical form \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$h_k(\vec {x}_{k_1},\vec {x}_{k_2})$$\end{document} MapLinear relation for binary factors; here \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$h_k=\vec {x}_{k_2}-\vec {x}_{k_1}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mu \in \mathbb {R}^2$$\end{document} , \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\Sigma \in \mathbb {R}^{2\times 2}$$\end{document} Mean, covarianceMoments parameters \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\eta \in \mathbb {R}^2$$\end{document} , \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\Lambda \in \mathbb {R}^{2\times 2}$$\end{document} Info, precisionCanonical parameters; \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\Lambda =\Sigma ^{-1}$$\end{document} , \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\eta =\Lambda \mu $$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\alpha _p$$\end{document} ScalarPrecision weighting used in Eq. (4) \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{damp}}\in [0,1]$$\end{document} ScalarMessage damping coefficient in Eqs. (8) and (9)Vectors are columns; Gaussians use moments form \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {N}(x;\mu ,\Sigma )$$\end{document} or canonical form \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {N}^{-1}(x;\eta ,\Lambda )$$\end{document} with \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$(\eta ,\Lambda )_G \equiv (\mu ,\Sigma )_G$$\end{document}
In this work, we use factor graphs; a bipartite graph \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {G}=(\mathcal {X},\mathcal {F},\mathcal {E})$$\end{document} of variables \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {X}$$\end{document} and factors \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {F}$$\end{document} connected by edges \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {E}$$\end{document} . The notation used is summarised in Table 1. We use only linear 2D state representations, and only factors of one or two variables. A Gaussian variable \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {x}$$\end{document} in the moments form \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {N}(\vec {x};\mu ,\Sigma )$$\end{document} can also be represented in the canonical/information form \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathcal {N}^{-1}(\vec {x};\eta ,\Lambda )$$\end{document} , connected by the identities: \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\Lambda =\Sigma ^{-1},\eta =\Lambda \mu $$\end{document} , where \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\Lambda $$\end{document} is the precision matrix and \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\eta $$\end{document} the information vector. Hereafter, we use the compact notation \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$(\eta ,\Lambda )_G\equiv (\mu ,\Sigma )_G$$\end{document} to express a Gaussian parameterisation; unless otherwise stated, beliefs and messages are given in canonical form. Unary factors \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$f_j(\vec {x}_j)$$\end{document} connects to a single variable and specifies a prior, defined by the Gaussian constraint \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {z}_j=(\eta _j,\Lambda _j)_G$$\end{document} . Binary measurement factors \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$f_k(\vec {x}_{k1},\vec {x}_{k2})$$\end{document} connects two variables \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {x}_{k1}, \vec {x}_{k2}$$\end{document} and specifies linear relationship relation \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {h}_k(\vec {x}_{k1}, \vec {x}_{k2})=\vec {x}_{k2}-\vec {x}_{k1}$$\end{document} , with Gaussian parameters \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {z}_k=(\eta _k,\Lambda _k)_G$$\end{document} . As described in more detail in Davison and Ortiz (2019), Ortiz et al. (2021), the standard GBP algorithm requires three steps: factor-to-variable message \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$m_{f\rightarrow x}$$\end{document} , variable-to-factor-message \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$m_{x\rightarrow f}$$\end{document} , and belief update b(x). All messages are in the form \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$(\eta ,\Lambda )_G$$\end{document} . The schedule of operations, and nodes upon which the operations take place, can be entirely arbitrary and asynchronous, though as noted in Ortiz et al. (2021) convergence time is affected by the form of the schedule.
Belief update: A variable \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {x}_i$$\end{document} has its belief updated as the product of messages from all connected factors. In canonical form, this is expressed as a sum:
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} b(\vec {x}_i)=\left( \sum _{f\in n(\vec {x}_i)}{\eta }_{f\rightarrow \vec {x}},\sum _{f\in n(\vec {x}_i)}{\Lambda _{f\rightarrow \vec {x}}}\right) _G \end{aligned}$$\end{document}where \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$n(\vec {x}_i)$$\end{document} are all the factors connected to \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {x}_i$$\end{document} .
Variable-to-factor message: The message to a connected factor is the product of the incoming messages from all other connected factors:
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} m_{\vec {x}\rightarrow f_j}=\left( \sum _{f\in n(\vec {x}_i)\setminus f_j}{\eta }_{f\rightarrow \vec {x}},\sum _{f\in n(\vec {x}_i)\setminus f_j}{\Lambda _{f\rightarrow \vec {x}}}\right) _G \end{aligned}$$\end{document}Factor-to-variable message: For a single variable factor, this is simply the factor.
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} f(\vec {x}):m_{f\rightarrow \vec {x}}&= \vec {z} \end{aligned}$$\end{document}For a two variable measurement factor, this is the product of the factor and the message from the other variable, marginalising out the other variable. Alternatively, the message to a variable is the precision weighted sum of the message from the other variable and the measurement vector \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {z}$$\end{document} .
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} \text {Let }\alpha _{p}&= \frac{\Lambda _f}{\Lambda _f+\Lambda _p}, \bar{\alpha }_p=1-\alpha _p \end{aligned}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} f(\vec {x}_i,\vec {x}_j):m_{f\rightarrow \vec {x}_i}&= \bigl ((\bar{\alpha }_{\vec {x}_j}\eta _f+\alpha _{\vec {x}_j}\eta _{\vec {x}_j}),\alpha _{\vec {x}_j}\Lambda _{\vec {x}_j}\bigr ) \end{aligned}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} f(\vec {x}_i,\vec {x}_j):m_{f\rightarrow \vec {x}_j}&= \bigl ((\bar{\alpha }_{\vec {x}_i}\eta _f+\alpha _{\vec {x}_i}\eta _{\vec {x}_i}),\alpha _{\vec {x}_i}\Lambda _{\vec {x}_i}\bigr ) \end{aligned}$$\end{document}One modification we make to the original algorithm is to note that:
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} m_{x\rightarrow f_j} = b(\vec {x}_i) - m_{f_j\rightarrow x} \end{aligned}$$\end{document}Variable nodes just calculate their beliefs and send them as messages, and factor nodes locally calculate what the variable-to-factor message would have been by subtracting the last message sent to that variable. This minimises the non-local knowledge needed in any node and more fairly distributes computation around very connected nodes.
Message damping As noted in Su and Wu (2015), message damping often improves convergence. We only damp messages from factors to variables, such that the message components \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$(\eta ,\Lambda )$$\end{document} are replaced:
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} \eta '_{t+1}&= (1-r_{\textrm{damp}})\eta _{t+1} + r_{\textrm{damp}}\eta _{t} \end{aligned}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} \Lambda '_{t+1}&= (1-r_{\textrm{damp}})\Lambda _{t+1} + r_{\textrm{damp}}\Lambda _{t} \end{aligned}$$\end{document}Throughout this work, we use a damping factor of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{damp}}=0.8$$\end{document} , arrived at empirically during initial experimentation as a compromise between slow convergence and a tendency to oscillation.
Shape formation
Shape or pattern formation in robot swarms is widely studied. The use of a swarm to perform a task is often implicitly or explicitly dependent upon that swarm maintaining a particular shape or covering a particular area. Often, a necessary part of forming a shape is the use of simpler behaviours such as dispersion, aggregation, alignment. Notably Reynolds (1987) introduced simple rules to produce complex flocking behaviour. Potential fields or virtual springs are a common approach (Pan et al., 2019; Sabattini et al., 2011) but often use assumptions such as knowledge of position. Rubenstein et al. (2014) used kilobots (Rubenstein et al., 2012) to build arbitrary 1-connected shapes by individual robots extending a seed formation and localising themselves against already positioned static robots, likewise Li et al. (2019) progressively construct a shape with new agents positioning themselves against those already present. Also there are bioinspired approaches such as morphogenesis (Carrillo-Zapata et al., 2019; Slavkov et al., 2018). See Oh et al. (2017) for a recent review.
Several systems have demonstrated shape formation on real robot swarms; with kilobots, Rubenstein et al. (2014), constructing a common reference frame as robot accumulate around four seed robots. Li et al. (2019) validate their method on Khepera IV robots (Soares et al., 2016), synthesising relative position information from a global tracking system. Stolfi and Danoy (2024) use E-puck2 robots with synthesised range-and-bearing sensing of other robots over distances much larger than the shape being formed. Montijano et al. (2016) use consensus to reach an agreed formation with three quadrotors. Relative positioning is inferred using cameras to observe visible features on floor or ceiling and homography, with scale agreed by consensus. Except for the Kilobot work, these systems typically operate directly in relative coordinates and do not construct an explicit shared global frame. In contrast, we treat the same local relative measurements (and odometry) as factors in a distributed inference problem, and shape formation is layered on top of this frame.
Swarm logistics
Swarm logistics can be regarded as a real-world application of foraging (Winfield, 2009; Liu & Winfield, 2010; Pitonakova et al., 2018; Talamali et al., 2020). Robots must leave a nest area and seek resources to be returned to the nest. The analogy is obvious. As a use case, we focus on robot swarms used for intralogistics. As noted above, we specifically focus on the out-of-the-box, easily deployable solutions for everyday environments, as described in Jones et al. (2020). Even simple random walk algorithms are capable of effective retrieval in logistics applications, (Milner et al., 2022a, b). Messages from users propagate from robot to robot, robots store and retrieve items, all in parallel and without central resources. Although simple, this scenario encapsulates many issues that will need to be solved for viable swarm logistics systems to become a reality.
Methods
We work in simulation and the real world. As such, for simulation we use an abstract model of our real robots, the DOTS (Jones et al., 2022); each robot is modelled as a 2 kg disk, 250 mm in diameter, that can move holonomically at up to \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{max}}=\, 1\hbox{ms}^{-1}$$\end{document} , and can sense and identify other robots and objects to a distance of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{sense}}=$$\end{document} 0.5 m in simulation, 1 m for the real robots. Communication is possible between any robots that can sense each other. In simulation, each robot has imperfect sensors distorted by Gaussian noise; a velocity sensor \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {v}_{\textrm{sense}}=GT(v_{\textrm{robot}}) \cdot \mathcal {N}(1,\sigma ^2_{\textrm{velocity}})$$\end{document} , and a relative position sensor \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {p}_{\textrm{object}}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$GT(\textrm{object})-GT(\textrm{robot})$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$+\mathcal {N}(0,\sigma ^2_{\textrm{position}})$$\end{document} where GT(k) is the ground truth from the simulator. The robot maintains odometry \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {p}_{\textrm{odom}} = \sum \vec {v}_{\textrm{sense}}\Delta t$$\end{document} , \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\Delta t=\frac{1}{60}$$\end{document} s, integrated velocity since the last variable node was created.
Simulator
The simulator is written in C++, using the Box2D (Catto, 2009) physics engine. Updates to the simulation occur at 60 Hz. The arena is a square area with fixed walls. Robot motion is modelled as a disk with friction sliding on the arena surface, collisions between robots and with walls follow physics. Low level proportional control of the force applied to the robot body is used to satisfy the commanded velocity. As well as robots, there can be objects that can be detected by the robots. For each robot a list of robots and objects within \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{sense}}$$\end{document} range is maintained, and at each timestep a set of abstract senses is constructed and a controller routine is executed on those senses to generate a commanded velocity. The simulated senses and actuator are shown in Table 2.Table 2. Robot senses and actuatorSensesDescription \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {v}_{\textrm{sense}}$$\end{document} Velocity \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {p}_{\textrm{odom}}$$\end{document} Odometry since last variable node \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {p}_{\textrm{robot}}=\vec {x}_i+\vec {p}_{\textrm{odom}}$$\end{document} Inferred position of robot relative to swarm centroid \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\{\vec {p}^1_{\textrm{object}},..,\vec {p}^n_{\textrm{object}}\}$$\end{document} List of other robots and objects within \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{sense}}$$\end{document} radius \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\angle _{\textrm{neighbours}}$$\end{document} Direction of nearest neighbour robots \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\text {converged}$$\end{document} True when elapsed time \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$>t_{\textrm{convproxy}}$$\end{document} Actuator \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {v}_{\mathrm {cmd\_vel}}$$\end{document} Commanded velocity of robot
The baseline controller performs a random walk moving at \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{fast}}=0.5\,\hbox{ms}^{-1}$$\end{document} in a random direction \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{rwduration}}=\max (0.1,\mathcal {N}(2,1))$$\end{document} seconds, before choosing a new direction and duration. Collisions are treated as ballistic. This behaviour is denoted DSA-RW (Distributed Spatial Awareness - Random Walk) (Fig. 1).Fig. 1. Simulation of robots performing DSA-RW to locate cargo carriers, with various elements of the visualisation labelled. Within the carrier squares are overlayed the current swarm estimates of the carrier position, already showing good correspondence after two minutes of simulated time
Distributed spatial awareness
We construct a shared reference frame for the swarm in the following way: Each robot builds a local factor graph with variables representing 2D pose, unary anchor factors connecting to a single variable, and binary relative measurement factors connecting two variables. Each robot shares synchronised time, and at regular intervals \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}}$$\end{document} from a starting epoch a new timestep \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$ts_i$$\end{document} is issued and a new variable node \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {x}_i$$\end{document} is created. At any given time, the pose of the robot relative to the shared reference frame is \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {p}_{\textrm{robot}} = \vec {x}_i + \vec {p}_{\textrm{odom}}$$\end{document} .
The first node to be created will have an anchor factor connected to it with a weak prior of pose (0, 0). Successive nodes have a measurement factor connecting them: \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$f_{\vec {x}_i\rightarrow \vec {x}_{i-1}}(\mu =\vec {p}_{\textrm{odom}},\Sigma =\sigma ^2_{\textrm{velocity}}I_2)$$\end{document} . As new variable nodes are created, old ones are removed to maintain a maximum number of nodes \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$n_{\textrm{window}}$$\end{document} , with the now oldest variable having an anchor factor attached to it set to the belief of that variable. At every \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{message}}$$\end{document} interval, a factor node is chosen at random, and messages are propagated to each connected variable according to the GBP algorithm. Each connected variable then has its belief updated.
As described, this local factor graph is purely a bounded time window sampling of the odometry of the robot. In order to connect the local factor graphs together into a swarm whole, we add two things: 1) When a new variable node is created, robots observe other robots within their sensory range and create measurement factors \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$f(\mu =\vec {p}_{\textrm{object}},\Sigma =\sigma ^2_{\textrm{position}}I_2 )$$\end{document} that link the new variable node on the observing robot to the observed robot. These factors, termed outward-facing, are given the current timestep \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$ts_i$$\end{document} and the ID of the other robot. This uniquely identifies the variable at the other robot that it links to. 2) Robots in sensory range exchange GBP messages. At the same \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{message}}$$\end{document} interval as above, the robot randomly chooses a single other robot from any within \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{sense}}$$\end{document} range and sends a message request. This consists of a list of the timesteps of all outward-facing factors that connect to the other robot. The responding robot replies with a list of beliefs from the variables those factors uniquely connect to, and a list of messages from remote factors that connect to local variables of the requesting robot.
The factors linking variable nodes on different robots create the constraints that produce convergence of local pose estimates into a shared consistent state, i.e a shared reference frame. This process is illustrated in Fig. 2.Fig. 2. Robots creating connected factor graphs. Left: Two robots move on trajectories, making odometry measurements, that bring them within sensing range of each other. Right: The internal factor graphs that are created on each robot, assuming \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$n_{\textrm{window}}=4$$\end{document} and some time has passed. The oldest variable node in each graph, \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$x_3,y_3$$\end{document} , has an anchor factor. Between each variable node is an odometry measurement factor node. In timestep \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$ts_5$$\end{document} , robot y has observed robot x, creating an outward-facing relative position measurement factor. In \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$ts_6$$\end{document} , both robots have observed each other
Assume that robot x has sensed robot y, this is the process that follows. It sends a list of the timesteps of outward-facing factors connected to robot y, which will just be \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\{6\}$$\end{document} , for the factor connecting \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$x_6$$\end{document} to \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$y_6$$\end{document} . Robot y replies with \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\{b(y_6), m_{f\rightarrow x_6}, m_{f\rightarrow x_5}\}$$\end{document} . Robot x has now got message information needed to update the beliefs of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$x_5$$\end{document} , \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$x_6$$\end{document} , and to create an outward message from \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$f_m$$\end{document} attached to \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$x_6$$\end{document} .
Convergence
As robots encounter each other and exchange messages, and individual robots perform message passing on their individual fragments of the factor graph, the pose of each robot becomes constrained against others, thus the assumed origin or reference frame of each robot converges, Fig. 3. A necessary condition for convergence is that all parts of the graph must have, at some point, been connected. For n robots, we expect this to happen after \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$n_{\textrm{enc}}=\frac{1}{2}n\ln n$$\end{document} new robot pair encounters, Erdős et al. (1960).
It is important to note that all factor messages convey relative constraints, and all variable belief messages convey local probabilistic historical pose estimates, over a stochastically connected joint factor graph. This process performs inference to solve for a consistent swarm-centric frame, with no prior alignment. It is dynamic, and will never completely converge, since robots are moving and have noisy perceptions, and the whole graph does not remain fully connected.
The error is the mean deviation from the swarm centroid of the robot origins:
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} r_{\textrm{error}}=\frac{1}{n_{\textrm{robots}}}\sum _{i=1}^{n_{\textrm{robots}}}|\vec {r}^i_{\textrm{origin}}-\mu _{\textrm{origin}}| \end{aligned}$$\end{document}This is only knowable from the global perspective of the simulator. We define the convergence time \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{conv}}$$\end{document} as the time taken for error to reach \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{error}}<2\sigma _{\textrm{position}}$$\end{document} since position observation noise dominates convergence error.
To make the shared reference frame useful to a swarm system, individual agents within the swarm need to know when they can rely on estimates but they have no access to the global measure \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{error}}$$\end{document} . In characterising the system, we collect data on the time taken for agents to encounter at least half the robots in the swarm, denoted \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\mathrm {met\_half}}$$\end{document} . We reason that this, with some constant, should be a reasonable proxy for the time taken to build a fully converged reference frame:
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} t_{\textrm{convproxy}} = \beta \cdot t_{\mathrm {met\_half}} \end{aligned}$$\end{document}To implement this, each robot keeps a count of the number of unique other robots it has encountered. When this exceeds half the size of the swarm, the time is noted and \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{convproxy}}$$\end{document} calculated. Only when the elapsed time is greater than this is it possible to use the shared reference frame derived \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {p_{\textrm{robot}}}$$\end{document} . The two example algorithms below wait for this before switching behaviour from the baseline random walk controller DSA-RW.Fig. 3. Illustration of shared reference frame convergence. 1) All robots start by thinking they are at the centre of the swarm. 2) Observation and communication imposes constraints on location of the swarm centroid; the top two robots communicate.. 3) .. and each robot updates its own estimate. 4) More communication imposes further constraints. 5) Origin estimates move closer.. 6) ..and approach convergence
Application: shape formation
Because each robot has access to the shared reference frame, it is easy to construct algorithms for swarm-wide shape formation. To demonstrate this, we use a simple algorithm called DSA-SF (Distributed Spatial Awareness - Shape Formation) where the shape is defined functionally, \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$f_{\mathrm {in\_shape}}(p_{\textrm{robot}})$$\end{document} , and shown in Algorithm 1. Each robot has two modes of behaviour; when not inside the shape according to its estimated position \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {p}_{\textrm{robot}}$$\end{document} , it uses the baseline behaviour DSA-RW. When inside the shape, it slows down to \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{slow}}=0.1\cdot v_{\textrm{fast}}$$\end{document} , and if there are any neighbours, it moves towards them, causing classic swarm aggregation.
Algorithm 1DSA-SF Shape formation We define some simple shapes such as a circle, vertical and horizontal lines, and wavy lines, and switch between them at intervals.
Application: intralogistics
As noted above, even random walkers are capable of performing simple swarm logistics operations. We want to enhance the performance of such systems using the shared reference frame. A key component to a logistics system is knowledge of the location of cargo carriers. Carriers C are dynamic objects that may move. Each robot maintains a set of Gaussian variables and an associated time of observation \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$c(i)\equiv (\vec {x}_{\textrm{carrier}}^{i},t_{\textrm{observed}}^i), i \in C$$\end{document} , one for each possible carrier. When a robot observes a carrier k, it sets the tuple \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$c(k)=((\vec {p}_{\textrm{robot}}+\vec {p}_{\textrm{carrier}},\sigma ^2_{\textrm{position}}+\sigma ^2_{\textrm{robot}})_G,t)$$\end{document} . Each time a robot exchanges messages with another robot, it sends a list of the observation times \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{observed}}^i,i\in C$$\end{document} it has, including \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t=0$$\end{document} for carriers for which it has no observation. The other robot replies with any carrier observations it has that are more recent, these are used to replace the older observations. More recent observations are privileged, even if they may have greater positional uncertainty, since the carrier may have moved.
Algorithm 2DSA-KE Knowledge Enhancement
The way the robots move has an impact on the acquisition of knowledge about the environment. When the swarm has no knowledge, the goal is to cover as much of the arena area as possible. For this, we use the baseline behaviour DSA-RW. Once the swarm has a certain level of awareness, it becomes possible to use this information to guide swarm exploration so as to maximise knowledge. This behaviour is called DSA-KE (Distributed Spatial Awareness - Knowledge Enhancement), and is identical to DSA-RW until a robot has some information about all the carriers in the environment. At that point, instead of choosing a direction at random, a robot will choose the direction of the carrier which it has oldest knowledge about, thus the swarm as a whole seeks to minimise overall uncertainty, Algorithm 2.
In order to test the quality of knowledge acquisition in a dynamic environment, we make the carriers move position. This is specified with a single parameter, the aggregate mean carrier velocity \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\mathrm {c\_agg}}$$\end{document} . Given \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$n_{\textrm{carrier}}$$\end{document} carriers, a carrier is selected at random and moved a fixed distance of 1 m into a random location at a velocity of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\mathrm {c\_agg}}\cdot n_{\textrm{carrier}}$$\end{document} . The mean perception error of the swarm is given by:
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} s_{\textrm{error}}&= \frac{1}{n_{\textrm{robots}}n_{\textrm{carrier}}}\\ &\quad\sum _{j=1}^{n_{\textrm{robots}}}\sum _{i=1}^{n_{\textrm{carrier}}}|c(i)_{\textrm{est}}^j-c(i)_{\textrm{gt}}| \end{aligned}$$\end{document}where \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$c(i)_{\textrm{gt}}$$\end{document} is the ground truth position of a carrier, transformed into the swarm frame.
Application: DOTS robots
Bringing techniques from simulation into the real world is an important step toward applications. Here we detail how we implement DSA on the DOTS robots. The robots run within a custom-built arena of 3.7m \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\times $$\end{document} 3.7 m. Each has a controlling RockPi 4 single board computer (SBC) running ROS2, and is connected to a dedicated WiFi access point. A PC server is used for data collection.
In our simulation model, robots can sense the relative position of all other nearby objects within a fixed range with added Gaussian noise; an idealised sense. How should we construct relative object sensing in our real robots?
In reality, each robot has four wide-angle cameras around their periphery, with approximately \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$90^{\circ }$$\end{document} horizontal field of view. The two front cameras are closer together, with overlapping views. Cameras are not calibrated individually; during construction of the robots, the intrinsics and distortion parameters were measured and averaged for a subset of cameras. Because visual processing for arbitrary object recognition is expensive and the controlling RockPi 4 single board computer (SBC) is relatively low power, we rely on fiducial tags in the environment to facilitate comprehension of the world. We reason that future deployment will have access to much higher computation ability, making this a reasonable analog for full object recognition. Each camera is connected to a Raspberry Pi Zero, which runs fiducial detection on-board (Jones & Hauert, 2023), making a stream of tag detections from each camera available to the SBC.
Sensing other robots
In this application, we use a band of fiducial markers around the body of the robots to allow them to sense each other visually. Each robot is allocated a unique tag ID. This tag is replicated 16 times around the robot, see Fig. 4, with the ith tag rotated by \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$i2\pi /16$$\end{document} about its z-axis.1 This band of ID tags have known and fixed locations relative to the robot. Because we know that both the observing and observed robots are on the same plane, by calculating the z-angle of observed markers, we can disambiguate which of the 16 tags are being observed and thus know the physical location of the marker corners in the frame of the observed robot. Using the OpenCV solvePnP function with the camera intrinsics and distortion parameters, we get the transform \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$^\textrm{cam}T_{\textrm{idband}}$$\end{document} . Combining with with observing camera extrinsics \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$^\textrm{robot}T_{\textrm{cam}}$$\end{document} we get the relative poses of the two robots \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$^\textrm{robot}T_{\textrm{idband}}$$\end{document} . The full relative pose in SE(2) is not necessary for this application but we intend to use it in future work. Initial experiments showed that achieving reliable fiducial detection between relatively moving robots was difficult. Fiducial detection relies on sharp image edges, but relative movement between robots results in lateral movement of fiducials across a camera view, inducing image blur, increasing with reduced robot-robot distance. This limits both the velocity of the robots and the close range detection distance. Reducing robot velocity \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{robot}}$$\end{document} to \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$0.25 \,\hbox{m s}^{-1}$$\end{document} (from \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$0.5\,\hbox{m s}^{-1}$$\end{document} in simulation) and extending the allowed sensing range \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{sense}}$$\end{document} to 1 m (from 0.5 m in simulation) resulted in a reasonable detection rate.Fig. 4. Two DOTS robots, with identifying fiducial tags around their bodies. Also visible are the two front-facing cameras and large top mounted fiducial tags for ground truth tracking
The odometry noise model of metres per metre travelled still approximates the real robots. But for relative position measurement using cameras, the application of a fixed uncertainty to the relative (x, y) position is not realistic; position measurement is effectively range and bearing, which is non-linear. Pixel measurement noise P maps predominately to range r; at \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r=1 \hbox {m}, \frac{dr}{dP}\approx -0.11\,\hbox{m}$$\end{document} whereas angle \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\frac{d\theta }{dP}\approx 0.18^{\circ }$$\end{document} , or about 3.2 mm displacement. To handle this properly, we would need to use non-linear factors, and perform linearisation during inference, but to keep the same code as used in simulation, we opt to simply add a larger uncertainty, scaled to distance: \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\sigma _{\textrm{position}} = 0.02 + 0.04r$$\end{document} .
Implementation in ROS2
All robots run on a common ROS2 system, and they all have synchronised clocks. Each robot receives a signal on the /launch topic and this is used to trigger the ROS node handling the DSA processing to start. These nodes on different robots run in lockstep with skew between them of less than 50 ms. The simulation code relies on unique identifiers being created for each factor graph node based on the robot ID and the current timestep \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$ts_i$$\end{document} , so these can be referenced during message exchange. This scheme is unchanged except to tie the robot ID to the physical robot name, rather than simulation object creation order. Update period for the factor graph was fixed at \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{message}}=0.1 s$$\end{document} . As with the simulation, a list of robots within \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{sense}}$$\end{document} range that have been detected is maintained, the relevant transform being broadcast to the local tf tree. At each update, if there are any robots within range, as determined by whether they are visually identified and measured as less than 1 m away, one is picked randomly and the message exchange protocol described in Sect. 3.2 executed. This is implemented as a ROS2 service request. The response is not guaranteed, and is not synchronous, but this does not matter for the GBP algorithm; returning data if it arrives is placed in the inward message slots of relevant factor and variable nodes.
Robot odometry from wheel velocity \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {p}_{\textrm{odom}}$$\end{document} together with the latest variable node \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {x}_i$$\end{document} is used to maintain the estimated position relative to the swarm \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\vec {p}_{\textrm{robot}}$$\end{document} . This is also broadcast to the local tf tree.
In order to maintain the requirements of the DSA code that robot pose is pure 2D, we use a synthetic compass sense and low level proportional controller acting only on angular velocity to maintain a constant robot heading. All robots run localisation on-board, using odometry, IMU, and observation of twelve large fiducial markers in fixed known locations on the arena walls to establish their global pose within the arena. The compass sense is synthesised from this pose orientation; there is no access by DSA to the position information, it is not used within this work.
Pattern formation
We perform experiments with six DOTS robots. As well as their peripheral bands of identifying fiducials, each robot is fitted with a larger unique fiducial marker on its top surface for ground truth tracking. At the start of a run, the robots are placed in random locations within the arena and the DSA estimation is reset. Each run consists three repetitions of Algorithm 3; a mixing period of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{mix}=30 s$$\end{document} consisting of a random walk at a velocity of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{robot}}=0.25\,\hbox{ms}^{-1} $$\end{document} . Once the mixing period is complete, the robots try and form a hexagon of radius 0.7 m around the origin of the shared reference frame. Each robot has a fixed target vertex in the hexagon pattern, with a simple proportional controller driving robot linear velocity. At all times collision avoidance is active, with obstacle detection via 16 laser Time-of-Flight sensors. Any obstacle less than 0.2 m away, causes a random direction roughly opposite the obstacle to be chosen.
Once the hexagon pattern has stabilised, two more sets of mixing period and hexagon formation were initiated, with the DSA estimation retaining accumulated state. Five runs in total were performed. For each run, we collect video recordings, and ROS2 bag files of relevant topics, for later analysis.
Algorithm 3DOTS demonstration
Results
We measured the performance of various metrics of interest. In each case, simulations were run 50 times with different random seeds for each datapoint. Standard parameters are shown in Table 3. DOTS robot experiments recorded video and bag file data which was post-processed to get unified datasets.Table 3DSA parametersParameterValueDescription \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$n_{\textrm{window}}$$\end{document} 10Max number of variable nodes in local factor graph \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}}$$\end{document} 0.5 sNew variable node creation interval \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$d_{\textrm{robot}}$$\end{document} 0.25 mRobot diameter \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{sense}}$$\end{document} 0.5 mObject and robot sense and communication radius \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\sigma _{\textrm{velocity}}$$\end{document} 0.1 m/mVelocity sense noise (metres per metre travelled) \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\sigma _{\textrm{position}}$$\end{document} 0.02 mPosition sense noise \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{fast}}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$${0.5}\,\hbox{ms}^{-1}$$\end{document} Movement performing DSA-RW and DSA-KE \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{slow}}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$0.05\,\hbox{ms}^{-1}$$\end{document} Movement within shape while performing DSA-SFDOTS parameters where different \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{sense}}$$\end{document} 1.0 mObject and robot sense and communication radius \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\sigma _{\textrm{position}}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$0.02+0.04r$$\end{document} mPosition sense noise, r is distance to robot \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{robot}}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$0.25\,\hbox{ ms}^{-1}$$\end{document} Robot movement speed
Convergence, computation, and bandwidth
We examine how long the distributed factor graph takes to converge, \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{conv}}$$\end{document} , and how much computation and bandwidth is used in different scenarios. An example video of 10 robots reaching convergence is available at https://youtu.be/3S9Ko356eiY.
Firstly, we look at different \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{message}}$$\end{document} update periods of the factor graph, and different numbers of robots in an arena sized to keep a constant robot density of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$0.4\,\hbox{ ms}^{-2}$$\end{document} . See Fig. 5 top. Convergence takes longer when there are robots over a larger area, this is expected, since all robots have to be able to influence each other, though not necessarily by direct communication, that is, the factor graph over time must be connected. Increasing density with constant area has a small linear effect on convergence (bottom) due to the larger total size of the graph, but the dominating factors are update rate and arena area. There is little gain from rapid updates over 10 Hz.Fig. 5Top: Convergence time with different factor graph update rates and different arena areas, with fixed robot density. Reduction in convergence time is minimal below 0.1 s update period. Middle: Convergence time with different \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}}$$\end{document} periods, at three different window lengths, using 20 robots in a \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$50\,\hbox{m}^{2}$$\end{document} arena. Bottom: For a fixed update period of 0.1 s and fixed arena area of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$25\,\hbox{m}^2$$\end{document} , computation, convergence time, and particularly bandwidth are dependent on robot density
Because convergence requires the factor graph be connected, we next examine how \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}}$$\end{document} controls the rate of new edge formation. The number of nodes \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$n_{\textrm{window}}$$\end{document} is less informative than the total history length, so the middle graph represents interpolated slices through three different fixed history windows. This shows a clear knee in convergence time \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{conv}}$$\end{document} at \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}}=1\,\hbox{s}$$\end{document} . We explain this with reference to the time that meeting robots remain within sense range \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\tau _{\textrm{in}}\approx 1\,\hbox{s}$$\end{document} . For \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}} <\tau _{\textrm{in}}$$\end{document} , we are effectively continuously sampling the environment, so we record new edges at a rate independent of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}}$$\end{document} , determined only by the physics of encounters ( \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$n_{\textrm{robots}},v_{\textrm{robot}},r_{\textrm{sense}}$$\end{document} , and arena area), which is constant here. For \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}}>\tau _{\textrm{in}}$$\end{document} , sampling, not physics, limits progress; more robot encounters start to take place between the sample points, so the time to reach connectivity, and thus convergence, grows roughly linearly with \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}}$$\end{document} . Smaller history lengths have worse convergence, there is more ‘forgetting’, and fewer constraints for the GBP algorithm to work with; a six second window with \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{node}}=2\,\hbox{s}$$\end{document} only has three nodes.
Computation and bandwidth are both proportional to update frequency so we use a fixed sized arena of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$25\,\hbox{m}^2$$\end{document} and update rate of 10 Hz to look at other factors. All operations are performed in 32 bit floating point. Since we are working in 2D space with linear factors, the precision matrix \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\Lambda $$\end{document} can be represented as a single number. Belief update needs 3 operations per attached factor (Eq. 1). Factor message generation only needs computation for measurement factors; \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$2\,\text{(Eqn}$$\end{document} 4) \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$+1+2+2+1\,\text{(Eqn}$$\end{document} 5) \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$+2+2+1\,\text{(Eqn}$$\end{document} 6) \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$=13$$\end{document} operations. Assuming a naive message protocol, every request has an overhead of 12 bytes, and 4 bytes per outward-facing factor, and each response has an overhead of 12 bytes, and 16 bytes per returning belief and factor-to-variable message.
There is a weak dependence of computation on density, as there are more chances to create additional outward-facing factors. Bandwidth is strongly dependent on robot density, as there are many more opportunities for a robot to communicate. It should be noted that the raw figures for supporting the shared reference frame are remarkably low; for a \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$25\,\hbox{m}^2$$\end{document} arena, the swarm will converge in less than 60 s, with each robot using only a few hundred floating point operations and exchanged message bytes per second. This is achievable even on low cost processors.
In order to determine an appropriate value for \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\beta $$\end{document} (Eq. 11), we ran a set simulations over different numbers of robots between 5 and 100, and different arena sizes between \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$4\,\hbox{m}^2$$\end{document} and \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$100\,\hbox{m}^2$$\end{document} , fixing the update period \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{message}}=0.1$$\end{document} s. Simulations were run for 1000 simulated seconds. Out of 4200 simulations, 2886 were able to find an initial placement for the robots in the arena size, and reached convergence within the simulation run time. We can see from the graphs in Fig. 6 that using a value of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\beta =3$$\end{document} ensures the proxy measure \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{convproxy}}$$\end{document} exceeds the true measure \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{conv}}$$\end{document} in 95% of simulations.Fig. 6. The distribution of the robot encounter measure \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\mathrm {met\_half}}$$\end{document} over 2886 simulations with different numbers of robots between 5 and 100, and different arena sizes between \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$4\,\hbox{m}^2$$\end{document} and \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$100\hbox {m }^2$$\end{document} . Using a constant \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\beta =3$$\end{document} ensures the proxy for convergence time \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{convproxy}}$$\end{document} exceeds the true convergence time \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{conv}}$$\end{document} in more than 95% of simulation. Bottom scatter plot coloured according to arena area
Shape formation
We ran simulations using 150 robots, with an arena size of 7.5 m per side. After reaching convergence, each robot followed Algorithm 1, with the shape function switching at regular intervals. Figure 7 shows snapshots of the process, with the desired shapes emerging within about 40 s in each case. The video at https://youtu.be/ps5Wf-3UHr0 shows this process in full.Fig. 7. Shape formation. 150 robots perform random walk DSA-RW when outside shape, and slow down and aggregate when inside shape, taking about 40 s to form each pattern
The algorithm is extremely simple, yet forms and cleanly transitions between shapes even without fine tuning of parameters.
Intralogistics
Figure 8 shows how the two movement behaviours DSA-RW and DSA-KE perform in acquiring knowledge about the dynamic carrier environment. At zero carrier velocities, the swarm quickly reaches low \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$s_{\textrm{error}}$$\end{document} values, around 0.04 m. Since the position sense has injected noise of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\sigma _{\textrm{psense}}=0.02\,\hbox{m}$$\end{document} , this is a reasonable lower bound. As the velocity of the carrier movement increases, the error goes up as unobserved carriers can move further from their last know positions.Fig. 8. Swarm carrier location error \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$s_{\textrm{error}}$$\end{document} with the two robot behaviours while varying the carrier velocity (top) and the number of carriers in the arena (bottom). The number of robots is fixed at 10. Shaded areas indicate \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\pm \sigma $$\end{document} over 50 different random seeds. As the mean carrier velocity increases, so does the error, which is expected. With a fixed mean carrier velocity and sweeping the number of carriers, we see roughly constant error with the random walk of DSA-RW, but much lower error with DSA-KE as it actively seeks to enhance knowledge, particularly with low carrier numbers. In all cases, the DSA-KE behaviour results in much lower swarm perception error
The effectiveness of DSA-KE in improving carrier position estimation in the swarm by actively moving to areas where knowledge is weaker, is clearly apparent. In all cases over different numbers of carriers and different carrier velocities, we see improvement, 38% with 10 carriers and mean velocity \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$0.01\,\hbox{ms}^{-1}$$\end{document} . When looking at performance vs the number of carriers, we see that DSA-RW is roughly constant, whereas DSA-KE performs extremely well with lower numbers of carriers. Clearly visible in the video available at https://youtu.be/r53Z1O1pfxk is a rather interesting emergent flocking behaviour; as one robot heads towards the carrier it knows least about, the backwards propagation of information from the leading robot on average causes trailing robots to be in a better position to seek the next low-knowledge carrier, cohering the swarm. We illustrate the dynamics of this in more detail with the video at https://youtu.be/5Ad-HzJYnM8, colour coding the least recently visited carrier, and colouring robots with their current target.
The good relative performance of DSA-KE falls off at higher numbers of carriers, though it is still 24% better than DSA-RW with 20 carriers. It does suggest that a better strategy for high carrier numbers would ensure a greater degree of dispersion.
DOTS robots
Firstly, we characterise the performance of the visual recognition system. Figure 9 left shows all the robot detections of other robots while the swarm was performing a random walk, a total of 38,663 datapoints. These are plotted as a polar graph, and show that the receptive field is not perfect, with significant blind spots at about \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$75^{\circ }$$\end{document} and \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$180^{\circ }$$\end{document} from the robot forward direction. Most detections were in the distance range 0.5–1.0 m. Overlaid on the graph of the robot receptive field are six orange circles, which illustrate where neighbouring robots will appear when in the ideal hexagon pattern. Two of the six robots in the pattern have little to no visibility of their neighbours; to the rear and to the front left or right.Fig. 9Left: The visual receptive field of the DOTS robots. All visual detections by robots of other robots while performing random walk are shown, totalling 38,663, with density contours. Also shown are the angles of the four cameras, and orange circles at the locations where other robots would need to be detected for optimal hexagon shape formation. Right: Locations of robots and each of their inferred frames for all converged pattern formation attempts. Circles show ideal robot positions and robot index, crosses mark mean actual locations
Range and bearing error are shown in Table 4. Bias and spread of error for range was quite low, given the value of \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\frac{dr}{dP}=-0.11\,\hbox{m}$$\end{document} , demonstrating effective subpixel localisation of fiducial corners. Bearing error was much higher than could be explained by pixel error, and more likely due to inaccuracies in keeping robots pointing to a fixed heading.Table 4. Range and bearing measurement errorMeasurementMean error \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\sigma $$\end{document} errorRange (m)0.0120.044Bearing ( \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\angle ^{\circ }$$\end{document} )− 1.23.4Table 5Mean deviation \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{error}}$$\end{document} of robot DSA framesRunPattern 1Pattern 2Pattern 310.1020.0780.06720.1380.1810.09430.328****0.3440.16840.0620.0580.14650.1230.0500.062Bolded numbers show pattern formation where convergence was not successfulTable 6Mean vertex positions, vertex error \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{error}}$$\end{document} , and effective pattern radius in metres (excluding Run 3 Patterns 1,2)Robot \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\bar{x}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\bar{y}$$\end{document} \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{error}$$\end{document} rRobot 10.7400.0300.0690.741Robot 20.3700.6940.1020.786Robot 3− 0.3720.6590.0710.757Robot 4− 0.738− 0.0160.0600.738Robot 5− 0.389− 0.6780.0880.782Robot 60.389− 0.6870.1150.7900.0840.766Note that robot 2 and robot 6 have worse \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{error}}$$\end{document} , as would be expected from the visual receptive field
We then look at the performance of pattern formation. Firstly, for the pattern to form correctly, the DSA shared reference frame needs to have converged. Table 5 shows the mean deviation \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{error}}$$\end{document} in metres for each pattern in each run. In Sect. 3.2.1 we define convergence as \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$2\sigma _{\textrm{position}}$$\end{document} which, in worse case with our distance dependent alteration to position sensing uncertainty, is 0.12 m. Only eight of the 15 pattern formation attempts meet this, although this criteria assumes unbiassed odometry measurement. It is clear that Run 3, Patterns 1 and 2 are significantly worse, and upon examination, in each case the swarm has two disconnected graphs; with two and four robots in each, and with the distance sufficiently far apart that no messages can pass between them to bring about convergence. The best and worst convergences are shown in Fig. 10, which are annotated frames from videos of the runs. Snapshots from Run 1 are shown in Fig. 11 to illustrate the pattern formation process. A video of all runs is available at https://youtu.be/6ZwQHJnqL7M, with annotation of the robot ground truth and the DSA frames.Fig. 10. Worst and best \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{error}}$$\end{document} of robot DSA reference frames. From video with overlaid annotation of robot ground truth and DSA frames. Left is Run 3 Pattern 2, with \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{error}}=0.344\,\hbox{m}$$\end{document} , and right is Run 5 Pattern 2, with \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$r_{\textrm{error}}=0.05\,\hbox{m}$$\end{document} . The swarm on the left has not converged on a single frame of reference, and at this point in pattern formation, the two groups of robots are too far apart to communicateFig. 11Formation of hexagon arrangement by DOTS robots repeated three times, frames from Run 1
Figure 9 right shows data from all pattern formation attempts except for Run 3, Patterns 1 and 2, and Table 6 summarises the vertex position error defined as:
\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\begin{aligned} v_{error} = \frac{1}{6\cdot n_{pats}} \sum _{p \in pats} \sum _{i=1}^6 {|r^i_{position}-r^i_{ideal}|} \end{aligned}$$\end{document}Overall \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{error}}= 0.084\,\hbox{m}$$\end{document} . Robots 2 and 6 have significantly worse \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$v_{\textrm{error}}$$\end{document} , and this would be expected given the characteristics of visual receptive field of the robots. Each cannot sense either of their neighbours. Despite this, because their neighbours can observe them, make outward-facing factors that connect to their local fragments of the factor graph, and thus initiate exchanges of GBP messages to them, they are still able to converge to within \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$0.12\,\hbox{m}$$\end{document} of ideal position.
We have done very little tuning to the uncertainty assigned to the odometry and relative position measurements, and it is likely that the odometry uncertainty is too low, as well as failing to capture velocity differences in the robots; the effective wheel diameter varies from nominal resulting in different robots having different biasses, which are not currently captured by calibration. Despite the non-optimal uncertainty model, and the approximations of applying the 2D linear system, the system still works surprisingly well.
Discussion
It is important to note that both DSA-KE (Knowledge Enhancement) and DSA-SF (Shape Formation) are simple algorithms that serve to illustrate the possibilities that distributed spatial awareness can offer for swarm algorithms. The behaviour of the GBP message passing algorithm underlying the DSA shared reference frame is robust across parameters and the computational cost per robot of maintaining its local factor graph is low, of the order of a few hundred floating point operations per second, and the communications likewise is low cost, being a few hundred bytes per second. This is within reach of even cheap microcontrollers.
There are some limitations to this work, particularly the use of linear factors in \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\mathbb {R}^2$$\end{document} . As we mention below, we intend to move to \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$\textrm{SE}(2)$$\end{document} . Implementing the algorithm updates to support this is necessary to generalise to real-world applications. Likewise, the current assumption of time synchronicity is unnecessary for the general GBP algorithm, one simple approach we are considering is to tag outgoing messages with their local age; the receiving robot can allocate them to its closest equivalent age node. The convergence time proxy \documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$t_{\textrm{convproxy}}$$\end{document} requires knowledge of the size of the swarm, we are investigating unique encounter rates and possible swarm size consensus approaches to remove this limitation. Our shape-formation algorithm is extremely simple, and will fail in environments with obstacles, but this was to demonstrate the utility of the DSA shared reference frame. More robust algorithms can be constructed on this framework.
Successfully transferring DSA to real robots, without making alterations to the algorithm beyond slight changes to the uncertainty model further demonstrates the robustness of DSA to quite large deviations from the more idealised simulator. Even with gaps in the robot visual perception effectively blinding two of the six robots in the hexagon patten, the swarm collectively was still able to perform well. The resilience of the distributed factor graph and message passing underlying DSA is encouraging for future real-world deployment.
Conclusion
We use exploratory movement, local observation, and distributed factor graph construction, combined with GBP evaluation, to give a global sensing ability to a swarm system without violating the core tenet of decentralisation, and with low per-robot computation and communication cost.
Using two simple algorithms that make use of this new distributed spatial awareness ability, we show the possibilities. Moving DSA to real robots demonstrates potential applicability and robustness beyond the lab. Applying some of the automatic design techniques commonly used for swarm controllers to systems using DSA is interesting; evolution often discovers non-obvious uses of new abilities. We consider GBP to be a powerful technique to infer global knowledge within a completely distributed paradigm, and wish to bring other items of state and knowledge within this overarching framework; it will be interesting to compare some more traditional swarm consensus algorithms with GBP, many best-of-n swarm algorithms rely on local message passing Valentini et al. (2017), perhaps suggesting deeper similarities.
We look forward to exploring new swarm algorithm possibilities that may combine DSA acquired knowledge with other swarm techniques. The current system relies on graph construction happening in synchronisation at regular timesteps, but we don’t see this as necessary and plan to remove this restriction, e.g. by estimating between states at time of observation. Finally, we are planning to move beyond the restrictions of pure 2D linear systems for further work on our DOTS robots moving towards a fully decentralised swarm logistics demonstration.
The reference list from the paper itself. Each links out to its DOI / PubMed record.
- 1Agarwal, S., Mierle, K., & Team, T. C. S. (2023). Ceres solver. https://github.com/ceres-solver/ceres-solver
- 2Bickson, D., Shental, O., & Dolev, D. (2008). Distributed Kalman filter via Gaussian belief propagation. In: 2008 46th annual allerton conference on communication, control, and computing (pp. 628–635).
- 3Catto, E. (2009). Box 2D: A 2D physics engine for games. http://box 2d.org/
- 4Crosscombe, M., Lawry, J., Hauert, S., Homer, M. (2017). Robust distributed decision-making in robot swarms: Exploiting a third truth state. In: T. Maciejewski (Ed.), 2017 IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 4326–4332). Vancouver, Canada.
- 5Davison, A.J., & Ortiz, J. (2019). Future Mapping 2: Gaussian belief propagation for spatial AI. ar Xiv:1910.14139.
- 6Dellaert, F., & Contributors, G. (2022). borglab/gtsam. Georgia Tech Borg Lab. https://github.com/borglab/gtsam
- 7Dellaert, F., Kaess, M., et al. (2017). Factor graphs for robot perception. Foundations and Trends® in Robotics, 6(1–2), 1–139. 10.1561/2300000043
- 8Erdős, P., Rényi, A., et al. (1960). On the evolution of random graphs. Publications of the