Optimize global path plan