Optimal Trajectory Planning of Manipulators With Collision Detection and Avoidance

This article presents an optimal trajectory-planning method for robot manipulators with collision detection and avoidance. The obstacles and robot segments are represented by a set of convex polyhedra. The collision detection is performed at each discretized robot configuration by an efficient procedure devel oped with the computational geometry method, which computes a distance function of the robot segments and the obstacles. By introducing this function for specifying the collision-free con straint, the path-planning problem is formulated as an optimal control problem using the augmented Lagrangian, which may be considered as a combination of the duality, penalty and con straint relaxation methods. The problem is solved by a robust UZAWA-like algorithm, where a subgradient method is applied for the primal optimization, as the distance function is not ev erywhere differentiable. An example is given for the trajectory planning of a robot arm with three revolute joints.