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. I.

As a flexible needle with a bevel tip is pushed through soft tissue, the asymmetry of the tip causes the needle to bend. We propose that, by using nonholonomic kinematics, control, and path planning, an appropriately designed needle can be steered through tissue to reach a specified 3D target. Such steering capability could enhance targeting accuracy and may improve outcomes for percutaneous therapies, facilitate research on therapy effectiveness, and eventually enable new minimally invasive techniques. In this paper, we consider a first step toward active needle steering: design and experimental validation of a nonholonomic model for steering flexible needles with bevel tips. The model generalizes the standard three degree-of-freedom (DOF) nonholonomic unicycle and bicycle models to 6 DOF using Lie group theory. Model parameters are fit using experimental data, acquired via a robotic device designed for the specific purpose of inserting and steering a flexible needle. The experiments quantitatively validate the bevel-tip needle steering model, enabling future research in flexible needle path planning, control, and simulation. KEY WORDS—nonholonomic system, steerable needle, surgical

When inserted into soft tissues, flexible needles with bevel tips have been shown experimentally to follow a path of constant curvature in the direction of the bevel. By controlling 2 degrees of freedom at the needle base (bevel direction and insertion distance), these needles can be steered around obstacles to reach targets inaccessible to rigid needles. Motion planning for needle steering is a type of nonholonomic planning for a Dubins car with no reversal. We develop a motion planning algorithm based on dynamic programming where the path of the needle is uncertain due to uncertainty in tissue properties, needle mechanics, and interaction forces. The algorithm computes a discrete control sequence of insertions and direction changes so the needle reaches a target in an imaging plane while minimizing expected cost due to insertion distance, direction changes, and obstacle collisions. We efficiently sample the state space of needle tip positions and orientations and define bounds on the errors due to discretization. We formulate the motion planning problem as a Markov Decision Process (MDP) and use infinite horizon dynamic programming to compute an optimal control sequence. We first apply the method to the deterministic motion case where the needle precisely follows a path of constant curvature and then to the uncertain motion case where state transitions are defined by a probability distribution. Our implementation generates motion plans for bevel-tip needles that reach targets inaccessible to rigid needles and demonstrates that accounting for uncertainty can lead to significantly different motion plans. Index Terms--- steerable needle, medical robotics, nonholonomic motion planning, dynamic programming, Markov decision process.

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 beveltip 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 lookup table. We apply the method to generate motion plans

Abstract — Bevel-tip flexible needles have greater mobility than straight rigid needles, and can be used to reach targets behind sensitive or impenetrable areas. Accurately planning and executing the optimal motions for such steerable needles is difficult, however, and requires solving inverse kinematics for a nonholonomic system. This paper presents an approach to 3D motion planning for bevel-tip needles in an environment with obstacles. Instead of discretizing the configuration space as in earlier work, we discretize the control space, such that the trajectory of the needle can be expressed analytically without the need for approximate numerical simulation. This results in a fast optimization routine that finds a locally optimal path in a 3D environment with obstacles, requiring just a few seconds of computation time on a standard PC. We introduce two different discretization strategies that lead to differently structured paths and show that both produce valid trajectories from start to goal. To our knowledge, the presented method is the first to address motion planning for bevel-tip needles in a 3D environment with obstacles. I.

Abstract: We consider a variant of nonholonomic motion planning for a Dubins car with no reversals, binary left/right steering, and uncertainty in motion direction. We apply our new motion planner to steerable needles, a new class of flexible bevel-tip medical needles that clinicians can steer through soft tissue to reach 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 turning points 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 (MDP) based on an efficient discretization of the state space, models motion uncertainty using probability distributions, and computes turning points to maximize the probability of success using infinite horizon Dynamic Programming (DP). This approach has three features particularly beneficial for medical planning problems. First, the planning formulation only requires parameters that can be directly extracted from images. Second, we can compute the optimal needle insertion point by examining the DP look-up table of optimal controls for every needle state. Third, intra-operative medical imaging can be combined with the pre-computed DP look-up table to permit optimal control of the needle in the operating room without requiring time-consuming intra-operative re-planning. We apply the method to generate motion plans for steerable needles to reach targets inaccessible to stiff needles and illustrate the importance of considering uncertainty during motion plan optimization. 1

