Ранее я занимался планированием пути на основе поля, но отказался от него в пользу более подходящих подходов к моей проблеме. Он работает адекватно для сред, где у вас точная локализация и точные показания датчиков, но гораздо реже в реальных условиях (это не особенно удачное решение даже с точки зрения скорости и качества трассы, даже при моделировании). Учитывая, что в настоящее время существует множество хороших реализаций SLAM, доступных либо бесплатно, либо по низкой цене, я не стал бы беспокоиться о повторной реализации, если у вас нет особых проблем с повторным использованием. Для MRDS (над чем я работаю) есть Karto Robotics, ROS имеет реализацию SLAM, и есть несколько реализаций с открытым исходным кодом только в поиске Google.
Если вы хотите получить хороший обзор различных подходов к планированию пути, то вам может понадобиться копия «введения в автономные мобильные роботы» Segwart et al. Это довольно хорошая книга, и в разделе «Планирование пути» дается хороший обзор различных стратегий.