Planning locally optimal, curvature-constrained trajectories in 3D using sequential convex optimization

3D curvature-constrained motion planning finds applications in a wide variety of domains, including motion planning for flexible, bevel-tip medical needles, planning curvature-constrained channels in 3D printed implants for targeted brachytherapy dose delivery or channels for cooling turbine blades, and path planning for unmanned aerial vehicles (UAVs). In this work, we present a motion planning technique using sequential convex optimization for computing locally optimal, curvature-constrained trajectories to desired targets while avoiding obstacles in 3D environments. We report two main contributions in this work: (i) curvature-constrained trajectory optimization in 6D pose (position and orientation) space, and (ii) planning multiple trajectories that are mutually collision-free. We demonstrate the performance of our approach on two clinically motivated applications. Our experiments indicate that our approach can compute high-quality plans for medical needle steering in 1.6 seconds on a commodity PC, enabling re-planning during execution to correct for perturbations. Our approach can also be used for designing optimized channel layouts within 3D printed implants for intracavitary brachytherapy.