Abstract — 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. I.

In state estimation, we often want the maximum likelihood estimate of the current state. For the commonly used joint multivariate Gaussian distribution over the state space, this can be efficiently found using a Kalman filter. However, in complex environments the state space is often highly constrained. For example, for objects within a refrigerator, they cannot interpenetrate each other or the refrigerator walls. The multivariate Gaussian is unconstrained over the state space and cannot incorporate these constraints. In particular, the state estimate returned by the unconstrained distribution may itself be infeasible. Instead, we solve a related constrained optimization problem to find a good feasible state estimate. We illustrate this for estimating collision-free configurations for objects resting stably on a 2-D surface, and demonstrate its utility in a real robot perception domain.

Abstract — 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 freespace 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. I.

Abstract — 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. I.

Abstract — 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 freespace 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