37 9

2011 9

Vol. 37, No. 9

ACTA AUTOMATICA SINICA

September, 2011

Voronoi
1, 2

1, 3

(Simultaneous localization and mapping, SLAM)

, Voronoi VorSLAM.
, Voronoi , .
. Voronoi ,
. ,
. , ,
. VorSLAM .

, Voronoi , ,

DOI

10.3724/SP.J.1004.2011.01095

Simultaneous Localization and Mapping Through a Voronoi-diagram-based

Map Representation
GUO Shuai1, 2

MA Shu-Gen1, 3

LI Bin1

WANG Ming-Hui1

WANG Yue-Chao1

Abstract To solve the problem that the current map division methods in the hybrid metric map based simultaneous
localization and mapping (SLAM) are not complete, this paper proposes an algorithm VorSLAM, which uses a new map
representation based on Voronoi diagram. The VorSLAM builds a feature based map in the global reference frame firstly.
By operating Voronoi diagram on the feature map, it then divides the whole environment space uniquely into a series of
local regions, in which dense local maps are built. The feature map together with the local maps gives the environment
a continuous and dense description. The map representation based on Voronoi diagram ensures that the map division is
unique. In each region, the local environment coutour is proven to be described completely by the corresponding local
map. A basic character of the proposed map representation is that each global feature associates with a local dense
map, which is defined relative to this feature. Utilizing this character, a data association (DA) approach based on shape
matching is presented to solve the ambiguity problem of traditional DA algorithms. An experiment carried out in a long
arched corridor illustrates the effectiveness of VorSLAM algorithm and the shape matching data association method.
Key words

Simultaneous localization and mapping (SLAM), Voronoi diagram, data association (DA), mobile robot

(Simultaneous localization and mapping, SLAM)

,
, [12] .
.
1987 , Smith [3]
SLAM ,
(Extended Kalman filter, EKF)
2010-12-29
2011-02-18
Manuscript received December 29, 2010; accepted February 18,
2011
(60905058, 61075103)
Supported by National Natural Science Foundation of China
(60905058, 61075103)
1.
110016 2. 100039 3.
525-8577
1. State Key Laboratory of Robotics, Shenyang Institute of
Automation, Chinese Academy of Sciences, Shenyang 110016,
P. R. China 2. Graduate University, Chinese Academy of Sciences, Beijing 100039, P. R. China 3. Department of Robotics,
Ritsumeikan University, Kusatsu-Shi 525-8577, Japan

. 20 ,
SLAM [12] . ,
[48] .
,
.
SLAM
: 1)
SLAM . R-B (Raoblackwellized particle filter, RBPF)
, .
GridSLAM[9] DPSLAM[1011] ; 2)
SLAM .
Scan-SLAM[12] (Simultaneous localization and sampled environment
mapping, SLASEM)[1314] DenseSLAM[1516]
. SLAM
, ,

1096

. ,
SLAM
.
Scan-SLAM[12]
(Template) ,
.
,
. SLASEM[1314] (Sampled environment map, SEM) ,
.
DenseSLAM[1516]
(Local triangle regions, LTRs)
, LTR
. , Scan-SLAM[12]
SLASEM[1314] SLAM
, DenseSLAM[1516]
SLAM .
,
.

.
DenseSLAM[1516]
: 1) DenseSLAM
, LTRs ,
LTRs ; 2)
,
, .
LTRs ,
SLAM
.
, Voronoi
SLAM VorSLAM.
,
Voronoi
. ,
.
.
DenseSLAM[1516] , Voronoi
: 1) , Voronoi
,
; 2) ,
DenseSLAM ,
.
Voronoi
, .
SLAM ,
. (Nearest neighbor,

37

NN) , Neira Tardos[17]

,
(Joint compatibility branch
and bound, JCBB) .
.
, JCBB
. Voronoi ,
,
. ,
.
: 1 EKFSLAM; 2 VorSLAM ; 3
; 4
; 5 .

1 EKF-SLAM
EKF-SLAM .
, k
X r,k = [xr,k yr,k r,k ]T R3 , fi
T
m
X fi = [p1fi p2fi pm
fi ] R .
n 1
:
T

T
T
T
mn+3
XT
X k = [X
r,k X f1 X f2 X fn ] R

(1)

X k
. EKF-SLAM , X k
k Pk|k .
X
(Landmark)[48] ,
, X fi = [xfi yfi ]T R2 .
k + 1 X r,k X r,k+1 .
k k + 1 ,
X k+1 :
1.1
k k + 1

