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

• Published in: IEEE access Vol. 13; pp. 107657 - 107668
• Main Authors: Pan, Lijuan, Zhang, Zhenhui, Yin, Zhuyan, Li, Bai
• Format: Journal Article
• Language: English
• Published: IEEE 2025
• Subjects: Bevel-tip needle insertion; collision avoidance; computational optimal control; Cost function; Genetic algorithms; interior point method; Kinematics; Needles; nonlinear program; Optimal control; Splines (mathematics); Surface reconstruction; Surface topography; Trajectory; Trajectory planning
• ISSN: 2169-3536; 2169-3536
• DOI: 10.1109/ACCESS.2025.3580645

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 <inline-formula> <tex-math notation="LaTeX">{\mathrm {A}}^{\ast } </tex-math></inline-formula> 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.