## I Introduction

During the last decade, research and development in the area of autonomous mobile robots have made significant progress. Nowadays robots such as vacuum cleaners or lawn mowers can be found in many households fulfilling their intended tasks. However, most of the robots employ thereby simplistic navigation strategies, such as a random walk, due to the lack of suitable maps and accurate sensors required for successful path planning. While most existing work for mapping and localization utilizes long-range sensors, such as LIDAR sensors, RGB-D cameras or time of flight sensors, robots for private households lack such sensor-richness. The reason for that is that such sensors are either too expensive, aiming for low acquisition and maintenance costs, or not suitable for outdoor environments, e.g. to reflections and direct sunlight. Moreover, simultaneous localization and mapping (SLAM) algorithms require certain amount of computational power which is often not available. However, intelligent navigation from low cost hardware is essential for mobile robots to enter our daily life. For example, autonomous lawn mowers employed with random walk policies are limited to simple environments, e.g. they can not enter small corridors.

In this paper, we propose a sophisticated mapping algorithm that can generate a map estimate of the environment based only on odometry data. Such data can be generated by following the wall or border line for an area of interest. Such wall following strategies can be executed using simple wall sensors, bumpers or signal wire sensors. The generated odometry data can be used for the proposed map estimation method. We will focus in this paper on how to achieve the most suitable map using only the recorded odometry data.

In order to reduce computational complexity, we first prune our problem using path segmentation. Thereby, we cluster the individual path points generated from the odometry data into straight line segments. Based on the pruned data set we generate a pose graph in which we search for loop closing constraints using shape comparison. We include these loop closing constraints into our pose graph formulation and optimize the graph using standard pose graph optimization techniques, such as the Levenberg-Marquardt algorithm. Finally, we introduce a measure for the mapping error based on the deviation of areas between the estimated map and the true shape of the environment.

We first evaluate our method in a complex simulation environment using standard motion models. Afterwards, we show how our method performs in a real robot, a Viking MI 422P, set up in a real garden environment. In order to represent real conditions as best as possible we learned the odometry parameters for the motion model by means of Log Likelihood estimation. Moreover, we show in simulations that our approach is robust even under a large amount of odometric drift, which is essential for successfully mapping outdoor environments.

### I-a Contributions and Organization

The contributions of the paper are two-fold. First, an efficient and simple method for loop closure detection using odometry only data is presented and second, a map evaluation scheme based on comparing two map areas, the estimated map and a groundtruth, is proposed. Both contributions are necessary to generate accurate map estimates with only low-range or binary sensors, which then enables low-cost robots to implement intelligent navigation and path planning strategies. These contributions are discussed in Section II. In Section III we evaluate our approach in a realistic mowing scenario and in a challenging simulated apartment environment. We conclude in Section IV.

### I-B Related Work

Most of the existing work on SLAM [20] or graph-based SLAM [9] relies on long-range sensors, such as LIDARs or cameras [3], [7], [15], [8]. Most of these approaches are using sensor fusion and probabilistic reasoning, e.g. particle filter [11]

or extended kalman filter

[1]. However, there are some approaches which try to handle the SLAM problem using only sparse sensor data, e.g. [2], to avoid expensive sensing. Existing work for low-range sensors [22], [6], such as sonars or infrared sensors, requires linear features which are not necessarily present in outdoor environments. Indoor mapping with limited sensing using a wall following approach has been presented in [24]. However, this approach makes the assumption of an approximately rectilinear structure, which may be true for most indoor environments but not for outdoor applications. In contrast, our proposed approach is not restricted to such structural assumptions and can be used for either indoor or outdoor applications with arbitrary shapes.Generating the path from odometry data leads to a pose-graph representation, which is often used for the SLAM problem [21], [17] and has been first introduced by Lu and Milios 1997, [19]. Pose graph optimization has been addressed in several studies, e.g TORO [10], g2o [16], iSAM2 [14] and LAGO [5]. Thereby, TORO is based on gradient descent and is an extension of Olson’s algorithm [21]. It applies a tree-based parameterization to distribute residual errors across the graph that improves the performance. The ”general graph optimization” framework, g2o, has been designed to perform the optimization of different least squares problems, which can be represented as a graph. It thereby relies on the Gauss-Newton method. The iSAM2 method applies Bayes trees using incremental variable re-ordering and fluid relinearization to solve sparse nonlinear incremental optimization problems. LAGO addresses the pose graph optimization problem by decoupling the orientation and translation. We use the simpler Levenberg-Marquardt algorithm [9], which worked reasonably well for pose graph optimization. There, the function is approximated by its first order Taylor expansion around the current initial guess in order to then find the solution to the optimization problem iteratively.

