ARTIGO TÉCNICO David Cabecinhas, João Nascimento, João Ferreira, Paulo Rosa, Pedro Lima Instituto de Sistemas e Robótica Instituto Superior Técnico - Lisboa
SELF-LOCALIZATION BASED ON KALMAN FILTER AND MONTE CARLO FUSION OF ODOMETRY AND VISUAL INFORMATION ABSTRACT The main goal of this paper is to describe the application of two self localization methods, based on Kalman filter and Monte Carlo fusion of odometry and visual information, to omni-directional soccer robots. The two methods are compared, and the usage of several candidates for the robot posture, provided by the vision-based observation step, is discussed. Simulated and real robot results are presented. Keywords: self-localization, Kalman filter, Monte Carlo localization, sensor fusion, robot vision.
1. INTRODUCTION AND MOTIVATION One of the most important problems in mobile robotics is the self-localization of the robot in specific environments. In Robotic Soccer this is highly important because the robot needs to “know” its position on the field, to perform adequately most of its tasks. This paper introduces KF based localization and MCL methods [2][4][5], taking advantage of them to fuse sensor information, and applying it to an omnidirectional robot. By fusing different sensor measurements, such as image features and odometry, the robot should be able to get a better estimate of its location, rather than just using one of the two measurements separately. Furthermore, the KF and the MCL also output a measure of uncertainty of the estimate. This might be useful in some tasks. For instance, the robot should not get close to the lateral lines of the field if the estimate variance is too high, since it could be closer than it would expect.
the optimal estimator in the minimum mean square error sense, for linear systems driven by Gaussian noise. The following equations describe a discrete-time dynamic system and its sensors’ measurements:
What the KF does is to try to estimate the state of the dynamic system, based on its linear model, as it will be shown later on this paper, which can be written as
Where In the next section, we describe a vision-based self localization algorithm, introduced in [6] and improved in [3], which determines the robot’s posture, and that will be called the “self algorithm” throughout this paper. Finally, some results and conclusions are presented, and a few suggestions for future work on this area are presented.
2. KALMAN FILTER The Kalman filter block diagram is shown in Fig. 1.
The KF algorithm is recursive in the sense that it does not need to process all the previous data to estimate the next state. To estimate the state at k + 1 only the estimate at k, and the measurement in k + 1, are needed. There are two matrices that define the behaviour of the Kalman filter, expressing the covariance of the measurements errors, and the covariance of the model noise. These are the matrices R and Q, respectively, and can be seen as adjustment factors of the filter. Formally, they can be defined by:
2.1. Kalman Filter Algorithm Prediction Cycle Given the state estimate at k, x ( k | k ), the predicted state estimate at the next sampling time is: ^ Figure 1 . KF block diagram (reprinted from [1])
Furthermore, the conditional covariance is given by: The KF is an optimal recursive data processing algorithm, as explained in the sequel. In this paper, only its discrete time version will be used. The KF is
[22]
robótica
Table 2
If only odometry were used, then the errors would be quite larger. In the previous trials, the average error for the odometry was about 3 m for x and y, and 100º for the orientation. On the other hand, if only the self algorithm were used, then the posture of the robot would only be estimated a few times per minute, since the algorithm often fails to return a posture. The results show that there is bias in the error in x, which can be due to miscalibration of the catadioptric vision system. However, the KF algorithm is quite accurate concerning the orientation.
7.2. Experimental results for MCL Similar tests were made using the MCL algorithm as well. Tests were made for the global location problem, i.e., the robot did not know its initial localization, and using a different number of particles. The results after the robot moved for some time around the field are shown in Tables 4 and 5. More results were obtained for different number of particles, but are not reported here.
of the angular displacement of the wheels. The results could be better since the non linearity of the kinematics model of the robot is explicitly in the EKF. However, there are no guarantees of convergence of the filter. The matching step implemented checks if the estimate of the self algorithm is in a predefined neighbourhood of the latest KF estimate. This neighbourhood could also change over time, since in the beginning it is expected that the KF estimate is further away of the real posture of the robot, so this neighbourhood could be bigger. Another solution is to decrease it when the values of the diagonal of P also decrease. Finally, the initialization could be different than the one used. In this case, the initial estimate of the posture is (0, 0, 0). This may lead to slow convergence of the filter. Another way of doing this step is to tryout the self algorithm initially, and then use this estimate as the initial state. As for the MCL, future developments in this area are concerned with the problem of dynamical environments and cooperative multiple robot localization (with a team of robots). Although the robotic soccer environment is dynamical in order that other robots and the ball are also moving, the objects used by the self-algorithm for localization (football field, its lines, goals and posts) are static, so it would not influence the MCL results. Cooperative multiple robot localization is much more interesting for soccer robots. Its aim is to speed up the convergence of the process of self-localization (for the robots to “know” their positions as early as possible). Although relatively good precision is already obtained with a simple robot, the added precision obtained by cooperative work would be beneficial. Based on these facts, the cooperative multiple robot localization problem is a natural extension of the problems handled in this paper and is likely to be worked upon in the future.
Table 3 . 10 particles
REFERÊNCIAS [1] Maria Isabel Ribeiro, (2000). Introduction to Kalman Filtering: a set of two Table 4 . 1000 particles
lectures, IST. [2] Moshe Kam, Xiaoxun Zhu, and Paul Kalata (1997). Sensor Fusion for Mobile
The results obtained with the MCL algorithm were similar to those previously obtained with the Kalman filter. The results for x and y are accurate to the meter and the orientation is correct to 5 degrees. Moreover, the bias in the x error does not occur. Also worth noticing is that the error is similar in all the tests, regardless of the number of particles used.
Robot Navigation, Proceedings of the IEEE, 85, Nº. 1, pages 108-119 [3] Hugo Costelha (2002). A Modified Localization Method for a Soccer Robot Using a Vision-Based Omni-directional Sensor, ISR/IST report [4] D. Fox, S. Thrun, W. Burgard and F. Dellaert, (2000). Particle Filters for Mobile Robot Localization, Sequential Monte Carlo Methos in Practice, pages 470-498 [5] S. Thrun, D. Fox, W. Burgard and F. Dellaert (2000). Robust Monte Carlo Localization for Mobile Robots, Artificial Intelligence Journal, 128, No. 1-2,
8. CONCLUSIONS AND FUTURE WORK This paper presented the application of self-localization methods based on odometry and visual measurements applied to omnidirectional soccer robots. The Kalman filter showed to be a solution for integrating different sensorial information, which can be expanded in future, by adding accelerometers, gyroscopes, etc. The MCL algorithm also proved to be a good approach to the robot localization problem. The results obtained were a big improvement on using the sensor information separately. Fusion with more sensors is also possible, requiring however an adjustment of the motion model and perception model, to reflect information from the additional sensors. Several improvements can be done to the presented KF algorithm. The Extended KF was not tested when the prediction cycle is made with the readings
[28]
robótica
pages 99-141 [6] C. Marques and P.Lima (2000). A localization method for a soccer robot using a vision-based omnidireccional sensor, In: RoboCup 2000 Book, Springer Verlag, 2001 [7] Bentley, J. (1980). Multidimensional divide and Conquer, Comunications of the
ACM, 23(4): 214-229
SELF-LOCALIZATION BASED ON KALMAN FILTER AND MONTE CARLO FUSION OF ODOMETRY AND VISUAL INFORMATION
2, noting that the final posture is also different from the one of the previous tests.