Motion planning under uncertainty for medical needle steering using optimization in belief space

We present an optimization-based motion planner for medical steerable needles that explicitly considers motion and sensing uncertainty while guiding the needle to a target in 3D anatomy. Motion planning for needle steering is challenging because the needle is a nonholonomic and underactuated system, the needle's motion may be perturbed during insertion due to unmodeled needle/tissue interactions, and medical sensing modalities such as ultrasound imaging and x-ray projection imaging typically provide only noisy and partial state information. To account for these uncertainties, we introduce a motion planner that computes a trajectory and corresponding linear controller in the belief space - the space of distributions over the state space. We formulate the needle steering motion planning problem as a partially observable Markov decision process (POMDP) that approximates belief states as Gaussians. We then compute a locally optimal trajectory and corresponding controller that minimize in belief space a cost function that considers avoidance of obstacles, penalties for unsafe control inputs, and target acquisition accuracy. We apply the motion planner to simulated scenarios and show that local optimization in belief space enables us to compute higher quality plans compared to planning solely in the needle's state space.