  We deal with the problem of planning collision-free trajectories for robots
operating in a shared space. Given the start and destination position for each
of the robots, the task is to find trajectories for all robots that reach their
destinations with minimum total cost such that the robots will not collide when
following the found trajectories. Our approach starts from individually optimal
trajectory for each robot, which are then penalized for being in collision with
other robots. The penalty is gradually increased and the individual
trajectories are iteratively replanned to account for the increased penalty
until a collision-free solution is found. Using extensive experimental
evaluation, we find that such a penalty method constructs trajectories with
near-optimal cost on the instances where the optimum is known and otherwise
with 4-10 % lower cost than the trajectories generated by prioritized planning
and up to 40 % cheaper than trajectories generated by local collision avoidance
techniques, such as ORCA.
