Keywords

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

1 Introduction

In recent years the field of robotics and automation has undergone a dramatic ascendency in terms of its significance in industrial and military applications as well as its growing importance in service applications. Multi-robot systems (also known as multi-agent systems) is a branch of robotics that deals with the collaboration among teams of robots (either homogenous or heterogenous) in accomplishing certain tasks. This section reviews some of the work done in the field of multi-robot localization followed by an overview of the remainder of this chapter.

1.1 Introduction to Multi-Robot Localization

The path to true autonomy starts with robots knowing where they are in a given workspace. Such a problem is known as robot localization. According to [15], the localization problem can be categorized into two subproblems: (1) position tracking (local localization) which aims to compensate for small dead reckoning errors using sensor feedback; this approach is local in that the initial pose is assumed known and (2) global localization in which the robot “figures out” its position given no knowledge of its initial pose. A tremendous amount of effort has been devoted to effectively and efficiently solving the localization problem and the field has seen major advancements in the establishment of highly practical and easy to implement algorithms with the EKF (extended Kalman filter)-based and PF (particle filter)-based approaches the most widely accepted solutions to the problem. However, the majority of existing approaches are tailored to localizing a single robot. The field of multi-robot localization remains relatively fresh and to be explored [6].

Performing the localization task with multiple robots possesses the advantage of information sharing. Robots within a team can exchange information with other members so to increase the accuracy and reduce uncertainty in their own estimates. This advantage is shown both in simulation and experimentally in [6] in which two robots explore an indoor environment executing their own single robot localization scheme when they are far away from each other. And the proposed collaborative fusion algorithm is used when the two robots come into each other’s detection range. Results show that such an algorithmic reinforcement has the effect of significantly reducing the ambiguities existing in the original estimates. A collaborative architecture of this sort can effectively reduce the hardware cost of the entire team in that if at least one robot has a good knowledge of its location, then other team members can use this information along with relative measurements to infer their own position and reduce estimation errors.

1.2 Comparison of Existing Distributed Localization Methods

The problem of cooperative localization has been tackled with a wide variety of approaches over the years. And similar to single robot localization, many of the existing algorithms can be considered variations of two main categories. The first family of algorithms makes use of recursive Gaussian filters. Distributed versions of the Kalman filter are proposed in [1, 14] to solve the cooperative localization problem. The extended Kalman filter (EKF) is utilized in [12] while also providing analytical expressions for the upper bound of the estimate uncertainty. In [2] the EKF is also used, but the algorithm is reinforced with an entropic criterion to select optimal measurements that reduce global uncertainty. The advantage of using recursive Bayesian filters to fuse information lies in they are incremental in nature, which makes them applicable to real-time estimation. Closed-form expressions for state estimation and update also facilitate computational speed. However, these types of algorithms deal only with Gaussian noise which may not be the case for some real systems. And EKF linearizes the system dynamics around the state of estimate which is prone to failure when errors grow.

The second family of algorithms is built upon sampling-based nonparametric filters. Monte Carlo localization methods are used in [9] to estimate the pose of each member robot while using grid cells to describe the entire particle set. A global collaborative localization algorithm is presented in [6] that also builds upon sample-based Markov localization. In addition, [3, 8, 11] have all approached the problem with different variations of the particle filter and have also applied their algorithm in the SLAM (simultaneous localization and mapping) context. Further experimental validation is provided in [11] and [3]. Grid-based and sampling-based Markov localization techniques usually address the problem globally and can be improved via carefully designed resampling processes to counteract localization failures. They can also be used to accommodate non-Gaussian noise models. However, like all sampling-based approaches, a large number of grids/samples are usually needed to acquire reasonable outcomes, and the computational cost grows dramatically with the dimension of the problem. A table comparing the two families of methods is provided below.

These two main categories of localization techniques presented in Table 8.1 currently dominate the field. Both possess their own pros and cons and the choice of which depend heavily on the type of applications they are desired for. The two approaches can potentially be combined to yield superior outcomes. For more details regarding the extended Kalman filter (EKF) applied to multi-robot systems see [12, 14]. For details on collaborative Monte Carlo localization (MCL) see [6].

Table 8.1 Comparison between distributed EKF And MCL
Full size table

The following subsection explains how our approach differs.

1.3 Objectives, Contributions, and Outline

Existing approaches to the multi-robot localization problem usually consider only uncertainties in each robot’s pose estimate and sensor measurement. The goal of this chapter is to explore cooperative localization in a more generalized setting where uncertainties in the sources of relative measurements (neighboring robots’ pose estimates) are also considered. The distributed localization approach proposed in this chapter makes an effort to providing recursive closed-form expressions for real time cooperative sensor fusion used for pose updates of robots within a team. This work extends the method presented in [10], which considers cooperative localization with only one exact noise-free measurement (relative to a neighboring robot), whereas the technique proposed here can take into account any number of relative measurements while also considering sensor noise. This method is developed under the framework of exponential coordinates for Lie groups which gives this exotic sounding methodology a down-to-earth benefit: Gaussian distribution in Cartesian coordinates, \((x,y,\theta )\)-planar coordinates and heading angle, possesses elliptical probability density contours for each fixed \(\theta\) and for marginal densities in (x, y), whereas the banana-shaped distribution resulting from incremental motions of a stochastic differential-kinematic system (i.e., a probabilistic model of mobile robots with nonholonomic kinematic constraints) is better represented by a Gaussian in exponential coordinates which produce a more conformable density contour (see Fig. 8.2a). This underlying framework allows the proposed algorithm to tolerate higher errors without worrying about collapse of the normality assumption as uncertainty grows. Unlike most existing cooperative localization schemes that consider only uncertainty in the pose of the robot to be estimated and measurement noise, the presented method has also taken into account the uncertainty in the pose of nearby robots from which relative measurements are taken, making it a more realistic and dynamical localization technique. This approach is second order in its expansion of the Gaussians that describes the pose and measurement distributions using the Baker–Campbell–Hausdorff (BCH) formula [4], and no simplifications are made regarding the system kinematics, thus preserving the full nonlinear characteristics of the original system. Lastly, the form of sensor measurement in this method is kept generic without assuming the type of sensor or any underlying characteristics given the Gaussian-in-exponential-coordinate model can be applied.

Fig. 8.1
figure 1

Simple model for a two-wheeled differential drive mobile system

Full size image
Fig. 8.2
figure 2

Localization with only the prediction model. (a) Straight-line motion. (b) Circular motion

Full size image

The remainder of this chapter is outlined as follows. Section 8.2 introduces the mathematical foundation on which the proposed approach is based, namely the basics of matrix Lie groups and exponential mapping. Section 8.3 provides a detailed derivation of the proposed technique. Section 8.4 describes the experimental setup in simulation and provides a discussion of the results. Section 8.5 concludes this chapter.

2 Mathematical Background for the Group SE(n) and Exponential Mapping

2.1 The Special Euclidean Group and Exponential Coordinates

2.1.1 The Special Euclidean Motion Group

