Feature Based Occupancy Grid Maps for Sonar Based Safe-Mapping

1 Introduction

This paper provides a framework by which any number of environment features can be integrated within the OG paradigm. OG methods provide dense maps but the veracity of these maps is questionable under the duress of sensor noise especially with sonar. For example figure 1 shows a rectangular environment with the corresponding OG map overlaid. Large deviations from the ground truth are seen essentially because of specular reflection when the conical beam of the sonar is not normal to the surface that it is impinging. It is obvious from figure 1 that some walls have been destroyed by the OG map and shown as empty space. A decision taken by the robot based on this map may be prone to danger or at least run time collision avoidance overhead will be increased.

We are thankful to Ms. Shweta Kumari for her help in different phases of completion of this work.

There have been two broad methods of overcoming these problems. Within the grid-based mapping paradigm, Howard and others [Howard and Kitchen, 1996] introduced the notion of response grids to alleviate some of the problems due to sonar characteristics. Here a grid was updated with a response value if it gave a response in atleast one of the n considered directions. In [Thrun 2003] a method of finding maximal likelihood maps based on forward sensor models was presented. Other authors bypass the OG framework to represent sonar data in terms of features. Some of the prominent methods include [Leonard and Whyte 1992]. In these methods features in the workspace such as edges and corners are identified from a circular scan of the environment by initially identifying regions of constant depths (RCD). In [Strachniss and Burgard 2003] coverage maps were presented as alternatives to traditional OG maps [Elfes 1989]. Though not primarily from the point of view of alleviating sonar noise the method of giving coverage values to cells instead of the traditional occupancy/non occupancy values could mitigate some of the challenges posed by sonar data.

In this paper we present a novel method of integrating features within an OG framework and show performance gain of this method, in terms of accuracy and safeness, over the traditional OG paradigm in number of runs. As far as our knowledge goes we have not come across a similar method in literature. Hybrid maps [Nieto et. al, 2004] offer a method for integrating features into OG but do not make use of the probabilistic framework of OG. Moreover the method makes use of image data rather than sonar and is disparate from this effort of tackling sonar challenges. The other novelties of this method include its providing a convenient framework of integrating several features within the Bayesian update scheme of OG. Thereby it combines the elegance and ease of OG updates with the consistency of feature based mapping. Dense OG maps that are not subject to sonar errors find several utilities for autonomous navigation using inexpensive sensors.

2 Methodology

2.1 Categorizing RCD

Consider a sensor moving on a circle of radius R centered at O', the robot's center. Let successive measurement be at an angular spacing of .notdef.notdef. Then the measurement of a plane surface that returns back would have theoretically a difference of t d .notdef R (1-cos.notdef.notdef). The difference t d between two consecutive scan locations S1 and S2 is shown in figure 2a. The assumption is that as long as a part of the conical wavefront of the transducer hit the surface normally, it is returned back to the transducer. This assumption is along the lines reported in [Leonand and Whyte 1992]. For a corner the difference theoretically would be higher and also varies for every scan, since the corner reflection is returned to the transducer from a single point than from a surface [Leonard & Whyte, 1992]. In experiments t d = 1 cm was sufficient to detect wall RCD's from all other readings.

Figure 2b: The minimum number n for a valid RCD.versus distance

A minimum number n of such successive range measurements Z= {Z1, Z2, ..., Zn} that satisfies the threshold criteria is classified as a wall RCD. A plot of this minimum number versus mean distance of the candidate RCD points is shown in fig. 2b. Our algorithm extracts only wall RCD's from any circular scan.

2.2 Existence of a wall, Computing P(WE)

The existence of a wall is synonymous with existence of the wall RCD described above. The probability of existence of a wall, ( ) WE P , is hence either 1 or 0.

2.3 Existence of a corner, Computing P(CE)

The probability of existence of a corner is obtained as follows. Wall RCDs are parameterized as straight lines. The point of intersection of any pair of such lines is computed and denoted as IP in figure 3b. Let dc be distance of the closest range measurement Z projected on the plane from IP. Let distance d1 and d2 be the distances from the corner point to the closest end points of the wall obtained so far.

The probability that a corner exist is given by

