^{*}

This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

This paper presents a novel approach to mobile robot localization using sonar sensors. This approach is based on the use of particle filters. Each particle is augmented with local environment information which is updated during the mission execution. An experimental characterization of the sonar sensors used is provided in the paper. A probabilistic measurement model that takes into account the sonar uncertainties is defined according to the experimental characterization. The experimental results quantitatively evaluate the presented approach and provide a comparison with other localization strategies based on both the sonar and the laser. Some qualitative results are also provided for visual inspection.

A crucial requirement for a mobile robot to accomplish useful long term missions is that the robot has some knowledge about its pose with respect to a fixed reference frame. The process of estimating the robot pose with respect to such fixed reference frame is known as the

The quality of the pose estimates is, consequently, strongly related to the quality of the sensor measurements. That is why localization strategies usually rely on accurate sensors such as laser range scanners. Most of these sensors are able to provide thousands of readings per second with a sub degree angular resolution. Other sensors, such as standard

However, ultrasonic range finders are still appealing in the mobile robotics community for several reasons. Their price and power consumption are better than those of laser scanners. Moreover, their basic behavior is shared with underwater sonar, which is a sensor vastly used in underwater and marine robotics. A typical underwater sonar, although being far more complex than the ultrasonic devices used in this work, can take profit of those localization techniques accounting for the ultrasonic sensor limitations.

Some researchers have demonstrated the validity of standard ultrasonic range finders, like the Polaroid ultrasonic sensors, to perform localization. For instance, Tardós

However, looking for features has shown to be a complex, unreliable and time consuming task due to the noisy nature of sonar data. That is why different approaches have to be adopted when using this kind of sensors. For example, Burguera

Nowadays it is broadly accepted that probabilistic methods are the most promising ones to deal with sensor and pose uncertainties in real-time. In this context, Kalman filters are commonly used to perform localization and SLAM. However, they fail to represent ambiguities and to recover from localization failures. To confront some of these problems, Dellaert

