Localizability estimation using correlation on occupancy grid maps

Abstract

In the field of autonomous mobile robotics, reliable localization is important. However, there are real environments where localization fails. In this paper, we propose a method to estimate localizability based on occupancy grid maps. The localizability indicates the reliability of localization. There are several approaches to estimate localizability, we propose a method to estimate localizability as a covariance matrix of the Gaussian distribution using local map correlation. Our method can estimate the magnitude of the localization error and the characteristics of the error. To confirm the effectiveness of the proposed method, we constructed simulation environments that include representative shapes of indoor environments. We conducted an experiment to investigate the characteristics of the distribution of local map correlation. Furthermore, we also conducted an experiment of our method to estimate localizability on occupancy grid maps. The simulation experiment results showed that the proposed method could estimate the magnitude of the localization error and the characteristics of the error on occupancy grid maps. The proposed method was confirmed to be effective in estimating localizability.

Introduction

Autonomous navigation of mobile robots requires localization and mapping capabilities. This problem is called simultaneous localization and mapping (SLAM) and has been actively researched. These methods [1, 2] calculate the covariance matrix of the robot poses for the iterative closest point (ICP) registration. When the covariance is smaller, the localization uncertainty is smaller [3].

Reliable localization is important for autonomous mobile robots in tasks such as SLAM, navigation [4], and exploration [5]. Therefore, mobile robots are required to move based on the reliability of localization. However, in real environments, there are several environments where degeneration is likely to occur. The representative shapes of indoor environments are as follows:

• Complex shape environment is unlikely to degenerate because it can be distinguished from the surrounding shapes.

• Simple shape environment such as a corridor is likely to degenerate because the surrounding shapes overlap in the longitudinal direction of the corridor.

• Circular arc shape environment is likely to degenerate in the rotational direction because the surrounding shapes overlap in the rotational direction.

• Repeating pattern shape environment is likely to degenerate because the surrounding shapes overlap at constant intervals.

• Larger shape environment than sensor measurement range is likely to degenerate because the sensor cannot acquire surrounding shapes.

In this paper, we propose a method to estimate localizability based on occupancy grid maps. The localizability indicates the reliability of localization. If the localizability is high, the ability to accurately estimate robot poses at that location is high. If the localizability is low, the ability to accurately estimate robot poses at that location is low. By estimating the localizability of the occupancy grid map, we can estimate the reliability of localization for each location on the map. For example, it can detect locations likely to lose robot poses due to degeneration problems.

There are several approaches to estimate localizability, and we previously proposed a method to estimate localizability as a covariance matrix of the Gaussian distribution using local map correlation [6]. However, the method does not consider the robot rotation. In this paper, we use a covariance matrix of the Gaussian distribution considering robot rotation and estimate localizability.

The contributions of this study are as follows:

• The algorithm of our method was constructed to estimate localizability as a covariance matrix of the Gaussian distribution using local map correlation.

• The characteristics of the distribution of local map correlation were shown for representative shapes of indoor environments.

• The estimation of localizability by our method for the whole area of occupancy grid maps is demonstrated.

Related work

Probabilistic localization methods, such as Bayes filters, estimate the distribution of robot poses [7]. A larger distribution implies a weaker belief, and a smaller distribution means a stronger belief. Therefore, we can determine the degree of confidence based on the distribution size.

There are other studies that seek to find the achievable accuracy of localization [8, 9]. The covariance of the estimation is theoretically evaluated based on the Cramer-Rao lower bound, which is the inverse of the Fisher information matrix.

A different approach, which explicitly models and estimates the reliability of localization, has been researched [10]. Here, it is formulated in a localization graphical model with the reliability added as a hidden variable, and a rao-blackwellized particle filter (RBPF) is used to perform simultaneous robot pose and reliability estimation.

