Closed-Loop Planning and Control of Steerable Medical Needles

Steerable needles have the potential to increase the effectiveness of needle-based clinical procedures such as biopsy, drug delivery, and radioactive seed implantation for cancer treatment. These needles can trace curved paths when inserted into tissue, thereby increasing maneuverability and targeting accuracy while reaching previously inaccessible targets that are behind sensitive or impenetrable anatomical regions. Guiding these flexible needles along an intended path requires continuously inserting and twisting the needle at its base, which is not intuitive for a human operator. In addition, the needle often deviates from its intended trajectory due to factors such as tissue deformation, needle-tissue interaction, noisy actuation and sensing, modeling errors, and involuntary patient motions. These challenges can be addressed with the assistance of robotic systems that automatically compensate for these perturbations by performing motion planning and feedback control of the needle in a closed-loop fashion under sensory feedback. We present two approaches for efficient closed-loop guidance of steerable needles to targets within clinically acceptable accuracy while safely avoiding sensitive or impenetrable anatomical structures. The first approach uses a fast motion planning algorithm that unifies planning and control by continuously replanning, enabling correction for perturbations as they occur. We evaluate our method using a needle steering system in phantom and ex vivo animal tissues. The second approach integrates motion planning and feedback control of steerable needles in highly deformable environments. We demonstrate that this approach significantly improves the probability of success compared to prior approaches that either consider uncertainty or deformations but not both simultaneously. We also propose a data-driven method to estimate parameters of stochastic models of steerable needle motion. These models can be used to create realistic medical simulators for clinicians wanting to train for steerable needle procedures and to improve the effectiveness of existing planning and control methods. This dissertation advances the state of the art in planning and control of steerable needles and is an important step towards realizing needle steering in clinical practice. The methods developed in this dissertation also generalize to important applications beyond medical needle steering, such as manipulating deformable objects and control of mobile robots.Doctor of Philosoph

Stochastic Extended LQR for Optimization-Based Motion Planning Under Uncertainty

We introduce a novel optimization-based motion planner, Stochastic Extended LQR (SELQR), which computes a trajectory and associated linear control policy with the objective of minimizing the expected value of a user-defined cost function. SELQR applies to robotic systems that have stochastic non-linear dynamics with motion uncertainty modeled by Gaussian distributions that can be state- and control-dependent. In each iteration, SELQR uses a combination of forward and backward value iteration to estimate the cost-to-come and the cost-to-go for each state along a trajectory. SELQR then locally optimizes each state along the trajectory at each iteration to minimize the expected total cost, which results in smoothed states that are used for dynamics linearization and cost function quadratization. SELQR progressively improves the approximation of the expected total cost, resulting in higher quality plans. For applications with imperfect sensing, we extend SELQR to plan in the robot's belief space. We show that our iterative approach achieves fast and reliable convergence to high-quality plans in multiple simulated scenarios involving a car-like robot, a quadrotor, and a medical steerable needle performing a liver biopsy procedure

High-Frequency Replanning Under Uncertainty Using Parallel Sampling-Based Motion Planning

As sampling-based motion planners become faster, they can be re-executed more frequently by a robot during task execution to react to uncertainty in robot motion, obstacle motion, sensing noise, and uncertainty in the robot’s kinematic model. We investigate and analyze high-frequency replanning (HFR), where, during each period, fast sampling-based motion planners are executed in parallel as the robot simultaneously executes the first action of the best motion plan from the previous period. We consider discrete-time systems with stochastic nonlinear (but linearizable) dynamics and observation models with noise drawn from zero mean Gaussian distributions. The objective is to maximize the probability of success (i.e., avoid collision with obstacles and reach the goal) or to minimize path length subject to a lower bound on the probability of success. We show that, as parallel computation power increases, HFR offers asymptotic optimality for these objectives during each period for goal-oriented problems. We then demonstrate the effectiveness of HFR for holonomic and nonholonomic robots including car-like vehicles and steerable medical needles

Needle Steering in 3-D Via Rapid Replanning

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-of-magnitude 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

EG-RRT: Environment-guided random trees for kinodynamic motion planning with uncertainty and obstacles