In other words we say a corner exists if a measurement is obtained that is close enough to nearest end point of the two walls and close enough to the point of intersection of the two walls. Based on empirical results we found that above classification scheme never wrongly asserts the existence of a corner when there is none. It asserts the existence of a corner to an accuracy of 95 in 100 if there is actually one. Figure 3a shows graph of P(d1) versus d1 while 3b shows how d1, d2 and dc are computed. Similar graphs are used for computing P(d2) and P(dc).

2.4 Associating measurement to a wall

Here we compute the probability that a measurement Z is due to an existing wall, denoted by ( ) WE W Z P / = . Given a

set of walls W1, W2,…,W.notdef and a measurement Z, we find the perpendicular distance from Z projected on the plane to all the walls. If the perpendicular distance does not intersect the wall segment we then consider the distance to the nearest endpoint of the wall segment. Denoting this distance as dw we compute the probability that the measurement Z is due to an existing wall as

Figure 4 shows how dw is computed for two range measurements Z1, Z2. For Z1 the distance to the wall W1 is computed as the perpendicular distance dw1 to the wall and for Z2 the dw2 is the distance to the nearest endpoint on W2. Note that, ( ) WE W Z P / = is computed for a given measurement over all existing walls.

2.5 Associating measurement to corner

Here the probability that the measurement is due to an existing corner, ( ) CE C Z P t / = , is computed. Let Cr be an existing corner. Let Pt be the range measurement projected onto the plane. Consider a cone of beamwidth .notdef whose central axis connects Cr with the robot's position R. If Pt lies with this cone then ( ) CE C Z P t / = is computed as

Where cor d is the distance from Pt toCr , cor σ is fixed at 15cm based on experiments performed. Figure 5 shows t P , Cr and the computation of dcor.

2.6 Associating measurement to others,

