Active pose SLAM with RRT*

Trabajo presentado al ICRA celebrado en Seattle (US) del 26 al 30 de mayo de 2015.We propose a novel method for robotic exploration that evaluates paths that minimize both the joint path and map entropy per meter traveled. The method uses Pose SLAM to update the path estimate, and grows an RRT* tree to generate the set of candidate paths. This action selection mechanism contrasts with previous appoaches in which the action set was built heuristically from a sparse set of candidate actions. The technique favorably compares agains the classical frontier- based exploration and other Active Pose SLAM methods in simulations in a common publicly available dataset.This work has been supported by the Spanish Ministry of Economy and Competitiveness under project PAU+ DPI-2011-27510 and by the EU Project CargoAnts FP7-605598.Peer Reviewe

Dense entropy decrease estimation for mobile robot exploration

Presentado al ICRA 2014 celebrado en Hong Kong del 31 de mayo al 7 de junio.We propose a method for the computation of entropy decrease in C-space. These estimates are then used to evaluate candidate exploratory trajectories in the context of autonomous mobile robot mapping. The method evaluates both map and path entropy reduction and uses such estimates to compute trajectories that maximize coverage whilst min- imizing localization uncertainty, hence reducing map error. Very efficient kernel convolution mechanisms are used to evaluate entropy reduction at each sensor ray, and for each possible robot position and orientation, taking frontiers and obstacles into account. In contrast to most other exploration methods that evaluate entropy reduction at a small number of discrete robot configurations, we do it densely for the entire C-space. The computation of such dense entropy reduction maps opens the window to new exploratory strategies. In this paper we present two such strategies. In the first one we drive exploration through a gradient descent on the entropy decrease field. The second strategy chooses maximal entropy reduction configurations as candidate exploration goals, and plans paths to them using RRT*. Both methods use PoseSLAM as their estimation backbone, and are tested and compared with classical frontier-based exploration in simulations using common publicly available datasets.This work has been supported by the Spanish Ministry of Economy and Competitiveness under Project DPI-2011-27510 and by the EU Project CargoAnts FP7-605598.Peer Reviewe

Large Scale 3D Mapping of Indoor Environments Using a Handheld RGBD Camera

The goal of this research is to investigate the problem of reconstructing a 3D representation of an environment, of arbitrary size, using a handheld color and depth (RGBD) sensor. The focus of this dissertation is to examine four of the underlying subproblems to this system: camera tracking, loop closure, data storage, and integration. First, a system for 3D reconstruction of large indoor planar environments with data captured from an RGBD sensor mounted on a mobile robotic platform is presented. An algorithm for constructing nearly drift-free 3D occupancy grids of large indoor environments in an online manner is also presented. This approach combines data from an odometry sensor with output from a visual registration algorithm, and it enforces a Manhattan world constraint by utilizing factor graphs to produce an accurate online estimate of the trajectory of the mobile robotic platform. Through several experiments in environments with varying sizes and construction it is shown that this method reduces rotational and translational drift significantly without performing any loop closing techniques. In addition the advantages and limitations of an octree data structure representation of a 3D environment is examined. Second, the problem of sensor tracking, specifically the use of the KinectFusion algorithm to align two subsequent point clouds generated by an RGBD sensor, is studied. A method to overcome a significant limitation of the Iterative Closest Point (ICP) algorithm used in KinectFusion is proposed, namely, its sole reliance upon geometric information. The proposed method uses both geometric and color information in a direct manner that uses all the data in order to accurately estimate camera pose. Data association is performed by computing a warp between the two color images associated with two RGBD point clouds using the Lucas-Kanade algorithm. A subsequent step then estimates the transformation between the point clouds using either a point-to-point or point-to-plane error metric. Scenarios in which each of these metrics fails are described, and a normal covariance test for automatically selecting between them is proposed. Together, Lucas-Kanade data association (LKDA) along with covariance testing enables robust camera tracking through areas of low geometrical features, while at the same time retaining accuracy in environments in which the existing ICP technique succeeds. Experimental results on several publicly available datasets demonstrate the improved performance both qualitatively and quantitatively. Third, the choice of state space in the context of performing loop closure is revisited. Although a relative state space has been discounted by previous authors, it is shown that such a state space is actually extremely powerful, able to achieve recognizable results after just one iteration. The power behind the technique is that changing the orientation of one node is able to affect other nodes. At the same time, the approach --- which is referred to as Pose Optimization using a Relative State Space (POReSS) --- is fast because, like the more popular incremental state space, the Jacobian never needs to be explicitly computed. Furthermore, it is shown that while POReSS is able to quickly compute a solution near the global optimum, it is not precise enough to perform the fine adjustments necessary to achieve acceptable results. As a result, a method to augment POReSS with a fast variant of Gauss-Seidel --- which is referred to as Graph-Seidel --- on a global state space to allow the solution to settle closer to the global minimum is proposed. Through a set of experiments, it is shown that this combination of POReSS and Graph-Seidel is not only faster but achieves a lower residual than other non-linear algebra techniques. Moreover, unlike the linear algebra-based techniques, it is shown that this approach scales to very large graphs. In addition to revisiting the idea of using a relative state space, the benefits of only optimizing the rotational components of a trajectory in order to perform loop closing is examined (rPOReSS). Finally, an incremental implementation of the rotational optimization is proposed (irPOReSS)