The proposed technique is largely based on the notion of Lie groups and their parameterizations. According to [4], a group is defined as a pair (G, ∘) consisting of a set G and a binary operator ∘ such that g 1 ∘ g 2 ∈ G whenever g 1, g 2 ∈ G, the operator is associative in the sense that (g 1 ∘ g 2) ∘ g 3 = g 1 ∘ (g 2 ∘ g 3) for all elements g 1, g 2, g 3 ∈ G, there exists an identity element e ∈ G such that for all elements g ∈ G, \(g \circ e = e \circ g = g\), and for each g ∈ G there exists an inverse element g −1 ∈ G such that \(g \circ g^{-1} = g^{-1} \circ g = e\). For engineering applications, a group of great interest is the Special Euclidean Group, SE(n), that describes rigid-body motions in n-dimensional Euclidean space. The elements of SE(n) can be represented as \((n + 1) \times (n + 1)\) homogeneous transformation matrices of the form

$$\displaystyle{ \mathrm{SE}(n) = \left \{\left [\begin{array}{*{10}c} R &\mathbf{t}\\ \mathbf{0} ^{T } &1 \end{array} \right ]\middle\vert R \in \mathrm{ SO}(n),\mathbf{t} \in \mathfrak{R}^{n}\right \}, }$$
(8.1)

where SO(n) is the special orthogonal group consisting of n × n rotation matrices and \(\mathfrak{R}^{n}\) is the n-dimensional vector space representing translations. The binary operation in this context is simply the matrix multiplication. The Special Euclidean Group is also a matrix Lie group since each element is a real-valued matrix, the whole set is a differentiable manifold, and both the operations of multiplication and inversion of homogeneous transformation matrices are smooth operations. We note that in most practical problems n takes only two values: n = 2 for planar motion and n = 3 for 3-dimensional space motion.

Now we introduce the concept of Lie Algebra. Again following [4], elements of a matrix Lie group can be written as \(g =\mathop{ \mathrm{exp}}\nolimits (X)\) for \(X \in \mathcal{G}\) where the set \(\mathcal{G}\) is the matrix Lie algebra of G. The Lie Algebra for SE(2) [denoted as se(2)] can be represented by the linear combination of a set of basis

$$\displaystyle{E_{1}^{\mathrm{se}(2)} = \left [\begin{array}{*{10}c} 0&0&1\\ 0 &0 &0 \\ 0&0&0 \end{array} \right ],\quad \quad E_{2}^{\mathrm{se}(2)} = \left [\begin{array}{*{10}c} 0&0&0\\ 0 &0 &1 \\ 0&0&0 \end{array} \right ],\quad \quad E_{3}^{\mathrm{se}(2)} = \left [\begin{array}{*{10}c} 0&-1&0\\ 1 & 0 &0 \\ 0& 0 &0 \end{array} \right ].}$$

The exponential coordinates for an element of SE(2) can be defined as x se(2) = [v 1, v 2, α]T and under this definition an element of the Lie algebra se(2) can be written as a 3 × 3 matrix

$$\displaystyle{ X^{\mathrm{se}(2)} = \left [\begin{array}{*{10}c} 0&-\alpha &v_{1}\\ \alpha & 0 &v_{ 2} \\ 0& 0 & 0 \end{array} \right ] =\sum _{ i=1}^{3}E_{ i}^{\mathrm{se}(2)}x_{ i}^{\mathrm{se}(2)}. }$$
(8.2)

The “hat” and “vee” notation is convenient to identify an element of se(2) with a vector in \(\mathfrak{R}^{3}\) as follows:

$$\displaystyle{\hat{\mathbf{x}}^{\mathrm{se}(2)} = X^{\mathrm{se}(2)}\,\,\,\mathrm{and}\,\,\,X^{\mathrm{se}(2)\vee } = \mathbf{x}^{\mathrm{se}(2)}.}$$

The exponential map \(\mathop{\mathrm{exp}}\nolimits:\mathrm{ se}(2) \rightarrow \mathrm{ SE}(2)\) is surjective, but is not injective since α = π and −π correspond to the same group element of SE(2). But by removing from SE(2) the set of measure zero, M, corresponding to α = π, it is possible to define an inverse map \(\log: (\mathrm{SE}(2) - M) \rightarrow \mathrm{ se}(2)\). Since the integrals of well-behaved functions over SE(2) and SE(2) − M are the same, we do not distinguish between SE(2) and SE(2) − M in the remainder of this chapter.

For SE(2), exponentiation gives

$$\displaystyle{ R = \left [\begin{array}{cc} \cos (\alpha )&\sin (\alpha )\\ \sin (\alpha ) &\cos (\alpha ) \end{array} \right ]\,\,\,\,\mathrm{and}\,\,\,\,\mathbf{t} = \left [\begin{array}{c} [v_{2}(\cos (\alpha ) - 1) + v_{1}\sin (\alpha )]/\alpha \\ {}[v_{1}(1 -\cos (\alpha )) + v_{2}\sin (\alpha )]/\alpha \end{array} \right ]. }$$
(8.3)

2.1.2 Adjoint Matrices

The adjoint operators Ad(g) and ad(X) are two important concepts in the derivations that follow, and so their definitions as well as relevant properties are introduced in this section. To define the adjoints, we need to first define the inner product and Lie bracket operations for Lie algebras. According to [4], an inner product between arbitrary elements of the Lie algebra \(Y =\sum _{i}y_{i}E_{i}\) and \(Z =\sum _{i}z_{i}E_{i}\) can be defined such that

$$\displaystyle{ (Y,Z) =\sum _{ i}^{d}y_{ i}z_{i}, }$$
(8.4)

where d is the dimension of G. In particular, for G = SE(n) the dimension is \(d = n(n + 1)/2\). Choosing a basis {E i } and requiring that (E i , E j ) = δ ij , where δ ij is the Dirac delta function, defines an inner product for \(\mathcal{G}\) and a metric for G.

The Lie bracket of \(Y,Z \in \mathcal{G}\) is defined as

$$\displaystyle{ [Y,Z]\doteq Y Z - ZY. }$$
(8.5)

With the above definitions in place, and any g ∈ G, the adjoint operators are

$$\displaystyle\begin{array}{rcl} \mathrm{Ad}(g)X& \doteq& \frac{d} {dt}(g \circ \mathop{\mathrm{exp}}\nolimits (tX) \circ g^{-1})\vert _{ t=0} = \frac{d} {dt}\left.\mathop{\mathrm{exp}}\nolimits (tgXg^{-1})\right \vert _{ t=0} = g\,X\,g^{-1}, \\ \mathrm{ad}(X)Y & \doteq& \left. \frac{d} {dt}(\mathrm{Ad}(e^{tX})Y )\right \vert _{ t=0}. {}\end{array}$$
(8.6)

Since the adjoint operators are both linear operators, they can both be written as matrices that represent linear mapping. We call these matrices adjoint matrices and define them as (in component form) [4]

