Pmeeedings of the 1999 IEEE InternationalConference on Robotics & Automation Detroit, Michigan ● May 1999
Real time position estimation for mobile robots by means of sonar sensors C. Urdiales, A. Bandera, R. Ron and F. Sandoval Dpto. Tecnologfa Electr6nica, E. T.S.I. Telecomunicaci6n, University of M61aga
[email protected] Abstract This paper presents a fast localisation algorithm for autonomous mobile agents in dynamic environments based on the definition of a new very small sized landmark type. These landmarks are calculated by obtaining the coordinates of the circular depth jimction obtained from a ring of equally spaced sonar sensors projected on a bidimensional base of a vectorial space. Finally, a pyramidal structure is used to enhance and fasten the performance of the localisation algorithm.
depend on the robot orientation. It allows a large number of landmarks with no decay of the global performance because: i) the size of the landmarks is small enough to store as many as desired, ii) the characterisation algorithm is fast enough to be conducted in real time in non-dedicated processors in a concurrent way with map-building activities and iii) pyramidal segmentation provides a fast and reliable technique to locate the robot by regions instead of by positions. II. CHARACTERISATION
OF THE ENVIRONMENT
A) Normalised circular depth jimction. I.
INTRODUCTION
The sensorymotor system of a robot basically depends on where the robot is located and where it wants to go. This information is not trivial, because the goal position is set according to a reference that is not necessarily the agent itself. Also, after some navigation, estimation of the robot position can not rely on dead reckoning due to slippage. A classical solution for repositioning consists of using landmarks, but bootstrapping makes it difficult to rely on them if there are only a few available. Unfortunately, if too many landmarks are stored, it is slow to analyse all of them in real time, especially if they present a large size or require computationally expensive algorithms to be processed. The problem is even more complicated when there is no odometric information available at all. Because of the importance of localisation, much research has been focused on it and many algorithms have been proposed, being landmark-based solutions the most popular ones. Landmarks can be roughly divided into dynamic landmarks, capable of transmitting their positions to the robot [2], and static ones, that need to be detected by means of laser [5], sonar or infrared sensors [1,4,6] or video cameras [3]. The following problems are related to most landmarkbased solutions: i) vision systems are computationally expensive and slow in non dedicated processors; ii) most dynamic systems require a priori knowledge of the environment to triangulate and therefore are not suitable for exploration; iii) sonar sensors present a wide arc of uncertainty; and iv) laser systems are very expensive. A final problem is that many systems depend on the orientation of the robot in which the landmark was created. Thus, either several orientations have to be tested or geometrical transformations have to be performed before making a decision, with the consequent loss of time. This paper presents a fast method to model landmark information into a localisation map. Our method does not 1650 0-7803-51 80-0-5/99 $10.00 @ 1999 IEEE
In our method, landmarks are created by means of sonar sensors. Since this type of sensor presents a wide arc of uncertainty, 64 equally spaced sonar measures from a ring placed around the robot are taken to build a circular depth Iimction so redundancy gets rid of some reflection peaks. For the same purpose, the function is low-pass filtered. Further pyramidal segmentation will remove most incongntent data. Fig. 1 shows a circular depth function associated to a random position. Localisation in totally or partially explored environments can be performed by comparing different functions. This technique presents two advantages: i) if similitude is measured by means of a circular procedure (e.g. circular correlation), the landmark does not depend on the orientation of the robot and ii) the landmark is fast and easy to obtain. However, it also presents two major drawbacks: i) since every map yields 64 elements, the localisation grid, consisting of all stored landmarks, is too heavy for real time processing and ii) when the number of landmarks grows processing time required to compare current landmark to all stored ones grows as well. In order to reduce the landmark length, it can be observed that the Fourier transform of all depth functions presents a large number of zeros in its central region (Fig. 2). Also, because of redundancy of sonar measures, it is plausible that non-zero components of that ITT could be linearly dependant. Considering the Fourier domain for a 64 point FFT a vectorial space of dimension 64, the FIT of any depth function belongs to a P-dimensional subspace, being P at most 64-M where M is the number of points that are zero for all FiTs. Thus, a depth function can be expressed with only 64-M values, but if an orthogonal base of the named subspace can be calculated, the minimum number of components (P) for any ITT can be granted if the co-ordinates of that ITT on such base are used for representation.
180
1
o
10
20
30
40
50
60
50
60
(b)
1
60
I 32
-60
(a)
10
20
30 40 (c)
Fig.1. a) Sonardepth map; b) circular depth function -64 equally spaced (5,7°) measures-; c) normalised and smoothed circular depth function.
The base in our algorithm is obtained in 2 steps: 1) Creation of a set of FFlk of random depth functions. The largerthenumberof elementsof the set, the betterthe orthogonalityof theresultingbase. 2) Segmentationof the set into P classes by using a kmeans clustering algorithm based on euclidean distance. The value of P is unsupervisedly chosen by a cluster validation process [8]. Since classes have to be as different as possible, their prototypes tend to be orthogonal. These prototypes are the vectors of the base, and the co-ordinates of any depth function onto this base are obtained by Eq. (3). This unique set of coordinates is the landmark and it will be different for each different geometrical location in the environment. In brief, the process to obtain a landmark in a given
The equation of change of co-ordinates between different bases is: (1) X’M.Y Ybeing the co-ordinates of the FIT on the old base, that is the ~ of the depth function, X the co-ordinates on the new one and M the matrix of the change. Any matrix M can be decomposed into a sum of two matrices, being one of them a diagonal one D. Thus, Eq. (1) is converted into:
X=(N+D).Y=IVY+D
Y
(2)
If the new base is orthogonal, N is a null matrix and the coordinates ai are very easy to obtain using the following expression: P-1 txi’
~
Xjvti
600
(3)
;.0 500 Xj being thejtb coefficient of the FFT of the depth function, and Vtico-ordinatej of vector i of the base of the subspace. However, calculation of an orthogonal base is very complicated in this particular problem, so our method does not grant orthogonality, but provides in exchange an enormous dimensional reduction. If the base vectors were orthogonal, the product N-Y would be equal to zero; in our case, the value of N.Y has been empirically proven to be very small, so errors induced by ignoring that expression are not significant when compared to those induced by physical imperfections of the sensors. 1651
400
300 200
100 .A 0
Al . 10
20
30
Fig. 2. FFT of the normalised Fig. lb).
40
50
60
sonar function of
3. P-classes
se~mentation
in chosen level k.
A
11 Id
Level k 2.
4. Propagation of P classes to the base.
Generation and Stabilisation of the pyramid. ‘0
)1‘v’
1. Landmark acquisition procedure.
Region rejection algon”hms.
Fig.3. Landmarkmapsegmentation byusinga pyramidal structure. location consists of i) obtaining 64 different sonar measures spaced 5,7° (depth function); ii) normalizing the depth function to a null average value to remove the continous component of the FIT of the function; iii) smoothing the depth function; iv) calculating the ~ of the normalised function; and v) obtaining the co-ordinates of the landmark by multiplying the input FIT by those of the vectors of the base. Because of the reduced length of the landmark, the algorithm can work with a localisation grid presenting a high degree of deeomposition. Also, the algorithm is fast enough to actualize the localisation map in real time to work with all the information available each time the robot needs to be located. It is important to note that to calculate the components of the base for dimension reduction it is not assumed that FITs yield a given number of zeroes even it it is so. This is because such zeroes are due to the low pass filter that removes spurean peaks ti-om the sonar readings, so in order to have a method as general as possible, it is important to work as if such zeroes do not exist. The absence of a filtering process might affect the number of elements of the base, but since the reduction is based on linear dependencies between components of the FIT, the procedure would be valid as well. B) Segmentation
of the environment.
Once a localisation map has been partially or totally built, to be capable of quickly and reliably locating a given landmark, a segmentation process is conducted by means of a pyramidal algorithm that enhances processing speed and removes incongruent values due to peaks, reflections and sonar uncertainty. Due to uncertainty of sonar sensors, if the localisation algorithm is applied to a grid yielding a large degree of decomposition, in some cases several cells will yield similar sonar values if they are close enough. Also, moving obstacles and reflections induce readings that will not match the sonar signature of neighbour cells. Therefore, it is interesting to perform a segmentation of the environment because: i) instead
1652
of matching a given landmark with all stored landmarks, it is only neeessary to compare it with the prototypes of the classes, ii) readings that do not fit their environments because they are related to errors or moving obstacles are absorbed to a proper value and iii) transitions between regions are defined for further localisation in several steps. When the segmentation process is finished, the map is divided into landmark regions. Unexplored regions are assigned a prefixed value. The segmentation process of the localisation map consists of the following steps (Fig. 3): 1) Generation of a multilevel pyramid, whose base level is the localisation map. Each new level is built by averaging the landmark value of every set of four nodes of the level immediately below into a new node and therefore it yields 1/4 of the nodes of the previous level. When this step is accomplished, eaeh node in a @en level N is linked to the four nodes of level N-1 that were used to generate it. Initially, the pyramid yields a 4-1 structure, for each node above the base level is linked to four sons underneath in the level immediately below and the landmark of each node is the average of those of its sons. 2) Stabilisation of the links using the Adaptive Link Principle [7]. This algorithm changes the value of the links between nodes according to son-parent similitude: each son (except those in the highest level) has four possible parents and each parent may have 16 possible sons. The closest parent according to the similarity of the father-son landmark value is located for each son in an iterative way and a link is forced between them. When the process is finished, every node in the pyramid is linked to an irregular area of nodes in the base. Nodes belonging to a given area tend to yield the same landmark value and they group into a single node in the highest level. 3) The algorithm chooses the most adequate level K to work in. If the chosen level is far from the base, the computational speed is higher, but the segmentation is worse.
4) Clustering process in level K by means of a k-means algorithm. Due to the nature of the Adaptive Lhk Principle and to the small number of nodes in the highest levels, this process is very fast and easy. 5) Class generation. The class values in level K are propagated down to the base through the links between nodes. When the process has been accomplished, the landmarks in the map are segmented into the number of classes fixed by the k-means algorithm. Most abnormal points due to scattering, spurious peaks, reflections and mobile obstacles are removed because this pyramidal architecture takes spatial proximity into account when including a landmark into a given class.
l-III. LOCALISATION
ALGORITHM
No
-----
,,.,”
‘-... 1.sthere a .vin@! choice ?“;> ‘.
.... >,” YES
Once the map has been segmented, it is easy to identify a given landmark by comparing it with the prototypes of the different classes. Thus, the process is much faster, because instead of comparing each landmark with all stored ones, only L similarity measures are required, being L. the number of classes of the map. However, in a given environment some locations might present the same landmark value if their depth functions are too similar. Therefore, a final step is required to determine the real location of the robot when there are several possibilities available. The whole process (Fig. 4) consists of 1) Generation of the landmark associated to current location. 2) Determination of all possible landmark regions prototypes of the map whose similitude to current input landmark is bigger than a threshold U. 3) If the number of possible regions is bigger than one, the rob ot performs a random movement and the algorithm
I
b
END
Fig. 4. Localisation algorithm. returns to step 1, but it keeps knowledge about the previous landmarks and movements. Thus, by comparing the distance between the prototypes of the upcoming class and the previous one with the magnitude of the performed movement, possibilities are incrementally restricted until there remains an unique choice. IV. TESTS AND RESULTS. The proposed system has been implemented on a Nomad 200 robot equipped with a Sensus 200 Sonar Ranging System consisting of a ring of 16 sonar sensorsequally spaced around the cylindrical body of the robot. The first set of tests was
—
:::1!: :::’=:
:.:;
::::
‘.1...1
:’::’1
1~ 29.4 m
(a)
(b)
Fig.5. a) Simulated environment, b) Segmented localisation map. 1653
o.
El
0.5
\ o--
10 20
30 40 50 60
(a)
0.9-
(d)
(c)
0.5
0.193670
I 0.159969
,1 1= ;
I
I(
0:‘
/
10 20 30 40 50 60
(e)
(b) Fig.6. a-b)FFTsofthebaseelements, c) sonardepthmap,d) normalised depthfunction,e) Landmark. conductedin a laboratory, but because of its reduced size, all landmarkregions were very different and the process gave only occasional errors. Further tests were conducted in simulationsof largeenvironmentsto studythe usualproblems related to larger rooms. Fig. 5 presentsan example: the test environment is presented in Fig. 5a) and its associated segmented localisation map appears in Fig. 5b). To obtain the landmarks, the base of the subspace of landmarks was obtained off-line before any calculation. The k-means clustering algorithm created 2 classes, whose prototypes can be seen in Fig. 6a) and Fig. 6b). Thus, landmarks were two-dimensional (Fig. 6e)). Since k can be fixed to any value in such kind of algorithms, the correct number of classes is heuristically calculated off-line by means of a genetic algorithm that optimises the output error of landmark recognition in a controlled situation. The localisation procedure is illustrated in Fig. 7, where the robot locates its position through 6 steps. The values of the landmarks in all six positions can be seen in Table I. Fig.7a) shows the departure point, and Figs. 7b-f) show the regions where the robot might have been in each step. The real position of the robot is marked with a black cross. It can be observed that the procedure could have finished in the second step (Fig. 7b)), where a single possible region was left, but it was artificially forced to continue during 4 steps to show its performance by requiring a positioning error under 0.3?Z0, Table I. Measured Values during the localisation process Co-ordinates of the sonar depth function Point Point Point Point Point Point
A B C D E F
0.172547 0.081625 0.071850 0.122851 0.068194 0.112407
0.208733 0.064602 0.102306 0.139697 0.116971 0.140839
1654
whichis equivalent to a circular region of uncertainty of 2.159 square meters. Such error in step 2 was 0.96%. Two parameters need to be fixed in the localisation process: one is threshold U, which is used to compare the current landmark with the prototypes of the different landmark regions, and the other is parameter Dist, which is used to know if the robot may have reached a given region: if the centroid of the new region is (XW, YnJ, the centroid of the old region is (Xoj,j Y,,,(l)and the robot has moved a dis~nce S> the robot could be in the new region if
The similitude threshold U was empirically fixed to 0.028, and Dist was 15.0 pixels. V, CONCLUSIONS This paper presents a fastj easy and computationally cheap localisation procedure based on sonar sensors. The proposed algorithm has been tested in real and simulated environments, yielding a good performance in real time. The system has been integrated in the sensory-motor module of an autonomous robot and the localisation map was built at the same time that the probabilistic map used to model the environment for path-planning techniques. The localisation/probabilistic grid yielded three values per point two for its landmark plus its probability of occupation. Sonar uncertainty is handled by means of filtering, redundancy and pyramidal segmentation. Landmarks are so small that the system can store as many as necessary with no harm to the global performance. This algorithm is suitable for dynamic environments becasue of its speed: the pyramidal segmentation step takes less than half a second and therefore it can be conducted online each time a localisation is going to be performed. Thus,
-o
-
13
-26
o
14,7 (d)
29,4
0
14,7 (e)
29,4
o
14,7 (f)
29:4
Fig. 7. Localisation procedure in6 steps(allphysicaldimensions inmeters). all changes in the environment explored by the robot are taken into aecoun< specially because a landmark is built in less than 200 ms, so it is easy to keep the map actualised. Besides, the size of the landmark is so small that the system allows storage of a very large number of them and even if a large area of the map has changed, there is enough information to be localised. Further work will be focused on making the process as independent as possible of parameters U and Dist. Also, it is interesting to include the dispersion ratio of the resulting region as a new control parameter, in order to provide a more precise localisation. ACKNOWLEDGEMENTS This work was partially supported by the Spanish Comisi6n Interministerial de Ciencia y Tecnologia (CICYT), Project No. TIC095-0589. REFERENCES [1] Holenstein, A., Muller, A. and Badreddin, E., “Mobile robot localization in a structured environment cluttered with obstacles”, IEEE International Conference on Robotics and Automation, 3, Nice, France, pp. 25762581, 1992. [2] Kleeman, L., “Optical estimation of position and heading for mobile robots using ultrasonic beacons and deadreckoning”, IEEE International Conference on Robotics and Automation, 3, Nice, France, pp. 2582-2587, 1992. [3] Chenavier, F. and Crowley, J., “Position estimation for a mobile robot using vision and odometry”, IEEE International Conference on Robotics and Automation,
1655
3, Nice, France, pp. 2588-2593, 1992. [4] Rencken, W., “Concurrent localisation and map building for mobile robots using ultrasonic sensors”, IEEJMRSJ International Conference on Intelligent Robots and Systems, 3, Yokohama, Japan, pp. 2192-2197, 1993. [5] Devy, M. and Bulata, H., “Multi-sensory perception and heterogeneous representations for the navigation of a mobile robot in a structured environment”, International Symposium on Intelligent Robotic Systems, Lisbon, Portugal, pp. 361-368, 1996 [6] Kreezmer, B., “Orientation and position determination of a mobile robot equipped with ultrasonic sonars”, International Symposium on Intelligent Robotic Systems, Grenoble, pp. 231-237, 1994. [7] Bml P.J., Hong, T. and Rosenfeld, A., “Image smoothing based on neighbor linking”, IEEE Trans. Systems, Man Cybern., 11 (12), pp. 769-780, 1981. [8] Zhang, J. and Modestino, J.W., “A model-fitting approach to cluster validation with application to stochastic modelbased image segmentation”, IEEE Trans. Pattern Anal. and Machine Intell., 12 (10), pp. 1009-1016, 1990.
