Using Lie algebra for shape estimation of medical snake robots

Highly articulated robots have the potential to play a key role in minimally invasive surgeries by providing improved access to hard-to-reach anatomy. Estimating their shape inside the body and combining it with 3D preoperative scans of the anatomy enable the surgeon to visualize how the entire robot interacts with the internal organs. As the robot progresses inside the body, the position and orientation of every link comprising the robot, evolves over a coordinate-free Lie algebra, se(3). To capture the full motion and uncertainty of the system, we use an extended Kalman filter where the state vector is defined using elements of se(3). We show that this approach describes the shape of the robot more accurately, than the ones where the state vector is a conventional parametrization, such as Cartesian coordinates and Euler angles. We perform two experiments to demonstrate the effectiveness of this new filtering approach.