In this paper, we present a novel approach for mapping periodically moving visual features with a monocular camera. Our target application is the estimation of moving surfaces during minimally invasive surgery for the purpose of aiding in the guidance of surgical tools. Our approach uses a bank of Kalman filters to estimate FFT parameters that encode the periodic motion of visually detected features. To ensure convergent estimation for this highly nonlinear problem, we have developed an iterative update procedure that treats the Kalman filter measurement update step as an optimization problem. Unlike existing solutions that rely on stereo vision, our approach estimates periodic motion with a single moving camera. With an experiment involving a beating heart phantom, we have shown that our approach is able to successfully estimate the periodic motion of visual features.

This video elaborates on a new active and controllable over-tube addition to the highly articulated robotic probe; the HARP. This over-tube allows the current HARP mechanism to double its overall length and allows it to perform more complex tasks. We explain the design concept of the current HARP and the novel over-tube mechanism and show two proof-of-concept experiments demonstrating the use of the active over-tube.

In this paper, we present a novel algorithm for deterministically mapping an undirected graph-like world with multiple synchronized agents. The application of this algorithm is the collective mapping of an indoor environment with multiple mobile robots while leveraging an embedded topological decomposition of the environment. Our algorithm relies on a group of agents that all depart from the same initial vertex in the graph and spread out to explore the graph. A centralized tree of graph hypotheses is maintained to consider loop-closure, which is deterministically verified when agents observe each other at a common vertex. To achieve efficient mapping, we introduce an active exploration method in which agents dynamically request rendezvous tasks from other available agents to validate graph hypotheses.

This paper presents a novel filtering technique that uses contact detection data and environmental stiffness estimates to register and localize a robot with respect to an a priori 3D surface model. The algorithm leverages geometric constraints within a Kalman filter framework and relies on two distinct update procedures: 1) an equality constrained step for when the robot is forcefully contacting the environment, and 2) an inequality constrained step for when the robot lies in the free-space of the environment. This filtering procedure registers the robot by incrementally eliminating probabilistically infeasible state space regions until a high likelihood solution emerges. In addition to registration and localization, the algorithm can estimate the deformation of the surface model and can detect false positives with respect to contact estimation. This method is experimentally evaluated with an experiment involving a continuum robot interacting with a bench-top flexible structure. The presented algorithm produces an experimental error in registration (with respect to the end-effector position) of 1.1 mm, which is less than 0.8 percent of the robot length.

The generalized Voronoi graph (GVG) is a topological representation of an environment that can be incrementally constructed with a mobile robot using sensor-based control. However, because of sensor range limitations, the GVG control law will fail when the robot moves into a large open area. This paper discusses an extended GVG approach to topological navigation and mapping: the saturated generalized Voronoi graph (S-GVG), for which the robot employs an additional wall-following behavior to navigate along obstacles at the range limit of the sensor. In this paper, we build upon previous work related to the S-GVG and provide two important contributions: 1) a rigorous discussion of the control laws and algorithm modifications that are necessary for incremental construction of the S-GVG with a mobile robot, and 2) a method for incorporating the S-GVG into a novel multi-hypothesis SLAM algorithm for loop-closing and localization. Experiments with a wheeled mobile robot in an office-like environment validate the effectiveness of the proposed approach.

This paper uses KLD-based (Kullback-Leibler Divergence) Monte Carlo Localization (MCL) to localize a mobile robot in an indoor environment represented by 3D texture maps. A 3D texture map is a simplified model that includes vertical planes with colored texture information associated with each vertical plane. At each time step, a distance measurement and an observed texture from an omnidirectional camera are compared to the expected distance measurement and the expected texture according to each hypothesis of the robot's pose in an MCL framework. Compared to previous implementations of MCL, our proposed approach converges faster than distance-only MCL and localizes the robot more precisely than SIFT-based MCL. We demonstrate this new MCL algorithm for robot localization with experiments in several hallways.

We present a novel method for enforcing nonlinear inequality constraints in the estimation of a high degree of freedom robotic system within a Kalman filter. Our constrained Kalman filtering technique is based on a new concept, which we call uncertainty projection, that projects the portion of the uncertainty ellipsoid that does not satisfy the constraint onto the constraint surface. A new PDF is then generated with an efficient update procedure that is guaranteed to reduce the uncertainty of the system. The application we have targeted for this work is the localization and automatic registration of a robotic surgical probe relative to preoperative images during image-guided surgery. We demonstrate the feasibility of our constrained filtering approach with data collected from an experiment involving a surgical robot navigating on the epicardial surface of a porcine heart.