It is ( ) OE O Z P t / = .Note that ) (OE P , the existence of other features, is always 1 in the sense there always is, in the environment, things other than walls and corners. This could be also empty space. Since ) (OE P is always 1 we have :

Since (5) and (6) are already probability functions in that

their values are in [0,1] it is easy to rewrite (4) as ( ) ( ) ( ) ( ) ( ) ( ) 7 ... 1 1 W Z P C Z P O Z P = - = - = =

2.7 Integration into the OG framework

The standard OG framework computes the occupancy and non occupancy of relevant cells, each denoted by xy G , under usual independence assumption between cells, given measurements { } t t Z Z , 1 + , the most recent being 1 + t Z as

We denote ( ) t xy Z G P / in (8) as ( ) t xy G P . Under usual Markovian assumptions ( ) t xy t Z G Z P , / 1 + becomes ( ) xy t G Z P 1 + and further application of Bayes rule on ( ) xy t G Z P 1 + reduces (8) to ( ) ( ) ( ) ( ) 9 / 1 1 … t xy t xy t xy G P Z G P G P ⋅ = + + η .

Here .notdef is a normalization constant which gets cancelled out during analogous computed of ( ) 1 + t xy G P ; the non occupancy of cells Gxy and subsequent normalization.

In the current paradigm we integrate the existential probabilities of features and the association probabilities of measurements to features by modifying ( ) t xy G P in (9) as

Note that by this modification the basic recursive nature of (9) is still preserved and can be repeated over any sequence of measurements. However (10) has subtly removed the independence assumption between cells since ( ) W Z P t = and ( ) C Z P t = do take into consideration the closeness of measurements to features. Unlike OG paradigm, cells are not always updated where measurements are made but on cells where the features associated with the measurements are most likely. This is elaborated in the following sections.

2.8 Compute ( ) W Z G P t xy = /

Let Pt be the projection of measurement Zt. A cone of beam width 2.notdef whose central axis connects the robot's current position to Pt is considered. The end point of the conical front is projected onto the associated wall. In figure 6 the end points of the conical front, Pc1 and Pc2 are projected onto the wall at Pw1 and Pw2. The extension of Pc2 to the wall meets at we P . The probability function are framed in a manner such that cells on the line from Pw1 to Pw2 are high in occupancy, from Pw2 to Pwe are left unupdated (unknown) and remaining cells in the triangle S Pw1 Pwe are high in unoccupancy probabilities. The probability functions themselves are not shown for conciseness.

extended wall at Pw1 and Pw2. Pc2 extends to Pwe on the wall. Figure 7: Cr is the corner feature. Points Pt and Pc1 get projected on wall W1 at Pcr1 and Pcr2, while SPc2 meets W2 at Pce2.

2.9 Computing ( ) r t xy C Z G P = / :

The conical front is formed by Pc1, Pt and Pc2 in figure 7. The corner is denoted by Cr. W1 and W2 denote the two walls. Pc1 and Pt are projected onto W1 at 1 cr P and 2 cr P . The segment SPc1 and SPc2 intersect the walls at Pce1 and Pce2. The cells on the line from Cr to Pcr1 are updated to probabilities that are high in occupancy. The cells between Pcr1 and Pce1 and between Cr and Pce2 are not updated. The remaining cells in the area enclosed by quadrilateral SPce1CrPce2 are updated so that their non-occupancy probability is high.

2.10 Computing ( ) O Z G P t xy = / :

This is the routine OG update for all cells within the conical front of the sonar [Elfes 1989,Thrun 2003]. When t Z is associated with a feature, ( ) O Z P t = tends to zero and the contribution of ( ) O Z G P t xy = / in (10) is negligible. When ( ) O Z P t = is dominant the update of cell probabilities is delayed by one or more time instants. This delay helps to ascertain more certainly if there are no features around the cells corresponding to t Z . There can arise four situations: (i) If a feature is formed in the immediate instants around cells corresponding to the previous reading t Z it gets associated with the features and updated as mentioned in previous sections. (ii) If features are formed but they are hiding the cells of t Z , t Z will be considered as outlier and cells beyond the feature up to t Z are left as unknown. (iii) If in next few instants t Z cannot be associated with any feature though there are some features found in a wider conical neighborhood of that direction, then, only nonoccupancy will be updated up to a safe line, which is the line joining the ends of those neighbors' features, as shown encircled in figure 9. Strict space limits prevent further elaboration. (iv) Otherwise routine OG update takes place. This delayed update takes care of several inconsistencies, by suspending the OG behavior for sometime and by detecting outliers/noisy readings, that otherwise occur when a reading corresponding to a feature is updated in the standard OG sense. Figures 8a and 8b: Overlapping RCDs get associated as in 8a. Non overlapping RCDs as in 8b are retained as such

2.11 Associating RCD'S:

Every new 360 scan results in new RCD'S as well as RCD's that need to be associated with previous ones. Two RCD's are associated if their line segment (walls) fitted on them intersect at angles less than 5 degrees . In such a case the wall that was obtained at closer range from the robot is taken as the feature and points on the other wall are associated with this one and updated as mentioned before. RCD's that are separated by some spacing between them are not explicitly associated with one another. In figure 8a 2 RCD's are associated because of overlap and the wall gets extended. The 2 RCDs in figure 8b are not associated due to spacing between them and treated as independent walls.

3 Experimental Results and Analysis

Figure 9 is the map obtained by the current method of integrating features with the OG paradigm for the same rectangular environment of figure 1, with the scans taken exactly at the same locations. The performance gain of the current method is apparent as the map of figure 9 is significantly closer to the ground truth. The resolution of every cell in this and subsequent maps is a 2 4cm area. Figures 10a and 10b show two portions of a corridor as it bends around a corner that is not at right angles to the right shown by an arrow in 10a. Figure 10b also shows a water cooler/fountain while 10a shows a trashcan on the right. The maps obtained by the OG and current methods are shown in 11a and 11b. Once again the performance gain of the current method is evident as 11b is much closer to the ground truth vis-à-vis 11a. The corridors corresponding to 10a and 10b are marked A and B in fig. 11a. Also the region near the fountain is marked as F and the trash can as T in fig. 11b. Figure10c shows a corridor studded with cartons, 12a its OG map and 12c the enhancement with current method.

Maps were built from exactly the same places for both methods, visited by an Amigobot equipped with 8 sonar sensors for all these experiments.

3.1 Analysis:

A more rigorous scheme for comparing performances of the two methods is developed here. The actual map is overlaid on cells along with the maps obtained by OG and current methods. The actual map is compared with the two methods through a scheme that assigns error levels/values as shown in table 1. Test1 (table 1) finds if there is at least 1 occupied cell in the immediate neighborhood of the current cell, in the obtained map, while test2 does the same in the original map. Based on these assignments an error level greater than or equal to 3 are dangerous, for they show cells that are occupied or unknown as unoccupied. A robot visiting those cells based on these assignments is prone to collide with objects or will have run time collision avoidance overheads. Maps will be less accurate if neither the cell nor its neighbors resemble the original cells' states. A map will have exploration overhead if regions which should be known from a specific set of scans are still remaining as unknown. This will be slightly more in the present method because it finds out noisy readings as outliers and updates only till features, leaving some cells unknown as mentioned in 2.10

Figures 13a and 13b show the error plots for the maps of figure 1 and figure 9. In these plots areas shown in white are those that have been mapped accurately and have no errors. Areas that are shown in dark shades of gray have the highest errors and danger levels of 3 & 4. They are significantly lesser for the current method vis-à-vis OG paradigm. The

12a & 12b:The maps obtained by OG and present methods for 10c.The door(D), shown by solid arrow, and the zig-zag nature of the environment have been correctly retained by present method.

ascendancy of danger levels from white to black is shown though the danger level indices in the middle gray-level bar. Figures 13c and 13d show the histogram plots at error levels

Figures 14a-14d: 14a: Error plot for the map in figure 12a obtained by OG method. 14b: Error plot for map in figure 12b obtained by current method. 14c: Histogram error plot for map in figure 12a. 14d: Histogram error for map in figure 12b.

0 to 4 for the maps of figures 1 and 9. Figures 14a and 14b depict the error plots for the maps in 11a and 11b, 14c and 14d are the error histograms of these maps.

Table 2 shows corresponding values as follows. The dangerous cells (Z5 in table 2 corresponding to error levels > 2.5 in table 1) are at-least 20 to 50 times higher in the OG methods over the current method. Overall at-least 95% of the danger regions have been reduced by the current method (last column shows the percentage reduction of danger cells). The table also shows the number of cells classified as accurate (Z1) to be more in the present method. The number of cells with exploration overhead is slightly higher in the current method due to reasons mentioned before. The number of cells with error levels less than 3 (those that belong to safe zone or Z2=Z1+Z3+Z4) is once again higher for the present method. The number of overall inaccurate cells (Z5+Z4) is significantly lesser by current method. Note that all cells in danger zone are also inaccurate. In absolute terms there are at-least 2 x 10 3 more number of cells of 4cm 2 area that are mapped wrongly in the OG paradigm when compared with the current method for the rectangular environment. This number is even higher for the corridor environments. The performance gain of the current method was of the same order for various other experimental runs on our robot on diverse environments that are not reported here for brevity.

3.2 Retrospection

The reasons for enhanced performance of the current method are attributed to the following. Firstly by associating measurements to known features; the updates of ( ) C Z G P t xy = / and ( ) W Z G P t xy = / are very different

from the standard OG update; the physics of the sonar is being accounted for more in the current method. Typically at corners and at walls that are not at right angles to the conical front of the transmitted beam the measurement errors are significant due to specular and possibly multiple reflections. By understanding the presence of features at those locations through the RCD and by associating future measurements to those features the error is considerably reduced, consistency with ground truth increased. Secondly associating measurements to features also implicitly takes care to certain degree what is often called the bane of the OG paradigm; independence assumption between neighboring cells. Thirdly the method of delayed update, mentioned before, of measurements not associated to any features at the current instant overcomes the inconsistencies that arise when a measurement that actually is due to a feature is updated as such. Delayed update reduces inconsistencies that depict cells occupied behind features that should actually be depicted as unknown.

4 Conclusions

This paper has presented a method of overcoming some of the limitations of the OG paradigm which get pronounced especially with sonar data without sacrificing the elegancy associated with the OG method in the form of Bayesian updates. This it does by integrating features within the OG updates thereby alleviating problems associated with the peculiarities of sonar range data. From an OG perspective the method captures the physics of the sonar by initially identifying existence of features. By associating future measurements to these features it mitigates the chief problem of OG albeit implicitly; the independence assumption between neighboring cells during updates. The nature of the formulation presented is such that any number of features can be integrated within the OG paradigm. Also since it enhances the performance of OG and also extracts and uses the features like wall and corners, so any method of SLAM either based on feature or OG can perform with more accuracy. Extensive experiments and analysis reported confirm the performance gain of the current approach over the traditional OG method of building maps.