Path-constrained optimal trajectory planning for robot manipulators with obstacle avoidance

In this paper, we develop a novel path-constrained and collision-free optimal trajectory planning algorithm for robot manipulators in the presence of obstacles for the following problem: Given a desired sequence of discrete waypoints of robot configurations, a set of robot kinematic and dynamic constraints, and a set of obstacles, determine a time and jerk optimal and collision-free trajectory for the robot passing through the given waypoints with constant speed. Our approach in developing the robot path through the waypoints relies on the orthogonal collocation method where the states are represented with Legendre polynomials in the Barycentric form; the transcription process efficiently converts the continuous-time formulation of the optimal control problem (both time and jerk optimal) into a discrete non-linear program. In addition, we provide an efficient method for avoiding robot self-collisions (of joints and links) and collisions with workspace obstacles by modeling them as the union of spheres and cylinders in the workspace. The resulting collision free optimal trajectory provides smooth and constrained motion for the robot passing through all the waypoints in the given prescribed sequence. The proposed method is validated using numerical simulations and experiments on a six degree-of-freedom robot.