$$\displaystyle{ [\mathrm{Ad}(g)]_{ij} = (E_{i},\mathrm{Ad}(g)E_{j}) = (E_{i},g\,E_{j}\,g^{-1}),\,\,\,\,\ [\mathrm{ad}(X)]_{ ij} = (E_{i},\mathrm{ad}(X)E_{j}) = (E_{i},[X,E_{j}]). }$$
(8.7)

Written in terms of columns, the matrices have the form

$$\displaystyle{ [\mathrm{Ad}(g)] = [(gE_{1}g^{-1})^{\vee },\ldots,(gE_{ n}g^{-1})^{\vee }],\,\,\,\,\ [\mathrm{ad}(X)] = [[X,E_{ 1}]^{\vee },\ldots,[X,E_{ n}]^{\vee }]. }$$
(8.8)

Some important properties of the adjoint matrices that are used in the following calculations are listed as follow:

  1. 1.

    \(\mathrm{Ad}(\mathop{\mathrm{exp}}\nolimits (X)) =\mathop{ \mathrm{exp}}\nolimits (\mathrm{ad}(X))\), ad(X)X ∨ = 0, \(\mathrm{ad}(X)Y = XY - Y X = [X,Y ]\)

  2. 2.

    ad(X)Y ∨ = [X, Y ]∨, \(\mathrm{ad}([X,Y ]) =\mathrm{ ad}(X)\mathrm{ad}(Y ) -\mathrm{ ad}(Y )\mathrm{ad}(X)\), \(\mathrm{ad}(X)Y ^{\vee } = -\mathrm{ad}(Y )X^{\vee }\)

  3. 3.

    \(\mathrm{Ad}(g_{1})\mathrm{Ad}(g_{2})X = g_{1}(g_{2}Xg_{2}^{-1})g_{1}^{-1} = (g_{1}g_{2})X(g_{1}g_{2})^{-1} =\mathrm{ Ad}(g_{1}g_{2})X\)

  4. 4.

    \(\log ^{\vee }(g \circ e^{X} \circ g^{-1}) =\mathrm{ Ad}(g)\log ^{\vee }(e^{X})\)

For SE(2), the explicit form of the adjoint matrices are

$$\displaystyle{ \mathrm{Ad}(g) = \left [\begin{array}{*{10}c} R &M\mathbf{t}\\ \mathbf{0} ^{T } & 1 \end{array} \right ] \in \mathfrak{R}^{3\times 3},\quad \mathrm{ad}(g) = \left [\begin{array}{*{10}c} -\alpha M &M\mathbf{v} \\ \mathbf{0}^{T} & 0 \end{array} \right ] \in \mathfrak{R}^{3\times 3} }$$
(8.9)

where \(M = \left [\begin{array}{*{10}c} 0 &1\\ -1 &0 \end{array} \right ]\) and R and t are defined by Eq. (8.1). (v, α) = (v 1, v 2. α) are the exponential coordinates of SE(2).

2.1.3 The Baker–Campbell–Hausdorff Formula

The Baker–Campbell–Hausdorf (BCH) formula [4] serves as the core of the second-order estimation of Gaussian convolutions (described in more detail in the next section). In essence, the BCH formula establishes a relationship between the Lie bracket [defined in Eq. (8.5)] and the matrix exponential. Let \(X,Y \in \mathcal{G}\) and define \(Z(X,Y ) =\log (e^{X}e^{Y })\), the BCH formula then takes the form

$$\displaystyle\begin{array}{rcl} Z(X,Y )& =& X + Y + \frac{1} {2}[X,Y ] + \frac{1} {12}([X,[X,Y ]] + [Y,[Y,X]]) \\ & & + \frac{1} {48}([Y,[X,[Y,X]]] + [X,[Y,[Y,X]]]) + \cdots \,. {}\end{array}$$
(8.10)

This can be verified by expanding e X, e Y using matrix exponential Taylor series \(e^{X} =\sum _{ k=0}^{\infty }\frac{X^{k}} {k!}\) and substitute into the Taylor series for matrix logarithm

$$\displaystyle{ \log (e^{X}e^{Y }) =\log (I + (e^{X}e^{Y } - I)) =\sum _{ k=1}^{\infty }(-1)^{k+1}\frac{(e^{X}e^{Y } - I)^{k}} {k}. }$$
(8.11)

Applying the \(\vee \) operator to (8.10) results in

$$\displaystyle\begin{array}{rcl} \mathbf{z}& =& \mathbf{x} + \mathbf{y} + \frac{1} {2}\mathrm{ad}(X)\mathbf{y} + \frac{1} {12}(\mathrm{ad}(X)\mathrm{ad}(X)\mathbf{y} +\mathrm{ ad}(Y )\mathrm{ad}(Y )\mathbf{x}) \\ & & + \frac{1} {48}(\mathrm{ad}(Y )\mathrm{ad}(X)\mathrm{ad}(Y )\mathbf{x} +\mathrm{ ad}(X)\mathrm{ad}(Y )\mathrm{ad}(Y )\mathbf{x}) + \cdots \,.{}\end{array}$$
(8.12)

Equations (8.11) and (8.12) will be used extensively.

2.2 Gaussians on SE(n) and Second-Order Convolution Theory

A Gaussian on the SE(n) is defined as

$$\displaystyle{ f(g;\mu,\varSigma )\doteq \frac{1} {C(\varSigma )}\mathop{\mathrm{exp}}\nolimits \left \{-\frac{1} {2}[\log ^{\vee }(\mu ^{-1}g)]^{T}\varSigma ^{-1}[\log ^{\vee }(\mu ^{-1}g)]\right \}, }$$
(8.13)

where μ, g ∈ SE(n), \(C(\varSigma ) \approx (2\pi )^{\frac{d} {2} }\|\mathrm{det}(\varSigma )\|^{\frac{1} {2} }\) is the normalizing factor when \(\|\varSigma \|\) is small.

For a domain of integration G = SE(n), the mean of the above Gaussian is defined by the value μ ∈ G for which

$$\displaystyle{ \int _{G}\!\log ^{\vee }(\mu ^{-1}g)f(g)\,\mathrm{d}g = \mathbf{0}, }$$
(8.14)

and the covariance is given by

$$\displaystyle{ \varSigma \doteq\int _{G}\![\log ^{\vee }(\mu ^{-1}g)][\log ^{\vee }(\mu ^{-1}g)]^{T}f(g)\,\mathrm{d}g. }$$
(8.15)

For details on how to integrate on SE(n), please refer to [15, 16]. Given two Gaussians \(f_{1}(g) = f(g;\mu _{1},\varSigma _{1})\) and \(f_{2}(g) = f(g;\mu _{2},\varSigma _{2})\) in the form of (8.13), their convolution is defined as

$$\displaystyle{ (f_{1} {\ast} f_{2})(g) =\int _{G}\!f_{1}(h)f_{2}(h^{-1}g)\,\mathrm{d}h =\int _{ G}\!\rho _{1}(\mu _{1}^{-1}h)\rho _{ 2}(\mu _{2}^{-1}h^{-1}g)\,\mathrm{d}h, }$$

