Abstract
We present a new sampling-based method for planning optimal, collision-free, curvature-constrained paths for nonholonomic robots to visit multiple goals in any order. Rather than sampling configurations as in standard sampling-based planners, we construct a roadmap by sampling circles of constant curvature and then generating feasible transitions between the sampled circles. We provide a closed-form formula for connecting the sampled circles in 2D and generalize the approach to 3D workspaces. We then formulate the multi-goal planning problem as finding a minimum directed Steiner tree over the 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 our multi-goal planner significantly decreases tissue that must be cut when compared to sequential execution of single-goal plans.