IROS2019国际学术会议论文集0630

Active SLAM using Connectivity Graphs as Priors Alberto SoragnaMarco BaldiniDominik JohoRainer K ummerleGiorgio Grisetti AbstractMobile robots can be considered completely au- tonomous if they embed active algorithms for Simultaneous Localization And Mapping (SLAM). This means that the robot is able to autonomously, or actively, explore and create a reliable map of the environment, while simultaneously estimating its pose. In this paper, we propose a novel framework to robustly solve the active SLAM problem, in scenarios in which some prior information about the environment is available in the form of a topo-metric graph. This information is typically available or can be easily developed in industrial environments, but it is usually affected by uncertainties. In particular, the distinguishing features of our approach are: the inclusion of prior information for solving the active SLAM problem; the exploitation of this information to pursue active loop closure; the on-line correction of the inconsistencies in the provided data. We present some experiments, that are performed in different simulated environments: the results suggest that our method improves on state-of-the-art approaches, as it is able to deal with a wide variety of possibly large uncertainties. I. INTRODUCTION Autonomous mobile robots can create a map of the envi- ronment while they move in it through SLAM techniques. Historically SLAM systems are passive: a robot is driven by an operator, to collect environmental data with its on-board sensors. These data are processed by an algorithm in order to estimate the map and the trajectory of the robot. Yamauchi 1 introduced a simple but effective method for autonomous exploration based on the greedy discovery of frontiers. This algorithm has two main weak points: its greediness makes it sub-optimal in most scenarios and it is diffi cult to target specifi c areas for the exploration or to impose constraints on the movements of the robot. For these reasons, active SLAM is not catching on in industrial applications. These environments are generally large and users already know where a robot will operate. Exploring all the available space may be ineffi cient; moreover, some areas may not be accessible to the robot. In this paper we are going to present an exploration technique that overcomes these limitations, making it suitable for industrial robots. An effective way for addressing the drawbacks of frontier- based exploration and for taking into account the require- ments of the industry consists in providing some prior infor- mation to the robot. The structure of the environment will typically be available in an industrial scenario. We propose to represent the prior using a topo-metric graph 2. This object contains both topological as well as metrical information and it can be considered a stylized Voronoi diagram. The graph allows the robot to easily build a global exploration plan, A. Soragna and G. Grisetti are with the Department of Computer Science at La Sapienza University of Rome. M. Baldini, D. Joho and R. K ummerle are with KUKA Deutschland GmbH, Augsburg. Fig. 1: MIT CSAIL dataset. Red graph: prior information, green graph: automatically adjusted graph. using combinatorial optimization techniques, which results in a remarkable reduction in the time required to complete the task, as demonstrated by the recent literature 2, 3. The main contribution of this paper is a framework for active SLAM which exploits a graph structure in order to improve the exploration time and accuracy. Our system computes an optimal global plan as the solution of a Chinese Postman Problem (CPP). This is further refi ned while the robot moves in the environment in order to maximize its pose localization accuracy and trigger active loop closures. Errors in the provided graph could result in an ineffi cient exploration and could drive the robot toward areas where it was not allowed to move. We propose an online algorithm based on Least-Squares optimization for compensating the most common sources of errors, allowing the robot to reconstruct a more accurate graph, Fig. 1 illustrates a typical use-case of our approach. In sum, we make three key claims: our approach is able to (i) incorporate prior information in order to map an environment, (ii) exploit this information to pursue active loop closure, and (iii) automatically detect and correct in- consistencies in the provided data. II. RELATEDWORK Standard exploration techniques follow a greedy approach, continuously selecting as next goal the pose which max- 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE340 imizes the map information gain. The seminal work in this fi eld is the frontier exploration algorithm developed by Yamauchi 1. Since then, more sophisticated decision- making algorithms have been presented, as 4, 5, however few of them exploit prior information. Similarly, also the majority of active SLAM techniques evaluate the best action that the robot should perform, based only on the knowledge acquired online by the robot, as described by Stachniss et al. 6. In the last years, the exploitation of prior information for robotic mapping has grown in interest. An overview of the state of the art in this topic has been presented by Parsley 7. Cooperative approaches between robots are a common solution. Korah and Rasmussen 8 and K ummerle et al. 9 match aerial images with the robot perceptions. Information obtained by fl ying robots is exploited also by Delmerico et al. 10 for optimized path planning. These methods however are diffi cult to apply and limited to outdoor environments. Human readable maps are a source of information which is available for a vast amount of application scenarios and can also be interpreted by the robot in a relatively easy way. Vysotska et al. 11 and Mielle et al. 12 exploit these kind of priors and propose methods for matching and merging them with SLAM maps acquired by the robot. They can only cope with small and localized inconsistencies which are treated with robust kernel functions during the matching phases. The problem of compensating errors in the prior infor- mation is actively tackled by those systems that exploit sketched maps, since these are inherently wrong. Chronis and Skubic 13 describe one of the fi rst approaches of this kind, based on fuzzy logic. Boniardi et al. 14 infer the global connectivity of the environment from the sketch, estimate a scale transformation between the prior and the real world, and use it to include sensor measurements in the global plan. Mielle et al. 15 start from a drawing of the environment and then extract a Voronoi diagram from it. This is matched with a diagram computed on the map built by the robot. In contrast to that, Osswald et al. 2 directly exploit a graph structure as prior information. They solve a Traveling Salesman Problem to obtain a global plan and then combine it with a frontier based exploration. Their proposed method detects non-traversable edges as inconsistencies and performs a replanning step to exclude them from the routing problem. Xu and Stentz 3 model the exploration as a CPP and again perform a replanning whenever the environment differs from the original map. In this paper, we introduce an integrated approach to active SLAM and exploration which, differently from all the previous works, allows to dynamically correct the priors, making it extremely robust to inconsistencies. III. ACTIVESLAMONGRAPHS Given a prior topo-metric graph, representing the area that a mobile robot has to explore during a SLAM-based mapping procedure, our task consists in fi nding a way to navigate on each of its edges and to return to the starting location. It is important to always keep the localization accuracy high enough to be able to rely on the robot pose for a consistent mapping. We formulate this exploration task as a CPP. The optimal plan that would allow to complete the task is the one which traverses all the edges one and only one time. A necessary and suffi cient condition for the existence of such a closed plan (circuit) is that the associated graph is Eulerian. The circuit can be then computed in linear time. The CPP consists in fi nding this circuit for any general, non Eulerian, graph. In this case, some edges will have to be traveled more than once. The problem is solved using combinatorial optimization algorithms to fi nd the best subset of edges which have to be duplicated such that the resulting augmented graph is Eulerian. An overview of the CPP and of the standard algorithms for solving it on different types of graphs has been presented by Eiselt et al. 16. Costs must be assigned to all edges to allow the compu- tation of an optimal plan, i.e. the one minimizing the sum of the individual costs of the traversed edges. To minimize the exploration time, someone can use metrics such as the length of the edges or their traversal time. The CPP effi ciently computes the plan which maximizes the map information gain, but it does not take into account the robot localization uncertainty. This second quantity will typically increase as the exploration proceeds toward new areas, and it will decrease when the robot returns to pre- viously visited ones. When the robot recognizes that it has come back to an already mapped area, a loop closure will happen. This action can drastically reduce the localization uncertainty, provided that the robot is not simply retracing its steps. When dealing with an environment characterized by a topo-metric graph, a suffi cient condition for a loop closure is that the robot traverses an already visited edge, excluding the last traversed ones to avoid trivial closures. For this reason, given a plan it is possible to make predictions about when the next loop closure will happen, by inspecting the sequence of edges. This is not necessary because loop closures could be detected also while visiting new edges, depending on the topology of the environment. However, this type of closure cannot be easily predicted. We propose a new methodology for computing the cost of a plan that, differently from just summing the cost of its edges, will take into account the robot pose information. Defi ning a plan P as an ordered list of edges, we can split it into two parts denominated Ploop closureand Pend . The fi rst one is the sequence of edges from the current location to the fi rst predicted loop closure edge, while the second one is made of the remaining part that completes the exploration. We defi ne the modifi ed cost for a plan P as fm(P)=f(Ploop closure) + f(Pend).(1) In (1), the function f(x) refers to the standard cost for following a plan x, i.e. the sum of the cost of its edges. The modulation factor is a scalar term inversely proportional 341 (a) Initial confi guration(b) Robot actively searches for loop closures(c) Candidate loop closure edges Fig. 2: Initial confi guration of a graph exploration task: each edge has its cost written nearby and it is colored in orange once it is explored. The initial plan is P = 12345471098387651 (a). Lets check all the available plans when the robot reaches vertex 7. The suffi cient condition defi ned in Sec. III does not allow to predict any non-trivial loop closure in the current plan P. Thus a cost of fm (P) = 40 + 0 is computed through (1) (b). It is possible to fi nd two loop closure candidates outside the current plan, colored in pink. The candidate edge 54 can be reached through the path 7654 and then the exploration can be completed following 471098387451 with a total cost fm(P54) = 15 + 45. The candidate 34 can be reached through 7834 and then the exploration can be completed following 4710987651 with a total cost fm(P34) = 15+35 (c). Note that P34will always have a lower cost than P54. The best plan between the current one and P34 will depend on the localization uncertainty: the higher it is, the more will decrease and P34will become the minimum cost choice. to the robot pose uncertainty. As this last quantity increases, the contribution of f(Pend) to the total cost fm(P) will go to zero. If the analysis of the current plan does not allow to predict any loop closure, we are going to set Ploop closure equal to the plan itself, while Pendwill be empty. There exist different approaches and well known scalar formulations for expressing the robot localization uncertainty 17. After some tests, we chose to adopt the D-optimality criterion for our implementation, confi rming the results of Carrillo et al. 18. Through the inspection of the topo-metric graph and of the edges visited so far, it is possible to fi nd additional candidate edges, outside of the current plan, that will trigger a loop closure if the robot visits them again. To fi nd possible loop closure candidates, we create a temporary graph where the last traversed edges have been removed. Then, it is possible to compute which already visited vertices are reachable from the current position on this new graph: any visited edge exiting from one of those vertices is a valid candidate. This has the effect of excluding trivial closures, such as to go backward along the last edge. After we identify a candidate, we can create an exploration plan which includes it and then, thanks to (1), we can com- pare this new plan with the current one. We will obtain the plan as the concatenation of the two subplans Ploop closure and Pend, which have to be computed independently. The fi rst term will represent the shortest route from the robot current position to the candidate edge. We calculate this subplan through standard shortest path techniques applied on the temporary graph. The second term will represent the sequence of edges that are required to be traversed to complete the exploration starting from the loop closure candidate. We compute this subplan as the solution to a routing problem in which not all the edges of a graph are required to be visited. Moreover, the desired route will be open because most of the time the loop closure candidate will not be connected to the goal vertex. In the graph theory literature, a variant of the CPP with optional edges is called Rural Chinese Postman Problem (RCPP). This type of problem is NP-Hard and it is generally solved using branch- and-bound techniques which repeatedly apply the standard CPP algorithm 3. In order to deal with an open routing problem, we exploit a simple technique, presented in detail in Thimbeley 19. It consists in adding a fake edge with an infi nite cost between the current and the goal vertices, then we can apply standard CPP/RCPP algorithms to compute a circuit from the goal location. Finally we adjust the computed plan removing the fake edge and making it start from the current vertex. While the robot is exploring a graph, it can fi nd several candidate loop closure edges. We automatically discard all candidates for which the f(Ploop closure) value is higher than the one for the current plan. This is equivalent to say that the cost for reaching them is higher than the cost for reaching a loop closure or completing the exploration, whichever occur fi rst, by following the current plan. Then we solve the RCPP for all the remaining candidates. Now we can compute the modifi ed cost in (1) and we use this value to compare the new plans with the current one, to decide which one to follow. In an extreme scenario where the uncertainty is infi nite, the cost of a plan P, computed using (1), will be equal to f(Ploop closure), thus the best plan will be the one which allows to reach a loop closure with the lowest possible cost, without taking into account the additional overhead for completing the exploration task. We show a practical application of this active loop closure technique to a toy example in Fig. 2. Our solution is conservative, as it reduces the risk of wrong loop closures. Moreover, note that switching to a new plan will not always require a longer time to complete the exploration because the CPP can have more than one equivalent solution. Standard algorithms only provide one of these subset of edges which have to be duplicated. By computing new plans, our method also allows to evaluate 342 these alternative solutions and pick them if needed. IV. GRAPHONLINEADAPTATION The main diffi culty of using a topo-metric graph as prior information is that it will not represent the environment exactly, due to several sources of errors. First of all, if the graph is drawn by the user, it can only be correct up to a certain degree of approximation. Even in the case of a graph extracted from a precise source, like a CAD model, several differences with what the robot will perceive will be present. They are due to: Tolerance of the metric information of the model. Obstacles that are not represented in the model. Scaling or distortion in the robots mapping procedure. Uncertainty in the robots starting position. The robot needs to adapt the rough prior graph to its observed environment, to properly navigate as expected by the user. We have to make an additional assumption to correct these errors. The real environment should allow the existence of a connectivity graph with a topology similar to the prior graph, i.e. with the same number of vertices and edges. The concept of a connectivity graph has been defi ned by Sun et al. 20, as an approach to path planning. In our case the connectivity property implies that the robot can freely rotate at each vertex position and can safely traverse edges (either with one orientation only or with any orientation) without the risk of collisions. Whenever the robot detects that the prior graph violates this property, it has to perform an optimization step to enforce the physical constraints of the observed environment. A naive