Proceedings of the 2016 IEEE

International Conference on Robotics and Biomimetics

Qingdao, China, December 3-7, 2016

An Approach to Restaurant Service Robot SLAM

Jinglin Zhang, Yongsheng Ou∗ , Guolai Jiang and Yimin Zhou

Abstract— Recently service robots are becoming a part of our and Mapping(SLAM) problem. It is considered to be the key
daily life. There is a huge market for robot since many human to the realization of autonomous exploration. Kalman Filter
friendly service robots are designed. In this paper we describe based algorithms and Particle Filter based algorithms are
a system of Simultaneous Localization and Mapping(SLAM) on
restaurant service robot using a depth camera and a move base the most popular probability based methods. These statistical
with odometry and gyro. We create a virtual laserscan from techniques aim at computing an estimate of robot’s location
the depth camera, which provides depth image to be converted xt and a map of the environment mt according to the
into 2D laser scan. Meanwhile, the color image provided by observations zt on discrete time t. This objective can be
the depth camera could be used to extract feature descriptions described with probabilistic expression p (xt , mt |z1:t ).
which will be used in the loop closure process. The depth camera
act as a low cost laser scanner and a RGB camera in this case. The Extended Kalman Filter(EKF) is the nonlinear version
Our method is under particle filter frame. The robot builds of Kalman Filter(KF) [2][3][4], which use a first order Taylor
map incrementally when exploring new environment. This make expansion to approximate nonlinear models. The process and
it can accurately locate itself in the working environment observation noises are assumed to obey normal distribution
and complete routine navigation tasks autonomously. We will in EKF. It is sub-optimal filter based on minimum variance
introduce the fundamental mathematical model and conduct
experiments to show it. criterion when the measurement and state transition model
are nonlinear. Accordingly, this procedure would lead to
I. INTRODUCTION large linearization error when confronted with a nonlinear
When a service robot roaming in the room, a map created system. Computing the Jacobians of the state transition and
simultaneously is necessary to localize itself. Building a map the observation equations at each time step requires much
of an unknown environment is a key technique for mobile computational resources when linearizing the system. Also,
robots. The mapping problem is usually termed Simultaneous this approach is subject to big cumulative errors on the
Localization and Mapping(SLAM) [8][9][13][14][15][16]. In estimation of robot pose after a long run.
order to determine its own location a robot needs a map Particle filter(PF) is another statistical technique widely
while in order to acquire the map it requires a robot pose used in SLAM. Since Del Moral [5] put forward the term
estimation. These two aspects are the precondition of each in 1996, many improved methods based on particle filter
other, which makes it a complex problem being described as has been proposed. PF uses a set of weighted particles
chicken-and-egg problem. or samples to approximate the posterior distribution. This
Restaurant service robots, for example, require an effective nonparametric method get rid of the restriction that the
model of their workspace to carry out autonomous path states and noise distribution must be Gaussian distribution
planning and human friendly service. The robot localization in nonlinear system. That makes it possible to describe a
and navigation also need detail maps about the environment. wider range of model. It assumes that the sequence of state
variables is a Markov process [6] so that the current states
II. RELATED WORK only depend on the last one.
Since Smith and Cheeseman [1] proposed a probability III. MODELING
based frame to solve the SLAM problem in 1986, a lot of A. Mobile robot model
researches have focused on the Simultaneous Localization
Our robot has a two-wheel differential move base shown
This work was supported by the National High-Tech Research and as Fig. 1. In view of the fact that the robot don’t execute
Development Program of China (863 Program) (Grant No. 2015AA042303),
the National Natural Science Foundation of China (Grant No. 61273335),
velocity commands that can be obtained by measuring the
the Guangdong Innovative Research Team Program (201001D0104648280), spinning wheels [7], we apply an odometry model for pose
the Shenzhen overseas innovation and entrepreneurship Research Program estimation in consideration of higher accuracy. The robot
(KQCX2015033117354155) and the Shenzhen Fundamental Research Pro- T
gram (JCYJ20140718102705295, JCYJ2016428154842603)
pose is described by x = (x, y, θ) . (x, y) indicate the
The authors are with Guangdong Provincial Key Laboratory of Robotics coordinate of the center of the base and θ is the heading
and Intelligent System, Shenzhen Institutes of Advanced Technology, Chi- T
angle. The motion information is given by u = (∆s, ∆θ) ,
nese Academy of Sciences, P.R.China.
The authors are also with Key Laboratory of Human-Machine
where ∆s and ∆θ denote the robot translation and rotation
Intelligence-Synergy Systems, Shenzhen Institutes of Advanced Technology, respectively.
Chinese Academy of Sciences, P.R.China.
Jinglin Zhang and Guolai Jiang are also with Shenzhen College of B. Motion model
Advanced Technology, University of Chinese Academy of Sciences, 518055
Shenzhen, P.R.China Robot pose estimation is predicted by internal odometry
∗ Yongsheng Ou is the corresponding author. [email protected] and gyro sensor which report robot translation and rotation.

