Observation Likelihood Model Design and Failure Recovery Scheme Toward Reliable Localization of Mobile Robots

2.1. Observation likelihood model design

The first step in the design of the observation likelihood model is the determination of pose error E(x), where x is a robot pose. E(x) is determined by a sensor type, map representations, selection of features, map-matching function and so forth. E(x) is affected by sensor noise and unmapped obstacles.

Our pose error E(x) corresponds to the map-matching error between the scanned and predicted range images. The map-matching error should be minimized at the real pose. The error should be smoothly increased if the pose of the computed reference sample pose moves away from the real pose. This fact implies that sensitivity ∂E(x)/∂x should not be too high.

Another significant property is that E(x) should be insensitive to partial corruption of sensor data. Partial corruption occurs when there are unmapped obstacles around the robot and ∂E(x) / ∂x becomes smaller than the obstacle free case. However, it is desirable that E(x) maintains its overall convergence property with reduced accuracy.

After the definition of E(x), the next step is to design a probability mapping function P(E) to perform resampling. The term ∂P(x)/∂x determines the resultant convergence speed. If ∂P(x) / ∂x is too high, convergence property becomes worse. Since the pose estimation result is too much sensitive to sensor data, convergence to wrong position may take place when there exist sensor noise or data corruption due to obstacles. MCL may fail to track the robot pose because too fast convergence reduces the area of sample distribution. The larger area of sample distribution implies higher uncertainty handling capacity.

On the other hand, too small ∂P(x)/∂x results in poor performance of pose estimation due to slow convergence. Therefore, it can be concluded that P(E) and E(x) should be carefully designed in order to achieve fast convergence and appropriate sample distribution area.

2.2. Design of map-matching error

Fig. 1 illustrates a designed map-matching error function named “matching error”. The matching error E _m (x) is defined as the difference in the area between the free space of the scanned image and the expected reference image. The expected reference distance is computed from a given grid map. The term Scan distance Z _t ^j corresponds to jth measured distance from the scanned data. The term Reference distance Z _t ^j* is a computed distance obtained by using the ray-casting algorithm. The matching error is defined by the following equations:

A t j ∗ = ( Z t j ) 2 Δ θ

(1)

A t j = ( Z t j ) 2 Δ θ

(2)

Δ A t j = | A t j ∗ − A t j |

(3)

E m ( X ) = { ∑ j Δ A t j ∑ j ( A t j ∗ + A t j ) × 100 ( % ) }

(4)

OPEN IN VIEWER

Fig. 1.

Illustration of matching error.

Equations (1) and (2) imply the area of the free spaces. Equation (4) defines the matching error E _m (x). Δθ is the angular resolution of laser range finder. The denominator in eq. (4) carries out normalization. There are two roles of normalization. The first role is that the reliability of current map-matching performance can be evaluated as a bounded region from 0% to 100%. The designer can evaluate the localizer reliability by considering E _m (x). The second role of normalization is to modulate the effect of obstacle locations. E _m (x) becomes too large when obstacles are close to a robot without normalization.

E _m (x) was empirically designed after testing a large variety of candidate functions. The proposed matching error shows several advantages. In the conventional approach in (S. Thrun, et. al., 2005), the error is independently evaluated for each range data. The resultant positional probability is computed by multiplication of individual probabilities. Therefore, corrupted sensor data should be filtered out by using additional filtering techniques, as shown in (D. Fox, et. al., 1998). However, the proposed matching error in this paper is less sensitive to partially corrupted sensor data due to obstacles.

Fig. 2 shows the result of the matching error distribution simulation when the real robot pose is located at (101,101) in the given map. For simplicity, the environment is a square and we use a grid map. The boundary of a rectangle is occupied and represented by black dots. Fig. 2(b) shows the matching error distribution with four obstacles, which are denoted by black dots. From Fig. 2(a), it is verified that matching error E _m (x) is minimum at the real pose and the error surface smoothly increases according to the pose error. Fig. 2(b) shows that the overall shape of E _m (x) surface does not change a lot even though the accuracy was decreased due to data corruption by obstacles.

OPEN IN VIEWER

Fig. 2.

The matching error distribution plot as the robot position changes for two cases: (a) without obstacle and (b) with for obstacles (marked by black dots).

