prev

next

out of 2

Published on

01-Nov-2014View

44Download

0

DESCRIPTION

Transcript

- 1. 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, its 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, its 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 m A 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 k g 1 1 ( , ) ( ) n k k k k k j T t t t t x kj t x j j k k T x t x g u S A D S S S + = = + + + (2) Where k g is the k -th robots 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 0 T xS I= L (3) Where I is an identity matrix of the same dimension
- 2. as the robot pose vector tx 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 k h ( , ) (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 ( , | , )tp X Y M U over all robot poses tX 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 ( , ) ( ) ( , ) ( , ) n k k k k k j T t t t t x kj t x j j 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 j j 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 n j T k T x kj t x x t x j j 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 j j k b H g u S A D S = = + + Its 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 ( | ) 1 exp ( ( )) ( ) ( ( )) 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 k h . ( ) ( ) ( )[ ]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 ( | , ) ( | ) ( | , ) 1 exp{ ( ) ( ) ( ) ( ) 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.