978-1-5090-4364-4/16/$31.00 ©2016 IEEE 2122

 where xk is state variable, zk is the measurement in time
step k, and m is the map of the environment. Actually, zk is
a series of features observed by the robot at time k, hence
write:

zk = zk1 , zk2 , · · ·

(6)

In robotics, features correspond to the landmarks in the

physical world [7]. We denote the distance by r and the
bearing by φ, so that the feature vector is given by (7)
 1   2  
rk rk
zk = , ,··· (7)
φ1k φ2k

where rki and φik denote the distance and bearing of the i-
T
th feature zki relative to the robot pose (xk , yk , θk ) . Here
we adhibit Markov assumption that features are conditionally
independent although this is true only in ideal circumstances.
Hence, the measurement model p (zk |xk , m) can be approx-
Fig. 1. Robot model imated by the product of individual measurement likelihoods
as (8).
We assumed that the robot only does rolling motion in N
Y
the direction of movement without sideslip and the moving p zki |xk , m


p (zk |xk , m) = (8)
process is a rigid motion. The Motion model could be i=1
represented as:
The depth camera act as a range finder which provides a
p (xk |uk , xk−1 ) (1) measurement of distance rki and bearing φik of the measured
T
feature relative to the robot pose (xk , yk , θk ) . Then, the
where xk and xk−1 are both state variable indicating robot measurement model can be computed by (9):
poses, and uk is a robot control. This posterior distri-  r 2  2 
bution plays a role of the transition model. We compute  i 
m j
− x + m j
− y
rk x k y k
p (xk |uk , xk−1 ) according to (2): =  + vk
 
φik  j
my −yk

arctan mj −x − θk
x k
∆s cos θk−1 + ∆θ
    

xk xk−1 2

(9)

T
 yk  =  yk−1  +  ∆s sin θk−1 + ∆θ 
2
where the i-th feature zki = rki , φik at time k corresponds
θk θk−1 ∆θ (2)
T
2 to the j-th landmark mj = mjx , mjy in the map. The vk
+ wk−1 denotes the measurement noise.
where wk−1 is the processing noise, ∆θ is computed from IV. RAO-BLACKWELLIZED PARTICLE FILTER
tread L and the difference between the left and right wheels: FRAMEWORK
∆sr − ∆sl
∆θ= (3) RBPF is one of Bayesian filter based methods which
L approximate the posterior probability by a set of sample
T
(xk , yk , θk ) denotes the robot pose at time k. ∆s is calcu- particles draw form the posterior. Each particle corresponds
lated from mileage of right and left wheels: to a robot path and a map.
∆sl + ∆sr The key idea of Rao-Blackwellized particle filter [9] is
∆s = (4) to decompose the joint posterior p (x1:k , m|z1:k , u1:k ) into
2
posterior probability over maps p (m|x1:k , z1:k ) and posterior
Taking into account the time interval is very short, the
probability over trajectory p (x1:k |z1:k , u1:k ). This factoriza-
robot heading angle deviation is so small that we can apply
tion is shown as (10):
an approximate treatment to ∆d:
∆s ≈ ∆d p (x1:k , m|z1:k , u1:k ) = p (x1:k |z1:k , u1:k ) p (m|x1:k , z1:k )
(10)
C. Measurement model where the posterior over trajectory computed by particle
The measurement model is defined by a conditional prob- filter and then the map can be updated according to current
ability distribution measurement and the trajectory posterior.
The routine of the robot pose estimation and map update
p (zk |xk , m) (5) is shown as follows:

2123
1) 
Initialization:
Set an initial sample set with N particles 6) Resampling: Resampling process is executed when the
xi1 sharing the same wights and initialize an empty effective particles are too little. A parameter was intro-
grid map at the beginning. duced to measure the degree of the particle degradation
2) Data association: Use a scan matcher to find the [10] called Nef f :
correspondence between the observed features fki and 1
the landmark mj in increasing the map. The scan Nef f = N
matching process described by (11) finds the best