Fig. 3 shows the matching error under orientation errors. It is verified that E _m (x) is designed to achieve desirable convergence properties for all cases. Therefore, it can be concluded that the proposed map-matching function E _m (x) satisfies the design requirements in section 2-1.

OPEN IN VIEWER

Fig. 3.

The matching error plot along the changes of the orientation error.

Once map-matching function is defined, a developer should design the probability update rule, which is represented by p(o _t |x _i,t ,m) ← E _m (x). One example is to use the reciprocal error function in (D. Lee and W. Chung, 2006). However, it is difficult to estimate resultant performances of the localizer by using the reciprocal function.

Our suggestion is the use of a saturated Gaussian function as the probability density function. By monitoring the standard deviation of matching error, a designer can obtain quantitative insights into the convergence properties. Furthermore, excessive convergence can be prevented by using the saturation. The probability saturation region is designed in order to avoid erroneous concentration of probability. The localization accuracy cannot be higher than the resolution of a grid map and sensing accuracy. Therefore, it is advantageous to saturate the probability around the center of Gaussian distribution. The excessive number of iteration may reduce the pose distribution to a very small area and may disturb the uncertainty handling capability. Pfaff et.al. proposed a localization algorithm using an adaptive likelihood model in (P. Pfaff, et. al., 2006). The adaptive likelihood model is advantageous in dynamic environments. In this paper, we exploit this algorithm. We use the statistical data of the samples, which are the mean value and standard deviation of the sample's matching error, to adjust the convergence speed. The designed probability updating rule is given by the following equations:

p ( E m ( X ) ) = η 2 π σ t 2 ; E m ( X ) ≤ b i a s

(5)

p ( E m ( x ) ) = η 2 π σ t 2 exp ( − E m ( x ) − b i a s ) 2 2 σ t 2 ) ;

(6)

b i a s < E m ( x ) ≤ μ t + 3 σ t p ( E m ( x ) ) = 0 ; E m ( x ) > μ t + 3 σ t

(7)

We compute the probability using (5), (6), and (7).B is a normalizer. The σ _t and μ _t denote for the standard deviation and mean value of sample's the matching error at time t Bias determines the region of probability saturation. The adopted Bias is 3% because the grid resolution is 10cm. The region of saturation is inside of 3% E _m (x) contour in Fig. 2(a). From Fig. 2(a), it is clear that 3% contour region is much smaller than the grid resolution. This value implies that the saturation region is about 30cm, as shown in Fig. 2(a). Equation (5) implies the saturation region of probability. The bias means that if the matching error is smaller than the bias, the probability is saturated. Equation (7) shows if the matching error E _m (x) is larger than μ _t + 3σ _t . This fact implies that the region of convergence covers 99.7% of the entire probability distribution region. Equation (6) shows the Gaussian probability function. The bias implies the convergence limit of the observation likelihood model.

Fig. 4 shows the probability distribution of the Beam-model and the proposed model under the identical environmental conditions with Fig. 2(b). It is well-known that the continuous distribution is more robust than the discontinuous distribution. As shown in Fig. 4(a), the probability distribution shows local discontinuity around the true position. The positional error is larger than 20cm. This result implies that the Beam-model requires a large number of samples to deal with sensor noise. However, the probability distribution of the proposed scheme is smooth around the true position as shown in Fig. 2(b). Therefore, it is advantageous to adopt the proposed observation likelihood model.

OPEN IN VIEWER

Fig. 4.

The probability distribution simulation results as the robot position changes. (Red curve indicates the highest probability contour.) (a) Beam Model, (b) Proposed model.

Since the proposed scheme is a range sensor based localization scheme, it works only when sensor readings are partially corrupted by obstacles. In the highly dynamic environment, additional sensors such as upward pointing cameras should be installed.

3.1. Localizer status estimation using the matching error

The objective of the localizer status estimation is to decide whether the current localization status is localizer success or localizer failure. The matching error was used to detect a localizer failure. It is assumed that matching error is greater than a specific threshold when localization is failed. The major difficulty lies in the determination of the threshold. The threshold cannot be a single constant.

Fig. 5 shows experimentally measured the matching error history while the mobile robot performs 11,000 steps of the MCL successively. The navigation experiment is conducted in a typical office building which is shown in Fig. 7 and travel distance is about 670m. As shown in Fig. 5, the matching error is non-zero due to various errors, even for a successful localization. The matching error is affected by the scale of environment, unmapped obstacles, sensing accuracy and so forth. Therefore, it is difficult to estimate the localizer status with a constant threshold of the matching error. In this paper, the threshold h is the upper limit of the matching error of a localizer success.

