A Motion Planning Algorithm Based on Trajectory Optimization with Workspace Goal Region

We consider a motion planning problem with workspace goal region in obstacle environments and the task is to move the end-effector into a specific goal region without posture constraints. In order to improve the quality of the planning trajectory, we present a goal region constraint algorithm based on Gaussian process motion planning. We construct a distance field for the irregular goal region in advance. The closest distance and direction from any location of the workspace to the specific goal region can be easily computed. Combined with the original method, we define a goal-region-constrained likelihood which specifies the probability that the position of end-effector is within the specific goal region and move the end-effector to a better position by numerical optimization. Finally, multiple simulation experiments are carried out and the results show that the proposed algorithm can quickly plan an obstacle avoidance trajectory in joint space and the quality of the trajectory is improved effectively compared to randomly specifying a goal configuration.