  Future operating theatres will be equipped with robots to perform various
surgical tasks including, for example, endoscope control. Human-in-the-loop
supervisory control architectures where the surgeon selects from several
autonomous sequences is already being successfully applied in preclinical
tests. Inserting an endoscope into a trocar or introducer is a key step for
every keyhole surgical procedure -- hereafter we will only refer to this device
as a "trocar". Our goal is to develop a controller for autonomous trocar
docking.
  Autonomous trocar docking is a version of the peg-in-hole problem. Extensive
work in the robotics literature addresses this problem. The peg-in-hole problem
has been widely studied in the context of assembly where, typically, the hole
is considered static and rigid to interaction. In our case, however, the trocar
is not fixed and responds to interaction. We consider a variety of surgical
procedures where surgeons will utilize contact between the endoscope and trocar
in order to complete the insertion successfully. To the best of our knowledge,
we have not found literature that explores this particular generalization of
the problem directly.
  Our primary contribution in this work is an optimal control formulation for
automated trocar docking. We use a nonlinear optimization program to model the
task, minimizing a cost function subject to constraints to find optimal joint
configurations. The controller incorporates a geometric model for insertion and
a force-feedback (FF) term to ensure patient safety by preventing excessive
interaction forces with the trocar. Experiments, demonstrated on a real
hardware lab setup, validate the approach. Our method successfully achieves
trocar insertion on our real robot lab setup, and simulation trials demonstrate
its ability to reduce interaction forces.