In this paper, we present a filtering method for estimating the shape and end effector pose of a highly articulated surgical snake robot. Our algorithm introduces new kinematic models that are used in the prediction step of an extended Kalman filter whose update step incorporates measurements from a 5-DOF electromagnetic tracking sensor situated at the distal end of the robot. A single tracking sensor is sufficient for estimating the shape of the system because the robot is inherently a follow-the-leader mechanism with well defined motion characteristics. We therefore show that, with appropriate steering motion, the state of the filter is fully observable. The goal of our shape estimation algorithm is to create a more accurate and representative 3D rendered visualization for image-guided surgery.We demonstrate the feasibility of our method with results from an animal experiment in which our shape and pose estimate was used as feedback in a control scheme that semi-autonomously drove the robot along the epicardial surface of a porcine heart.

This paper presents a novel recursive maximum a posteriori update for the Kalman formulation of undelayed bearing-only SLAM. The estimation update step is cast as an optimization problem for which we can prove the global minimum is reachable via a bidirectional search using Gauss-Newton's method along a one-dimensional manifold. While the filter is designed for mapping just one landmark, it is easily extended to full-scale multiple-landmark SLAM. We provide this extension via a formulation of bearing-only FastSLAM. With experiments, we demonstrate accurate and convergent estimation in situations where an EKF solution would diverge.

We present a method for topological SLAM that specifically targets loop closing for edge-ordered graphs. Instead of using a heuristic approach to accept or reject loop closing, we propose a probabilistically grounded multi-hypothesis technique that relies on the incremental construction of a map/state hypothesis tree. Loop closing is introduced automatically within the tree expansion, and likely hypotheses are chosen based on their posterior probability after a sequence of sensor measurements. Careful pruning of the hypothesis tree keeps the growing number of hypotheses under control and a recursive formulation reduces storage and computational costs. Experiments are used to validate the approach.

Perceptual aliasing makes topological navigation a difficult task. In this paper we present a general approach for topological SLAM (simultaneous localisation and mapping) which does not require motion or odometry information but only a sequence of noisy measurements from visited places. We propose a particle filtering technique for topological SLAM which relies on a method for disambiguating places which appear indistinguishable using neighbourhood information extracted from the sequence of observations. The algorithm aims to induce a small topological map which is consistent with the observations and simultaneously estimate the location of the robot. The proposed approach is evaluated using a data set of sonar measurements from an indoor environment which contains several similar places. It is demonstrated that our approach is capable of dealing with severe ambiguities and, and that it infers a small map in terms of vertices which is consistent with the sequence of observations.

This paper discusses the importance of iteration when performing the measurement update step for the problem of bearing-only SLAM. We focus on an undelayed approach that initializes a landmark after only one bearing measurement. Traditionally, the extended Kalman filter (EKF) has been used for SLAM, but the EKF measurement update rule can often lead to a divergent state estimate due to its inconsistency in linearization. We discuss the flaws of the EKF in this paper, and show that even the well established inverse-depth parametrization for bearing-only SLAM can be affected. We then show that representing the bearing-only update as a numerical optimization problem (solved with an iterative approach such as Gauss-Newton minimization) prevents divergence of the Kalman filter state and produces accurate SLAM results for a bearing-only sensor. More specifically, we propose the use of an iterated Kalman filter to resolve the issues normally associated with the EKF measurement update. Two outdoor mobile robot experiments are discussed to compare algorithm performance.

This paper presents a hybrid localization scheme for a mobile robot using the hierarchical atlas. The hierarchical atlas is a map that consists of a higher level topological graph with lower level feature-based metric submaps associated with the graph edges. Our method employs both a discrete Bayes filter and a Kalman filter to localize the robot in the map. This framework accommodates localization in a map with no prior information (global localization) and localization in a map with an incorrect pose estimate (kidnapped robot). Our approach efficiently scales to large environments without sacrificing accuracy or robustness. We have verified our method with large-scale experiments in a multi-floor office environment.