X k , uk , w k )
X k+1 = f (X

(2)

, u k ; w k u k
, Qk ,
w k N(00, Qk ).
, k X k
k Pk|k ) k + 1
( X
X k+1 :

k+1|k = f (X
k , uk , 0)
X

(3)

Pk+1|k = F Pk|k F T + GQk GT

(4)

n ; i, j .

: Voronoi

1097

f
f
F =
,
G
=

X X k ,uuk
u X k ,uuk
X
u

VPi = gVoronoi(P, p i )

(2) X k
u k .

1.2

, X k :

X k, v k)
z k = g(X

(5)

, v k ,
Rk , v k N(00, Rk ).
k + 1 , X r,k+1
k+1|k
z k+1 . X

zk+1 = g(X k+1|k , 0 ).

X k+1 :

k+1 = z k+1 zk+1

(6)

k+1 = X
k+1|k + Wk+1 k+1
X

(7)

T
Pk+1|k+1 = Pk+1|k Wk+1 Sk+1 Wk+1

(8)

1
Wk+1 = Pk+1|k H T Sk+1

(9)

Sk+1 = HPk+1|k H T + KRk+1 K T

P p i
Voronoi . Voronoi :

kqq , p i k < kqq , p j k, j 6= i, q VPi

a, b k a , b .
, ka
Voronoi ,
.
[2021] ,
Voronoi (General voronoi graph,
GVG) , Voronoi
.
SLAM , :
,
1 .
M F ,

V D(M F ), .
,
.
, Voronoi
VorSLAM ,
.

(10)

g
g
, K=
H=

X X k+1|k ,vv k+1 =00

X
vv X k+1|k ,vv k+1 =00
(5) X k+1 v k+1
.
,
.

2 VorSLAM
VorSLAM , Voronoi ,
VorSLAM Voronoi .
Voronoi [1819] .
(R2 ) n
P = {ppi |i = 1, 2, , n} ( ),
Voronoi n
:

VD(P ) = Voronoi(P )

1 Voronoi
Fig. 1 An indoor environment divided by
Voronoi diagram

VorSLAM,
; ;
.

2.1 VorSLAM
2 VorSLAM .
EKF-SLAM ,
. ,
.

1098

37

.
,
.
Or .
1 .
1
Table 1

2
Fig. 2

VorSLAM

Flow diagram of VorSLAM

1 EKF-SLAM
.
. ( i fi )
Ffi , , x
.
1 , C4 XOY .
,
. ,
. fi ,
Ffi FG ,
X fi = [xfi yfi fi ]T R3 .
EKF-SLAM :

XT
X k = [X
r,k xf1 yf1 f1 xf2 yf2 f2
xfn yfn fn ] T R3n+3

Definition of symbols

FG

Fr

()

0
j

O r j

2.1.1

O r fj , S = {fj }

Ff 0

fj

orj

S O r Voronoi , fj

, O r = {orj }
0

oj

orj Fr Ff 0

fi

Ffi

fi

MF

, M F = {fi }

MiL

Ffi , fi

M = {M F , {MiL }}

(11)

, fi Ffi
Fr .

X r , X fi ) =
z fi = [x , x , ]T = h(X

(xfi xr )cos r +(yfi yr )sin r

(xfi xr )sin r +(yfi yr )cos r (12)

fi r

1) S = {fj },
Ffj0 . S
Voronoi ,
0
VPj = gVoronoi(S, fj ) ,
0
orj . orj Fr Ffj0 , oj .
3 , ,
.

(Landmark) , (12)
. ,
,
.
EKF-SLAM ,
,
M F .

2.1.2
VorSLAM , MiL
VPi = gVoronoi(M F , fi )
M F . , MiL fi

3
Fig. 3

Feature extraction and raw sensor data dividing

2) 2 {(i1 , j1 ), (i2 , j2 ), , (im , jm )}

2

3 .

: Voronoi
0

, fj1 fi1 , orj1

MiL1 . , orj2 , ,
orjm MiL2 , , MiLm .
MiL MiL
0
oj .
, 3
,
.
3) VorSLAM , MiL
VPi = gVoronoi(S, fi ) .
MiL
VPi . Voronoi ,
VPi fi
ft . fi ft
, fi ( Ffi )
,
VPi = gVoronoi(M F , fi )
. , fi
ft
:
" # "p
#
d
(xfi xft )2 +(yfi yft )2
D=
=
(13)

i t
:

