An Algorithm for Autonomous Formation Obstacle Avoidance