View
47
Download
0
Category
Preview:
DESCRIPTION
Citation preview
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.
Recommended