CD = BPk|k B T

(14)

D
D
, B = X
| k D X .
X X
CD , fi ft
, .

2.2 VorSLAM
: 1
; 2 VorSLAM
.

2.2.1
EKF-SLAM ,
,
.

.
, ,
1. Nieto [16] DenseSLAM
,
.
SLAM (Sparse extended
information filters, SEIF)[4] ,

2.3 .

1099

. ,
.
Nieto
.
Nieto :
,
, .
VorSLAM Voronoi
, fi
VPi . VPi MiL
Ffi . , fi
fm (m 6= i) ,
L
MiL Mm
. Voronoi
.

2.2.2
2.1.2 .
: {MiL , i = 1, 2, }
.
1. M F Voronoi fd
gVoronoi(M F , fd );
S Voronoi fd
gVoronoi(S, fd ). gVoronoi(M F , fd )
gVoronoi(S, fd ) .
. M F = {fi |i = 1, 2, ,
0
n}, S = {fj |j = 1, , m}.
M F , S
0
M F , S S = {fj } = {fij |j = 1, ,
m, ij [1, n]}.
Voronoi , q gVoronoi(S, fij ), :
d(q, fij ) d(q, fis ), is [1, n], is 6= ij

(15)

, p gVoronoi(M F , fv ), :

d(p, fv ) d(p, fs ), s = 1, , n, s 6= v

(16)

S M F , fd S,
fd M F , p gVoronoi(M F , fd )
(16) (15), p gVoronoi(S, fd ),
gVoronoi(M F , fd ) gVoronoi(S, fd ).

,
.
,
2.1.2 , MiL
0
gVoronoi(M F , fi ) oj (
0
fj fi ). 2.1.2
MiL VPi =
gVoronoi(M F , fi ) .
Voronoi ,
.

1100

2.3 VorSLAM
,
.
2.3.1

VorSLAM ,
. ,
Or , VorSLAM
(Iterative end point fit, IEPF)[2223]
.
,
,
. IEPF
,
. , ,
(Covariance propagating)[24]
. 3 ,
X ,
3 .
2.3.2
VorSLAM ,
. ,
,
0
0
M F . , Ffj oj
0
, oj
MiL . ,
.
0
oj
0
MiL ( fj fi ) , VorSLAM
(Scan match)[25] .

,
. SLAM , (Iterative closest
point, ICP)[2627] ,
. ICP
,
. Lu [28]
ICP :
0

[zz , C] = ICP scanmatch(MiL , oj )

(17)

, zz fj ,
: z fj0 = z fj0
zz (zz f 0 Fr Ff 0
j

, fj ). C (
(10) R), .
0
ICP , MiL oj
;
, VorSLAM

37

. ,
,
, .

3
SLAM
.
0
S = {fj } M F = {fi }
{(i1 , j1 ), , (im , jm )}.
NN JCBB
,
. Voronoi
, NN
.
VorSLAM , MiL
Ffi , fi ,
fi . ,
0
oj Ffj0 ,
0

fj . ,
0
(is , js ) (fis , fjs )
0
(MiLs , ojs ) ,
.
,
.
,
,
; ,
, .
, :
VS VB.
Z
1 2
VS =
kr1 r22 k d
(18)
2
Z
VB =
d
(19)

, ,
, r1 r2
. 4 ,
,
. ,
, VS = s1 + s2 , VB =
+ . (18) (19)

VS =
0.5 kr12 r22 k
(20)
512

X
VB =
(21)
512

: Voronoi

, r1 , r2 ,
.

4
Fig. 4

512

The diagram of environment visibility

:
0
1) fjs , NN
s = {fiz |z = 1, 2, }.
2)
0
(fiz , fjs ) .
0

1101

) Voronoi Voronoi(M F ).
, Voronoi
,
.
,
, ;
,
EKF-SLAM
, EKF-SLAM O(N 2 )
.
.
2 URG-04LX
Table 2 Parameters of the scanning laser range
finder URG-04LX
(r)

20 mm 4 000 mm

(b)

1 31 = 240o

200 ms/scan

/512 rad 0.36o

r = 20 1 000 mm: 10 mm;

[VS, VB]iz = Env visibility(MiLz , ojs )

r = 1 000 4 000 mm: 1 % r

3) [VS, VB]is .
VS, VB ,
(is , js ) ,
, ; ,
0
, fjs .

VorSLAM . 5 (c)
. ,