2 (19)
ŵki
P
alignment between the existing map mik−1 and the i=1
current measurement zk . We resample when Nef f is less then a threshold T to
x̂ik = arg max p x|mik−1 , zk , x̄ik

avoid sample impoverishment.
(11)
x 7) State output: We use the weighted sum as the posterior
pose estimation at time step k:
where the x̄ik
is predicted from robot control (2).
3) Computing proposal distribution: In general, the mo- N
X
tion model shown as (1) would be chosen to be xk = ŵki xik (20)
proposal distribution to generate new particles. But the i=1
motion model which just rely on the odometry will 8) Map update: The posterior over maps p (m|x1:k , z1:k )
lead to the need for more particles. The new sampling can be decomposed into sub-problems of estimating
distribution taking into account the measurement is


p mj |x1:k , z1:k for all grid cells mj [9].
(12).
Y  ij i 
p xk |xik−1 , uk , zk p mik |xi1:k , z1:k =


(12) p mk |x1:k , z1:k (21)
j
The proposal distribution can be estimated by a Gaus-
sian with parameters µik and σki based on K points Thanks to the factorization, the landmark or occupied
{xj } sampled around the pose x̂ik . grid positions in every particle could be computed
independently of each other.
K
1 X
µik = xj p zk |mik−1 , xj


i
η j=1 (13) Algorithm 1 Systematic Resampling

Require:  xi , wi
p xj |xik−1 , uk−1


Ensure: x∗j , w∗j
K 1: s0 = 0, s1 = w 1 
1 X
σki = p zk |mik−1 , xj 2: u1 ∼ U 0, N −1


i
η j=1
3: for i = 2 : N do
(14)
p xj |xik−1 , uk−1 si = si−1 + wi


4:

T 5: ui = ui−1 + N −1
xj − µik xj − µik


6: end for
where η i is 7: j = 1
8: for i = 1 : N do
K
X 9: ni = count {uk ∈ (si−1 , si ] , k = 1, · · · , N }
ηi = p zk |mik−1 , xj p xj |xik−1 , uk−1



(15) 10: for k = 1 : ni do
j=1
11: x∗j = xi
4) Sampling: New particles
are sampled from (12) ap- 12: w∗j = N −1
proximated by N µik , σki : 13: j++
14: end for
xik ∼ N µik , σki


(16) 15: end for
5) Weight update: The weight of each particle can be
iteratively estimated by (17).
V. IMPLEMENTATION ISSUES
wki = wk−1
i
ηi A. Occupancy grid mapping
K
i
= wk−1
X
p zk |mik−1 , xj p xj |xik−1 , uk−1


Occupancy grid map [17][18] is 2-D floorplan of the
j=1
environment which is simple and efficient when doing lo-
(17) calization, navigation and path planning task. Each particle
The normalized weight is corresponds to a map m and the map plane is discretized into
finitely many grid cells mj . The estimation of the posterior
wki probability of the map given the trajectory of that particle
ŵki = N (18) and the historical measurements for each grid cell is a binary
wkj
P
j=1 estimation problem. Due to the factorization shown in (21),

2124
a binary Bayes filter can be used here to compute these B. Systematic resampling
occupancy probabilities. We use a log-odds representation To deal with the weight degradation in the sampling step,
of occupancy in (22) many resampling methods are proposed. These methods in-


j p mj |z1:k , x1:k clude multinomial resampling, residual resampling, stratified
lk = log (22) resampling and systematic resampling. Although these four
1 − p (mj |z1:k , x1:k )
resampling schemes are found to provide comparable results
So that the probability of each grid cell is
in practice, systematic resampling method is often preferred
1
p mj |z1:k , x1:k = 1 − since it is the simplest method to implement [11][12]. It takes


(23)
n o
1 − exp lkj O (N ) time and minimizes the MC variation. The key idea
for resampling is eliminate the particles with low weights
The log-odds ratio for each cell in the perceptual field of zk
and copy those with high weights. Systematic resampling
can be computed iteratively by (24)
process can be summarized by the following steps:
lkj =lk−1
j
+ ∆lkj − l0 (24) 1) Compute the cumulative distribution of the particle
We initialize the log-odds to l0 for all grid cells by (25) weights and N intervals are generated.

