Shape estimation for image-guided surgery with a highly articulated snake robot

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.