. URG-04LX
( 5 (a)).
, 2.
, 180 mm,
10 .
.
ICP ,
,
.
diag{(10 mm)2 , (10 mm)2 , (4o )2 }.
, VorSLAM
6 (a) , ,
,
. 6 (b)
, (

(b)

(a)
URG-04LX
(a) Scanning laser range

(b) The handcart used as

finder URG-04LX

the carrier in our experiment

(c)
(c) Environment for experiment

5
Fig. 5

The experiment

6 (b)

1102

Voronoi(M F ) .
6 (b) 5 (c)
.
,
, VorSLAM
.

37

,
,
.

(a) VS VB (
VS , 0.5 VB
)
(a) VorSLAM (,
)
(a) The map built by VorSLAM (Black points denote
locations of corners and gray points denote

(a) The curves of values of VS and VB (Gray solid curve

denotes the VS and gray solid line denotes the threshold.
Gray dotted curve denotes the 0.5VB and gray dashed line
denotes 0.5VB thresholds.)

the local dense maps.)

(b) NN , VS = 2.123, VB = 0.71107

(b)

(b) An example of wrong DA, VS = 2.123 and VB = 0.71107

(b) A local dense map

6
Fig. 6

Fig. 7

Experimental results

7
500 VS VB . ,
VS VB 0.2 /6.
, VS VB
NN . 7 (a)
[70 140] [290 320] NN
. 7 (b) .

DA results

5
VorSLAM Voronoi
. EKF-SLAM
;
Voronoi ,
.
.

: Voronoi

, VorSLAM
. Voronoi ,

References
1 Durrant-Whyte H, Bailey T. Simultaneous localization and
mapping: part I. IEEE Robotics and Automation Magazine,
2006, 13(2): 99110
2 Bailey T, Durrant-Whyte H. Simultaneous localization and
mapping (SLAM): part II. IEEE Robotics Automation Magazine, 2006, 13(3): 108117
3 Smith R, Self M, Cheeseman P. A stochastic map for uncertain spatial relationships. In: Proceedings of the 4th International Symposium on Robotics Research. Massachusetts,
USA: The MIT Press, 1988. 467474
4 Thrun S, Liu Y F, Koller D, Ng A Y, Ghahramani Z,
Durrant-Whyte H. Simultaneous localization and mapping
with sparse extended information filters. The International
Journal of Robotics Research, 2004, 23(78): 693716
5 Guivant J E, Nebot E M. Optimization of the simultaneous localization and map-building algorithm for real-time
implementation. IEEE Transactions on Robotics and Automation, 2001, 17(3): 242257
6 Montemerlo M, Thrun S, Koller D, Wegbreit B. FastSLAM:
a factored solution to the simultaneous localization and
mapping problem. In: Proceedings of the 18th National
Conference on Artificial Intelligence. Edmonton, Canada:
AAAI, 2002. 593598
7 Montemerlo M, Thrun S, Koller D, Wegbreit B. FastSLAM
2.0: an improved particle filtering algorithm for simultaneous localization and mapping that provably converges. In:
Proceedings of the International Joint Conference on Artificial Intelligence. Acapulco, Mexico: Morgan Kaufmann,
2003. 11511156
8 Zhu Ji-Hua, Zheng Nan-Ning, Yuan Ze-Jian, Zhang Qiang.
A SLAM algorithm based on central difference particle filter.
Acta Automatica Sinica, 2010, 36(2): 249257
(, , , . SLAM
. , 2010, 36(2): 249257)
9 Hahnel D, Burgard W, Fox D, Thrun S. A highly efficient
FastSLAM algorithm for generating cyclic maps of largescale environments from raw laser range measurements. In:
Proceedings of the IEEE/RSJ International Conference on
Intelligent Robots and Systems. Washington D. C., USA:
IEEE, 2003. 206211
10 Eliazar A I, Parr R M. DP-SLAM: fast, robust simultaneous localization and mapping without predetermined landmarks. In: Proceedings of the International Joint Conference on Artificial Intelligence. Acapulco, Mexico: Morgan
Kaufmann, 2003. 11351142

1103

11 Eliazar A I, Parr R. DP-SLAM 2.0. In: Proceedings of the

IEEE International Conference on Robotics and Automation. Washington D. C., USA: IEEE, 2004. 13141320
12 Nieto J, Bailey T, Nebot E. Scan-SLAM: combining EKFSLAM and scan correlation. In: Proceedings of the 5th
International Conference on Field Robotics. Port Douglas,
Australia: Springer, 2005. 167178
13 Sun R C, Ma S G, Li B, Wang Y C. Simultaneous localization and sampled environment mapping. In: Proceedings of
the 48th IEEE Conference on Decision and Control and Held
Jointly with the 28th Chinese Control Conference. Shanghai, China: IEEE, 2009. 64846489
14 Sun Rong-Chuan, Ma Shu-Gen, Li-Bin, Wang Ming-Hui,
Wang Yue-Chao. Simultaneous localization and sampled environment mapping based on a divide-and-conquer ideology.
Acta Automatica Sinica, 2010, 36(12): 16971705
(, , , , .
. , 2010, 36(12): 16971705)
15 Nieto J I, Guivant J E, Nebot E M. The hybrid metric maps
(HYMMs): a novel map representation for DenseSLAM.
In: Proceedings of the IEEE International Conference on
Robotics and Automation. Washington D. C., USA: IEEE,
2004. 391396
16 Nieto J I, Guivant J E, Nebot E M. DenseSLAM: simultaneous localization and dense mapping. The International
Journal of Robotics Research, 2006, 25(8): 711744
17 Neira J I, Tardos J D. Data association in stochastic mapping using the joint compatibility test. IEEE Transactions
on Robotics and Automation, 2001, 17(6): 890897
18 Barber C B, Dobkin D P, Huhdanpaa H. The quickhull algorithm for convex hulls. ACM Transactions on Mathematical
Software, 1996, 22(4): 469483
19 Aurenhammer F. Voronoi diagrams a survey of a fundamental geometric data structure. ACM Computing Surveys,
1991, 23(3): 345405
20 Doh N L, Chung W K, Lee S, Oh S, You B. A robust general Voronoi graph based SLAM for a hyper symmetric environment. In: Proceeding of the IEEE/RSJ International
Conference on Intelligent Robots and Systems. Washington
D. C., USA: IEEE, 2003. 218223
21 Choset H, Nagatani K. Topological simultaneous localization and mapping (SLAM): toward exact localization without explicit localization. IEEE Transactions on Robotics and
Automation, 2001, 17(2): 125137
22 Duda R O, Hart P E. Pattern Classification and Scene Analysis. New York: John Wiley and Sons, 1973. 1115
23 Borges G A, Aldon M J. Line extraction in 2D range images for mobile robotics. Journal of Intelligent and Robotic
Systems, 2004, 40(3): 267297
24 Haralick R M. Propagating covariance in computer vision.
In: Proceedings of the 12th IAPR International Conference
on Pattern Recognition. Washington D. C., USA: IEEE,
1994. 493498

1104

25 Besl P J, McKay H D. A method for registration of 3-D

shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2): 239256
26 Guo R, Sun F C, Yuan L. ICP based on polar point matching
with application to graph-SLAM. In: Proceedings of the
International Conference on Mechatronics and Automation.
Changchun, China: IEEE, 2009. 11221127

