Markov-Kalman Localization for Mobile Robots Jens-Steffen Gutmann Digital Creatures Laboratory, Sony Corporation 6-7-35 Kitashinagawa, Tokyo 141-0001, Japan Email:
[email protected] Abstract Localization is one of the fundamental problems in mobile robot navigation. Recent experiments showed that in general grid-based Markov localization is more robust than Kalman filtering while the latter can be more accurate than the former. In this paper we present a novel approach called Markov-Kalman localization (ML-EKF) which is a combination of both methods. ML-EKF is well suited for robots observing known landmarks, having a rough estimate of their movements, and which might be displaced to arbitrary positions at any time. Experimental results show that our method outperforms both of its underlying techniques by inheriting the accuracy of Kalman filtering and the robustness and relocalization speed of the Markov method.
1. Introduction Self-localization is the task of estimating the pose (position and orientation) of a mobile robot given a map of the environment and a history of sensor readings and executed actions. It is one of the fundamental problems in mobile robot navigation and many solutions have been presented in the past including approaches employing Kalman filtering [1, 7, 8], grid-based Markov localization [3], or Monte Carlo methods [6, 10]. For a survey see [9]. By performing localization experiments with a mobile robot it has been ascertained that grid-based Markov localization is more robust than Kalman filtering while the latter – given good inputs – is more accurate than the former [4]. A combination of both approaches is likely to inherit the advantages of the underlying techniques. Our work is the consequent advancement of these comparison results. The major contributions are: 1. verification of the results found in [4] using different robot and sensors, 2. a novel localization method ML-EKF that combines grid-based Markov localization and Kalman filtering, 3. experimental verification that the ML-EKF method outperforms both of its underlying techniques.
2. Probabilistic Localization In probabilistic terms, localization is the process of determining the probability of the robot being at pose l given
sensor inputs sn and executed actions an (n = 1 : : :N ). By assuming independence between observations and between executed actions, the following update rules can be formulated in the Markov localization framework [3]:
p(sn j l) p(l) Z p(l j an; l0) p(l0 ) dl0
p(l) p(l)
(1) (2)
where (1) is performed on observations evaluating sensor model p(sn jl), which returns the likelihood of an observation for a given pose, multiplied by a normalizing factor , and (2) is employed on robot motion evaluating motion model p(ljan ; l0) which delivers the probability of the robot being at pose l given it was at l0 and executed action an . Depending on the application and the type of robot, different motion models can be considered. Throughout this paper we make use of 3 motion models sketched in Fig. 1 where (a) refers to a Gaussian model that is widely used in all kinds of robot systems, (b) is the max distance model that places equal probability to positions up to a certain range and is useful e.g. for legged robots that might get obstructed at obstacles, and (c) allows a robot to be manually displaced to any position in the environment with a certain displacement probability. (a)
0
d
(b)
0
d
(c)
0
Fig. 1. Motion models: (a) Gaussian, (b) max distance, (c) displacement model.
Many approaches assume Gaussian sensor models. Experimental evidence that a Gaussian sensor model is appropriate for our application, too, will be given in Section 4. Probabilistic localization methods can be categorized based on the representation of p(l). If p(l) is represented by a piece-wise linear function, we obtain grid-based Markov localization [3]. Advantages of this method are its global search space and flexibility for different motion and sensor models. Depending on the dimension, resolution and size of the grid, the method might not be feasible for real-time applications without further optimizations.
By representing p(l) as a set of particles, Monte Carlo localization (MCL) is obtained. Computational resources, accuracy and robustness can be controlled by the number of particles in this method. See [10] for a complete discussion. Kalman filtering emerges when representing all densities by Gaussians: p(l) N (^ l; l ), p(sn jl) N (^sn ; sn ), and p(ljan; l0) N (^an; an ). The update rules (1, 2) become:
^l ^l
^l + W v ; l f (^l ; a^n) ; l
l W v W T rf l rf T + an
The extended Kalman filter (EKF) in our system uses typical process and observation equations widely used in mobile robot navigation. Besides the commonly employed distance/heading sensor model, we also experimented with a heuristic model based on an idea of the UNSW legged robotic soccer team [5] where the true pose of the robot is assumed to lie on a straight line connecting ^l and the observed landmark (see Fig. 3).
(3) New state after fusion
(4)
l) where W = l rhT v 1 is the filter gain, v = s^n h(^ the innovation, v = rhl rhT + sn its covariance, h and f are the measurement and process equations, and rh and rf their Jacobians [1]. Note that the EKF equations do not necessarily assume Gaussian densities but can be used for estimating the first and second moments of the error. These moments exist for any error statistics [1]. 3. Markov-Kalman Localization We present a novel approach to probabilistic localization based on Markov localization and Kalman filtering which is useful for robots observing known landmarks, having rough estimates of their movements, and which can be displaced to arbitrary positions in their environment at any time. The basic idea is that Markov localization is used for global search of the robot position providing high robustness on sensor noise and fast recovering from manual robot displacement, whereas Kalman filtering is used locally for precisely computing the robot pose. Our Markov-Kalman (ML-EKF) system consists of 3 modules depicted in Fig. 2.
Current state
Observation
Fig. 3. Closest pose sensor model: the true robot pose is assumed to be on a straight line connecting robot and landmark.
Since in this approach the sensor model directly delivers pose estimates, h = I and (3) can be rewritten as [8]:
(l 1 + sn1) 1 (l 1^l + sn1s^n ) (l 1 + sn1) 1
^l l
The heart of our localization system is the EKF controller that filters observations and reinitializes the EKF when necessary. A flowchart of the logic is given in Fig. 4. Input: motion
Update ML
2D position prob. grid
Input: observation
Update ML
Observation plausible?
No
Yes Update EKF
ML 2D
(5)
EKF Controller
Update EKF
EKF state plausible?
Accept/Reject
Yes
No
Reinitialize EKF
Re-initialize landmark observation motion
EKF
EKF state
(x y θ)
Output: EKF state
Output: EKF state
Σ
Fig. 2. Markov-Kalman (ML-EKF) localization system
We employ a 2D grid-based Markov localization method at coarse resolution that represents possible (x; y) positions of the robot but does not contain information about orientation. Therefore, on observations only the distance to landmarks are considered and on motion all directions are treated with equal probability. We want to pinpoint that this design, and by representing probabilities in negative loglikelihood space, allows for very fast updates. In an environment of size 120x80 cells we achieve about 12000 observation, 225-300 max-dist motion, and 325 displacement motion updates per second on a PIII 1GHz computer.
Fig. 4. Flowchart of the ML-EKF system.
All robot motions are integrated into the Markov grid and the EKF. The latter uses a Gaussian motion model (optimistic assumption, Fig. 1(a)) whereas the Markov grid employs a mixture of max distance and displacement motion model (pessimistic assumption, Fig. 1(b) and 1(c)). Landmark observations are first integrated into the Markov grid. If the given observation is plausible based on the Markov state, it is also integrated into the EKF using the heuristic closest pose sensor model as described above. We l) where ~l is the maximum likely cell in the evaluate p(sn j~ grid. If this probability is smaller than a threshold tobs , the observation is rejected. After accepting and integrating an
observation into the EKF, the distributions of Markov grid and EKF are compared using a 2 test. If the test exceeds a threshold t 2 , the Kalman filter is reinitialized with ~l and the maximum likely orientation computed by projecting the l. last unfiltered observation to ~ The output of the ML-EKF system is the EKF state.
of battery. All sensor and motion data has been logged to a file adding a special tag each time the robot passed over one of the tape markers (as observed by the operator). Field size: 3 x 2 m Total run time: 58 min Landmarks seen: 48636 Motion steps: 8333
(-100, 50)
4. Results The ML-EKF system has been implemented on the ERS 2100 robot system, a developer version of the commercial AIBO robot (see Fig. 5(a)), connected to a Linux PC by wireless LAN. For our experiments we utilized color landmarks as shown in Fig. 5(b) that are also used in competitions in the RoboCup four-legged robot league.
(-100, 0)
(0, 0)
(50, 0)
(50, -50) Position markers
Fig. 7. Environment for performing localization experiments.
(b)
Fig. 5. (a) AIBO Entertainment Robot System and (b) color landmark tubes as observed by the robot’s CMOS camera.
The detection of color tubes is realized by employing the CMVision software library [2] on color labeled images provided by the robot’s vision hardware. By taking into account the kinematic chain of the robot’s head, distance and bearing to landmarks in 2D coordinates are computed. We conducted a series of experiments to compute the accuracy of our color landmark sensor. Fig. 6 displays plots of the probability distributions for distance and angle. As can be seen, the error in distance and angle can be approximated by Gaussian models.
1200
800 600 400 200 0
# Measurements
60
60
(a) # Measurements
70
50 40 30 20
0.9 0.92 0.94 0.96 0.98 1 Distance (in m, true distance = 1m)
1.02
1/1
1/2
1/4
1/8 1/16 1/32 Sensor input (fraction)
1/64
1/128
Fig. 8. Accuracy under different levels of sensor data sparsity.
40 30 20 10
10 0 0.88
50
(b)
ml ekf-orig ekf-closest ml-ekf
1000 Distance error (mm)
(a)
Using the recorded data we conducted a series of experiments to determine accuracy, robustness and relocalization speed of the ML-EKF system in comparison with its underlying techniques. The leftmost error bars in Fig. 8 show the mean absolute position error of the Markov 2D grid localization, the EKF using distance/heading sensor model, the EKF using closest pose sensor model, and the ML-EKF system. The absolute error is about 10cm for all methods. In this and all following figures error bars indicate the 95% confidence interval of the computed mean.
0 -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 Angle (in deg, true angle = 0 deg)
1
Fig. 6. Sensor model for landmark observations: distribution of (a) distance and (b) angular measurements.
Motion and sensor models are implemented on the robot providing their estimates to the localization system running on the PC in real-time. For evaluation, we built an environment of size 3x2m with 6 landmarks (see Fig. 7) similar to the setup used in the RoboCup four-legged robot league. Several positions inside the field were marked by tape and the robot was joysticked around while swinging its head for about 1h until it ran out
It should be noted that part of the absolute error is due to the problem of joysticking the robot exactly onto the tape markers, human observation of the robot being on a marker, and a marginal uncertainty in time when adding a tag to the log. For these reasons, we believe that the true absolute error is much smaller than the one reported here. We ran the Markov grid at a 5x5cm cell resolution. Obviously, increasing cell size will in general produce less accurate results for this method. By discarding landmark observations from sensor data we can infer how accurate the different methods are under sensor data sparsity. Fig. 8 plots the mean position error when only a fraction of available sensor data is presented to the localization methods. While the mean error increases
for all methods with increasing sparsity, Markov localization observes a significant larger error than the other methods most probably because the 2D grid does not contain information about orientation and therefore the maximum likely cell does not change on robot motion. The accuracy of the ML-EKF system is similar to the EKF using the heuristic sensor model and both are slightly better than the EKF using the standard sensor model. To find out about robustness under sensor noise, we replaced a certain fraction of landmark observation by random landmark data. Fig. 9 shows the mean error under different levels of such sensor noise. Whereas Markov localization and our ML-EKF system are still capable of providing accurate position estimates, Kalman filtering alone produces significantly worse results, even when the noise level is low. 1400
ml ekf-orig ekf-closest ml-ekf
Distance error (mm)
1200 1000 800 600 400 200 0
0
10
20 30 40 Sensor noise (percentage)
50
60
Fig. 9. Accuracy of methods under different levels of sensor noise.
Time needed for re-localisation (seconds)
Finally, we computed the average time the methods needed for relocalizing the robot after it has been manually displaced. Such situations occur when the robot is picked up and put to a new location, when the robot system is started, or when it lost track of its position. Thus, relocalization is not specific only to the RoboCup situation. Fig. 10 shows that Markov localization and our ML-EKF method recover much faster than pure EKF methods most probably because in our Markov approach we make use of an explicit motion model for robot displacement. 10 8 6 4 2 0
ml
ekf-orig ekf-closest Localization method
ml-ekf
Fig. 10. Time for recovering from manual robot displacement.
5. Discussion and Conclusion We presented a new method for mobile robot localization and experimentally compared it with other methods.
Our experiments verify the results from [4], that grid-based Markov localization is more robust than Kalman filtering whereas the latter can be more accurate than the former, using different robot and sensors than [4]. Our new ML-EKF method inherits the accuracy of the Kalman filter and the robustness and relocalization speed of the Markov method and thus is better than each of the underlying techniques. A limitation in our approach is that integration of observations into the 2D Markov grid must be feasible, i.e.
p(sn j lxy ) =
Z
0
2
p(sn j lx ; ly ; l ) dl
(6)
can be computed efficiently. This is true for landmark based navigation but might not be the case for methods using dense sensor matching [3, 4, 10]. An open question is how our approach compares to Monte Carlo localization and their extensions. In preliminary experiments we observed that SRL [6] and MixtureMCL [10] either perform poorly under large sensor noise (the methods reset when observing wrong landmarks) or need a long time for recovering from manual robot displacements, whereas our method still produces accurate results (Fig. 9) due to the global search space of the Markov grid. A deeper analysis is needed, however, to obtain more evidence about how the methods compare. We will present such a comparison in the future.
References [1] Y. Bar-Shalom and T. Fortmann. Tracking and Data Association. Academic Press, 1988. [2] J. Bruce, T. Balch, and M. Veloso. Fast and inexpensive color image segmentation for interactive robots. In Int. Conf. on Intelligent Robots and Systems (IROS), 2000. [3] D. Fox, W. Burgard, and S. Thrun. Markov localization for mobile robots in dynamic environments. Journal of Artificial Intelligence Research, 11, 1999. [4] J.-S. Gutmann, W. Burgard, D. Fox, and K. Konolige. An experimental comparison of localization methods. In Int. Conf. on Intelligent Robots and Systems (IROS), 1998. [5] B. Hengst, D. Ibbotson, S. B. Pham, J. Dalgliesh, M. Lawther, P. Preston, and C. Sammut. The UNSW RoboCup 2000 Sony legged league team. In P. Stone et al., editor, RoboCup 2000: Robot Soccer World Cup IV, Lecture Notes in Artificial Intelligence. Springer-Verlag, 2001. [6] S. Lenser and M. Veloso. Sensor resetting localization for poorly modeled mobile robots. In Int. Conf. on Robotics and Automation (ICRA), 2000. [7] J. Leonard and H. Durrant-Whyte. Mobile robot localization by tracking geometric beacons. IEEE Transaction on Robotics and Automation, 7(3):376–382, 1991. [8] P. S. Maybeck. The Kalman filter: An introduction to concepts. In I. Cox and G. Wilfong, editors, Autonomous Robot Vehicles, pages 194–204. Springer-Verlag, 1990. [9] C. F. Olson. Probabilistic self-localization for mobile robots. IEEE Transactions on Robotics and Automation, 16(1):55– 66, Feb. 2000. [10] S. Thrun, D. Fox, W. Burgard, and F. Dellaert. Robust Monte Carlo localization for mobile robots. Artificial Intelligence, 128(1-2):99–141, 2000.