In order to reduce computational complexity, we prune the data using path segmentation. Such a pruning can lead to a huge reduction of computational time at the price of a small increase in error [18].

## Ii Methods

We start by introducing the standard pose graph formulation:

Let be a set of poses representing the position and orientation of a mobile robot in a two dimensional space, hence . Here, is the cartesian position of the robot and the corresponding orientation as an euler angle with the integer . The relative measurement between two poses and is then given as

(1) |

where is a planar rotation matrix and the pose compounding operator which has been introduced in [19]. These relative measurements are, in general, affected by noise. Thus, including a zero mean Gaussian noise leads to the noisy relative measurements

(2) |

In general, there are two different types of relative pose measurements: Odometric constraints and loop closing constraints. Here, the first constraints are generated by the wheel odometry of the differential drive robot. The second type of constraints are provided by the robot recognizing a match between actual measurements and past measurements by revisiting places. Subsection II-B shows how to efficiently identify and add loop closing constraints to the pose graph for odometry only data.

The pose graph is thereby represented as a directed graph with vertices and edges, where is the number of odometric constraints and the number of loop closing constraints. The connection between the vertices by the edges can be compactly written using an incident matrix . There, every column represents an edge connecting two vertices with each other. The row number thereby represents the vertex from which the edge starts, denoted by , and the vertex where the edge leads to, denoted by . In Figure 1 a pose graph is exemplarily shown in combination with the according incidence matrix .

The overall optimization problem is then to minimize the sum of weighted residual errors in regard to the pose estimates

(3) |

with

(4) |

The covariance matrix corresponding to the relative measurements is thereby given as .

### Ii-a Path Segmentation / Data Pruning

Path segmentation is used in order to cluster the individual path points retrieved from the odometry data into straight line segments. It is mostly used to reduce the complexity of the mapping problem, e.g. [18] or [24]. The reduction of complexity will allow later improvements to the path by standard pose graph optimization techniques. Therefore, as shown in Figure 2, the errors between the individual odometry data points and a straight line segment are calculated and summed up to compare it with a certain threshold . If the sum of the errors exceeds the threshold a new line segment with different orientation is generated. The path segmentation scheme is presented in Algorithm (1) and is inspired by [24]. We now shortly state the idea of the path segmentation approach.

Assume the position estimates based on the odometry are given as . Here is the number of data points received from the odometry. First, a set of dominant points and a temporarily subset are initialized using the first position estimate . Hereby, the set of dominant points represents the pruned data set and the temporarily subset contains the data points which are approximated by the current straight line segment. We now successively include all odometry data points into the algorithm. Thereby, a new data point is integrated into the temporary subset . Afterwards, two conditions are checked:

(5) |

and

(6) |

where and are the last added items in the respective set. If both, Equation (5) and Equation (6), are true, the temporary subset is not any longer a valid representation for the current straight line segment. Thus, the position is added to the set and the temporary subset is set back to . Here, defines the shortest distance between the point

and the vector

and is the cardinality of the set . In Figure 2 the error calculation is exemplarily shown and in Figure 3 a resulting pruned data set is depicted compared to the original data set. The parameters and are problem specific and have to be tuned accordingly.Based on the pruned data set , the poses for the pose graph are generated as

(7) |

where is the cardinality of the set of dominant points and

(8) |

For the pose graph and the relative measurements can be calculated using Equation (1). In Figure 4 an example for the generation of the pose graph based on the set of dominant points is shown. The part of the corresponding incidence matrix containing the odometric constraints can be then written as

(9) |

### Ii-B Loop Closure Detection

Only the pose data generated in Subsection II-A can be used to find loop closing constraints. Therefore, it is required for the robot to cycle several times along the boundary line of the area of interest. We then compare the shape of the path, searching for poses with similar shaped neighborhoods in order to find vertices which can be matched onto each other. These matched vertices can then be added as loop closing constraints to the pose graph.

