Planning High-Quality Motions for Concentric Tube Robots in Point Clouds via Parallel Sampling and Optimization

We present a method that plans motions for a concentric tube robot to automatically reach surgical targets inside the body while avoiding obstacles, where the patient's anatomy is represented by point clouds. Point clouds can be generated intra-operatively via endoscopic instruments, enabling the system to update obstacle representations over time as the patient anatomy changes during surgery. Our new motion planning method uses a combination of sampling-based motion planning methods and local optimization to efficiently handle point cloud data and quickly compute high quality plans. The local optimization step uses an interior point optimization method, ensuring that the computed plan is feasible and avoids obstacles at every iteration. This enables the motion planner to run in an anytime fashion, i.e., the method can be stopped at any time and the best solution found up until that point is returned. We demonstrate the method's efficacy in three anatomical scenarios, including two generated from endoscopic videos of real patient anatomy.