Optimal routing for autonomus systems