Path-Constrained and Collision-Free Optimal Trajectory Planning for Robot Manipulators

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. 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 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 with a constant speed. The proposed method is validated using numerical simulations and experiments on a six degree-of-freedom robot. Note to Practitioners—This paper is motivated by planning collision-free optimal trajectories with constant Cartesian speed (norm of translation velocity) along a given list of waypoints. The primary applications include developing constant Cartesian speed trajectories for robotic surface finishing operations, spray painting operations and robot endurance testing. Sampling-based motion planning algorithms have been widely used for their high efficiency and robustness. However, those methods in general do not take into account the joint level constraints, motion jerk, robot dynamic model and kinematic constraints together. In addition, with these algorithms, it is difficult to generate a trajectory along a list of waypoints while maintaining a constant Cartesian speed. We provide an efficient robot trajectory planning algorithm for articulated robots that is capable of achieving time and jerk optimality while avoiding obstacles and satisfying robot kinematic and dynamic constraints. The scope of this work is limited to considering only static obstacles and pre-defined Cartesian waypoints; potential extensions include consideration of dynamic obstacles and incorporating tighter bounds for objects modeled by cylinders and spheres so that a larger workspace is available for trajectory planning, etc.