Cooperative Robot Control

The efficient utilization of cooperating robots in packing or assembly lines relies on balancing the robots' workload and ensuring collision-free trajectory planning. We consider this problem as the distribution of a large number of pick-and-place tasks, where different dynamic objects are involved, to several robots. The considered multi-robot system consists of stationary robots with limited but overlapping operation ranges. To obtain high packing/assembly throughput, the robot manipulators should perform the tasks in minimum possible time without colliding with each other or the direct environment. Therefore, two main tasks need to be addressed:

• optimal robot task allocation and scheduling for cooperative selection of non-stationary objects to balance the robots' workload, while accounting for some safety- and quality-related constraints

• model-based planing of collision-free trajectories for the robots to accomplish the assigned tasks successfully.

Hierarchical Control Algorithm
A hierarchical control algorithm consisting of two layers is employed to assign and execute the robot tasks optimally:

• The upper layer represents the scheduling algorithm. A discrete optimization problem is used to find the optimal allocation of the tasks to the robots (resources) by minimizing the total Euclidian distance covered by the robots' end effectors
• The scheduling layer generates set-points for the underlying layer, which includes a Model Predictive Control based planning algorithm running on top of the computed torque control low
• The computed torque controller as a nonlinear feedback is based on robot dynamic inversion and generates together with the robot dynamics the model used in the MPC algorithm
• The MPC layer computes the collision-free robot trajectories by minimizing the execution time of the tasks and is therefore referred to as time-optimal Model Predictive Control. 

The MPC-based trajectory generation layer can be realized as a centralized architecture common to both robots, or as a distributed architecture where each robot is considered separately, as a local system. 

Centralized Model Predictive Control (MPC)

• Single optimization problem centralized implemented for all subsystems
• Combined dynamic model for both robots
• Large optimization problem
• Joint transition time t_f for both robots

Distributed Model Predictive Control (DMPC)

• Local, robot specific controllers
• Smaller optimization problems
• Different transition times t_1f and t_2f for both robots
• Communication between the different controllers is needed
• Each local controller optimizes a local cost function
• Coupling of the local cost functions to synchronize the robot movements
• Coupling in the constraints for collision avoidance

Problem:
Allocate a set of objects n ∈ Ν to a set of slots s ∈ S, which are located on a set of trays p ∈ P.

Objective:
Minimize the total euclidean distance covered by the robots r ∈ R.

Inputs:

• Position of the objects xⁿ
• Position of the slots x^s
• Position of the robots x^r
• Minimal alloved distance between obejcts d^min.

Binary Variables:

• X_ns = 1, if obect n ∈ N is assigned to slot s ∈ S
• W_rp = 1, if robot r ∈ R is filling tray p ∈ P.
  

• Time-optimal trajectory generation problems result in a free terminal time optimization with the goal of minimizing the final time subject to various robot-relevant kinematic and dynamic constraints.
• We introduse a time scaling τ=(t-t₀)/(t_f-t₀) to transform the free terminal time optimization problem into a fixed time problem.
• The time optimal setting guaranteed that at each optimization step k, the desired state x_f(k)is reached at the end of the prediction horizon k + N, i.e. at τ= 1.
• Three different types of collision avoidance constraints are considered to prevent collisions between a robot's links, the robots and the environment and between the robots

The experimental setup consists of the following components:

• Two UR5 collaborative robots from Universal Robots
• Conveyor belt
• PLC / SPS control unit for speed control of the conveyor belt
• Lidar sensors for environment perseption
• Camera system for measuring / estimating the position and orientation of the objects
• Two grippers
• Pneumatic control unit to control the robot grippers
• Industrial PC and Robot Operating System (ROS) for robot control.