2) Sample a starting point u1 from a uniform distribution
p mj
U 0, N −1 .
 
l0 = log (25)
1 − p (mj ) 3) Generate the other N − 1 points by increasing u1 with
a step N −1 . These N points form a set {uj }.


where the prior probability p mj for i-th cell is set to a
value between 0.2 and 0.5. We assign it to 0.5 here. 4) Count the number of points fallen into the i-th interval
To determine the increment ∆lkj , we apply a inverse sensor as ni .
5) Copy the particle xi by ni times. These new copies

T
model here. For grid cell mj = mjx , mjy ,we calculate the
T form a new set in which the particles share the same
 pose (xk , yk , θk )
range and bearing of it relative to the robot
j weight N −1 .
as in (26). Similar to (9), we compute rmk , φjmk according
The overall process is summarized in Algorithm 1.
to the grid coordinate and robot pose rather than observe it
by sensor.
 r 2  2 
 j  j j
rmk mx − xk + my − yk
=  (26)
 
j
φmk
 j 
my −yk
arctan mj −x − θk
x k

Then we choose a beam with index of c which is closest to

the line between the cell and robot as in (27)
 
c = arg min φjmk − φik 
 
(27)
i
There are three cases exist:
1) ∆lkj = l0
When condition (28) is satisfied, the log-odds ratio of
the cell will not be updated. That means the cell is out
of the perceptual filed of the sensor.
 
j
rmk > min (rmax , zkc + α) or φjmk − φck  > β (28)
 

where rmax is the sensing range, β is set to the

minimum value between the difference of the adjacent
bearings in measurement zk and the the difference of Fig. 2. The restaurant service robot platform
φmk between adjacent grids and α is the size of the
grid cell.
2) ∆lkj = locc VI. EXPERIMENTS
When (29) is satisfied, locc > l0 is assigned to ∆lkj . A. Hardware platform
 
 j
zkc < rmax and rmk − zkc  < α

(29) The restaurant service robot we built is a two-wheel
differential mobile robot equipped with a ASUS Xtion PRO
3) ∆lkj = lf ree LIVE, as depicted in Fig. 2. The ASUS Xtion PRO LIVE
In this case, lf ree < l0 when (29) is satisfied. provides both depth image and RGB image. The horizontal
j field of view of the sensor is 58 degrees and the effective
rmk < zkc (30)
distance is between 0.8 meters and 3.5 meters. We convert
The value l0 , locc and lf ree measure the amount of evidence the depth image to a 2D laser scan so that we can use
a sensor reading carries for the three cases. it as a low-configure laser sensor. Other laser radars like

2125
RPLIDAR A2 can also do the same work without high
cost. The field of view of RPLIDAR A2 is 360 degrees, the
angular resolution is 0.9 degrees and the scanning frequency
is between 5Hz and 15Hz. The effective range is 6 meters.
Although the RPLIDAR A2 can provide better performance
in the condition of similar costs, we choose ASUS Xtion
PRO LIVE to carry out the experiment in consideration of
the loop closure in the later work.
B. Experiment
To evaluate our approach in loop environment we conduct
an experiment in the rectangular corridor of our office
building at 40 meters long and 20 meters wide. This is
shown in Fig. 3 (top row). And Fig. 3 (bottom row) is
another corridor in the floor. These areas share common
characteristics that the corridors are open and in regular
shape. This method has good performance in these open and
regular shaped environment.

Fig. 4. Maps from room B810 in the 8th floor and office area in the 4th
floor at A building at Shenzhen Institutes of Advanced Technology, Chinese
Academy of Sciences

us a lot of challenges. The results of these experiments shown

in Table I is compared with the EKF method.
TABLE I
POSITIONING ERRORS COMPARED WITH EKF METHOD

Method Position/(m) Heading angle/(◦ )

our method 0.038 1.56
Fig. 3. Maps from the passage of 5th floor of A building at Shenzhen EKF method 0.233 6.21
Institutes of Advanced Technology, Chinese Academy of Sciences

Considering that the robot will work in the restaurant, we

