arxiv Robust and Efficient Trajectory Planning for Formation Flight in Dense Environments