Возможно, вам лучше рассмотреть проблему, разбив ее на две части, чем пытаться решить ее с помощью одного алгоритма.
Движение персонажа состоит из двух компонентов: выбор цели высокого уровня и поиск пути, и локальное управление. Поиск пути решает проблему «я здесь, и мне нужно знать, как туда добраться». Местное рулевое управление решает проблему «я еду туда, а кто-то только что встал на моем пути».
Сохраняйте ваш путь, как сейчас. Что вам нужно добавить, так это способность персонажей обнаруживать препятствия во время движения по этому пути, а затем корректировать эту локальную часть пути, чтобы избежать препятствия.
В книге «Искусственный интеллект для игр» (автор сайта: http://ai4g.com/ и Amazon: http://amzn.to/k9K62F) подробно описаны несколько способов сочетания поиска пути с предотвращением столкновений. В этой статье также рассматриваются алгоритмы управления довольно хорошо на высоком уровне. Высокоэффективный метод, который я реализовал, - это управляющий конвейер, также известный как кооперативный арбитраж.
Любой полный ответ будет зависеть от вашего представительства в мире и других факторов, специфичных для вашей реализации, но я надеюсь, что это поможет.