The proposed method is an approach to estimate localizability for the whole map area offline in advance, rather than simultaneously estimating reliability during online localization. Our method estimates localizability using only occupancy grid map data as input, without sensor measurement data. Several methods for estimating localizability have also been researched. Early research on localizability is based on information entropy [11]. The method [11] uses the entropy of the probability function to model the uncertainty of the robot poses. Applying the formulation of [8, 9] to probabilistic grid maps, the method [12] obtained the Fisher information matrix as a localizability matrix using a laser beam scan measurement model. In contrast, our method does not scan the beam, but uses a correlation measurement model to directly obtain a covariance matrix. This has the advantage of utilizing information of not only occupied space but also free space and unknown space. The method [13] uses lidar and UWB to estimate localizability for geometrically featureless tunnel-like environments. In contrast, our method can estimate localizability for not only tunnel-like environments but also circular arc shape environments, repeating pattern shape environments, and larger shape environments than sensor measurement range. The proposed method and [12] estimate localizability for 2D maps, while some studies are for 3D maps. The method [14] computes localizability using constraints based on the surface normals of the 3D shape. In addition, method [15] performs segmentation of 3D point clouds and then obtains feature vectors from normals, eigenvalues, etc., and uses them to compute localizability. These methods [13, 14] and [15] is highly dependent on the environment and difficult to pre-determine in a reliable manner. To cope with this problem, [16] proposes a learning-based method for estimating localizability from a single lidar scan. The paper [17] also proposes a method for estimating localizability based on deep learning. Action selection and path planning using localizability have also been proposed by [18,19,20], and [21].

Our method uses a correlation measurement model to estimate localizability. Thus, it is similar in some respects to correlation-based localization methods. Correlation-based grid localization [22] uses a histogram filter and a correlation model to estimate robot poses. Correlative scan matching [23] uses least squares and a correlation model to estimate robot poses. Compared to localization, the proposed method estimates localizability for the whole map area, not the current robot pose.

Correlation of local maps for localizability estimation

Template matching methods

The proposed method uses local map correlation for estimate localizability. Specifically, the distribution of local map correlation is calculated by template matching from template images and local maps. Template matching is a method that the template image is slided over the input image and calculates the correlation of each location on the input image [24]. In this paper, the template image is slided over the local map and calculates the distribution of correlations. In addition, the template images are created circular shapes based on a 2D lidar with a scanning angle: 360 [deg].

Figure 3 shows the distribution of correlation calculated by template matching from the Fig. 1 template image and the Fig. 2 local map. Figure 1 size is a circular shape with a radius of ± 6 [m] and Fig. 2 size is a ± 8 [m]. The sliding range of the template images is ± 2 [m]/0.2 [m]. Here, Fig. 3 shows that the shape of the distribution of correlations depends on the template matching algorithm. Therefore, an appropriate template matching algorithm should be selected.

The following template matching methods are available. The equations for each method are shown in Eqs. (1, 2, 3, 4, 5, 6, 7) [24]. In Eqs. (1, 2, 3, 4, 5, 6, 7), $M(x^{\prime }+x, y^{\prime }+y)$ represents the pixel values of the local map, and $T(x^{\prime }, y^{\prime })$ represents the pixel values of the template image.

• Sum of Squared Difference (SSD)

• Sum of Absolute Difference (SAD)

• Normalized Sum of Squared Difference (NSSD)

• Normalized Cross Correlation (NCC)

• Zero-mean Normalized Cross Correlation (ZNCC)

• Zero-mean Cross Correlation (ZCC)

• Cross Correlation (CC)

$$\begin{aligned} S_{\text {SSD}}(x, y)&=\sum _{x^{\prime }, y^{\prime }}\left( T\left( x^{\prime }, y^{\prime }\right) -M\left( x^{\prime }+x, y^{\prime }+y\right) \right) ^{2} \end{aligned}$$

(1)

$$\begin{aligned} S_{\text {SAD}}(x, y)&=\sum _{x^{\prime }, y^{\prime }} \left| T\left( x^{\prime }, y^{\prime }\right) -M \left( x+x^{\prime }, y+y^{\prime }\right) \right| \end{aligned}$$

(2)

$$\begin{aligned} S_{\text {NSSD}}(x, y)&=\frac{\sum _{x^{\prime }, y^{\prime }}\left( T\left( x^{\prime }, y^{\prime }\right) -M\left( x^{\prime }+x, y^{\prime }+y\right) \right) ^{2}}{\sqrt{\sum _{x^{\prime }, y^{\prime }} T\left( x^{\prime }, y^{\prime }\right) ^{2}} \sqrt{\sum _{x^{\prime }, y^{\prime }} M\left( x^{\prime }+x, y^{\prime }+y\right) ^{2}}} \end{aligned}$$

