Also ich kann dir da mit dem Begriff "Algorithmus A*" (sprich: "A Stern") vielleicht weiterhelfen. Der soll auch Hindernisse erkennen, braucht zwar ein ziemlich großes Konstrukt, ist aber ein sog. "idealer Algorithmus"..
Vielleicht hilft dir auch
http://de.wikipedia.org/wiki/A*