Generation of Probabilistically Optimal Paths for Nonholonomic Needle Insertion

Abstract

Rapidly-exploring Random Tree (RRT) is a sampling-based algorithm which is designed for path planning problems. It is efficient to handle high-dimensional configuration space (C-space) and nonholonomic constraints. Under the nonholonomic constraints, the RRT can generate paths between an initial state and a goal state while avoiding obstacles. Since this framework assumes that a system is deterministic, more improvement should be added when the method is applied to a system with uncertainty. In robotic systems with motion uncertainty, probability for successful targeting and obstacle avoidance are more suitable measurement than the deterministic distance between the robot system and the target position. In this paper, the probabilistic targeting error is defined as a root-mean-square (RMS) distance between the system to the desired target. The proximity of the obstacle to the system is also defined as an averaged distance of obstacles to the robotic system. Then, we consider a cost function that is a sum of the targeting error and the obstacle proximity. By numerically minimizing the cost, we can obtain the optimal path. In this paper, a method for efficient evaluation and minimization of this cost function is proposed and the proposed method is applied to nonholonomic flexible medical needles for performance tests.