with \(\rho _{i}(g) = f(g;e,\varSigma _{i})\) denoting a Gaussian centered at the identity. It is proven (refer to [16]) that the convolution (f 1 ∗ f 2)(g) results (to the second order) in a Gaussian with mean and covariance

$$\displaystyle{ \mu _{1{\ast}2} =\mu _{1}\mu _{2},\,\,\,\,\ \varSigma _{1{\ast}2} = A + B + F(A,B) }$$
(8.16)

with the terms A, B, and F defined by

$$\displaystyle{ A =\mathrm{ Ad}(\mu _{2}^{-1})\varSigma _{ 1}\mathrm{Ad}(\mu _{2}^{-1})^{T}\,\,\mathrm{and}\,\,B =\varSigma _{ 2}, }$$
(8.17)

where

$$\displaystyle\begin{array}{rcl} F(A,B)& =& \frac{1} {4}\sum \limits _{i,j=1}^{d}\mathrm{ad}(E_{ i})\,\,\,B\,\,\,\mathrm{ad}(E_{j})^{T}A_{ ij} + \frac{1} {12}\left \{[\sum \limits _{i,j=1}^{d}A_{ ij}^{''}]B + B^{T}[\sum \limits _{ i,j=1}^{d}A_{ ij}^{''}]^{T}\right \} \\ & & + \frac{1} {12}\left \{[\sum \limits _{i,j=1}^{d}B_{ ij}^{''}]A + A^{T}[\sum \limits _{ i,j=1}^{d}B_{ ij}^{''}]^{T}\right \} {}\end{array}$$
(8.18)

and

$$\displaystyle{ A_{ij}^{''} =\mathrm{ ad}(E_{ i})\mathrm{ad}(E_{j})A_{ij},\,\,\,\,\ B_{ij}^{''} =\mathrm{ ad}(E_{ i})\mathrm{ad}(E_{j})B_{ij}. }$$
(8.19)

The above results will be used for SE(2) in the next section, where the basis elements E i as well as Ad and ad matrices as defined in the previous section.

3 Derivation of Second-Order Bayesian Sensor Fusion on the SE Group

This section presents a detailed derivation of the proposed technique. Again the technique focuses on fusing the relative measurements of neighboring robots and their pose information to reduce the estimation uncertainty of the current robot. A probabilistic approach is adapted where uncertainties in the robot positions and sensor measurements are modeled by Gaussians (refer to [5] for a more generalized formulation of nonlinear measurement model approximation on Lie groups). In addition, since the motion of a stochastic system with differential constraints is modeled more accurately with Gaussians in exponential coordinates than that in Cartesian coordinates, the proposed technique is developed under exponential coordinates. The theory will first be developed for a system of two robots (which builds on [10] by taking sensor noise into consideration) and be extended to the multi-robot scenario.

3.1 Localization for a Robot Pair

The problem is given by two mobile robots i and j moving in the plane whose priors in position and orientation are Gaussians \(f(a_{i}^{-1}g_{i};\mu _{i},\varSigma _{i})\) and \(f(a_{j}^{-1}g_{j};\mu _{j},\varSigma _{j})\). Here a i , a j  ∈ SE(2) are the known initial positions of the robots relative to the world frame at t = 0. At time t, μ i , μ j  ∈ SE(2) and \(\varSigma _{i},\varSigma _{j} \in R^{3\times 3}\) are the means (defined relative to the initial frames a i , a j ), and covariances obtained from a previous prediction step which we will also assume to be known. In addition, a sensor measurement of robot j relative to i is also obtained at time t and is given by the homogeneous matrix m ij  ∈ SE(2). Since we assume the sensor has Gaussian noise, its distribution is then characterized by a Gaussian of the form \(M_{ij}(g_{i},g_{j}) = f(g_{j};g_{i}m_{ij},\varSigma _{m})\) which says that according to the sensor, the position of robot j with respect to robot i has a mean of m ij and covariance of \(\varSigma _{m}\).

The goal is then to calculate a posterior for the position of robot i using the sensor measurement to update its prior. Because the sensor provides a relative measurement, we first formulate the joint prior of robots i and j making the assumption that the priors are independent of each other, giving

$$\displaystyle{ p_{ij}(g_{i},g_{j}) = f(a_{i}^{-1}g_{ i};\mu _{i};\varSigma _{i})f(a_{j}^{-1}g_{ j};\mu _{j};\varSigma _{j}). }$$
(8.20)

Then according to Bayes’ Theorem, the joint posterior is given by

$$\displaystyle{ \overline{p}_{ij} =\eta _{1}p_{ij}M_{ij}, }$$
(8.21)

where η 1 is a constant normalizing factor. Similar normalizing factors result in all fusion processes that follow and will be denoted by η i . To save space in the derivations, we will denote \(\rho _{i}(\mu _{i}^{-1}g_{i}) = f(g_{i};\mu _{i};\varSigma _{i})\) and the rest follows where ρ i (g) is a Gaussian with mean at the identity. The marginal distribution of the joint posterior for robot i is then

$$\displaystyle\begin{array}{rcl} \overline{p_{i}}(g_{i})& =& f(g_{i};\overline{\mu }_{i},\overline{\varSigma }_{i}) =\eta _{2}\int _{G}\!p_{ij}(g_{i},g_{j})M_{ij}(g_{i},g_{j})\,\mathrm{d}g_{j} \\ & =& \eta _{2}\rho _{i}(\mu _{i}^{-1}a_{ i}^{-1}g_{ i})\int _{G}\!\rho _{j}(\mu _{j}^{-1}a_{ j}^{-1}g_{ j})\rho _{m}(m_{ij}^{-1}g_{ i}^{-1}g_{ j})\,\mathrm{d}g_{j}.{}\end{array}$$
(8.22)

The goal is to find closed-form expressions for \(\overline{\mu }_{i}\) and \(\overline{\varSigma }_{i}\). Since ρ m is symmetric around the mean, we have \(\rho _{m}(m_{ij}^{-1}g_{i}^{-1}g_{j}) =\rho _{m}(g_{j}^{-1}g_{i}m_{ij})\). Letting \(g^{{\prime}} = g_{i}m_{ij}\), Eq. (8.22) becomes

$$\displaystyle\begin{array}{rcl} \overline{p_{i}}(g_{i})& =& \eta _{2}\rho _{i}(\mu _{i}^{-1}a_{ i}^{-1}g_{ i})\int _{G}\!\rho _{j}(\mu _{j}^{-1}a_{ j}^{-1}g_{ j})\rho _{m}(g_{j}^{-1}g_{ i}m_{ij})\,\mathrm{d}g_{j} \\ & =& \eta _{2}\rho _{i}(\mu _{i}^{-1}a_{ i}^{-1}g_{ i})\int _{G}\!\rho _{j}(\mu _{j}^{-1}a_{ j}^{-1}g_{ j})\rho _{m}(e^{-1}g_{ j}^{-1}g^{{\prime}})\,\mathrm{d}g_{ j},{}\end{array}$$
(8.23)