University, Japan. He is also holding professor position

at Shenyang Institute of Automation, Chinese Academy of
Sciences. His research interest covers biomimetic robots,
mobile robots, and multi-robot cooperation.)

. ,
.
E-mail: libin@sia.cn
(LI Bin Professor at Shenyang Institute of Automation, Chinese Academy
of Sciences. His research interest covers
biomimetic robots, mobile robots, and

27 Zhang Z Y. Iterative point matching for registration of freeform curves and surfaces. International Journal of Computer
Vision, 1994, 13(2): 119152
28 Lu F. Shape Registration Using Optimization for Mobile Robot Navigation [Ph. D. dissertation], University of
Toronto, Canada, 1995

.
.
. E-mail: guoshuai@sia.cn
(GUO Shuai
Ph. D. candidate at
Shenyang Institute of Automation, Chinese Academy of Sciences. His research
interest covers mobile robot and simultaneous localization and mapping (SLAM). Corresponding
author of this paper.)
,
.
,
.
E-mail: shugen@se.ritsumei.ac.jp
(MA Shu-Gen Professor at the
Department of Robotics, Ritsumeikan

37

robot control.)

. ,
.
E-mail: mhwang@sia.cn
(WANG Ming-Hui Associate professor at Shenyang Institute of Automation, Chinese Academy of Sciences. His
research interest covers mobile robots,
robot control, and multi-robot cooperation.)

. .
E-mail: ycwang@sia.cn
(WANG Yue-Chao
Professor at
Shenyang Institute of Automation, Chinese Academy of Sciences. His main research interest is robotics.)