SLAM of Multi-Robot System Considering

Its Network Topology

H. W. Dong 1, Z. W. Luo

1,2

1 Kobe University, Kobe 657-8501, Japan

2 Biomimetic Control Research Center, RIKEN, Nagoya 463-0003, Japan

donghaiwei@cs11.cs.kobe-u.ac.jp

Abstract: As the technology of one-robot Simultaneous Localization and Mapping (SLAM) becomes mature, it’s time to solve SLAM problem for multi-robot SLAM case. Compared with one robot, multi robots could cooperate to accomplish SLAM task more efficiently, accurately and robustly. Previous research solves multi-robot SLAM problem by expanding the existing one-robot SLAM algorithms structure directly. However, the characteristics of multi-robots network change while SLAM. In this paper, we add the network structure into the estimation of multi-robot’ SLAM case and complete the mathematical derivation of multi-robot system SLAM.

Keywords: Multi-robot system, SLAM, Network topology

1. Introduction

As the technology of one-robot Simultaneous Localization

and Mapping (SLAM) becomes mature, it’s time to solve

SLAM problem for multi-robot SLAM case. Compared

with one robot, multi robots could cooperate to accomplish

SLAM task more efficiently, accurately and robustly.

Moreover, more complex task based on SLAM, such as

rescue in very large scale environment, planetary

exploration and so on can be completed with multi-robots.

However, Multi-robot SLAM is really a complicated

problem for lots of issues, such as sub-map merging,

complex data association and so on.

In fact, all the SLAM methods for one robot case can be

expanded to multi-robot case. By now there are many

approaches for multi-robot SLAM, such as extended

information filter (EIF), extended Kalman filter (EKF),

particle filter, decoupled SLAM and so on. There are two

kinds of solution for multi-robot SLAM. One is centralized

method in which all the SLAM estimation calculation is

done on the host computer. Sometimes these methods are

called as wireless sensor network; the other is distributed

method where each robot estimates the near-by

environment by itself. Therefore robots in such method are

really intelligent agents. In fact, centralized method seems

simple, but such method requires the host computer

high-efficient. Decentralized method is very flexible but

complex in computation.

Previous research solves multi-robot SLAM problem by

expanding the existing one-robot SLAM algorithms

structure directly. However, the characteristics of

multi-robots network change while SLAM. If we apply the

network structure into the estimation of multi-robot’ SLAM

case, the efficiency and accuracy will enhance a lot. We

address the less-well-studied problem of multi-robot

SLAM, motivated by the fact that previous multi-robot

SLAM research neglects the relationship between robots

(i.e. subsystems).

2. Multi-robot SLAM problem

For continence, define variables as followed: k

tξ The pose vector of the k -th robot and

landmarks at time t . k

tz The measurement vector of k -th robot at time t . k

tu The motion command of k-th robot at time t .

k

tµ Mean vector of

k

tξ .

k

tΣ Covariance matrix of k

tξ .

k

tη Information vector of

k

tξ .

k

tΛ Information matrix of k

tξ .

Definition The Kronecker product of matrices A and B is defined as

( )ij ij n mA B A B

×⊗ = × (1)

Where A and B are n by m matrices and A B⊗ is

an n by m matrix.

We are given M mobile robots equipped with

environment sensors. The robots operate in an environment

populated by N stationary landmarks whose location are

denoted as 1

[ ]T

NY y y= L .

At each point in time, k

tu alters the pose of k -th

robot. This pose transition is governed by a function kg

1

1

( , ) ( )n

k k k k k j T

t t t t x kj t x

jj k

k T

x t x

g u S A D S

S S

ξ ξ ξ ξ

δ

+=≠

= + + ⊗

+

∑ (2)

Where kg is the k -th robot’s motion model, a

vector-valued function which is non-zero only for the robot

pose coordinates, as feature locations are static in SLAM.

The stochastic part of this change is modeled by k

tδ , a

Gaussian random variable with zero mean and covariance k

tU . Here

xS is a projection matrix for the form

[ ]0 0T

xS I= L (3)

Where I is an identity matrix of the same dimension

as the robot pose vector t

x and as of t

δ . Each 0 refers

to a null matrix. A is the link adjacency matrix of robots

which indicates the topological structure of the robot

network, D denotes the link strength between robots. Each

robot can also sense relative information to nearby

landmarks (e.g., range and bearing). The measurements are

governed through the function kh

( , ) (0; )k k k k

t tz h Y N Zξ= + (4)

Where k

tQ is the covariance of the measurement noise.

The objective of multi-robot SLAM is to estimate a

posterior ( , | , )t

p X Y M U over all robot poses t

X and

all landmark locations Y from all available data,

M and U . Here M is the set of all measurements

acquired by all robots from time t; U is the set of all

controls.

3. Detail derivation of the solution

3.1 Motion update

The section concerns the update of the filter in accordance

to robot motion. The function g in (2) is approximated by

its first degree Taylor series expansion:

1 1 1 1 1

( , ) ( , ) ( , )[ ]k k k k k k k k k k k

t t t t t t t tg u g u g uξξ µ µ ξ µ− − − − −≈ + ∇ − (5)

Here 1

( , )k k k

t tg uξ µ −∇ is the derivative of g with

respect to kξ at 1

k k

tξ µ −= and k

tu . Plugging this

approximation into (2) leads to an approximation of k

tξ ,

the state at time t :

1 1

1

1 1 1

( , ) ( )

( , ) ( , )

nk k k k k j T

t t t t x kj t x

jj k

k k k k k k k k T

t t t t t x t x

I g u S A D S

g u g u S S

ξ

ξ

ξ µ ξ ξ

µ µ µ δ

− −=≠

− − −

≈ + ∇ + ⊗

+ − ∇ +

∑(6)

Hence, under this approximation the random variable

tξ is again Gaussian distributed. Its mean is obtained by

replacing t

ξ and t

δ in (6) by their respective means:

1 1

1

( , ) ( )n

k k k k k j T

t t t t x kj t x

jj k

g u S A D Sµ µ µ µ− −=≠

≈ + + ⊗

∑ (7)

The covariance of t

ξ is simply obtained by scaling and

adding the covariance of the Gaussian variables on the

right-hand side of (6):

1 1 1

1

( , ) ( , )

( )

Tk k k k k k k k

t t t t t t

nj T k T

x kj t x x t x

jj k

I g u I g u

S A D U S S U S

ξ ξµ µ− − −

=≠

Σ = + ∇ Σ + ∇

+ ⊗ +

∑ (8)

Update equations (7) and (8) are in EKF form, that is,

they are defined over means and covariance. The

information form is now easily recovered from the

definition of the information form.

1( )k k

t tH −= Σ (9)

And

( )k k T k

t t tb Hµ= (10)

Where

1

1 1 1

1

( ) ( , ) ( )n

k k k k k k T j T

t t t t t x kj t x

jj k

b H g u S A D Sµ µ µ−− − −

=≠

= + + ⊗

∑

It’s easy to see that motion updates can be decomposed

into individual robot updates, hence can be run decentrally.

3.2 Observation update

Put into probabilistic terms, (4)specifies a Gaussian

distribution over the measurement space of the form

1

( | )

1exp ( ( )) ( ) ( ( ))

2

k k

t t

k k T k k k

t t t t

p z

z h Z z h

ξ

ξ ξ− ∝ − − −

(11)

Approximate this Gaussian by linearizing the

measurement function kh .

( ) ( ) ( )[ ]k k k k k k

t t t t th h hξξ µ µ ξ µ≈ + ∇ − (12)

Where ( )k k

thξ µ∇ is the first derivation of k

h with

respect to the state variable kξ , taken k k

tξ µ= . This

approximation leads to the following Gaussian

approximation of the measurement density (11)

1

1

1( | ) exp{ ( ) ( ) ( ) ( )

2

( ( ) ( ) ) ( ) ( ) }

k k k T k k T k k k k

t t t t t t

k k k k k k T k k k k

t t t t t t

p z h Z h

z h h Z h

ξ ξ

ξ ξ

ξ ξ µ µ ξ

µ µ µ µ ξ

−

−

∝ − ∇ ∇

+ − + ∇ ∇

(13)

We are now in the position to state the measurement

update equation, which implement the probabilistic law [1]

( )

( )

1

1

1

( | , ) ( | ) ( | , )

1exp{ ( ) ( ) ( ) ( )

2

( ( ) ( ) ) ( ) ( ) }

k k k k k k k k

t t t t t t t t

k T k k k T k k k k

t t t t t

k k k k k k k T k k k

t t t t t t t

p M U p z p M U

H h Z h

b z h h Z h

ξ ξ

ξ ξ

ξ ξ ξ

ξ µ µ ξ

µ µ µ µ ξ

−

−

−

=

∝ − + ∇ ∇

+ + − + ∇ ∇

(14)

Thus, the measurement update is given by the following

additive rules:

1

1

( ) ( ) ( )

( ( ) ( ) ) ( ) ( )

k k k k T k k k

t t t t

k k k k k k k k T k k k

t t t t t t t

H H h Z h

b b z h h Z h

ξ ξ

ξ ξ

µ µ

µ µ µ µ

−

−

= + ∇ ∇

= + − + ∇ ∇

(15)

Notice that the updates (15) are additive; they are easily

decomposed into individual robot updates, making it

amenable to a decentralized implementation.

4. Discussion

With the development of SLAM technology, SLAM problem consisting multi robots attracts more and more attention. This paper presents a new solution of multi-robot system which could make full use of network topology. Our future work will focus on some specific issue in multi-robot SLAM problem, such as sub map merging and etc.

References

[1] S. Thrun and Y. Liu, Multi-robot SLAM with sparse extended information filters, Robotics Research, Vol. 15, pp. 254-266, 2005.