Existing sampling-based robot motion planning methods are often inefficient at finding trajectories for kinodynamic systems, especially in the presence of narrow passages between obstacles and uncertainty in control and sensing. To address this, we propose EG-RRT, an Environment-Guided variant of RRT designed for kinodynamic robot systems that combines elements from several prior approaches and may incorporate a cost model based on the LQG-MP framework to estimate the probability of collision under uncertainty in control and sensing. We compare the performance of EG-RRT with several prior approaches on challenging sample problems. Results suggest that EG-RRT offers significant improvements in performance.Postprint (author’s final draft

Toward certifiable optimal motion planning for medical steerable needles

Medical steerable needles can follow 3D curvilinear trajectories to avoid anatomical obstacles and reach clinically significant targets inside the human body. Automating steerable needle procedures can enable physicians and patients to harness the full potential of steerable needles by maximally leveraging their steerability to safely and accurately reach targets for medical procedures such as biopsies. For the automation of medical procedures to be clinically accepted, it is critical from a patient care, safety, and regulatory perspective to certify the correctness and effectiveness of the planning algorithms involved in procedure automation. In this paper, we take an important step toward creating a certifiable optimal planner for steerable needles. We present an efficient, resolution-complete motion planner for steerable needles based on a novel adaptation of multi-resolution planning. This is the first motion planner for steerable needles that guarantees to compute in finite time an obstacle-avoiding plan (or notify the user that no such plan exists), under clinically appropriate assumptions. Based on this planner, we then develop the first resolution-optimal motion planner for steerable needles that further provides theoretical guarantees on the quality of the computed motion plan, that is, global optimality, in finite time. Compared to state-of-the-art steerable needle motion planners, we demonstrate with clinically realistic simulations that our planners not only provide theoretical guarantees but also have higher success rates, have lower computation times, and result in higher quality plans

Efficient Motion and Inspection Planning for Medical Robots with Theoretical Guarantees

Medical robots enable faster and safer patient care. Continuum medical robots (e.g., steerable needles) have great potential to accomplish procedures with less damage to patients compared to conventional instruments (e.g., reducing puncturing and cutting of tissues). Due to their complexity and degrees of freedom, such robots are often harder and less intuitive for physicians to operate directly. Automating robot-assisted medical procedures can enable physicians and patients to harness the full potential of medical robots in terms of safety, efficiency, accuracy, and precision.Motion planning methods compute motions for a robot that satisfy various constraints and accomplish a specific task, e.g., plan motions for a mobile robot to move to a target spot while avoiding obstacles. Inspection planning is the task of planning motions for a robot to inspect a set of points of interest, and it has applications in domains such as industrial, field, and medical robotics. With motion and inspection planning, medical robots would be able to automatically accomplish tasks like biopsy and endoscopy while minimizing safety risks and damage to the patient. Computing a motion or inspection plan can be computationally hard since we have to consider application-specific constraints, which come from the robotic system due to the mechanical properties of the robot or come from the environment, such as the requirement to avoid critical anatomical structures during the procedure.I develop motion and inspection planning algorithms that focus on efficiency and effectiveness. Given the same computing power, higher efficiency would shorten the procedure time, thus reducing costs and improving patient outcomes. Additionally, for the automation of medical procedures to be clinically accepted, it is critical from a patient care, safety, and regulatory perspective to certify the correctness and effectiveness of the algorithms involved in procedure automation. Therefore, I focus on providing theoretical guarantees to certify the performance of planners. More specifically, it is important to certify if a planner is able to find a plan if one exists (i.e., completeness) and if a planner is able to find a globally optimal plan according to a given metric (i.e., optimality).Doctor of Philosoph

Towards Robot Autonomy in Medical Procedures Via Visual Localization and Motion Planning

Robots performing medical procedures with autonomous capabilities have the potential to positively effect patient care and healthcare system efficiency. These benefits can be realized by autonomous robots facilitating novel procedures, increasing operative efficiency, standardizing intra- and inter-physician performance, democratizing specialized care, and focusing the physician’s time on subtasks that best leverage their expertise. However, enabling medical robots to act autonomously in a procedural environment is extremely challenging. The deforming and unstructured nature of the environment, the lack of features in the anatomy, and sensor size constraints coupled with the millimeter level accuracy required for safe medical procedures introduce a host of challenges not faced by robots operating in structured environments such as factories or warehouses. Robot motion planning and localization are two fundamental abilities for enabling robot autonomy. Motion planning methods compute a sequence of safe and feasible motions for a robot to accomplish a specified task, where safe and feasible are defined by constraints with respect to the robot and its environment. Localization methods estimate the position and orientation of a robot in its environment. Developing such methods for medical robots that overcome the unique challenges in procedural environments is critical for enabling medical robot autonomy. In this dissertation, I developed and evaluated motion planning and localization algorithms towards robot autonomy in medical procedures. A majority of my work was done in the context of an autonomous medical robot built for enhanced lung nodule biopsy. First, I developed a dataset of medical environments spanning various organs and procedures to foster future research into medical robots and automation. I used this data in my own work described throughout this dissertation. Next, I used motion planning to characterize the capabilities of the lung nodule biopsy robot compared to existing clinical tools and I highlighted trade-offs in robot design considerations. Then, I conducted a study to experimentally demonstrate the benefits of the autonomous lung robot in accessing otherwise hard-to-reach lung nodules. I showed that the robot enables access to lung regions beyond the reach of existing clinical tools with millimeter-level accuracy sufficient for accessing the smallest clinically operable nodules. Next, I developed a localization method to estimate the bronchoscope’s position and orientation in the airways with respect to a preoperatively planned needle insertion pose. The method can be used by robotic bronchoscopy systems and by traditional manually navigated bronchoscopes. The method is designed to overcome challenges with tissue motion and visual homogeneity in the airways. I demonstrated the success of this method in simulated lungs undergoing respiratory motion and showed the method’s ability to generalize across patients.Doctor of Philosoph