First, generate a piecewise linear function of the orientation in regard to the length of the path . The function is defined as

(10) |

The cumulated orientations and path lengths can be calculated as

(11) |

starting with and and going through all poses of the pruned graph. Here and with . In Figure 5 an example function based on the segmented data from Figure 3 is shown.

Second, a comparison between the shapes of the neighborhood of the poses using the introduced piecewise orientation function is done. Therefore, let be the length to both sides of a pose which defines its neighborhood. A correlation error matrix is then generated as follows:

Comparing the neighborhood of the pose and , the orientation function is grounded according to the orientations , and lengths , such that two neighborhood functions and are generated, e.g. . Both functions are then evaluated at linearly distributed points from to which result into two vectors and . The correlation error then add up to

(12) |

In Figure 6 the correlation errors between the vertices are shown.

Third, we search for local minima which are below a threshold in the correlation error data. This leads to convenient pairs for loop closure for . In general, this procedure leads to a set of loop closing pairs , where is the number of loop closing constraints found. The selection of an appropriate value for is hereby crucial to find sufficiently accurate pairs for loop closure. In general, picking a larger value for will lead to a more cautious selection and vice versa. Also, the threshold affects the loop closing detection significantly. Thereby, a huge neighborhood parameter and large odometry errors should be compensated by choosing a large value for and vise versa.

The loop closing constraints can now be included into the pose graph representation by adding the relative measurements between the poses and for the loop closing pair . The incident matrix for the loop closing measurements can be built as

(13) |

with the vector representing the loop closing pair by denoting and and the rest of the entries with zeros. The complete incidence matrix for the pose graph is then defined as

(14) |

### Ii-C Pose Graph Optimization

Let such that Equation (3) can be rewritten as

(15) |

where is the information matrix for the relative measurement . To solve Equation (15) the information matrices have to be determined with . For the odometric constraints we generate the covariance matrices as

(16) |

on the basis of the odometry model presented in [23] and under the assumption that only one translation and one rotation occur. The parameters are robot specific and must be determined. For the loop closing constraints, the covariance matrices can be calculated using the correlation error from Equation (6) and the parameters and

(17) |

The parameters and can be used to improve the map estimate in complex environments. In our experiments all parameters were set to one. We then use the popular Levenberg-Marquardt algorithm as presented in [9] in order to solve the pose graph optimization problem from Equation (15).

### Ii-D Map Generation and Evaluation

With Subsection II-C we optimized our pose graph, for which an example can be seen in the mid panel of Figure 7. Now, we shortly explain how to generate a closed trajectory from the optimized pose graph which can then be stored as a map of the environment. First, we cut off the prefix and the suffix of the pose graph, because these parts have not been optimized due to missing loop closing constraints. Thereby, the prefix is the part of the pose graph before the first loop closing constraint and the suffix the part of the pose graph after the last loop closing constraint. In order to generate a closed path, we investigate further the generated loop closing pairs. Here, we search for the first loop closing pair which represents a complete turn around the borderline. Setting the points of the this loop closing pair onto each other leads to a closed trajectory as presented in the right panel of Figure 7.

To compare different map estimates with each other we calculate the deviation of area between the estimated map generated using the above algorithms and the true shape of the environment. Therefore, we assume that the true shape of the environment is given as a polygon defined by the points . The idea is to determine a rotation matrix and a translation vector which transforms the set of points , representing the closed map estimate, onto the set of points

(18) |

such that

(19) |

is minimized. Here, represents the difference between the areas of the map estimate and the true shape of the environment. We use simple gradient descent in order to find a convenient rotation matrix and translation vector for Equation (18). Since gradient descent requires a good initial guess in order to not get stuck into a local minimum, we use Horns method [13] to first find such initial guess. Therefore, we use the in Subsection II-B described method for loop closure detection in order to find pairs of points between the estimated map and the true shape of the environment which can be mapped onto each other. In Figure 8 the deviation between the true shape of the environment and the estimated map is depicted in light blue.

## Iii Results

0.0849 | 0.0412 | 0.0316 | 0.0173 |

