Informed RRT*: Optimal Incremental Path Planning Focused through an Admissible Ellipsoidal Heuristic