(3)

$$\begin{aligned} S_{\text {NCC}}(x, y)&=\frac{\sum _{x^{\prime }, y^{\prime }} \left( T\left( x^{\prime }, y^{\prime }\right) M\left( x^{\prime }+x, y^{\prime }+y\right) \right) }{\sqrt{\sum _{x^{\prime }, y^{\prime }} T\left( x^{\prime }, y^{\prime }\right) ^{2}} \sqrt{\sum _{x^{\prime }, y^{\prime }} M\left( x^{\prime }+x, y^{\prime }+y\right) ^{2}}} \end{aligned}$$

(4)

$$\begin{aligned} S_{\text {ZNCC}}(x, y)&=\frac{\sum _{x^{\prime }, y^{\prime }} \left( T\left( x^{\prime }, y^{\prime }\right) -\mu _{z}\right) \left( M\left( x^{\prime }+x, y^{\prime }+y\right) -\mu _{m}\right) }{\sqrt{\sum _{x^{\prime }, y^{\prime }}\left( T\left( x^{\prime }, y^{\prime }\right) -\mu _{z}\right) ^{2}} \sqrt{\sum _{x^{\prime }, y^{\prime }}\left( M\left( x^{\prime }+x, y^{\prime } +y\right) -\mu _{m}\right) ^{2}}} \end{aligned}$$

(5)

$$\begin{aligned} S_{\text {ZCC}}(x, y)=\sum _{x^{\prime }, y^{\prime }}\left( T\left( x^{\prime }, y^{\prime }\right) - \mu _{z}\right) \left( M\left( x^{\prime }+x, y^{\prime }+y\right) -\mu _{m}\right) \end{aligned}$$

(6)

$$\begin{aligned} S_{\text {CC}}(x, y)&=\sum _{x^{\prime }, y^{\prime }} \left( T\left( x^{\prime }, y^{\prime }\right) M\left( x^{\prime }+x, y^{\prime }+y\right) \right) \end{aligned}$$

(7)

Comparison in 1D sliding window

To select the best template matching method for estimate localizability, we performed a simple experiment. We used the template images of Figs. 4 and 7 and the local maps of Figs. 5, 6, and 8, respectively, in one-dimensional horizontal dimension. Figure 4 template image is a simplified example, picking up only the free space. This is because the algorithm used when constructing the occupancy grid maps does not assume a situation where only occupied space or unknown space exist. Figure 5 local map has free space on the left and occupied space on the right. Figure 6 local map has free space on the left and unknown space on the right. These are because Fig. 4 template image is only free space and the local map is only unknown space or occupied space from the region of free space. Assuming a real environment, typical template images and local maps are shown in Figs. 7 and 8. The template image size is $6\times 6$ [m] and the local map size is $15\times 6$ [m]. Next, the template image was slided by 9 [m] / 0.05 [m] from the left edge of the local map to calculate correlation. Correlation values were compared in a one-dimensional sliding window in the horizontal direction. Here, the pixel value of free space, occupied space, unknown space were, 254, 0 and 127, respectively.

Figure 9 shows the results of the correlations in the case of using Figs. 4 and 5 as the template image and local map. Figure 10 shows the results using Figs. 4 and 6. Figure 11 shows the results using Figs. 7 and 8.

Figure 9 shows that SSD, SAD, NSSD, NCC, and CC have constant correlation in the 0–1.5 [m] sliding range. This is because the free space of Figs. 4 and 5 overlap in the 0–1.5 [m] sliding range. On the other hand, ZNCC always has constant correlation peaks, and ZCC has correlation peaks that are not stable. Thus, ZNCC and ZCC are considered inappropriate.

Figure 10 shows that SSD, SAD, NSSD, NCC and CC have constant correlation in the 0–1.5 [m] sliding range. This is because the free space of Figs. 4 and 6 overlap in the 0–1.5 [m] sliding range. However, NCC in Fig. 10d has two correlation peaks. Thus, NCC is considered inappropriate.

