| Initialize grid maps with obstacle matrix |
Mark N(xs, ys) as open_list |
while open_listempty set do |
find the node in open_list with least value f marked as N(xs, ys) |
| mark N(xi, yi) as open_list |
if N(xi, yi) = N(, ) do |
Construct the path |
return “path is found” |
else |
Mark N(xi, yi) as close_list |
generate N(xi, yi)’s 8 successors |
for each successor do |
if successor does not belong to close_list or obstacle_node do |
calculate successor’s value f and marked as f_new = f (successor) |
If f_new is lower than successor’s original value f or successor is not in open_list |
mark successor as open_list |
successor.f = f_new |
set successor’s parent to N(xi, yi) |
end if |
end if |
end for |
end if |
end while |