h = μ i − k : i + 3 σ i − k : i + b i a s

(8)

OPEN IN VIEWER

Fig. 5.

Experimentally measured matching error during a navigation. (a) Matching error history, (b) Matching error histogram.

Equation (8) shows the computation of threshold h. μ _i-k:i is the mean value of the matching error from (i-k)th step to i-th step, σ _i-k:i is the standard deviation of the matching error.

Equation (8) exploits the sliding window scheme based on Hypothesis testing using Gaussian and χ² test in (F. Gustafsson, 2000). The proposed equation is a simplified representation of the Hypothesis testing. Since bias indicates the upper bound of the Gaussian probability, the threshold is shifted by bias. The confidence interval is determined by 3 σ, since the matching error does not exceed the confidence level in most cases without wheel slippage. Finally, the sliding window size k is empirically determined from experiments. k is 500, which corresponds to 100s.

The localizer status estimation is independently carried out with the MCL loop for updating cycle of sensor data, since the MCL cycle time is more slower than the sensor cycle time. The matching error can be higher than h for a short time period. In most cases, instantaneous failures can be recovered by conventional MCL owing to its robustness. Hence, we use a median filter (E. Olson, et. al., 2005) to eliminate the short-term outlier values. A median filter removes the outlier by replacing a data with the median value of the n surrounding points. In the experiment, n was set to 15. The localization status is switched to localizer failure when the computed value of the matching error by the median filter is greater than h.

We propose a semi-global localization scheme to deal with exceptional localization situations. Many uneven floors exist in indoor environments. In such a case, abrupt wheel slippage may occur, which cannot be compensated by conventional MCL techniques. Using the localization status history of past movements, we can detect the moment that robot fell into localizer failure. Therefore, we can specify the reachable region of the robot. Semi-global localization implies that the sample distribution can be limited to the robot's reachable region.

Fig. 6 shows the maximum motion boundary region as a circle after the localizer fails to estimate a robot pose. Suppose that a robot starts navigation at time t₀ following the reference path. The localizer status is estimated as a localizer success at time t₀. While the robot moves forward, the robot's wheel slips at time t₁. In Fig. 6, slip distance of the left wheel was larger than that of the right wheel. Since it takes time for a robot to detect localization failure, the robot's localization status was switched into localization failure at t₃. It is clear that the local tracking may fail at t₃ as shown in Fig. 6. The maximum motion boundary region of a robot after slippage is a circle, as represented in Fig. 6. The maximum motion boundary region is computed by assuming that the maximum moving distance is the accumulated distance from time t₀ to t₃ in all directions.

S max = ∑ i ( 1 + 3 σ ) | Δ s i |

(9)

OPEN IN VIEWER

Fig. 6.

Conceptual illustration of semi-global localization.

Equation (9) shows S _max , which is the maximum moving distance. σ is the standard deviation of the motion model. Δs _i is the moving distance from wheel encoders. Using a motion model that has the Gaussian probability distribution, we assume the maximum moving distance by abrupt wheel slippage as max(δ _trans ) ≈ 3σ. We perform the semi-global localization in the maximum motion boundary area with intentionally added 300 samples.

4.1. Experimental environments

The experiments with the proposed observation likelihood model are conducted in a large building, which is shown in Fig. 7. Fig. 7(a) shows the target workspace. The environment contains some difficulties: a mirror-like surface around the elevator hall, which is located around x=64m∼70m and a stair, which is an open space. Fig. 7(b) shows the environment as a grid map with a grid resolution 10cm. Fig. 8 shows a two wheeled differential drive mobile robot. The robot is equipped with two SICK laser scanners in the front and rear. The laser range finder scans 180° with 1° angular resolution and the maximum range was set to 9m.

OPEN IN VIEWER

Fig. 7.

Experimental environments and its grid map. (a) Target workspace, (b) Grid map.

OPEN IN VIEWER

Fig. 8.

A mobile robot used for experiments.