As a nonparametric implementation of Bayes filters, particle filters have two main advantages. One is that they can approximate a wide range of probability distributions. The other is that even the most straightforward implementation of them exhibits very good results when applied to localization. Particle filters have been successfully applied to SLAM [

The goal of this paper is to define, develop and experimentally evaluate an algorithm to perform MCL without

More specifically, in this paper we propose the use of a probabilistic correlation technique as measurement model in a particle filter to perform mobile robot localization using sonar sensors. Each sonar reading is modeled by a bivariate Normal distribution. In order to properly model the sonar readings, an experimental characterization of the sonar sensor is performed in Section 2.. Thanks to these models, the correlation between two sets of sonar readings can be performed by means of statistical compatibility tests. The particle filter operation is described in Section 3.. Also, in this work, the particles are augmented with local environment information. This local information is updated at each time step, and allows the localization process to be performed without any

During the 80s, Polaroid developed a TOF ultrasonic range sensor for automatic camera focusing. Different versions of these sensors appeared. The third generation was based on the 6,500 ranging module. This module had the ability to detect and report multiple echoes. Moreover, a developer's kit was released for these sensors, allowing the user to configure the different parameters, such as frequency, gain or maximum range. Because of this, the 6,500 ranging module was extensively used in mobile robotics.

Although the original Polaroid sensors are not being used by recent robotic platforms, the 6,500 series ranging module is still commonly used. The ultrasonic sensors used in this paper are those endowed in a

In mobile robotics, sonar sensors are commonly used to detect objects which are in the same plane that the sensor itself. This idea will be referred to as the

Being endowed on a specific robotic platform, some of the sensor capabilities are not accessible to the user while some other are limited because of the robot configuration. For example, the multiple echo capability is not accessible by our mobile robot. Moreover, although a firing rate of 40 ms is possible, only 2 of the 16 available sensors are accessed every 40 ms. The details of our specific robotic platform are provided in [

Let us introduce some basic notation. If the 2D assumption is performed, the position of an object with respect to the sensor can be denoted by the polar coordinates (

The term _{1} is the Bessel function of the first kind of order 1. Let us parametrize the previous equation for the Polaroid sensors used in this paper. According to the sensor data sheet [

Now, let us focus on the dependence of the echo pressure on the azimuth, assuming a constant value for

The first thing to be noticed is that the sound pressure concentrates along the sonar acoustic axis. This area of high sensitivity is called the _{0}, _{0}], where _{0} is the lowest positive azimuth where _{0} ≃ 12.5°. The interval [−_{0}_{0}] lies between the dashed lines in

The sonar response mainly depends on the main lobe, where the most part of the signal strength is concentrated. Because of this, it is reasonable to assume that the received echoes have been produced in the main lobe and, thus, that the azimuth of the detected object lies in [−_{0}, _{0}]. Consequently, there is an azimuth uncertainty of 2_{0}. In the particular case of the Polaroid sensor under analysis, this theoretical azimuth uncertainty is, thus, 25°.

Taking into account the importance of the main lobe, it is common to model a sonar reading as the circular sector defined by the angular interval [−_{0}, _{0}] and centered at the sensor position. The sonar wedge, as defined by Moravec [_{0} is then referred to as the sensor _{0}. This model, which is called the

Some studies [

In order to build the sensor characterization, we have taken into account the following two aspects. First, although the sonar wedge model is widely used, it may be too simplistic. In consequence, the experiments are intended to find the limits of this model. Second, as the sensor is endowed on a specific robot platform, the usual way to access its measurements is by means of the robot's operating system. Accordingly, the characterization relies on reading the range information as it is provided by the robot's operating system under different conditions and, afterwards, comparing the gathered data to a ground truth. For the sake of simplicity, henceforth, the range readings provided by the robot's operating system from the Polaroid sensors will be referred to as, simply, the sensor (or the sonar) readings.

The presented analysis is based on the experimental evaluation of the following sensor parameters: resolution, minimum and maximum range, maximum angle of incidence, sensor opening and accuracy. Only the most relevant results are shown here. An exhaustive description of the evaluation procedures and the results are available in [

The

From these measurements, three important details have been observed. Firstly, it has been observed that the sensor is not able to measure distances up to 5 m. Secondly, we have also observed that there is a minimum detection distance. These two aspects will be discussed later. Finally, it has been observed that the smallest range that the sensor is able to detect is one millimeter. The value is constant along the whole sensor range, and is the same for all the 16 robot sensors that have been tested and for the three aforementioned materials. Accordingly the Polaroid sensor's resolution is, when it is accessed through the robot's operating system, 1 mm. This value slightly differs from the specifications, whereas the resolution is said to be 3 mm.

The

The maximum range is related to the sound attenuation with distance. Our experiments show a maximum range of 4,910 mm. This value significantly differs from the sensor specification, where the maximum range is said to be 10.7 m. Taking into account this large difference it is clear that this limit is imposed by the robot's manufacturer [

The angle of incidence is defined as the angle between the sonar acoustic axis and the perpendicular to the detected object. The

As stated previously, the term

It can be observed how the opening is not constant. For the short range of 0.5 m, the measured opening is significantly larger than the theoretical value. This is likely to be produced by the side lobes. For large ranges the obtained opening is significantly below the theoretical value, which is probably due to the attenuation of the sound with distance.

The term

The errors between the actual and the measured ranges have been computed. The mean and the standard deviation of these errors are shown in

The obtained sensor characterization will be used in Section 4. to build the measurement model for MCL.

Bayes filters address the problem of estimating the state _{t}_{t}_{t}_{t}_{t}_{t}_{t}

The use of particle filters in localization has centered the attention of the localization community since their introduction in this context. Dellaert _{t}^{T}

The term

Let

Finally, let the superindex [1 :

Particle filters build the particle set _{t}_{t}_{−1} one time step earlier. Thus, it is necessary to initialize the recursion by defining _{0}. In general, this initialization only involves the initial population of the state space. For instance, if global localization has to be performed, the initialization usually consists on distributing the particles over the empty regions of the global map randomly according to a uniform distribution. To the contrary, if pose tracking is the goal, the whole particle set can be initialized to the starting robot pose, which is commonly assumed to be [0,0,0]^{T}

In order to initialize the particle set, including the local maps, the robot has to move during

According to the described initialization process, the value of

Although having to rely on dead reckoning during 10 s may seem a long time, it is important to emphasize that this initialization is needed because of the low Polaroid's sensing rate, which is, under the described robot configuration, close to 50 range measurements per second. When compared to other sensors, such as standard laser range finders, which provide thousands of readings per second, the need for a few seconds initialization time becomes clear. A similar issue is found in other studies such as [

When the initialization has been performed, the particle filter is ready to start. The proposed particle filter implementation is shown in Algorithm 1. Next, the particularities of this algorithm and the main differences with respect to the standard particle filtering approach are described.

Line 3 is in charge of sampling from the motion model. In general, the motion model involves sampling from the distribution _{t}_{t}_{− 1}, _{t}_{t}_{t}_{t}_{t}_{t}_{−1}_{t}

Line 4 incorporates the measurement _{t}_{t}

There are two main approaches in the literature to compute

Our proposal is to approximate these probabilities by measuring the degree of matching between the current readings and the particles' local maps. As the local maps are not included into the state vector, the problem of high dimensional state spaces does not appear. In other words, we approximate the weights by

Line 7 is in charge of the resampling. At this point, the algorithm draws with replacement

As stated previously, our motion model provides local robot poses. Thus, an additional step is required in order to update the global robot poses held by the particle set. This is accomplished in line 8 by compounding the global robot pose at time step

Line 9 incorporates the current sonar measurements into the local maps. In order to do this, the robot motion
_{t}

_{t}

During the mission execution, it may be necessary not only to execute the particle filter algorithm but also to perform a

Although the presented pose tracking approach may lead to multimodal distributions, we have experimentally observed that the Gaussian approximation does not introduce significant errors. Consequently, our proposal is to perform this type of density extraction when needed.

The measurement model is in charge of computing the importance factors of the particles. In particle filter localization, the importance factor represents the likelihood of having the current set of readings _{t}

One of the advantages of the presented approach is that it does not require any

The proposal in this paper models the sonar readings by Normal distributions. Then, statistical compatibility tests are used to compute the degree of matching between the current sonar readings and each of the local maps.

The sonar reading provided by the _{xx}_{yy}(r)

The values of _{xx}(r)

For a given particle _{t}_{t})

Finally, the relative position of the sonar sensor _{i}

As stated previously, the particles are weighted by matching the current set of readings _{t}_{t}_{t}_{t}_{t}_{t}

Let _{new}_{t}

Each item in _{new}

To ease notation, let us denote by _{old}_{old}

First, the readings in
_{t}_{old}_{old}

_{new}_{old}

There exist many algorithms to match sets of range readings [_{new}_{old}_{new}_{old}_{new}_{old}

The proposed measurement model is based on matching the current set of readings against the local maps. However, instead of using the Euclidean distance, this method proposes the use of the Normal distributions in _{new}_{old}

Let _{j}_{j}, P_{j})_{old}_{j}_{new}_{1⊕} and _{2⊕} denote the Jacobian matrices of the compounding operator with respect to the first and second operands respectively, evaluated at the mentioned linearization points. Then, the covariance can be computed as follows:

The Mahalanobis distance in _{j}_{old}_{j}

To ease notation, let _{i}, b_{i}_{i}_{new}_{i}_{old}_{new}_{old}

At this point, our approach to Monte Carlo Localization using sonar sensors has been fully defined.

The experimental results presented in this section are aimed at evaluating the quality of the presented particle filter localization approach and to compare the two proposed measurement models, both in terms of quality and time consumption.

To evaluate the sMCL, a

Each data set contains the odometry information, the sonar range readings and the laser range readings. The laser readings have only been used to obtain ground truth pose estimates. In order to obtain such ground truth, the ICP scan matching algorithm [^{2} = 0.0025) to simulate worse floor conditions.

In order to quantitatively compare odometry and the different particle filter configurations, the following procedure has been used. First, the trajectories obtained by odometry, particle filter and the ground truth are approximated by polylines. The vertex density of each polyline increases in those regions with significant amount of robot rotation. Also, the maximum robot motion between two vertexes has been set to 1 m. This kind of approximation is useful to overcome the local perturbations in the individual motion estimates, both for odometry, particle filter and ground truth.

Then, the individual edges of the trajectory being evaluated are locally compared to those of the ground truth. The Euclidean distance between their end points is used as a measure of the edge error. Finally, the edge errors for the trajectory being evaluated are summed. This sum is normalized, using the path lengths between vertexes and the number of edges, and constitutes the

The sonar-based particle localization algorithm described in Section 3. has also been implemented using a different measurement model. This different measurement model is the well known ICP error, which uses Euclidean distance to establish correspondences. Then, the probability distribution

The first experiment evaluates the quality and the execution time of the algorithms with respect to the number of particles,

The first thing to be noticed is that the two measurement models presented in this paper significantly reduce the odometric error. Also, the results provided by the spMCL are significantly better than those obtained when the ICP error is used. Even if only 10 particles are used, the spMCL provides trajectories which are, in mean, a 74.9% better than the odometric estimates and a 42.8% better than the icpMCL. If 400 particles are used, spMCL provides trajectories which are, in mean, a 101.3% better than odometry. Also, the standard deviations of the particle filter trajectories are significantly lower than those of odometry. This suggests that the quality of the particle filter estimates is barely influenced by the errors in odometry. Moreover, the reduced standard deviation also suggests that the trajectory error after the particle filter operation is similar for all of the tested environments.

The second thing to be noticed is that only a very small error reduction appears if more than 200 particles are used. This suggests that the proposed weighting methods are able to accurately select the right particles. It also suggests that using a number of particles between 100 and 200 would be a good choice, more if the execution times are taken into account.

The mean and the standard deviation of the execution times per data set item for the particle filter algorithm using each of the measurement models presented in this paper are shown in

It can be observed how the execution times are strongly linear with the number of particles. The small standard deviations suggest that there is a very small dependence on the number of effective readings in each _{old}_{new}

Finally, it is clear that the icpMCL is significantly faster than spMCL in terms of the number of particles. Still, the quality of the pose estimates has to be taken into account when analyzing the time consumption. For example, using only 10 particles in spMCL provides, in mean, trajectories a 10.11% better than those provided by icpMCL when using 100 particles. Moreover, in this case, the probabilistic approach is a 59% faster than the ICP error based approach. As a matter of fact, using 10 particles in spMCL leads to better results than the icpMCL with any of the tested number of particles. Thus, when analyzing the time consumption required to achieve a certain trajectory error, the spMCL also provides significantly better results than the icpMCL.

The previous experiment has been performed using a local map size

It can be observed that the effects of the local map size are more noticeable than those of the number of particles. For example, in the spMCL case, the trajectory error using

The icpMCL does not perform very well when low local map sizes are used. For instance, in the case

The first thing to be noticed is related to the nonlinear relation between _{t}_{t}

It can also be observed that the ICP error based approach is significantly faster than spMCL with respect to the local map size. The situation in this experiment is different to the previous experiment. For instance, the computation time for spMCL using

Nonetheless, when taking into account both the number of particles and the local map size, spMCL provides better results. For example, the use of only 10 particles and a local map size of 100 in spMCL provides a trajectory error which is a 5.8% lower than the ICP error based approach using 100 particles and a map size of 200. Moreover, in this case, the spMCL approach is significantly faster than the icpMCL.

Overall, it is clear that spMCL is not as well suited as the icpMCL to deal with large local maps in terms of computational requirements. Still, spMCL is able to provide significantly better results with very reduced numbers of particles and small local map sizes than the icpMCL using larger particle sets.

In order to illustrate the previous results, some data sets have been plotted according to the obtained trajectories for visual inspection. _{t}

According to the previous experiments, the spMCL with only 10 particles lead the particle filter to better results than the use of the ICP error based with much higher numbers of particles.

There are some details that deserve some attention in this Figure. First, the two measurement models are able to significantly improve the odometric estimates and to provide results which are close to the ground truth. Second, the spMCL is able to match the readings more accurately than the icpMCL. This is especially clear in the second row, where the readings drawn according to the spMCL define thinner walls than those obtained by means of the ICP error based one. Finally, the results provided by spMCL provide, in general, better trajectory estimates for

In

Overall, the two sMCL approaches presented in this paper provide significant improvements in the pose estimates with respect to raw odometry. In particular, the spMCL has shown to provide trajectories which are very close to the ground truth. As stated previously, the ground truth has been obtained by manually improving the ICP pose estimates using laser readings. Moreover, the odometry estimates used in the particle filters are worse than those used for the ICP to build the ground truth. Thus, the results obtained with spMCL are comparable to those obtained with the well known ICP algorithm and laser sensors.

This paper is concerned to the use of sonar sensors to perform mobile robot localization. To this end, the Polaroid ultrasonic range finder has been experimentally characterized. Thanks to this characterization, parameters such as the resolution, the minimum and maximum ranges, the maximum angle of incidence, the opening or the accuracy have been obtained. Among them, the opening and the accuracy have shown to depend on the range to the detected obstacle.

Afterwards, a novel approach to mobile robot localization using sonar sensors has been presented. This approach relies on the use of particle filters. Each particle is augmented with a set of sonar readings, which is updated during the mission execution. These sets of sonar readings, which constitute the particles' local views of the environment, have two main goals. First, to overcome the sparseness of the sets of readings provided by ultrasonic range finders. Second, to avoid the need for

In order to weight the particles, a probabilistic correlation method is proposed. This method models the sonar readings as bivariate Normal distributions, allowing the use of statistical compatibility tests to evaluate the degree of matching between two sets of sonar readings. The parameters of the Normal distributions modeling the sonar readings come from the opening and the accuracy that were previously obtained.

The method has been evaluated by measuring the quality of its estimates for different numbers of particles and history sizes. Our measurement model has been compared to the well known ICP error approach, showing significantly better results. Also, the results show how the proposed sonar-based particle localization approach is well suited to deal with the sparseness and low angular resolution of sonar readings and provide good estimates of the robot pose using sonar sensors without any

This work is partially supported by DPI 2008-06548-C03-02 and FEDER funding.

(a) Position of an object with respect to the sensor in polar coordinates. (b) Normalized sound pressure as a function of the azimuth. (c) Polar representation of the sound pressure level. The azimuth is expressed in degrees and the SPL in dB. (d) The wedge model.

Measured distance vs. actual distance plot. For each actual distance, 100 measurements are shown. (a) Minimum range. (b) Maximum range. The resolution of 1mm can be observed in both images.

(a) Graphical representation of the sonar opening. The horizontal dashed line represents the sensor acoustic axis. The theoretical opening is shown as the two dotted lines. (b) Accuracy. Mean and standard deviation of the errors for different ranges.

Notation used for sMCL. The triangles represent the robot at consecutive poses.

(a) Example of the particle filter operation. The local maps
_{t}

(a) The proposed sonar model. The ellipses represent the 99% confidence interval of the Normal distributions modeling the readings for different ranges. (b) Relation between the sensor models. The circular sector represents the sonar beam. The dashed cross is the robot reference frame.

Example of sonar readings _{new}_{old}

Some of the data sets using in the experiments. The left column shows the sonar readings according to the initial odometric estimates. The right column shows the sonar readings according to the ground truth trajectory.

(Top) Fragment of a real trajectory. (Bottom) Polyline approximation. The dots represent the vertexes.

(a) Means and standard deviations of the trajectory errors. The experimental results have been obtained using different numbers of particles and setting the history size

(a) Means and standard deviations of the trajectory errors. The experimental results have been obtained using different local map sizes and setting the number of particles

Sonar readings plotted according to the particle filter trajectories. The left column corresponds to icpMCL using 100 particles. The right column corresponds to spMCL using only 10 particles. The local map sizes are

Sonar readings plotted according to the particle filter trajectories. The left column corresponds to icpMCL. The right column corresponds to spMCL. The local map sizes are