An operating theatre of the future will contain robots capable of enacting automated sequences to perform surgical sub-tasks. Inserting an endoscope into a trocar is a critical step required in all minimally invasive procedures. The goal of our work is to develop a robot controller capable of performing this insertion task fully autonomously. From a robotics perspective, automated trocar docking is a particular generalisation of the peg-in-hole problem and (to the best of the author's knowledge) there are little works exploring this task at a scale for procedures such as endoscopic lumbar discectomy/decompression, and cholecystectomy.

In this work, we develop a hardware realisation of an autonomous trocar docking system. Furthermore, we describe our developed trocar simulator used to develop our optimal control approach. Our method incorporates state estimation for the trocar and contact information (measured from the external torques sensed at the robot joint encoders). We use a nonlinear optimization program as our optimal control formulation. Optimal joint configurations are found by minimising a weighted sum of cost terms (that models the task objectives) subject to several constraints (representing physical limitations of the system).

Our experiments demonstrate that our developed controller is capable of solving the problem on a realistic hardware lab setup. Furthermore, through a study, in simulation, we highlight that the incorporation of contact information reduces the interaction forces between the endoscope and trocar, improving patient safety.