where e ∈ SE(2) is the identity element of SE(2). According to the definition of convolution in Sect. 8.2, the integral in Eq. (8.23) defines a convolution \((f_{1} {\ast} f_{2})(g^{{\prime}})\) where \(f_{1}(g^{{\prime}}) = f(g^{{\prime}};a_{j}\mu _{j},\varSigma _{j})\) and \(f_{2}(g^{{\prime}}) = f(g^{{\prime}};e,\varSigma _{m})\). Let \(f_{1{\ast}2}(g^{{\prime}};\mu _{1{\ast}2},\varSigma _{1{\ast}2}) = (f_{1} {\ast} f_{2})(g^{{\prime}})\), then (8.16)–(8.19) can be used to calculate the closed-form expressions of μ 1∗2 (which equals to a j μ j ) and \(\varSigma _{1{\ast}2}\). With the integral taken care of, (8.23) becomes

$$\displaystyle{ \overline{p_{i}}(g_{i}) = f(g_{i};\overline{\mu }_{i},\overline{\varSigma }_{i}) =\eta _{2}f(\mu _{i}^{-1}a_{ i}^{-1}g_{ i};e,\varSigma _{i})f(g_{i}m_{ij};a_{j}\mu _{j},\varSigma _{1{\ast}2}). }$$
(8.24)

For a posterior of robot i formulated in the form of (8.24), the fusion technique developed in [10] can be used to derive the closed-form expressions for \(\overline{\mu }_{i}\) and \(\overline{\varSigma }_{i}\).

3.2 Localization for Multi-Robot Team

Now we are ready to extend the technique to multi-robot localization. Similar to the previous subsection, the posterior of robot i is what we are trying to estimate, but instead of taking measurement from a single neighboring robot, multiple measurements are taken from however many neighboring robots that are in the sensing range (for derivation purposes we label the neighboring robots as 1, 2, …, n). Following a similar approach we have the joint prior

$$\displaystyle\begin{array}{rcl} p_{i,1,\ldots,n}& =& f(a_{i}^{-1}g_{ i};\mu _{i};\varSigma _{i})f(a_{1}^{-1}g_{ 1};\mu _{1};\varSigma _{1})\ldots f(a_{n}^{-1}g_{ n};\mu _{n};\varSigma _{n}) \\ & =& \rho _{i}(\mu _{i}^{-1}a_{ i}^{-1}g_{ i})\rho _{1}(\mu _{1}^{-1}a_{ 1}^{-1}g_{ 1})\ldots \rho _{n}(\mu _{n}^{-1}a_{ n}^{-1}g_{ n}).{}\end{array}$$
(8.25)

Let \(M_{in} = f(g_{n};g_{i}m_{in},\varSigma _{in})\) be the distribution of the sensor measurement of robot n relative to robot i and assume independence among all the measurements. Then we have the joint measurement distribution

$$\displaystyle{ M_{i,1,\ldots,n} = M_{i1}M_{i2}\ldots M_{in}. }$$
(8.26)

To further save space, we will write in short \(\rho _{i} =\rho _{i}(\mu _{i}^{-1}a_{i}^{-1}g_{i})\) as the position priors and \(\rho _{in} =\rho _{in}(m_{in}^{-1}g_{i}^{-1}g_{n}) = M_{in}\) as the measurement distributions. We will further define \(g_{in}^{{\prime}} = g_{i}m_{in}\). The posterior for robot i is then

$$\displaystyle\begin{array}{rcl} \overline{p_{i}}(g_{i})& =& f(g_{i};\overline{\mu }_{i},\overline{\varSigma }_{i}) =\eta _{3}\int _{G}\!\int _{G}\!\ldots \int _{G}\!p_{i,1,\ldots,n}M_{i,1,\ldots,n}\,\mathrm{d}g_{1}\,\mathrm{d}g_{2}\ldots \,\mathrm{d}g_{n} \\ & =& \eta _{3}\rho _{i}\left (\int _{G}\!\rho _{1}\rho _{i1}\,\mathrm{d}g_{1}\right )\left (\int _{G}\!\rho _{2}\rho _{i2}\,\mathrm{d}g_{2}\right )\ldots \left (\int _{G}\!\rho _{n}\rho _{in}\,\mathrm{d}g_{n}\right ). {}\end{array}$$
(8.27)

Let \(f_{n}(g_{in}^{{\prime}}) = f(g_{in}^{{\prime}};a_{n}\mu _{n},\varSigma _{n})\) and \(f_{in}(g_{in}^{{\prime}}) = f(g_{in}^{{\prime}};e,\varSigma _{in})\), then (8.27) becomes

$$\displaystyle{ \overline{p_{i}}(g_{i}) = f(g_{i};\overline{\mu }_{i},\overline{\varSigma }_{i}) =\eta _{3}\rho _{i}(\mu _{i}^{-1}a_{ i}^{-1}g_{ i})(f_{1}{\ast}f_{i1})(g_{i1}^{{\prime}})(f_{ 2}{\ast}f_{i2})(g_{i2}^{{\prime}})\ldots (f_{ n}{\ast}f_{in})(g_{in}^{{\prime}}). }$$
(8.28)

Calculating the convolutions using (8.16)–(8.19), we finally arrive at

$$\displaystyle\fbox{$\begin{array}{rcl}\begin{array}{l} \overline{p_{i}}(g_{i}) = f(g_{i};\overline{\mu }_{i},\overline{\varSigma }_{i}) =\eta _{3}f(\mu _{i}^{-1}a_{i}^{-1}g_{i};e,\varSigma _{i})f(g_{i}m_{i1};a_{1}\mu _{1},\varSigma _{1{\ast}i1}) \times \ldots \times f(g_{i}m_{in};a_{n}\mu _{n},\varSigma _{n{\ast}in}) \end{array}& &{}\end{array}$}$$
(8.29)

An extension of the method provided by [10] (which fuses only one measurement) gives the equations to calculate \(\overline{\mu }_{i}\) and \(\overline{\varSigma }_{i}\) and is presented as follows:

For neighboring robots 1, …, k, …. , n

  1. 1.

    Define \(q_{k} = m_{ik}\mu _{k}^{-1}a_{k}^{-1}a_{i}\mu _{i}\), \(\mathop{\mathrm{exp}}\nolimits (\hat{x}_{k}) = q_{k}\)., \(\varGamma _{k} = (I + \frac{1} {2}\mathrm{ad}(\hat{x}_{k}))\), \(S_{i} =\varGamma _{ i}^{T}\varSigma _{i}^{-1}\varGamma _{i}\)

  2. 2.

    Define \(S_{k} =\varGamma _{ m}^{T}\mathrm{Ad}^{-T}(m_{ik})\varSigma _{k{\ast}ik}^{-1}\mathrm{Ad}^{-1}(m_{ik})\varGamma _{k}\)

  3. 3.

    \(\overline{S}^{{\prime}} = S_{i} +\sum \limits _{ k=1}^{n}S_{k}\), \(\overline{x}^{{\prime}} =\bar{ S}^{{\prime}-1}\sum \limits _{k=1}^{n}S_{k}x_{k}\)