Figure 11 shows that SSD, SAD, NSSD, NCC, and ZNCC have a correlation peak at 2 [m] sliding range. This is because the free space and occupied space of Figs. 7 and 8 overlap at 2 [m] sliding range. In contrast, NSSD in Fig. 11c has constant correlation in the 3.75–9 [m] range, and CC always have constant correlation in the 0–2 [m] and 5–7 [m]. Thus, NSSD and CC are considered inappropriate. Based on these results, SSD and SAD are considered to be the appropriate template matching methods. In response, we decided to choose, SAD as the correlation model used in this paper.

Estimation of localizability as covariance

We propose a method to estimate localizability as a covariance matrix using local map correlation by SAD. Figure 12 shows the localizability estimation algorithm of the proposed method. First, we trim the template image and the local map from the occupancy grid maps Next, the template image is slided over the local map and the distribution of correlation is calculated by SAD. There are several ways to express localization error in terms of probability distributions, typical methods is to use the Gaussian distribution. Therefore, in this paper we use local map correlation to estimate localizability as a covariance matrix of the Gaussian distribution. As in the correlation distribution in Fig. 12, if the correlation values in the grid have values corresponding to the weights, the expression of the covariance matrix of the Gaussian distribution is as in Eq. (8).

$$\begin{aligned} \Sigma _{S_\text {SAD}}=k \left[ \begin{array}{ccc} \frac{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}(x_{i}-\mu _{x})^{2}}{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}} &{} \frac{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k} (x_{i}-\mu _{x})(y_{j}-\mu _{y})}{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}} &{} \frac{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k} (x_{i}-\mu _{x})(\theta _{k}-\mu _{\theta })}{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}} \\ \frac{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}(y_{j}-\mu _{y})(x_{i}-\mu _{x})}{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}} &{} \frac{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}(y_{j}-\mu _{y})^{2}}{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}} &{} \frac{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k} (y_{j}-\mu _{y})(\theta _{k}-\mu _{\theta })}{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}} \\ \frac{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k} (\theta _{k}-\mu _{\theta })(x_{i}-\mu _{x})}{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}} &{} \frac{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k} (\theta _{k}-\mu _{\theta })(y_{j}-\mu _{y})}{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}} &{} \frac{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}(\theta _{k}-\mu _{\theta })^{2}}{\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}} \\ \end{array}\right] \end{aligned}$$

(8)

Here, $w_{i, j, k}$ in Eq. (8) is weights. We have conducted basic experiment on several patterns of how the correlation values obtained by SAD are converted into weights. The higher the similarity between the template image and the local map, the lower the correlation value for SAD, so the weights were determined as in Eq. (9).

$$\begin{aligned} w_{i, j, k} := \frac{1}{S_\text {SAD}\left( x_{i}, y_{j}, \theta _{k}\right) ^2} \end{aligned}$$

(9)

In Eq. (8), $x_{i}$, $y_{j}$ and $\theta _{k}$ are the positions of the weights in the distribution of correlation in Fig. 12, and $\mu _{x}$,$\mu _{y}$ and $\mu _{\theta }$ are the average positions of the weights. The denominator $\sum _{i=0}^{x \prime } \sum _{j=0}^{y \prime } \sum _{k=0}^{\theta \prime } w_{i, j, k}$ means normalisation. The k in Eq. (8) is a parameter that determines covariance magnitude, called the noise level [25]. We determined the k so that the magnitude of the covariance was appropriate as a localization error. The k is a scalar value. It is also possible to express localizability in terms of scalar values by using the determinant of the covariance matrix as in Eq. (10).

$$\begin{aligned} e := \sqrt{ \text {det}(\Sigma _{S_\text {SAD}}) } = \sqrt{ \lambda _1 \lambda _2 \lambda _3} \end{aligned}$$

(10)