run this method in a conference room at 7 meters long and 3 VII. CONCLUSIONS
meters wide with a lot of chairs. The map we built is shown This paper presents an effective approach to achieve
in 4 (top row). localization and navigation for restaurant service robot. This
To measure the positioning error of our approach, we method uses depth camera and low cost laser to instead of
carried out experiments in a more complicated environment heavy and expensive laser sensor such as ASUS Xtion PRO
around the office area in the 4th floor in the building. 4 LIVE, Kinect for Xbox 360 and RPLIDAR A2 so that can be
(bottom row) shows the map built by our approach. This area extended to other restaurant service robots. The experiments
with many separated small areas and many other things such taken in different environments indicate that this approach
as chairs, desks, bonsais, boxes and walking people brought has good performance in complex indoor environments. We

2126
are trying to improve it by loop closure detection based
on laser scan only. On the other hand, more work will be
done on 3D mapping with those depth cameras to enable
the restaurant service robot to do collision-free trajectory
planning for robot manipulators with holonomic constraints.
R EFERENCES
[1] Smith R C, Cheeseman P. On the representation and estimation of
spatial uncertainty[J]. The international journal of Robotics Research,
1986, 5(4): 56-68.
[2] Kalman R E. A new approach to linear filtering and prediction
problems[J]. Journal of basic Engineering, 1960, 82(1): 35-45.
[3] Kalman R E. Contributions to the theory of optimal control[J]. Bol.
Soc. Mat. Mexicana, 1960, 5(2): 102-119.
[4] Kalman R E, Bucy R S. New results in linear filtering and prediction
theory[J]. Journal of basic engineering, 1961, 83(1): 95-108.
[5] Del Moral P. Non-linear filtering: interacting particle resolution[J].
Markov processes and related fields, 1996, 2(4): 555-581.
[6] Thrun S, Fox D, Burgard W, et al. Robust Monte Carlo localization
for mobile robots[J]. Artificial intelligence, 2001, 128(1): 99-141.
[7] Thrun S, Burgard W, Fox D. Probabilistic robotics[M]. MIT press,
2005.
[8] Montemerlo M, Thrun S, Koller D, et al. FastSLAM: A fac-
tored solution to the simultaneous localization and mapping prob-
lem[C]//Aaai/iaai. 2002: 593-598.
[9] Murphy K P. Bayesian Map Learning in Dynamic Environ-
ments[C]//NIPS. 1999: 1015-1021.
[10] Doucet A, De Freitas N, Gordon N J. Sequential Monte Carlo
Methods in Practice. Series Statistics For Engineering and Information
Science[J]. 2001.
[11] Douc R, Capp O. Comparison of resampling schemes for particle fil-
tering[C]//ISPA 2005. Proceedings of the 4th International Symposium
on Image and Signal Processing and Analysis, 2005. IEEE, 2005: 64-
69.
[12] Arulampalam M S, Maskell S, Gordon N, et al. A tutorial on particle
filters for online nonlinear/non-Gaussian Bayesian tracking[J]. IEEE
Transactions on signal processing, 2002, 50(2): 174-188.
[13] Dissanayake G, Durrant-Whyte H, Bailey T. A computationally ef-
ficient solution to the simultaneous localisation and map building
(SLAM) problem[C]//Robotics and Automation, 2000. Proceedings.
ICRA’00. IEEE International Conference on. IEEE, 2000, 2: 1009-
1014.
[14] Gutmann J S, Konolige K. Incremental mapping of large cyclic envi-
ronments[C]//Computational Intelligence in Robotics and Automation,
1999. CIRA’99. Proceedings. 1999 IEEE International Symposium on.
IEEE, 1999: 318-325.
[15] Hahnel D, Burgard W, Fox D, et al. An efficient FastSLAM algorithm
for generating maps of large-scale cyclic environments from raw laser
range measurements[C]//Intelligent Robots and Systems, 2003.(IROS
2003). Proceedings. 2003 IEEE/RSJ International Conference on.
IEEE, 2003, 1: 206-211.
[16] Montemerlo M, Thrun S, Koller D, et al. FastSLAM 2.0: An Improved
Particle Filtering Algorithm for Simultaneous Localization and Map-
ping that Provably Converges. 31 David Trnqvist, Thomas B[J]. Schn,
Rickard Karlsson and Fredrik Gustafsson. Particle Filter SLAM with
High Dimensional Vehicle Model.
[17] Morales-Menendez R, Freitas N D, Poole D. Real-time monitoring
of complex industrial processes with particle filters[C]//Advances in
Neural Information Processing Systems. 2002: 1433-1440.
[18] Moravec H P. Sensor fusion in certainty grids for mobile robots[J].
AI magazine, 1988, 9(2): 61.

2127