Dynamic model based formation control and obstacle avoidance of multi-robot systems