Abstract — Flexible, tip-steerable needles promise to enhance physicians ’ abilities to accurately reach targets and maneuver inside the human body while minimizing patient trauma. Here, we present a functional needle steering system that integrates two components: (1) a patient-specific 2D pre- and intraoperative planner that finds an achievable route to a target within a planar slice of tissue (Stochastic Motion Roadmap), and (2) a low-level image-guided feedback controller that keeps the needle tip within that slice. The planner generates a sequence of circular arcs that can be realized by interleaving pure insertions with 180 ◦ rotations of the needle shaft. This preplanned sequence is updated in realtime at regular intervals. Concurrently, the low-level image-based controller servos the needle to remain close to the desired plane between plan updates. Both planner and controller are predicated on a previously developed kinematic nonholonomic model of beveltip needle steering. We use slighly different needles here that have a small bend near the tip, so we extend the model to account for discontinuities of the tip position caused by 180 ◦ rotations. Further, during large rotations of the needle base, we maintain the desired tip angle by compensating for torsional compliance in the needle shaft, neglected in previous needle steering work. By integrating planning, control, and torsion compensation, we demonstrate both accurate targeting and obstacle avoidance. I.

Abstract—Steerable needles composed of a highly flexible material and with a bevel tip offer greater mobility compared to rigid needles for minimally invasive medical procedures. In this paper, we apply sampling-based motion planning technique to explore motion planning for the steerable bevel-tip needle in 3D environments with obstacles. Based on the Rapidly-exploring Random Trees (RRTs) method, we develop a motion planner to quickly build a tree to search the configuration space using a new exploring strategy, which generates new states using randomly sampled control space instead of the deterministically sampled one used in classic RRTs. Notice the fact that feasible paths might not be found for any given entry point and target configuration, we also address the feasible entry point planning problem to find feasible entry points in a specified entry zone for any given target configuration. To solve this problem, we developed a motion planning algorithm based on RRTs with backchaining, which grow backward from the target to explore the configuration space. Finally, simulation results with a approximated realistic prostate needle insertion environment demonstrate the performance of the proposed motion planner. I.

Abstract This paper presents a technique for planning and controlling bevel-tip steerable needles towards a target location in 3-D anatomy under the guidance of partial, noisy sensor feedback. Our approach minimizes the probability that the needle intersects obstacles such as bones and sensitive organs by (1) explicitly taking into account motion uncertainty and sensor types, and (2) allowing for efficient optimization of sensor placement. We allow for needle trajectories of arbitrary curvature through duty-cycled spinning of the needle, which is believed to make a needle path small-time locally “trackable ” [13]. This enables us to use LQG control to guide the needle along the path. For a given path and sensor placement, we show that a priori probability distributions of the needle state can be estimated in advance. Our approach then plans a set of candidate paths and sensor placements and selects the pair for which the estimated uncertainty is least likely to cause intersections with obstacles. We demonstrate the performance of our approach in a modeled prostate cancer treatment environment. 1

Abstract: Steerable needles can be used in medical applications to reach targets behind sensitive or impenetrable areas. The kinematics of a steerable needle are nonholonomic and, in 2D, equivalent to a Dubins car with constant radius of curvature. In 3D, the needle can be interpreted as an airplane with constant speed and pitch rate, zero yaw, and controllable roll angle. We present a constant-time motion planning algorithm for steerable needles based on explicit geometric inverse kinematics similar to the classic Paden-Kahan subproblems. Reachability and path competitivity are analyzed using analytic comparisons with shortest path solutions for the Dubins car (for 2D) and numerical simulations (for 3D). We also present an algorithm for local path adaptation using null-space results from redundant manipulator theory. The inverse kinematics algorithm can be used as a fast local planner for global motion planning in environments with obstacles, either fully autonomously or in a computer-assisted setting. 1