We present a new approach to motion planning under sensing and motion uncertainty by computing a locally optimal solution to a continuous partially observable Markov decision process (POMDP). Our approach represent beliefs (the distributions of the robot's state estimate) by Gaussian distributions and is applicable to robot systems with non-linear dynamics and observation models. The method follows the general POMDP solution framework in which we approximate the belief dynamics using an extended Kalman filter and represent the value function by a quadratic function that is valid in the vicinity of a nominal trajectory through belief space. Using a belief space variant of iterative LQG (iLQG), our approach iterates with secondorder convergence towards a linear control policy over the belief space that is locally optimal with respect to a user-defined cost function. Unlike previous work, our approach does not assume maximum-likelihood observations, does not assume fixed estimator or control gains, takes into account obstacles in the environment, and does not require discretization of the state and action spaces. The running time of the algorithm is polynomial (O[n 6 ]) in the dimension n of the state space. We demonstrate the potential of our approach in simulation for holonomic and nonholonomic robots maneuvering through environments with obstacles with noisy and partial sensing and with non-linear dynamics and observation models.

Abstract-We present a new motion planning framework that explicitly considers uncertainty in robot motion to maximize the probability of avoiding collisions and successfully reaching a goal. In many motion planning applications ranging from maneuvering vehicles over unfamiliar terrain to steering flexible medical needles through human tissue, the response of a robot to commanded actions cannot be precisely predicted. We propose to build a roadmap by sampling collision-free states in the configuration space and then locally sampling motions at each state to estimate state transition probabilities for each possible action. Given a query specifying initial and goal configurations, we use the roadmap to formulate a Markov Decision Process (MDP), which we solve using Infinite Horizon Dynamic Programming in polynomial time to compute stochastically optimal plans. The Stochastic Motion Roadmap (SMR) thus combines a sampling-based roadmap representation of the configuration space, as in PRM's, with the well-established theory of MDP's. Generating both states and transition probabilities by sampling is far more flexible than previous Markov motion planning approaches based on problem-specific or grid-based discretizations. We demonstrate the SMR framework by applying it to nonholonomic steerable needles, a new class of medical needles that follow curved paths through soft tissue, and confirm that SMR's generate motion plans with significantly higher probabilities of success compared to traditional shortest-path plans.

Abstract-Medical procedures such as seed implantation, biopsies, and treatment injections require inserting a needle to a specific target location inside the human body. Flexible needles with bevel tips are known to bend when inserted into soft tissues and can be inserted to targets unreachable by rigid symmetric-tip needles. Planning for such procedures is difficult because needle insertion causes soft tissues to displace and deform. In this paper, we develop a 2D planning algorithm for insertion of highly flexible bevel-tip needles into tissues with obstacles. Given an initial needle insertion plan specifying location, orientation, bevel rotation, and insertion distance, the planner combines soft tissue modeling and numerical optimization to generate a needle insertion plan that compensates for simulated tissue deformations, locally avoids polygonal obstacles, and minimizes needle insertion distance. Soft tissue deformations are simulated using a finite element formulation that models the effects of needle tip and frictional forces using a 2D mesh. The planning problem is formulated as a constrained nonlinear optimization problem which is locally minimized using a penalty method that converts the formulation to a sequence of unconstrained optimization problems. We apply the planner to bevel-right and bevel-left needles and generate plans for targets that are unreachable by rigid needles.

Steerable needles have the potential to improve the effectiveness of needle-based clinical procedures such as biopsy and drug delivery by improving targeting accuracy and reaching previously inaccessible targets that are behind sensitive or impenetrable anatomical regions. We present a new needle steering system capable of automatically reaching targets in 3-D environments while avoiding obstacles and compensating for real-world uncertainties. Given a specification of anatomical obstacles and a clinical target (e.g., from preoperative medical images), our system plans and controls needle motion in a closed-loop fashion under sensory feedback to optimize a clinical metric. We unify planning and control using a new fast algorithm that continuously replans the needle motion. Our rapid replanning approach is enabled by an efficient sampling-based rapidly exploring random tree (RRT) planner that achieves orders-ofmagnitude reduction in computation time compared with prior 3-D approaches by incorporating variable curvature kinematics and a novel distance metric for planning. Our system uses an electromagnetic tracking system to sense the state of the needle tip during the procedure. We experimentally evaluate our needle steering system using tissue phantoms and animal tissue ex vivo. We demonstrate that our rapid replanning strategy successfully guides the needle around obstacles to desired 3-D targets with an average error of less than 3 mm.

We develop a new motion planning algorithm for a variant of a Dubins car with binary left/right steering and apply it to steerable needles, a new class of flexible bevel-tip medical needles that physicians can steer through soft tissue to reach clinical targets inaccessible to traditional stiff needles. Our method explicitly considers uncertainty in needle motion due to patient differences and the difficulty in predicting needle/tissue interaction. The planner computes optimal steering actions to maximize the probability that the needle will reach the desired target. Given a medical image with segmented obstacles and target, our method formulates the planning problem as a Markov Decision Process based on an efficient discretization of the state space, models motion uncertainty using probability distributions, and computes optimal steering actions using Dynamic Programming. This approach only requires parameters that can be directly extracted from images, allows fast computation of the optimal needle entry point, and enables intra-operative optimal steering of the needle using the pre-computed dynamic programming look-up table. We apply the method to generate motion plans for steerable needles to reach targets inaccessible to stiff needles, and we illustrate the importance of considering uncertainty during motion plan optimization.