Survey of Robot 3D Path Planning Algorithms

Algorithm 5

Dijkstra’s algorithm.

is the priority queue; is the goal;

return the cost to apply action from ;

is the best cost-to-come known so far;

is the finite action space;

is a state transition function;

(1) ;

(2) ;

(3) while not empty

(4)  

(5)  for all

(6)   

(7)   if  

(8)   

(9)   if

(10)  

(11)   END; END

(12) END; END;