Optical-inertial tracking of an input device for real-time robot control

Minimally invasive robotic surgery systems are usually controlled by input devices, that are mechanically linked to the environment. These input devices often have a limited workspace, which makes intuitive operation difficult. This paper presents a tracking algorithm of a handheld input device, which combines inertial and optical measurements to obtain accurate and robust state estimates with high update rates and low latency. It is based on the fusion of inertial and optical data in an error state extended Kalman filter. To achieve a high degree of robustness with respect to partial device occlusions, active optical markers are tracked and their 2D positions in the camera planes are directly forwarded to the fusion process. The algorithm can handle partial occlusions of the device in one or all of the cameras. A quality measure is defined, which indicates if tracking performance is sufficient to control a robot. An exemplary task in a medical robotics context verifies the assumption that the tracking system can be used for real-time robot control despite frequent marker occlusions.