With the above definitions, the posterior distribution for robot i can be calculated by

$$\displaystyle{\fbox{$\begin{array}{lll} \overline{\varSigma }_{i}& =&\bar{\varGamma }^{{\prime}}\bar{S}^{{\prime}-1}\bar{\varGamma }^{{\prime}T} \\ \overline{\mu }_{i}& =&\mu _{i}\mathop{ \mathrm{exp}}\nolimits (-\hat{\bar{x}}^{{\prime}}) \end{array} $}}$$
(8.30)

with the operator \(\wedge \) and \(\vee \) defined in section 8.2.1.

3.3 A Complete Distributed Localization Algorithm Using Bayesian Filter in Exponential Coordinates

The fusion technique introduced above defines the state update step for the proposed localization method. However, like all Bayesian filters a complete recursive filter for state estimation consists of a state prediction step as well as a state update step. This section serves to provide the proposed algorithm in such a form.

Similar to the above setting, suppose at time t k robot i is the robot to be localized, robots 1, …, k, …, n are its n neighbors. Their means are μ i (t k ), μ k (t k ) and covariances \(\varSigma _{i}(t_{k}),\varSigma _{k}(t_{k})\), respectively. Let the stochastic differential equation (SDE) governing the motion of the robots be of the form

$$\displaystyle{ (g^{-1}\dot{g})^{\vee }dt = \mathbf{h}dt + Hd\mathbf{w}, }$$
(8.31)

where h is constant. When g ≈ e and for a sampling time Δ t a constant command u is given to the system resulting in motion of the system from t k to t k+1, the distributed localization scheme that estimates the location of robot i at time t k+1 follows two steps (letting \(\varDelta t = t_{k+1} - t_{k}\)). These are the prediction and update steps.

3.3.1 Prediction

$$\displaystyle{\fbox{$\begin{array}{ll} \mu _{i}(\varDelta t) & =\mathop{ \mathrm{exp}}\nolimits \left (\int _{0}^{\varDelta t}\!\hat{\boldsymbol{h}_{i}}\,\mathrm{d}\tau \right ) \\ \varSigma _{i}(\varDelta t) & =\int _{ 0}^{\varDelta t}\!\mathrm{Ad}(\mu _{i}^{-1}(t-\tau ))H_{i}H_{i}^{T}\mathrm{Ad}^{T}(\mu _{i}^{-1}(t-\tau ))\,\mathrm{d}\tau \\ \mu _{i}(t_{k+1}^{-})& =\mu _{i}(t_{k}) \circ \mu _{i}(\varDelta t) \\ \varSigma _{i}(t_{k+1}^{-})& = A_{i}(t_{k}) + B_{i}(t_{k}) + F(A_{i}(t_{k}),B_{i}(t_{k})) \end{array} $}}$$
(8.32)

where

$$\displaystyle\begin{array}{rcl} A_{i}(t_{k})& =& \mathrm{Ad}(\mu _{i}(\varDelta t)^{-1})\varSigma _{ i}(t_{k}^{+})\mathrm{Ad}(\mu _{ i}(\varDelta t)^{-1})^{T},\,\,\,\,\ B_{ i}(t_{k}) =\varSigma _{i}(\varDelta t) \\ A_{i}(t_{k})_{ij}^{''}& =& \mathrm{ad}(E_{ i})\mathrm{ad}(E_{j})A_{i}(t_{k})_{ij},\,\,\,\,\ B_{i}(t_{k})_{ij}^{''} =\mathrm{ ad}(E_{ i})\mathrm{ad}(E_{j})B_{i}(t_{k})_{ij}{}\end{array}$$
(8.33)
$$\displaystyle\begin{array}{rcl} F_{i}(A_{i}(t_{k}),B_{i}(t_{k}))& =& \frac{1} {4}\sum \limits _{i,j=1}^{d}\mathrm{ad}(E_{ i})B_{i}(t_{k})\mathrm{ad}(E_{j})^{T}A_{ i}(t_{k})_{ij} \\ & & + \frac{1} {12}\left \{\Bigg[\sum \limits _{i,j=1}^{d}A_{ i}(t_{k})_{ij}^{''}\Bigg]B_{ i}(t) + B_{i}(t)^{T}\Bigg[\sum \limits _{ i,j=1}^{d}A_{ i}(t)_{ij}^{''}\Bigg]^{T}\right \} \\ & & + \frac{1} {12}\left \{\Bigg[\sum \limits _{i,j=1}^{d}B_{ i}(t_{k})_{ij}^{''}\Bigg]A_{ i}(t_{k}) + A_{i}(t_{k})^{T}\Bigg[\sum \limits _{ i,j=1}^{d}B_{ i}(t_{k})_{ij}^{''}\Bigg]^{T}\right \}{}\end{array}$$
(8.34)

The second equation in (8.32) follows from equation (19) in [13] given by

$$\displaystyle{ \varSigma _{i}(\varDelta t) =\int _{ 0}^{\varDelta t}\!Ad^{-1}[\mu _{ i}^{-1}(\tau )\mu _{ i}(t)]H_{i}H_{i}^{T}Ad^{-T}[\mu _{ i}^{-1}(\tau )\mu _{ i}(t)]\,\mathrm{d}\tau. }$$
(8.35)

Since in our context the mean takes the form of \(\mu =\mathop{ \mathrm{exp}}\nolimits (Xt)\) where \(X \in \mathcal{G}\) is a constant, it follows that \(\mu _{i}^{-1}(t)\mu _{i}(\tau ) =\mu _{i}(\tau )\mu _{i}^{-1}(t) =\mu _{ i}^{-1}(t-\tau )\). Combined with the property of adjoint \(Ad^{-1}(\mu ) =\mathrm{ Ad}(\mu ^{-1})\) gives the final expression in (8.32).

Also in the above equations, μ i (Δ t) and \(\varSigma _{i}(\varDelta t)\) define the incremental distribution resulting solely from the input given at the Δ t time frame with location given with respect to μ i (t k ), not the fixed world frame. In order to take into account the uncertainties already present at time t given by \(\varSigma _{i}(t_{k})\), the distribution at time t k is convolved with the incremental distribution resulting in the predicted distribution given by \(\mu _{i}(t_{k+1}^{-}),\varSigma _{i}(t_{k+1}^{-})\).

3.3.2 Update

Now to incorporate the relative measurements, for each of the neighboring robots 1, …, k, …, n, obtain the measurement distribution \(m_{ik}(t),\varSigma _{ik}(t)\), then

$$\displaystyle\begin{array}{rcl} A_{ik}(t_{k})& =& \mathrm{Ad}(m_{ik}(t_{k})^{-1})\varSigma _{ k}(t_{k})\mathrm{Ad}(m_{ik}(t_{k})^{-1})^{T},\,\,\,\,\ B_{ ik}(t_{k}) =\varSigma _{ik}(t_{k}) \\ A_{ik}(t_{k})_{ij}^{''}& =& \mathrm{ad}(E_{ i})\mathrm{ad}(E_{j})A_{ik}(t_{k})_{ij},\,\,\,\,\ B_{ik}(t_{k})_{ij}^{''} =\mathrm{ ad}(E_{ i})\mathrm{ad}(E_{j})B_{ik}(t_{k})_{ij}{}\end{array}$$
(8.36)

