Использовал готовый бесплатный ассет "A* Pathfinding Project". Это сэкономило много времени на реализацию. Встроенный поиск пути в юнити встревает в углы, даже после тщательной настройки ¯\_(ツ)_/¯
Только не закончил обход юнитов, на релизе, но в ближайшем патче будет многое исправлено