Physics-Embedded Motion Planning With Contact Handling for Continuum Surgical Robots

Motion planning for Continuum Surgical Robots (CSRs) faces significant challenges during minimally invasive surgery (MIS) when operating within highly constrained anatomical workspaces. Although traditional approaches aim to prevent tissue damage by finding collision-free paths, robot-tissue interactions are often unavoidable in practical surgical scenarios. To address this, we present a physics-embedded motion planning framework where dynamic interactions between CSRs and tissues are modeled using real-time finite element method (FEM). Our framework integrates computed contact forces as safety constraints into an adaptive sampling-based motion planning algorithm enhanced by Gaussian mixture model (GMM) optimization for efficient exploration. We validate the proposed framework in anatomically accurate scenarios involving the brain ventricles and the tracheobronchial tree, using two types of CSRs: the concentric tube robot and the cable-driven continuum robot. The results demonstrate that our method outperforms the traditional collision-free strategy in terms of path quality and efficiency. Notably, our method provides feasible solutions in scenarios where collision-free paths are unattainable. Lastly, the applicability of the proposed framework is validated in a real-world experiment.