Algorithms

Note

• This page contains the pseudo code outlining the algorithms implemented.
• An effort has been made to implement the algorithm in a way that allows easy correspondence with the pseudo code, facilitating learning.

Important

• The pseudo code is written based on the original papers from which the algorithms are derived; slight modifications might be made, while retaining the essence of the algorithm.
• Minor modifications have been made to the implementation of Dijkstra to highlight it’s similarity of A*.
• Using the grid on which the algorithm runs to maintain cost would make the speed up the algorithms significantly for both A* and Dijkstra, and remove the necessity for using the closed list, maintaining cost in the open list.

Todo

• Highlight changes between pseudocode and implementation

Contents

• Algorithms
  • Dijkstra
  • A*
  • LPA*:
  • D* Lite:
  • RRT
  • RRTStar
  • Ant Colony Optimization
  • Genetic Algorithm

Dijkstra

procedure main()

OL = 0 //Initialize the open list. Priority queue sorted by cost

CL = 0 //Initialize the closed list

OL <— s_start

while(!OL.empty())

q <— OL.pop()

for all s in Succ(q)

if(s=s_goal) return path found

if(s is obstacle) continue

s.parent <— q

s.cost = q.cost + distance between successor and q

if(s` = s in OL && s`.cost < s.cost)

continue

if(s` = s in OL && s`.cost < s.cost)

continue

OL <— s

CL <— q

return no path

A*

procedure main()

OL = 0 //Initialize the open list. Priority queue sorted by cost + heuristic cost value

CL = 0 //Initialize the closed list

OL <— s_start

while(!OL.empty())

q <— OL.pop()

for all s in Succ(q)

if(s=s_goal) return path found

if(s is obstacle) continue

s.parent <— q

s.cost = q.cost + distance between successor and q

s.h_cost = heuristic(s, s_goal)

if(s` = s in OL && s`.cost + s`.h_cost < s.cost + s.h_cost)

continue

if(s` = s in CL && s`.cost + s`.h_cost < s.cost + s.h_cost)

continue

OL <— s

CL <— q

return no path

LPA*:

procedure CalculateKey(s)

return [min(g(s), rhs(s)) + h(s_start, s); min(g(s),rhs(s))];

procedure Initialize()

U = 0;

for all s in S rhs(s) = g(s) = inf

rhs(s_start) = 0;

U.insert(s_start, CalculateKey(s_start));

procedure UpdateVertex()

if(u!=s_start) rhs(s) = minfor all _{s’ in Succ(u)}c(u, s’) + g(s’)

if(u in U) U.remove(u)

if(g(u)!=rhs(u)) U.insert(u, CalculateKey(u));

procedure ComputeShortestPath()

while(U.TopKey() < CalculateKey(s_goal) OR rhs(s_goal) != g(s_goal))

u = U.Pop();

if(g(u) > rhs(u))

g(u) = rhs(u);

for all s in Succ(u) UpdateVertex(s);

else

g(u) = inf;

for all s in Succ(u) (union) {u} UpdateVertex(s)

procedure Main()

Initialize();

forever: // This runs for n times by default in current implementation

ComputeShortestPath();

Scan graph for changed edge costs;

if any edge cost changed // this occurs with probability 1/n for every iteration

for all directed edges (u,v) with changed edge costs

Update the edge cost c(u,v);

UpdateVertex(u)

D* Lite:

procedure CalculateKey(s)

return [min(g(s), rhs(s)) + h(s_start, s) + k_m; min(g(s),rhs(s))];

procedure Initialize()

U = 0;

k_m= 0;

for all s in S rhs(s) = g(s) = inf

rhs(s_goal) = 0;

U.insert(s_goal, CalculateKey(s_goal));

procedure UpdateVertex()

if(u!=s_goal) rhs(s) = minfor all _{s’ in Succ(u)}c(u, s’) + g(s’)

if(u in U) U.remove(u)

if(g(u)!=rhs(u)) U.insert(u, CalculateKey(u));

procedure ComputeShortestPath()

while(U.TopKey() < CalculateKey(s_start) OR rhs(s_start) != g(s_start))

k_old= U.TopKey();

u = U.Pop();

if(k_old< CalculateKey(u))

U.insert(u, CalculateKey(u))

else if(g(u) > rhs(u))

g(u) = rhs(u);

for all s in Pred(u) UpdateVertex(s);

else

g(u) = inf;

for all s in Pred(u) (union) {u} UpdateVertex(s)

procedure Main()

s_last= s_start

Initialize();

ComputeShortestPath();

while(s_start!=s_goal)

/ if(g(s_start)=inf) there is no known path /

s_start= arg min<sub>for all s’ in Succ(s:sub:`start</sub>)`(c(s_start,s’)+g(s’)

Move to start

Scan graph for changed edge costs;

if any edge cost changed

k_m= k_m+ h(s_last,s_start);

s_last= s_start

for all directed edges (u,v) with changed edge costs

Update the edge cost c(u,v);

UpdateVertex(u)

ComputeShortestPath()

RRT

procedure main()

T = 0; // Initialize list/tree

for k = 1 to K

q_rand← GenerateRandomNode()

q_parent← FindNearestPoint(q_rand, T, threshold) // Returns closest node based on distance

if(q_parentexists)

q_parent<— q_parent

if(q_rand= goal) return path found

return no path

RRTStar

procedure rewire()

for all node in close_node_list != q_parent

if(q_rand.cost + dist(node ,q_rand) < node.cost)

node.parent <— q_rand

node.cost <— q_rand.cost + distance(node, q_rand)

procedure main()

T = 0; // Initialize list/tree

for k = 1 to K

q_rand← GenerateRandomNode()

q_parent, close_node_list ← FindNearestPoint(q_rand, T, threshold) // Returns node that results in lowest total cost and list of all close nodes // Updates q_rand.cost to q_parent.cost + distance(q_rand, q_parent)

if(q_parentexists)

q_parent<— q_parent

if(q_rand= goal) return path found

rewire()

return no path

Ant Colony Optimization

procedure main()

Initialize ant colony

for each iterations

initialize ants

for each ant

while ant position != goal and number of steps < max steps

calculate probabilities of possible next edges to traverse

choose next position based on probability

if not next positions possible, break

if reached revisited points, remove loop and reduce number of steps taken

update pheromone trail

update ant in ant colony

update pheromone trail by evaporation equation

update pheromone trails based on ants that reached the goal

if path found return last best path

else return no path

Genetic Algorithm

procedure main()

Initialize paths

for each generation

choose paths based on criteria

mutate paths (using CrossoverMutation()) to find new paths if number of paths less than set number

for each path

calculate cost and validity of path

if path valid add to paths list

if path found return last best path

else return no path

procedure CrossoverMutation()

randomly choose 2 chromosomes (paths) in path list

initialize new chromosome

for each gene in chromosome

choose either the gene from the first chromosome, form the second chromosome or a random gene based on probabilities set

add this gene to new chromosome

return new chromosome