3D Bevel-Tip Needle Insertion Trajectory Planning via Computational Optimal Control

Manual insertion of flexible bevel-tip needles often leads to unpredictable tissue deformation and compromised targeting accuracy, emphasizing the need for robust trajectory planning. To address this challenge, we formulate the insertion problem as a time-energy optimal control problem (OCP) subject to nonlinear kinematic and collision-avoidance constraints. Due to its large-scale and nonconvex nature, directly solving the nominal OCP is difficult. Instead, we first obtain a coarse collision-free trajectory via A* search in the abstracted 3D workspace. Next, we create spatiotemporal safe corridors around this trajectory, replace the nominal collision-avoidance constraints with corridor-based constraints, and iteratively relax the kinematic equations as external penalties to refine feasibility. The refined solution subsequently warm-starts a final solve of the nominal OCP with strict kinematic constraints and reduced-scale collision-avoidance constraints. Simulations confirm that our proposed optimization-based trajectory planner converges reliably to numerically optimal needle trajectories, surpassing existing optimization-based trajectory planners in modeling accuracy, solution robustness, and efficiency.