We tested the above proposed mapping method for odometry only data in simulations with challenging environments and on real data. For the real system, a Viking MI 422P, a purchasable autonomous lawn mower, has been used. For the simulation environment and for the estimation of the covariance matrices Equation (16), we used the odometry motion model presented in [23]. We calibrated the odometry model by tracking lawn mower movements using a visual tracking system (OptiTrack) and computed the parameters using maximum likelihood estimation. The calibrated parameters for the Viking MI 422P are presented in Table I. For generating real data, we drove the lawn mower manually along the boundary line of the courtyard depicted in Figure 9(a). Additionally, we simulated a differential drive robot with the specifications of the lawn mower in a challenging apartment environment. Moreover, we successively increased the odometry error in order to show that the proposed approach works even with large odometry errors and leads to sufficiently accurate results. As error measurement we used the deviation of areas between the true shape of the environment and the estimated map, as presented in Subsection II-D. Parameter specifications regarding the robot and the proposed method can be found in the Appendix, Section V.

### Iii-a Apartment Environment - Simulation

We used simulations in order to test our algorithm in a challenging environment, namely an apartment floor as depicted in Figure 10. Since the apartment environment is complex we set the threshold to . Simulating the differential drive robot using the in Table I presented odometry parameters and a simple wall following algorithm results into the estimated path depicted in the left panel of Figure 11. The resulting map estimate can be seen in the right panel together with the true shape of the apartment. The error between the true shape and estimated map is .

To simulate a drastic odometry drift, we set the odometry parameters to for . This leads to an estimated path of the simulated robot as depicted in the left panel of Figure 12. The results of our map estimate is presented in the right panel of Figure 12. The mapping error is .

In order to show the robustness of our approach, we evaluated our method on the apartment environment for different values of with for . Therefore, we simulated the robot for every parameter setting ten times and calculated the mean error

, which can be seen in Table II. As expected, the mean rises monotonously with increasing as well as the standard deviation .### Iii-B Courtyard Environment - Real Data

For the real data set the parameter is set to to take into account the shape of the estimated path generated from the odometry data. This path can be seen in the left panel at Figure 9(b). In the right panel, the map estimate generated using the proposed method is depicted together with the true shape of the courtyard environment. The error between both, according to the method presented in Subsection II-D, is . The data for the courtyard has been retrieved from available CAD data.

## Iv Conclusion

We have presented a method for map estimation based on odometry only data. This method is essential for cheap or small robots, such as vacuum cleaners or lawn mowers. Our method does not require any additional assumptions like for example a rectilinear structured environment [24] or linear features [22], [6]. The required odometry data can be collected using a wall following scheme for which low range sensors or binary sensors are sufficient. Such sensors are widely used in actual purchasable household robots. Furthermore, we showed that our approach performs well in challenging environments, such as an apartment. Even when simulating large odometry noise, our approach can generate an accurate map estimate.

The map estimate of the environment allows the robot to plan its intended task, such as cleaning the floor or mowing the lawn, instead of executing a random walk behavior. In a related work [12]

, probability distributions were used to update a dirt coverage map to keep the dirt level below a certain threshold. In another work

[4], complete coverage is proven under the assumption of a bounded error. According to the first example a probability distribution encoded by a particle filter can be used to update a coverage map. This map allows then the execution of path planning strategies to avoid random walk behavior. This approach seems promising and will be further investigated.## V Appendix

The velocity of the lawn mower driving along the boundary has been set to . The odometry data has been sampled with a frequency of approximately . The algorithmic parameters used for the map estimation can be found in Table III. Here the chosen parameters , and for the path segmentation part reduce the complexity of the problem sufficiently while maintaining the path given by the odometry data. Moreover, the parameter for loop closure detection , since the used test environments have circumferences of and respectively. Thus, slightly over of the length of the circumference is used for shape comparison.

## References