Fig. 9 shows a localization result using the proposed matching error. Fig. 9 shows contour of the matching error on the left side and probability distribution on the right side. Fig. 9(a) shows first sampling result in which the samples are randomly distributed. After the first update, the samples are converged to a region in which the matching error is smaller than 60% as shown in Fig. 8(b). After 3 steps of sensor update, the samples converge to the 10% region as shown in Fig. 8(d). The experiment showed that the samples converged to the real robot pose after iterations. The samples did not converge excessively when the saturation was used. In addition, a designer can monitor the localization result intuitively using the matching error. For example, the map-matching result can be clearly monitored quantitatively by drawing the matching error contour.

OPEN IN VIEWER

Fig. 9.

Convergence of samples by MCL. (a) Initial sampling, (b) Sample distribution after 1st sensor data update, (c) Sample distribution after 2nd sensor data update, (d) Sample distribution after 3rd sensor data updates.

Fig. 10 shows the matching error contour under the different corruption ratios due to obstacles. A corruption ratio is a percent of corrupted sensor readings, where the range error is greater than m. As shown in Fig. 10, the contour shape does not change greatly even when obstacles block the sensor. Therefore, the convergence property is still maintained, even though absolute accuracy was decreased.

OPEN IN VIEWER

Fig. 10.

Matching error contour from corruption of sensor data by obstacles. (a) Corruption ratio 0%, (b) Corruption ratio 30%.

Fig. 11(a) shows the measured matching error that was monitored when the robot moved in the experimental environment for about 200s. A navigation experiment was conducted in the environment shown in Fig. 7 and the localizer successfully tracked the robot pose. This means that the localization status was localizer success during the experiment.

OPEN IN VIEWER

Fig. 11.

Matching error history during a navigation. (a) without median filter, (b) with median filter.

Even though the status of the localizer is localizer success, the matching error is non-zero due to various error sources. Fig. 11(a) shows the matching error which exceeds the threshold h instantaneously, and those instants are denoted by red-circle. False alarms occurred 6 times. This result implies that the localizer estimation result might return a localizer failure, which is a wrong estimation result without the median filter. Fig. 11(b) shows the matching error with the median filer. As shown in Fig. 11(b), outlier values are filtered out using the median filter. Therefore, the result of the localizer status estimation was always successful without any false alarms.

As shown in Fig. 12, there are two different floors in our experimental environment: one is cased with marble and the other one is cased with rubber tile. Sometimes, two wheels of the robot are stuck at the boundary of two different surfaces. As a result, the robot cannot move forward, even though the control input is pure translational motion.

OPEN IN VIEWER

Fig. 12.

Wheel slippage situation due to uneven floor condition.

Fig. 13 shows grid map and scanned image before the active wheels are stuck over the floor. As shown in Fig. 13, the scanned image (denoted by blue dots) and the grid map (denoted by black dots) matches sufficiently. This fact implies that the localizer successfully tracked the robot's pose.

OPEN IN VIEWER

Fig. 13.

Grid map and scanned image obtained with laser scanner.

Fig. 14 shows localization failure case by abrupt wheel slippage. Even though the mobile robot cannot move to forward due to the wheel slippage, the MCL samples move forward from the real robot pose by the motion model. As a result, the localizer failed to track the robot pose because there were no samples anymore at the real robot pose.

OPEN IN VIEWER

Fig. 14.

Localization failure.

Fig. 15 shows the matching error history. Localizer failed to track the robot pose at 870s due to abrupt wheel slippage. As shown in Fig. 15, the matching error exceeds threshold h, which is represented as red circles. As a result of the estimation, localization status is switched to localizer failure. The semi-global localization is carried out after 870s.

OPEN IN VIEWER

Fig. 15.

Matching error history at localization failure 870s.

Fig. 16(c) shows that the localizer starts the semi-global localization because the matching error exceeded threshold h by the wheel slippage. As shown in Fig. 16(c), the sampling region is spread over the reachable region after slippage at (2, 17). The localizer recovers from the localization failure after 20s. The area of the reachable region after slippage was about 2.6m². If we adopt conventional global localization instead of the proposed semi-global localization scheme, the samples should be spread over the whole workspace. The area of whole workspace is about 1539 m². The computational cost is proportional to the number of samples. If the uniform sample density is assumed, the number of samples is proportional to the area of sample distribution. The required sample size for the successfully global localization in the given environments is about 5,000 ∼ 10,000 samples. However, the samples size for the semi-global localization is 300 samples. It is advantageous to use the proposed semi-global localization because it is computationally efficient than the conventional global localization.

OPEN IN VIEWER

Fig. 16.

Semi-global localization.