and using (8.18),

$$\displaystyle{ \varSigma _{k{\ast}ik}(t(t_{k})) = A_{ik}(t_{k}) + B_{ik}(t_{k}) + F(A_{ik}(t_{k}),B_{ik}(t_{k})) }$$
(8.37)
  1. 1.

    Define \(q_{k}(t_{k}) = m_{ik}(t_{k})\mu _{k}(t_{k})^{-1}a_{k}^{-1}a_{i}\mu _{i}(t_{k+1}^{-})\), \(\mathop{\mathrm{exp}}\nolimits (\hat{x}_{k}(t_{k})) = q_{k}(t_{k})\)

  2. 2.

    Define \(\varGamma _{k}(t_{k}) = (I + \frac{1} {2}\mathrm{ad}(\hat{x}_{k}(t_{k})))\), \(S_{i}(t_{k}) =\varGamma _{i}(t_{k})^{T}[\varSigma _{i}(t_{k+1}^{-})]^{-1}\varGamma _{i}(t_{k})\)

  3. 3.

    Define \(S_{k}(t_{k}) =\varGamma _{m}(t_{k})^{T}\mathrm{Ad}^{-T}(m_{ik}(t_{k}))\varSigma _{k{\ast}ik}(t_{k})^{-1}\mathrm{Ad}^{-1}(m_{ik}(t_{k}))\varGamma _{k}(t_{k})\)

  4. 4.

    \(\overline{S}^{{\prime}}(t_{k}) = S_{i}(t_{k}) +\sum \limits _{ k=1}^{n}S_{k}(t_{k})\), \(\overline{x}^{{\prime}}(t_{k}) =\bar{ S}^{{\prime}}(t_{k})^{-1}\sum \limits _{k=1}^{n}S_{k}(t_{k})x_{k}(t_{k})\)

Then

$$\displaystyle{\fbox{$\varSigma _{i}(t_{k+1}^{+}) =\bar{\varGamma } ^{{\prime}}(t_{ k})\bar{S}^{{\prime}}(t_{ k})^{-1}\bar{\varGamma }^{{\prime}}(t)^{t_{k} },\,\,\,\,\ \mu _{i}(t_{k+1}^{+}) =\mu _{ i}(t_{k+1}^{-})\mathop{\mathrm{exp}}\nolimits (-\hat{\bar{x}}^{{\prime}}(t_{ k})) $}}$$
(8.38)

4 Simulation and Discussions

This section provides verification for the proposed technique in a Matlab-simulated environment. A team of two-wheeled differential drive robots are moving in the field. The given inputs are such that all robots move along a straight line or a circular arc. However, due to the stochastic nature of the systems, errors accumulate over time such that odometry or dynamics alone is insufficient in estimating the robot poses. The results from the previous sections can therefore be used to update the robots’ knowledge of their poses with the help of measuring their positions relative to neighboring robots.

Figure 8.1 depicts a simple model of the two-wheeled differential drive robot which is very useful in modeling segway-like mobile bases and various multi-robot experimental platforms (E-pucks, iRobot create, Khepera, etc.). According to [10], the kinematics of such a mobile robot can be characterized by the stochastic differential equation

$$\displaystyle{ (g^{-1}\dot{g})^{\vee }dt = \left [\begin{array}{*{10}c} \frac{r} {2}(\omega _{1} +\omega _{2})\\ 0 \\ \frac{r} {2}(\omega _{1} -\omega _{2}) \end{array} \right ]dt+\sqrt{D}\left [\begin{array}{*{10}c} \frac{r} {2} & \frac{r} {2} \\ 0 & 0\\ \frac{r} {l} &-\frac{r} {l} \end{array} \right ]\left [\begin{array}{*{10}c} dw_{1} \\ dw_{2} \end{array} \right ], }$$
(8.39)

where g ∈ SE(2) is the homogenous matrix representing the pose of the robot, r is the wheel radius, l is the axle length, ω 1, ω 2 are the wheel angular velocities, dw i are unit strength Wiener processes, and D is a noise coefficient. This stochastic differential system can be simulated using Euler–Maruyama method described in [7]. Equation (8.39) can be written in short as

$$\displaystyle{ (g^{-1}\dot{g})^{\vee }dt = \mathbf{h}dt + \mathbf{H}d\mathbf{w} }$$
(8.40)

when g is close to the identity, given an input pair [ω 1, ω 2]T, the mean and covariance of system (8.39) can be estimated by

$$\displaystyle\begin{array}{rcl} \mu (t)& =& \mathop{\mathrm{exp}}\nolimits \left (\int _{0}^{t}\!\hat{\mathbf{h}}\,\mathrm{d}\tau \right ), \\ \varSigma (t)& =& \int _{0}^{t}\!\mathrm{Ad}(\mu ^{-1}(t-\tau ))\mathbf{H}\mathbf{H}^{T}Ad^{T}(\mu ^{-1}(t-\tau ))\,\mathrm{d}\tau.{}\end{array}$$
(8.41)

For simple motions like straight-line motion when ω 1 = ω 2, (8.41) can be evaluated analytically as

$$\displaystyle{ \mu (t)_{\mathrm{st}} = \left [\begin{array}{*{10}c} 1&0&r\omega t\\ 0 &1 & 0 \\ 0&0& 1 \end{array} \right ],\,\,\,\,\ \varSigma (t)_{\mathrm{st}} = \left [\begin{array}{*{10}c} \frac{1} {2}Dr^{2}t& 0 & 0 \\ 0 & \frac{2D\omega ^{2}r^{4}t^{3}} {3l^{2}} & \frac{D\omega r^{3}t^{2}} {l^{2}} \\ 0 & \frac{D\omega r^{3}t^{2}} {l^{2}} & \frac{2Dr^{2}t} {l^{2}} \end{array} \right ]. }$$
(8.42)

The same can be done with circular motion of constant curvature

$$\displaystyle\begin{array}{rcl} & & \mu (t)_{\mathrm{cir}} = \left [\begin{array}{*{10}c} \cos (\dot{\alpha }t)&-\sin (\dot{\alpha }t)& a\sin (\dot{\alpha }t) \\ \sin (\dot{\alpha }t)& \cos (\dot{\alpha }t) &a(1 -\cos (\dot{\alpha }t)) \\ 0 & 0 & 1 \end{array} \right ],\,\,\,\,\ \varSigma (t)_{\mathrm{cir}} = \left [\begin{array}{*{10}c} \sigma _{11} & \sigma _{12} & \sigma _{13}\\ \sigma _{ 21} & \sigma _{22} & \sigma _{23}\\ \sigma _{31 } & \sigma _{32 } & \sigma _{33} \end{array} \right ].{}\end{array}$$
(8.43)

