Environment Identification by Comparing Maps of Landmarks Jens-Steffen Gutmann Masaki Fukuchi Kohtaro Sabe Digital Creatures Laboratory Sony Corporation -- Kitashinagawa, Shinagawa-ku Tokyo, 4- Japan Email: steffen,fukuchi,sabe@pdp.crl.sony.co.jp Abstract This paper describes a method for identifying an environment a robot is operating in by comparing the geometry of landmarks of a map the robot is currently building with a set of previously created maps. Landmark maps are created using the stochastic map approach originally presented by Smith, Self and Cheeseman [2]. The paper provides a method for measuring the similarity of such maps and presents a closed form solution for the special case that covariances are constant. Experiments carried out on a developer version of the AIBO robot and on a prototype of the humanoid SDR-4X robot show that the approach is applicable even for robots with poor odometry.. Introduction Navigation is an essential part of a mobile robot system. Two fundamental problems that need to be considered when designing a navigation system for an unknown environment are that of localization and map building. The former provides information about the pose (position and orientation) of a robot given knowledge of a map of the robot s environment and data obtained by the sensors on the robot. The latter initially starts with an empty map in an unknown environment and builds a representation of the environment from the actions carried out by the robot and the measurements of its sensors. For both problems, many solutions have been presented in the past [, 4,,,,, 4]. In general, map building is the harder of the two problems since it requires to not only estimate the position of the robot but also the position of landmarks (walls, corners, etc.) in the environment. In many cases special conditions might be needed when building a map such as having the environment free from obstacles or humans, not allowing robot kidnapping, etc. For these reasons, many solutions to mobile robot navigation do not perform map building during normal operation but separate the task into two steps: (a) an initial step for learning an environment by using a map building approach, and using the learned map for self-localization in normal operation mode. This means, whenever a robot is carried to a new environment, a map learning task is started and once it successfully finished, the system switches to an operating mode where it autonomously localizes in the obtained map. In case the robot operates in several different environments, information about in which environment the robot currently resides is needed in order to utilize the right map for localization. An operator might tell the system which map to use, however, for achieving full autonomy the system should be capable of finding out by itself. We call this task of determining the environment a robot is operating in environment identification. The contribution of this work is to provide an approach for environment identification where a robot autonomously creates a map of landmarks, compares the map to previously obtained maps and identifies the environment by selecting the one which best fits to the currently acquired one. For simplicity, we assume that landmarks are unique which can be assured, e.g., when placing artificial landmarks in the environment. Furthermore, the robot should keep a Gaussian estimate for the location of each landmark, e.g. provided by the stochastic map approach [2]. Comparison of a current map with one in the database is then performed by finding a rotation and translation that minimizes a Mahalanobis distance between the two maps. The rest of this paper is organized as follows. The next section discusses work related to environment identification. Section presents our general approach for environment identification and Section 4 contains the algorithm for matching different maps of landmarks. In Section, experiments are carried out. We conclude in Section.
2. Related Work To the best of our knowledge, environment identification as presented in this paper has not been addressed yet. However, the problem could be reformulated as a localization problem when arranging all maps in the database of environments into one global reference frame in such a way that no pair of environment maps overlap. Identifying the environment could then be solved by a global localization approach such as Markov Localization [] where from the estimated pose the corresponding environment could be determined. Taking this point of view, map correlation as presented by Konolige and Chou [] could be qualified as a related approach. In their work, the robot builds a patch (a small map) consisting of the last few frames of a laser range finder and correlates this patch with an occupancy grid-like map in order to estimate the pose of the robot. A similar approach is the re-localization method developed by Rencken et al. [] where the robot builds a small map of landmark features from the last few readings of a laser range finder and then finds corresponding matches of this map in a global reference map. Our work is different in that the maps of environments are kept separate and matching of maps is done on a symbolic level where landmarks are associated with an identifier (which in the current version has to be unique). Furthermore, a Gaussian estimate for the location of each landmark is considered in order to deal with errors in map building.. Environment Identification System The general system for environment identification we consider is depicted in Fig.. It consists of a map-building module, a control system for exploration, the environment identification unit and a database of previously acquired maps. landmark observation motion Map Building (Stochastic Map) map Exploration (Robot Control) Environment Identification Database of Environments Fig.. System for environment identification env id confidence The task of the module for map-building is to create and update a world map Ñ Ð Ö Ð ½ Ð Ò µ consisting of the estimates for the robot pose Ð Ö and the positions Ð ½ Ò of all landmarks. It receives information about the relative movement of the robot (e.g. from an odometry unit) and landmark observations in local robot coordinates from a lower-level sub system. We assume that the estimates for robot pose and landmark positions are Gaussian distributions, that is, Ð Ö Æ Ð Ö Ö µ and Ð Æ Ð µ, ½ Ò. Such estimates can readily be obtained e.g. by the application of the stochastic map approach [2]. The map-building system delivers its map information to the exploration control and the environment identification unit. The control system for exploration is responsible for moving the robot around in the environment in order to obtain as much landmark information as possible for the map-building and identification unit. The strategies for the design of this system are behind the scope of this paper. However, as an example one could think of a behavior that explores an environment by moving to each landmark in the map obtained so far until all landmarks have been visited. If each landmark is visible from at least another landmark and the induced graph of visibility is connected, this strategy would ensure that eventually information about all landmarks is obtained. For our experiments reported in Section, however, we used a different approach where a human joysticked the robot in order to get as much landmark information as possible. The heart of the system is the environment identification unit which receives the current map from the map building system and information about the state of the exploration control. It interacts with a database that holds environment maps. Whenever the current map Ñ provided by the mapbuilding system is updated, it compares it to all maps Ñ in the database. Each comparison provides a measurement telling how well the two maps match (in terms of a Mahalanobis distance as defined below). The index of the map in the database that fits best is taken as the result of the environment identification. A confidence value can also be derived from its residual matching error. The latter can be used, e.g. for thresholding in case there is no good match for any map in the database. The connection of the exploration control to the map identification unit can be used e.g. for signaling that exploration has been finished. This is useful for adding new world maps to the database of maps. When exploration signals that it has been finished and the confidence of the best matching map is still poor, the robot is likely to be in an unknown environment it has not been mapped yet. The obtained world map from map building can then be given a new index and inserted into the database.
4. Matching of Landmark Maps The main part of our environment identification is the algorithm for matching the current world map to another map from the database. This algorithm consists of two steps:. Data association 2. Minimization of matching error In the data association step, for each landmark in the current map corresponding landmarks in the other map have to be found. In general the number of corresponding landmarks is unlimited. However, in our current implementation we assume that landmarks are unique and therefore for each landmark there can be only zero or one corresponding landmarks in the other map. We proceed to the 2nd step only if the number of landmarks that could be associated to landmarks in the other map is greater or equal to a certain minimum number. In the experiments we report below, we use a minimum of 2. After performing the data association step we can assume that without loss of generality the landmarks in both maps are re-ordered such that for ½ Ò, landmark Ð in the current map is associated with landmark Ð in the other map (Ò is the number of landmarks that matched). What is left to be done is to find out how well the positions of landmarks left in the current map matches to the positions of their corresponding ones in the map from the database. This is done in the 2nd step. One problem that needs to be taken care of here is that maps usually have different coordinate systems, that is, the maps can be rotated and translated. In order to deal with this, we formulate a matching error Ï that computes the normalized sum of Mahalanobis distances of landmarks in the current map, rotated and translated according to a rotation and translation Ì, to the associated landmarks from the map in the database: where Ï Ì µ ½ Ò Ü Ì ½ Ü Ü () Ü Ê µ Ð Ì Ð (2) Ü Ê µ Ê µ Ì () Ê µ Ó µ Ò µ Ò µ Ó µ The task now is to find a rotation and translation Ì that minimizes the error Ï Ì µ. The error value itself then gives a measure how well the maps match and can be used for identifying the map in the database. The map having the least error value is most probably the environment the robot is currently operating in. The error value itself can also be converted into a confidence measure (e.g. by taking the inverse) which is the larger the better the two maps match. (4) By considering a minimum threshold on the confidence, unlikely matches can be rejected and new environments can be detected. Unfortunately, minimizing () is non-trivial because of the non-linear rotational parameter. An iterative approach like steepest decent can be applied for computing a solution that minimizes the error. If we consider a special case of the problem where all covariances are constants in the form of Á () where is a constant and Á the identity matrix then the problem is equivalent to minimizing a least squares problem: Ï Ì µ ½ Ò Ü Ì Ü () For this problem, the following closed-form solution can be derived [9, pp. -]: where Ë ÜÜ Ë ÝÜ ØÒ ½ Ë ÜÝ Ë ÝÜ Ë ÜÜ Ë ÝÝ () Ì Ð Ê µ Ð () Ë ÜÝ Ë ÝÝ ½ Ð Ò Ð ½ Ò Ð (9) Ð () Ре Ð Ð µ Ì () This closed-form solution can be used as an initial solution for finding the true minimum of () using an iterative approach. In the experiments reported below, however, we did not implement the iterative minimization of the Mahalanobis distance yet and therefore simply use the solution of () for environment identification.. Results We implemented the system for environment identification on the quadruped mobile ERS 2 robot, a developer version of the commercial AIBO robot. The robot, depicted in Fig. 2(a), is connected to a Linux PC by wireless LAN. For our experiments we utilized color landmarks as shown in Fig. 2 mounted on vertical surfaces such as walls or the sides of book shelves. The landmarks consist of three geometric primitives each in a different color. The combination of colors when read from left to right defines a unique identification for each landmark.
(a) B (c) B A C B D D A D C Fig.. Landmark maps learned by the stochastic mapping approach. (a) Hidai s, Ugo s, (c) Steffen s place. Landmarks are marked with letters A, B, C, and D for easier reference. A C (a) Fig. 2. (a) AIBO Entertainment Robot System and color landmark pictures as observed by the robot s camera. The detection of color landmark pictures is realized by extracting connected regions of color labeled images provided by the robot s vision hardware. By taking into account the camera calibration parameters and the kinematic chain of the robot s head, distance and bearing to landmarks in 2D coordinates are computed. Our map-building software is an implementation of the stochastic map approach [2] with an iterated Kalman filter and a two-stages update rule [] in order to limit errors caused by linearizations in the motion and sensor models. We are aware of the fact that simultaneous localization and map-building as formulated in the stochastic map framework can diverge [] especially in the case odometry is poor [2]. For the size of environments and duration of map-building considered in this paper, however, we did not notice such failures. For performing our experiments we mounted sets of landmarks at three different locations in our laboratory. Each set consisted of 4 unique landmarks. We used the same set of landmarks at each location but mounted them at different places relative to each other. By taking the same set for each environment we make it more difficult for the system to identify the right environment than when using different sets where the environment could already be identified just by finding the map where most of the landmarks can be associated. In order to obtain an initial set of maps for the database of environments, we joysticked the robot around at the three locations in our laboratory. Fig. shows the resulting map of landmarks for each environment. For each landmark, its Gaussian pose estimate is indicated by an error ellipse for the position together with an error cone for its orientation. We manually verified the quality of the obtained maps and found that landmark positions contain an average distance error to their true pose of about cm. Most of this error can be addressed to the poor odometry estimates the robot provides (basically odometry is estimated by interpreting the motion commands carried out on the robot) and the noise in estimating the pose of landmarks from color labeled images and the joint angles of the robot s head. Nevertheless, these results suggest that the stochastic map approach is suitable for this kind of robot in environments of the size considered in this paper. At the same three locations we ran the system for environment identification again in order to see how the residual least square error as defined in () evolves over time. Fig. 4 displays this error for each of the three environments measured over time. In the beginning, when no or only one landmark is visible in the current world map provided by the map-building system, an undefined environment id is returned by our algorithm. This is because a minimum of two landmarks have to be associated to landmarks in the database maps before matching of landmark maps becomes meaningful. When two landmarks in the world map become available, matching of the current map with the ones in the database is performed. However, since only two landmarks are used, the results are sometimes very similar as can be seen in all three diagrams of Fig. 4. After the third landmark comes into sight, the error plots are becoming more expressive and the environment with the lowest error refers to the actual environment the robot was operating in. The fourth landmark in some cases made distinguishing between the environments more clear. However, it might also increase the error of the best match as seen e.g. in Fig. 4(c) or causes disturbances as displayed in Fig. 4(a). As for the latter we believe that the rise in error when observing the fourth landmark is due to an erroneous first observation
mean residual error (mm) mean residual error (mm) mean residual error (mm) 4 2 4 (a) Hidai Ugo Steffen 2 2 2 4 4 Time (sec) 4 2 4 Hidai Ugo Steffen 2 2 4 2 4 Time (sec) 4 2 4 (c) Hidai Ugo Steffen 2 2 4 2 2 Time (sec) Fig. 4. Mean residual error when running the system for environment identification in the environment at (a) Hidai s, Ugo s, and (c) Steffen s desk. Circled numbers mark the time new landmarks are added to the stochastic map. of that landmark and the fact that we do not minimize the true Mahalanobis distance of () yet but the special one of () where all covariances are constant. From the experimental results we conclude that at least landmarks should be visible and associated to landmarks in a map in the database before matching of maps becomes useful. Furthermore for the environments considered in our experiments a value between 2 mm and mm seems to be a good threshold for rejecting poor matches. We also carried out experiments on a prototype of the humanoid SDR-4X robot []. SDR-4X (see Fig. ) is a biped robot with a similar system design as the AIBO robot allowing to easily adapt software from the AIBO robot. Fig.. Prototype of the humanoid SDR-4X robot. For these experiments we manually defined world maps, each composed of the same unique landmarks and covering an area of about 2 m ¾ (see Fig. ). Please note that some of the world maps (f-j) are very close to each other (e.g. only two landmarks are exchanged). For each of these maps we placed landmarks accordingly and manually controlled the SDR-4X robot until all landmarks have been observed. The resulting map was then compared to all pre-defined maps using our environment identification system. Fig. shows the mean residual error of the best and the 2nd best match for this experiment. Clearly the gap between the best and 2nd best match is sufficiently large for distinguishing between the different world maps. In all cases the best match (lowest error) corresponds to the correct environment. It should be noted that the absolute mean residual error is much smaller than in our previous experiment. We believe this is because: (a) the density of landmarks in the last experiment is much higher than the one in the first experiment yielding a smaller error for map-building, the pre-defined maps contain less error than the ones built by the mapping system itself, and (c) the odometry information on the SDR-4X robot is much more accurate than that of the AIBO robot which tends to slip a lot. Thus, for the SDR-4X robot using landmark setups as employed in this experiment, a combination of an absolute threshold (around mm) together with a relative one (relation of best to 2nd best match) can be applied for correct environment identification.. Conclusion We presented an approach for environment identification that builds a map of the current environment and matches it to previously obtained maps stored in a database. Experimental results suggest that this approach is applicable in environments containing unique artificial landmarks on robots with low accuracy in sensor and odometry measurements. There are many ways in which the system could be extended. First of all, the uniqueness of landmarks could be relaxed by allowing duplicates of landmarks. This would
(a) 2 4 2 4 (c) 4 2 (d) 2 4 (e) 2 4 (f) (g) (h) (i) (j) 2 4 Fig.. Pre-defined world maps used for experimenting with the environment identification system. Landmarks are marked with numbers. Mean residual error (mm) 2 error of best match error of 2nd best match a b c d e f g h i j Environment Fig.. Mean residual error of best and 2nd best match after performing environment identification using SDR-4X in the pre-defined environments of Fig.. In all cases, the best match correctly identifies the environment. cause the finding of a minimum for the distance error being computationally more expensive since then many different combinations for data association would have to be considered. Pruning might help to reduce the overall complexity, however, in the most general case, where all landmarks look alike, the complexity becomes exponential in the number of landmarks. Furthermore, instead of only using the result of minimizing the error function where covariances are assumed to be constant (), the identification quality could be improved by using this result as a seed for minimizing the true Mahalanobis distance () between the current map and the maps from the database. This is part of future work. References [] D. Fox. Markov Localization: A Probabilistic Framework for Mobile Robot Localization and Navigation. PhD thesis, University of Bonn, Germany, 99. [2] U. Frese and G. Hirzinger. Simultaneous localization and mapping a discussion. In IJCAI Workshop Reasoning with Uncertainty in Robotics, Seattle, 2. [] M. Fujita, Y. Kuroki, T. Ishida, and T. Doi. A small humanoid robot SDR-4X for entertainment applications. In Int. Conf. on Advanced Intelligent Mechatronics (AIM), Kobe, Japan, 2. [4] J.-S. Gutmann and D. Fox. An experimental comparison of localization methods continued. In IROS, 22. [] J.-S. Gutmann and K. Konolige. Incremental mapping of large cyclic environments. In CIRA 99, November 999. [] S. Julier and J. Uhlmann. A counter example to the theory of simultaneous localization and map building. In Int. Conf. on Robotics and Automation (ICRA ), 2. [] K. Konolige and K. Chou. Markov localization using correlation. In Proc. International Joint Conference on Artificial Intelligence (IJCAI 99), Stockholm, 999. [] J. Leonard and H. Durrant-Whyte. Mobile robot localization by tracking geometric beacons. IEEE Transaction on Robotics and Automation, (): 2, 99. [9] F. Lu. Shape Registration using Optimization for Mobile Robot Navigation. PhD thesis, University of Toronto, 99. [] P. Moutarlier and R. Chatila. Stochastic multisensory data fusion for mobile robot location and environment modelling. In th International Symposium on Robotics Research, pages 94, 99. [] W. D. Rencken, W. Feiten, and R. Zöllner. Relocalisation by partial map matching. In Sensor Based Intelligent Robots, volume 24 of Lecture Notes in Computer Science, pages 2. Springer, 999. [2] R. Smith, M. Self, and P. Cheeseman. Estimating uncertain spatial relationships in robotics. In Autonomous Robot Vehicles, pages 9. Springer-Verlag, 99. [] S. Thrun, D. Fox, and W. Burgard. A probabilistic approach to concurrent mapping and localization for mobile robots. Machine Learning, :29, 99. Also appeared in Autonomous Robots :2-2. [4] S. Thrun, D. Fox, W. Burgard, and F. Dellaert. Robust Monte Carlo localization for mobile robots. Artificial Intelligence, 2(-2):99 4, 2.