3D Motion Planning for Steerable Needles using Inverse Kinematics and Numerical Optimization

Vincent Duindam, Ron Alterovitz, Jijie Xu, Shankar Sastry, and Ken Goldberg

Steerable needles are flexible needles that provide steerability due to asymmetric forces acting at the tip, for example due to a beveled surface or a kink. By rotating the needle at the base, the orientation of the tip can be changed and hence the trajectory of the needle can be controlled. This extra mobility compared to rigid needles can be harnessed in medical applications such as brachytherapy to reach difficult targets behind sensitive or impenetrable areas.

In this project, we develop algorithms to compute optimal trajectories for steerable needles in a 3D environment with obstacles. The first algorithm is based on optimization of a cost function that numerically quantifies the planning objective. Various terms in this cost function describe penalties for deviation from the target location, large control actions, and obstacle penetration. The algorithm uses a suitable discretization of the control space to quickly compute a needle path with minimal cost.

The second algorithm relies on an explicit expression of the inverse kinematics of the needle to generate a range of valid needle paths from start to target, from which the best solution can be selected. Although this algorithm generally does not compute a (locally) optimal solution, it does not require iteration to converge to a solution and is hence much faster than the first algorithm. Depending on the required balance between speed and optimality, either algorithm can be advantageous.

Both planning algorithms have been implemented in C++ code. A graphical user interface allows for easy interaction with the system and visual inspection of the computed solutions. Inverse kinematics solutions are computed nearly instantaneously while numerical optimization takes no longer than a few seconds on a standard PC. Simulation studies suggest that the algorithms find solutions for environments with few obstacles but may return sub-optimal results for more complex environments. We envision the algorithms to be components in a larger path planning system that guarantees global optimality.

System setup with steerable needle, start and target locations, and obstacles.	Graphical user interface for the various motion planning algorithms implemented in C++. An example of a solution path is shown.