- [1] (2006) Consistency of the ekf-slam algorithm. In Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, pp. 3562–3568. External Links: Link Cited by: §I-B.
- [2] (2006) SLAM with sparse sensing.. In ICRA, pp. 2285–2290. Cited by: §I-B.
- [3] (2003) Using 3d laser range data for slam in outdoor environments. In Intelligent Robots and Systems, 2003.(IROS 2003). Proceedings. 2003 IEEE/RSJ International Conference on, Vol. 1, pp. 188–193. External Links: Link Cited by: §I-B.
- [4] (2013) Robust coverage by a mobile robot of a planar workspace. In Robotics and Automation (ICRA), 2013 IEEE International Conference on, pp. 4582–4587. External Links: Link Cited by: §IV.
- [5] (2014) A fast and accurate approximation for planar pose graph optimization. The International Journal of Robotics Research 33 (7), pp. 965–987. External Links: Link Cited by: §I-B.
- [6] (2008) A line feature based slam with low grade range sensors using geometric constraints and active exploration for mobile robot. Autonomous Robots 24 (1), pp. 13–27. External Links: Link Cited by: §I-B, §IV.
- [7] (2011) Towards semantic slam using a monocular camera. In Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on, pp. 1277–1284. External Links: Link Cited by: §I-B.
- [8] (2011) Real-time 3d visual slam with a hand-held rgb-d camera. In Proc. of the RGB-D Workshop on 3D Perception in Robotics at the European Robotics Forum, Vasteras, Sweden, Vol. 180, pp. 1–15. External Links: Link Cited by: §I-B.
- [9] (2010) A tutorial on graph-based slam. IEEE Intelligent Transportation Systems Magazine 2 (4), pp. 31–43. External Links: Link Cited by: §I-B, §I-B, §II-C.
- [10] (2009) Nonlinear constraint network optimization for efficient map learning. IEEE Transactions on Intelligent Transportation Systems 10 (3), pp. 428–439. External Links: Link Cited by: §I-B.
- [11] (2007) Fast and accurate slam with rao–blackwellized particle filters. Robotics and Autonomous Systems 55 (1), pp. 30–38. External Links: Link Cited by: §I-B.
- [12] (2014) A probabilistic approach to high-confidence cleaning guarantees for low-cost cleaning robots. In Robotics and Automation (ICRA), 2014 IEEE International Conference on, pp. 5600–5605. External Links: Link Cited by: §IV.
- [13] (1988) Closed-form solution of absolute orientation using orthonormal matrices. JOSA A 5 (7), pp. 1127–1135. External Links: Link Cited by: §II-D.
- [14] (2012) ISAM2: incremental smoothing and mapping using the bayes tree. The International Journal of Robotics Research 31 (2), pp. 216–235. External Links: Link Cited by: §I-B.
- [15] (2008) Outdoor mapping and navigation using stereo vision. In Experimental Robotics, pp. 179–190. External Links: Link Cited by: §I-B.
- [16] (2011) G 2 o: a general framework for graph optimization. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pp. 3607–3613. External Links: Link Cited by: §I-B.
- [17] (2013) Robust loop closing over time for pose graph slam. The International Journal of Robotics Research 32 (14), pp. 1611–1626. External Links: Link Cited by: §I-B.
- [18] (2013) Go straight, turn right: pose graph reduction through trajectory segmentation using line segments. In Mobile Robots (ECMR), 2013 European Conference on, pp. 144–149. External Links: Link Cited by: §I-B, §II-A.
- [19] (1997) Globally consistent range scan alignment for environment mapping. Autonomous robots 4 (4), pp. 333–349. External Links: Link Cited by: §I-B, §II.
- [20] (2002) FastSLAM: a factored solution to the simultaneous localization and mapping problem. Aaai/iaai 593598. External Links: Link Cited by: §I-B.
- [21] (2006) Fast iterative alignment of pose graphs with poor initial estimates. In Robotics and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE International Conference on, pp. 2262–2269. External Links: Link Cited by: §I-B.
- [22] (2016) Simultaneous loclization and mapping with limited sensing using extended kalman filter and hough transfrom. Tehnicki vjesnik/Technical Gazette 23 (6). External Links: Link Cited by: §I-B, §IV.
- [23] (2002) Probabilistic robotics. Communications of the ACM 45 (3), pp. 52–57. Cited by: §II-C, TABLE I, §III.
- [24] (2010) Real-time indoor mapping for mobile robots with limited sensing. In Mobile Adhoc and Sensor Systems (MASS), 2010 IEEE 7th International Conference on, pp. 636–641. External Links: Link Cited by: §I-B, §II-A, §IV.

Comments

There are no comments yet.