Hierarchical real-time optimal planning of collision-free trajectories of collaborative robots

Abstract

In collaborative robotics the manipulator trajectory has to be planned to avoid collisions, yet in real-time. In this paper we pose the problem as minimization of a quadratic functional among piecewise linear trajectories in the angular (joint) space. The minimization is subjected to novel nonlinear inequality constraints that simplify the original non-penetration constraints to become cheap to evaluate in real time while still preserving collision-avoidance. The very first and most critical step of the computation is to find an initial trajectory that is free of collisions. To that goal we minimize a weighted sum of the violated constraints until they become feasible or a maximal number of steps is reached. Sometimes an incremental growing of the obstacle helps. By incremental growing we mean that we sequentially solve auxiliary subproblems with obstacles growing from ground or falling from top and use as the initial trajectory the one optimized in the previous step. The initial trajectory is then optimized while preserving feasibility at each step. We solve a sequence of simple-bound constrained quadratic programming problems formulated in the dual space of Lagrange multipliers, which are related to the original linearized inequality constraints that are active or close-to-active. Finally, we refine the trajectory parameterization and repeat the optimization, which we refer to as an hierarchical approach, until an overall prescribed time limit, being well below a second, is reached.