The localizability uncertainty value e represents the estimated magnitude of the localization error. The $\lambda _1$ and the $\lambda _2$ and the $\lambda _3$ in Eq. (10) show the first and second and third eigenvalues of Eq. (8). We can visualize the localizability in terms of scalar values by calculating the localizability uncertainty value e. The covariance matrix is calculated for each cell on occupancy grid map to estimate the localizability of the whole map area. In this way, we can estimate the magnitude of the localization error and the characteristics of the error of the occupancy grid maps.

Experiments

Distribution of correlation

To confirm the effectiveness of the proposed method, we construct simulation environments that include representative shapes of indoor environments, and conduct experiments for these environments: complex shape, simple shape, circular arc shape, repeating pattern shape, larger shape environment than sensor measurement range. The simulator is Gazebo. The built environment is shown in Figs. 13, 14. The mobile robot performed SLAM on these environments to obtain occupancy grid maps. Figure 15 shows a mobile robot running and performing SLAM. The resulting occupancy grid maps are shown in Figs. 16, 17. Here, the pixel value of free space, occupied space, unknown space were, 254, 0 and 127, respectively.

We conducted an experiment to investigate the characteristics of the distribution of local map correlation for representative shapes of indoor environments. The environments used complex shape environment (Fig. 16A), simple shape environment (Fig. 16B), circular arc shape environment (Fig. 16C), repeating pattern shape environment(Fig. 16D) and larger shape environment than sensor measurement range (Fig. 16E). The template image and the local map are trimmed from these environments, and the local map correlation is calculated using SAD. The template images are created circular shape based on a 2D lidar with scanning angle: 360 [deg] and detection distance: ± 6 [m]. The local map sizes are ± 8 [m]. The sliding range of the template images are ± 2 [m]/0.2 [m], ± 60 [deg]/10 [deg], and the correlation is calculated.

We confirmed the characteristics distribution of correlation where complex shape environment and degeneration is unlikely to occur. First, the template image and local map were trimmed from Fig. 16 A. The trimmed template image and the local map show Figs. 18a and b. Figure 19 shows the results of the distribution of correlation between the template image Fig. 18a and the local map Fig. 18b. Figure 19 shows that the correlations become smaller near the center. This means the high similarity between Fig. 18a and b near the center.

Next, we confirmed the characteristics distribution of correlation where simple shape environment and degeneration is likely to occur. First, the template image and local map were trimmed from Fig. 16B. The trimmed template image and the local map show Fig. 20a and b. Figure 20 shows the results of the distribution of correlation between the template image Fig. 20a and the local map Fig. 20b. Figure 21 shows a valley of correlation in the longitudinal direction of the corridor. This means the high similarity between Fig. 20a and b in the longitudinal direction.

Next, we confirmed the characteristics distribution of correlation where circular arc shape environment and degeneration is likely to occur. First, the template image and local map were trimmed from Fig. 16C. The trimmed template image and the local map show Fig. 22a and b. Figures 23 and 24 show the results of the distribution of correlation between the template image Fig. 22a and the local map Fig. 22b. Figures 23 and  24 show a valley of correlation in the $\theta$ direction. This means the high similarity between even if Fig. 22a is rotated around the center and Fig. 22b.

Next, we confirmed the characteristics distribution of correlation where repeating pattern shape environment and degeneration is likely to occur. First, the template image and local map were trimmed from Fig. 16D. The trimmed template image and the local map show Fig. 22a and  b. Figure 25 shows the results of the distribution of correlation between the template image Fig. 25a and the local map Fig. 25b. Figure 26 shows several correlation peaks at constant intervals in the longitudinal direction of the corridor. This means the high similarity between Fig. 25a and b at a constant interval in the longitudinal direction.

Next, we confirmed the characteristics distribution of correlation where larger shape environment than sensor measurement range and degeneration is likely to occur. First, the template image and local map were trimmed from Fig. 17E. The trimmed template image and the local map show Fig. 27a and b. Figure 27 shows the results of the distribution of correlation between the template image Fig. 27a and the local map Fig. 27b. Figure 28 shows that the correlations are plotted at a constant value. This is because Fig. 27a has only free space, so sliding Fig. 27a does not change the correlation.

Localizability for whole map areas