where

$$\displaystyle\begin{array}{rcl} \sigma _{11}& =& \frac{c} {8}[(4a^{2} + l^{2})(2\dot{\alpha }t +\sin (2\dot{\alpha }t)) + 16a^{2}(\dot{\alpha }t -\sin (2\dot{\alpha }t))], \\ \sigma _{12}& =& \sigma _{21} = \frac{-c} {2} [4a^{2}(-1 +\cos (2\dot{\alpha }t)) + l^{2}]\sin (\dot{\alpha }t/2)^{2}, \\ \sigma _{13}& =& \sigma _{31} = 2ca(\dot{\alpha }t -\sin (2\dot{\alpha }t)), \\ \sigma _{23}& =& \sigma _{32} = -2ca(-1 +\cos (\dot{\alpha }t)), \\ \sigma _{33}& =& 2c\dot{\alpha }, \\ c& =& \frac{Dr^{2}} {l^{2}\dot{\alpha }}. {}\end{array}$$
(8.44)

With the pose priors calculated with (8.41)–(8.44), (8.36)–(8.38) are then applied with sensor measurements to update the priors. For arbitrary inputs (ω 1, ω 2) an approximation will be applied to (8.41) which will be discussed in the next section.

This example simulates localization of a robot team in straight and circular motion. In the setup of this simulation, the model based parameters are set as \(r = 0.033,l = 0.2\). The simulation parameters for straight-line motion are \(D = 5,v = 0.5,T = 1.3\), \(\omega _{1} =\omega _{2} = \frac{v} {r}\), and T = 2, \(\omega _{1} = 26.166,\omega _{2} = 21.408\) (for circular motion). The true robot motions are simulated 500 times using the Euler–Maruyama method [7] and the end position of each trial is plotted in the following figures. It can be observed that the posterior of such a stochastic differential system (SDE) results in a banana shaped distribution as is also discussed in [15].

In this simulation, all four robots are given the command to travel in a straight line for 1.3 % at 0.5 m/s or along an arc of constant curvature of 1 m at 45 deg/s for 1 s. The blue dashed lines in the figures represent the desired path of travel with the blue points at the two ends representing initial to final position. However, due to process noise each robot will eventually end up somewhere near the desired goal and our objective is to estimate its true position along with a quantification of our confidence of this estimate. Specifically for this example, the true pose of the middle robot (cyan colored) is what we are trying to estimate which we will call robot i, while the neighboring robots (yellow) are members of this team where relative measurements are obtained from. Among all the sampled end positions, one position for each robot is chosen as the true end pose (red point) and this is used to generate the mean of the measurement distribution m in .

As the first step, the prior mean and covariance of robot i is calculated using (8.42) and (8.43), and plotted in Fig. 8.2a, b, the resultant prediction aligns perfectly with the desire path (blue dash line), and the error “ellipse” marginalized over the heading angle is also plotted from the calculated covariance (magenta loop). Since this error “ellipse” is a contour of the resultant distribution, it can be observed that a Gaussian distribution under exponential coordinates is a much better fit for characterizing the uncertainties in an SDE of this kind than that under Cartesian coordinates. It is obvious that this prediction gives the same resultant distribution regardless of the true position and is only effected by the system dynamics and input commands. Therefore the next step is to update this prediction with measurements relative to neighboring robots.

It is assumed that robot i can exchange information with its neighbor when it comes into its sensing range, which means when a relative measurement is taken of neighbor j relative to robot i, the belief (mean and covariance in this case) that j holds for its current position can be communicated to i so that i can make use of this information in its localization process (update step). In this example, this belief (\(\mu _{j},\varSigma _{j}\)) for each neighboring robot j is taken to be the pose prediction calculated from (8.42) or (8.43), but in reality this can very well be the posterior from its own localization results. The covariance of the measurement distribution is chosen to be

$$\displaystyle{ \varSigma _{m} = \left [\begin{array}{*{10}c} 0.01 & 0.02 &0.001\\ 0.02 & 0.25 &0.015 \\ 0.002&0.0025& 0.15 \end{array} \right ]. }$$

Figures 8.3a, b show the updated posterior of robot i calculated from fusing the relative measurements taken from its three neighbors. The result indicates a more accurate position mean (black dot) and a shrinked error “ellipse” representing higher confidence in the estimate. Since this is a distributed localization technique aimed to be implemented on the embedded processor of each individual robot, the procedure is demonstrated only for one robot and the same goes for all other.

Fig. 8.3
figure 3

Pose update after sensor measurement and fusion. (a) Straight-line motion. (b) Circular motion

Full size image

5 Conclusion

This chapter proposed a distributed cooperative localization technique that can incorporate multiple sensor measurements to achieve higher estimation accuracy. Robots in a team can take measurements and exchange information among each other to update their knowledge of the current position. Simulation is used to validate the performance of the approach in Matlab. Results from the Matlab simulation show a good localization accuracy of the presented approach. The proposed multi-robot localization technique is distributed in that each robot can perform this localization process without the help of a centralized processor and is scalable for the computation time does not increase as the robot team enlarges and increases only linearly with the number of measurements taken. The generality of this scheme lies in the fact that uncertainties in the belief of the current robot, neighboring robots, and sensor measurements have all been considered which yields a more realistic estimate. Unlike sampling-based approaches, the proposed approach provides closed-form expressions which significantly increases computational efficiency. Most existing cooperative localization schemes possess a subset of the above attributes but rarely all. Lastly, this technique is of second order in its estimation of an updated posterior which is expected to be more accurate and reliable than first-order methods.

The limitation of this method is its dependency on Gaussian noises. Moreover, at present this is a local technique in that it depends on known initial poses and does not recover from localization failures (defined by [15]). In its current state, this approach does not possess the ability to serve as the sole scheme to localize a team of robots in that as errors accumulate in the beliefs of neighboring robots, erroneous information will be given to the current robot that leads to localization failures. However, this technique is local and prone to error accumulation only when none of the member robots have a reasonable estimate of their positions. As long as one robot possesses a good knowledge of its current pose (via more accurate sensors or sophisticated but computationally expensive algorithms) then this information can be used to drastically reduce the uncertainty of the entire team which introduces a level of robustness to the system and can also significantly reduce hardware and computational cost of the team. Table 8.2 shows a comparison of the proposed method with two of the most representative and accepted approaches.

Table 8.2 Final comparison
Full size table

The accuracy of the exponential localization method is expected to see great increase compared to results shown previously if the algorithm parameters (initial pose covariance, process and measurement noise covariances, etc.) can be fine-tuned. Establishing a systematic way of tuning these parameters can be a topic of its own. It is also incredibly beneficial if the proposed method can be combined with sampling-based approaches for their global localization and state recovery abilities. Lastly, experiments on hardware are required to fully establish the advantage of the proposed scheme. Overall this chapter has provided an alternative distributed cooperative localization technique in the domain of Lie group and exponential coordinates and has validated in simulation the potential of this technique as the next state of the art.