Я перечисляю все контрольные точки, потом вычисляю расстояния по прямой между ними. Добавляю штраф за каждую остановку, суммирую это с расстоянием и выбираю маршрут с наименьшей общей суммой. Просто математика, без всяких заморочек.