To estimate the localizability of the whole map area, we use the occupancy grid maps in Figs. 16 and 17. First, the correlation distribution is calculated for each cell on the occupancy grid maps. Here, we assume that the robot does not invade unknown or occupied space. Then, the distribution of correlation is calculated only for the points corresponding to the free space. The template images are created circular shape based on a 2D lidar with scanning angle: 360 [deg] and detection distance: ± 6 [m] and ± 8 [m]. The local map sizes are ± 8 [m] and ± 10 [m]. The sliding range of the template images is ± 2 [m]/0.2 [m], ± 60 [deg]/10 [deg], and the correlation is calculated. Next, the covariance matrix is calculated from the distribution of correlation. Figures 29 and 31 show the error ellipses, first eigenvectors, and standard deviation of angle calculated from the covariance matrix. Here the error ellipses are plotted in blue, the first eigenvectors in black, and the standard deviation of angle in red. Furthermore, Figs. 30 and 32 visualize the localization of the entire map area using the localizability uncertainty value e. Figures 30 and  32, the localizability uncertainty value e in the unknown and occupied spaces is set to 0. The localizability uncertainty value e in Fig. 32a and b have limits of 100 and 20, respectively, because the maximum value is very high.

The results for the template image ± 6 [m] are shown in Figs. 29a, 30a, 31a, 32a, and the results for the template image ± 8 [m] are shown in Figs. 29b, 30b, 31b, 32b. Comparing the size of the error ellipses in Fig. 29a and b, it can be seen that the error ellipse in Fig. 29b is smaller. Also, comparing the localizability uncertainty value e in Fig. 30a and b, it can see that Fig. 30b is smaller. Comparing the size of the error ellipses in Fig. 31a and b, it can be seen that the error ellipse in Fig. 31b is smaller. Also, comparing the localizability uncertainty value e in Fig. 32a and b, it can see that Fig. 32b is smaller. These results confirm that the larger the template image, the higher the reliability of localization.

Figure 29A shows that the complex shape environment have a smaller error ellipse than other environments. This indicates that degeneration is unlikely to occur. Also, Fig. 30 shows that complex shape environment have lower localizability uncertainty value e than other environments. This means higher reliability of localization than in other environments. These results confirm that the proposed method can estimate the locations where degeneration is unlikely to occur from the occupancy grid map.

Figure 29B shows that the simple shape environment have a larger error ellipse in the longitudinal direction of the corridor than other environments. This indicates that degeneration is likely to occur in the longitudinal direction of the corridor. Also, Fig. 30 shows that simple shape environment have a higher localizability uncertainty value e than other environments. This means lower reliability of localization than in other environments. These results confirm that the proposed method can estimate the location where degeneration is likely to occur in the longitudinal direction of the corridor from the occupancy grid map.

Figure 29C shows that the circular arc shape environment have a larger standard deviation of angle than the other environments. This indicates that degeneration is likely to occur in the rotation direction. Also, Fig. 30 shows that circular arc shape environment have a high localizability uncertainty value e. This means low reliability of localization. These results confirm that the proposed method can estimate the locations where degeneration is likely to occur in the rotation direction from the occupancy grid map.

Figure 29D shows that the repeating pattern shape environment have a larger error ellipse than the error ellipse for the complex shape environment (Fig. 29A). This indicates that degeneration is likely to occur. Also, Fig. 30 shows that repeating pattern shape environment have a higher localizability uncertainty value e than the complex shape environment (Fig. 29A). This means low reliability of localization. These results confirm that the proposed method can estimate the locations where degeneration is likely to occur in the repeating pattern shape environment from the occupancy grid map.

Figure 31E shows that the larger shape environment than sensor measurement range have a larger error ellipse than the other environment. This indicates that degeneration is likely to occur. Also, Fig. 30 shows that the lager than the sensor measurement range shape environment have a higher localizability uncertainty value e than other environments. This means low reliability of localization. These results confirm that the proposed method can estimate the locations where degeneration is likely to occur in the lager than the sensor measurement range shape environment from the occupancy grid map. The previous results confirm that it is effective to use the covariance matrix of the distribution of correlation by SAD using occupancy grid maps to estimate localizability.

Conclusion