Potential information fields for mobile robot exploration

We present a decision theoretic approach to mobile robot exploration. The method evaluates the reduction of joint path and map entropy and computes a potential information field in robot configuration space using these joint entropy reduction estimates. The exploration trajectory is computed descending on the gradient of this field. The technique uses Pose SLAM as its estimation backbone. Very efficient kernel convolution mechanisms are used to evaluate entropy reduction for each sensor ray, and for each possible robot orientation, taking frontiers and obstacles into account. In the end, the computation of this field on the entire configuration space is shown to be very efficient. The approach is tested in simulations in a pair of publicly available datasets comparing favorably both in quality of estimates and in execution time against an RRT∗-based search for the nearest frontier and also against a locally optimal exploration strategy.This work has been supported by the Spanish Ministry of Economy and Competitiveness under Project DPI-2011-27510 and by the EU Project CargoANTs FP7-605598.Peer Reviewe

Mapping, planning and exploration with Pose SLAM

This thesis reports research on mapping, path planning, and autonomous exploration. These are classical problems in robotics, typically studied independently, and here we link such problems by framing them within a common SLAM approach, adopting Pose SLAM as the basic state estimation machinery. The main contribution of this thesis is an approach that allows a mobile robot to plan a path using the map it builds with Pose SLAM and to select the appropriate actions to autonomously construct this map. Pose SLAM is the variant of SLAM where only the robot trajectory is estimated and where landmarks are only used to produce relative constraints between robot poses. In Pose SLAM, observations come in the form of relative-motion measurements between robot poses. With regards to extending the original Pose SLAM formulation, this thesis studies the computation of such measurements when they are obtained with stereo cameras and develops the appropriate noise propagation models for such case. Furthermore, the initial formulation of Pose SLAM assumes poses in SE(2) and in this thesis we extend this formulation to SE(3), parameterizing rotations either with Euler angles and quaternions. We also introduce a loop closure test that exploits the information from the filter using an independent measure of information content between poses. In the application domain, we present a technique to process the 3D volumetric maps obtained with this SLAM methodology, but with laser range scanning as the sensor modality, to derive traversability maps. Aside from these extensions to Pose SLAM, the core contribution of the thesis is an approach for path planning that exploits the modeled uncertainties in Pose SLAM to search for the path in the pose graph with the lowest accumulated robot pose uncertainty, i.e., the path that allows the robot to navigate to a given goal with the least probability of becoming lost. An added advantage of the proposed path planning approach is that since Pose SLAM is agnostic with respect to the sensor modalities used, it can be used in different environments and with different robots, and since the original pose graph may come from a previous mapping session, the paths stored in the map already satisfy constraints not easy modeled in the robot controller, such as the existence of restricted regions, or the right of way along paths. The proposed path planning methodology has been extensively tested both in simulation and with a real outdoor robot. Our path planning approach is adequate for scenarios where a robot is initially guided during map construction, but autonomous during execution. For other scenarios in which more autonomy is required, the robot should be able to explore the environment without any supervision. The second core contribution of this thesis is an autonomous exploration method that complements the aforementioned path planning strategy. The method selects the appropriate actions to drive the robot so as to maximize coverage and at the same time minimize localization and map uncertainties. An occupancy grid is maintained for the sole purpose of guaranteeing coverage. A significant advantage of the method is that since the grid is only computed to hypothesize entropy reduction of candidate map posteriors, it can be computed at a very coarse resolution since it is not used to maintain neither the robot localization estimate, nor the structure of the environment. Our technique evaluates two types of actions: exploratory actions and place revisiting actions. Action decisions are made based on entropy reduction estimates. By maintaining a Pose SLAM estimate at run time, the technique allows to replan trajectories online should significant change in the Pose SLAM estimate be detected. The proposed exploration strategy was tested in a common publicly available dataset comparing favorably against frontier based explorationPostprint (published version

Exploration on continuous Gaussian process frontier maps

© 2014 IEEE. An information-driven autonomous robotic exploration method on a continuous representation of unknown environments is proposed in This paper. The approach conveniently handles sparse sensor measurements To build a continuous model of The environment That exploits structural dependencies without The need To resort To a fixed resolution grid map. A gradient field of occupancy probability distribution is regressed from sensor data as a Gaussian process providing frontier boundaries for further exploration. The resulting continuous global frontier surface completely describes unexplored regions and, inherently, provides an automatic stop criterion for a desired sensitivity. The performance of The proposed approach is evaluated Through simulation results in The well-known Freiburg and Cave maps

Efficient active SLAM based on submap joining

This paper considers the active SLAM problem where a robot is required to cover a given area while at the same time performing simultaneous localization and mapping (SLAM) for understanding the environment and localizing the robot itself. We propose a model predictive control (MPC) framework, and the minimization of uncertainty in SLAM and coverage problems are solved respectively by the Sequential Quadratic Programming (SQP) method. Then, a decision making process is used to control the switching of two control inputs. In order to reduce the estimation and planning time, we use Linear SLAM, which is a submap joining approach. Simulation results are presented to validate the effectiveness of the proposed active SLAM strategy