Planning Curvature-Constrained Paths to Multiple Goals Using Disk Sampling

We present a new path planning method for robots with curvature constraints on their motion to visit multiple goals in any order. We first introduce a disk-based roadmap that facilitates computation of curvature-constrained paths that optimize an application specific metric. This roadmap, which generalizes to both 2D and 3D workspaces, is constructed by sampling disks of bounded curvature and generating feasible transitions between these sampled disks. We then formulate the path planning problem to multiple goals as a Steiner directed tree problem over this roadmap. Since optimally solving the multi-goal planning problem requires exponential time, we propose greedy heuristics to efficiently compute a path that visits multiple goals. We apply the planner in the context of medical needle steering where the needle tip must reach multiple goals in soft tissue, a common requirement for clinical procedures such as biopsies, drug delivery, and brachytherapy cancer treatment. We demonstrate that the proposed heuristics converge to within 5% of optimal and that considering the multi-goal planning problem significantly decreases tissue that must be cut compared to sequential execution of single goal plans.