In this paper, we proposed a method to estimate localizability as a covariance matrix of the Gaussian distribution using local map correlation based on occupancy grid maps. First, an experiment was conducted to compare several correlation models to select the best template-matching method for localizability estimation. As a result, SAD was selected. The proposed method trims the template image and local map for each cell on the occupancy grid map and calculates the local map correlation by SAD. Using local map correlation by SAD, localizability is then estimated as a covariance matrix. To confirm the effectiveness of the proposed method, we constructed simulation environments that include representative shapes of indoor environments, and conducted experiments for these environments: complex shape, simple shape, circular arc shape, repeating pattern shape, larger shape environment than sensor measurement range. We conducted an experiment to investigate the characteristics of the distribution of local map correlation for representative shapes of indoor environments. The experiment results show that the correlation distribution characteristics differ depending on the environment shapes. Then, we conducted an experiment to estimate localizability on occupancy grid maps. The experiment results showed that our method could estimate the magnitude of the localization error and the characteristics of the error for complex shape, simple shape, circular arc shape, repeating patterns shape, and larger shape environment than sensor measurement range. The localizability for the whole area of occupancy grid maps could then be estimated. Therefore, the proposed method was confirmed to be effective in estimating localizability.

Fig. 1

Template image

Fig. 2

Local map

Fig. 3

Distribution of correlation using Figs. 1 and 2 as a template image and a local map

Fig. 4

Template image of free space only

Fig. 5

Local map of free space on the left and occupied space on the right

Fig. 6

Local map of free space on the left and unknown space on the right

Fig. 7

Template image of free space on the left and occupied space on the right

Fig. 8

Local map of free space on the left, occupied space in the middle, and unknown space on the right

Fig. 9

Distribution of correlation using Figs. 4 and 5 as a template image and a local map

Fig. 10

Distribution of correlation using Figs. 4 and 6 as a template image and a local map

Fig. 11

Distribution of correlation using Figs. 7 and 8 as a template image and a local map

Fig. 12

Localizability estimation algorithm using correlation model on occupancy grid maps

Fig. 13

Simulation environment containing complex shape, simple shape, circular arc shape, and repeating pattern shape

Fig. 14

Simulation environment containing complex shape and larger shape than sensor measurement range

Fig. 15

Mobile robot in a simulation environment

Fig. 16

Occupancy grid map containing complex shape, simple shape, circular arc shape, and repeating pattern shape

Fig. 17

Occupancy grid map containing containing complex shape and larger shape than sensor measurement range

Fig. 18

Location where the shape is complex and degeneration is unlikely to occur

Fig. 19

Distribution of correlation x and y at the location where the shape is complex and degeneration is unlikely to occur

Fig. 20

Location where the shape is simple and degeneration is likely to occur

Fig. 21

Distribution of correlation x and y at the location where the shape is simple and degeneration is likely to occur

Fig. 22

Location where the shape is a circular arc and degeneration is likely to occur

Fig. 23

Distribution of correlation x and $\theta$ at the location where the shape is a circular arc and degeneration is likely to occur

Fig. 24

Distribution of correlation y and $\theta$ at the location where the shape is a circular arc and degeneration is likely to occur

Fig. 25

Location where the shape is repeating patterns and degeneration is likely to occur

Fig. 26

Distribution of correlation x and y at the location where the shape is repeating patterns and degeneration is likely to occur

Fig. 27

Location larger than sensor measurement range and degeneration is likely to occur

Fig. 28

Distribution of correlation x and y at the location larger than sensor measurement range and degeneration is likely to occur

Fig. 29

Localizability as covariance at sampled robot poses on the occupancy grid map containing complex shape, simple shape, circular arc shape, and repeating pattern shape

Fig. 30

Localizability map of Fig. 29: the localizability uncertainty value e represents the estimated magnitude of the localization error

Fig. 31

Localizability as covariance at sampled robot poses on the occupancy grid map of larger shape environment than sensor measurement range

Fig. 32

Localizability map of Fig. 31: The localizability uncertainty value e represents the estimated magnitude of the localization error

As a future work, we would like to conduct experiments using a real robot equipped with sensors and in outdoor environments.