-
Notifications
You must be signed in to change notification settings - Fork 12
Navigating known Terrain: Introduction to Pathfinding
#Introduction
In this section we will go over pathfinding for our robot, and in particular how Dstar Lite works. In order to do this, for those who have not yet taken Algorithms, we will have to introduce some other algorithms and concepts first.
#Dijkstra’s Algorithm
Dijkstra’s Algorithm is a SSSP algorithm, or Single Source Shortest Path, an algorithm which finds the shortest path from one vertex to all other vertices in a graph ( a collection of connected points called vertices (V) related by connections known as edges (E)) Dijkstras algorithm, with out any major modifications will run in O(V^2) time, not horrible for finding the shortest path from one point to every other. The psuedo code for Dijkstras is as follows:
function Dijkstra(Graph[], source):
dist[source] = 0 // Distance from source to source
parents[source] = undefined
PQ.add(source, dist[source]) // PQ is a priority queue
while PQ is not empty:
u = PQ.pop() // Source node in first case
for each neighbor v of u: // where v is still in PQ.
path_cost = dist[u] + cost(u, v) // determined by edge length and weight
if path_cost < dist[v]: // A shorter path to v has been found
dist[v] = path_cost
parents[v] = u
if v is in PQ
PQ.remove(v)
PQ.add(v, path_cost)
end if
end for
end while
return dist[], parents[]
end function
Slightly modified from wikipedia https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm. The algorithm essentially works like this:
step 1: initialize distance for source node in graph, initialize a priority queue (PQ) and add the source node to it
step 2: pop off the item with the minimum distance in the priority queue, if the priority queue is empty return the set of distances calculated, and parents
step 3: for each of the connected nodes still inside in the priority queue, calculate the path cost (cost so far and the cost from the parent node u to child v), if no more children return to step 2
step 4: if the new cost calculated is smaller than the cost currently logged, remove that item from the priority queue if it exists, and add the node with the new key (path cost) and return to step 3
Dijkstra's however, is not great for real time path-finding, i.e. path-finding required to be calculated continuously over the course of the run-time of a program with the possibility of new information given (such as autonomous navigation). In order to be useful for real time applications, we will need to add more parameters to our function, adding a "destination" parameter and stopping computation will help, however the run time complexity remains the same, if your destination is far enough away. For example, if your source and destination are on opposite corners on a uniform weighted square grid, the above function would end up taking the same amount of time as the destination modified function. To significantly reduce the cost for real time applications, we can add another variable for the priority queue key, called a heuristic.
#A*
A* is a modification of Dijkstra’s algorithm that in addition to the distance so far from previous nodes, takes into consideration something called a heuristic to determine which node to select for a path. A heuristic in our case is an additional piece of information that takes into consideration an estimation of the path from node A to B before we've reached our goal, for example, in a square grid we may choose to traverse paths in the direction closest to the straight line path from the current node to the goal. In Dijkstra's, path cost was merely the cost so far to go to the current node, in A* it changes to g(x), the current cost of getting to the given node, + h(x), the cost of getting to that node plus the heuristic function. In the previous example for a heuristic, an example of h(x) might be h(x) = sqrt((x2-x1)^2 + (y2-y1)^2)(C), where x1 and y1 are the current node coordinates and x2 and y2 are the goal node coordinates and C is some constant to multiply the distance formula by to modify its bias (how much the heuristic influences its nodes priority key in the priority queue). Because A is only concerned with a start and goal point, it no longer qualifies for finding the answer to SSSP problems with out constant multiplication (IE you run A* for a bunch of nodes) and thus from now on none of the rest of the algorithms mentioned are considered Single Source Shortest Path algorithms.
The psuedo code for A* is as follows.
function Astar(Graph[], start, goal):
g_score[start] = 0 // Distance from source to source
f_score[start] = g_score[start] + heuristic(start, goal)
parents[start] = undefined
openQ.add(start, f_score[start])
while openQ is not empty:
u = openQ.pop() // Source node in first case
if u = goal
return
for each neighbor v of u: // where v is still in PQ.
path_cost = g_score[u] + cost(u, v) // determined by edge length and weight
if path_cost < dist[v]: // A shorter path to v has been found
g_score[v] = path_cost
f_score[v] = g_score[v] + heuristic(v, goal)
parents[v] = u
if v is in openQ
openQ.remove(v)
openQ.add(v, f_score[v])
end if
end for
closedList.push(u)
end while
return g_score[], parents[]
end function
function reconstruct_path(graph[], parents[],current)
total_path.add(graph[current])
while current in parents[]:
current = parents[current]
total_path.append(current)
return total_path
Slightly modified from wikipedia https://en.wikipedia.org/wiki/A*_search_algorithm. The algorithm follows Dijkstra's with the slight modifications of A: stopping once the goal has been found, and B: implementing an additional weight to the priority queue with the heuristic, as explained above, instead of merely using distance costs, f_score, distance cost + heuristic prediction is factored in. In addition, a new function that takes in the parents[] array, reconstruct_path, creates a new path by using the goal with the previous shortest path parent.
But what happens with A* graphs that can change through out the life time of the program (for example, an obstacle has been suddenly put in a robots way)? In order to deal with this we need to introduce a new concept, a RHS value.
#Life Long Planning A*
Life long planning A* (LPA*) modifies A* by taking into account a new parameter of distances from parent to child nodes. This value, the RHS value or right hand side value, is essentially only a different conceptualization of the distance so far to a parent node, and the distance between a parent and a child. With RHS we introduce the new idea of inconsistencies. The idea is that g(x) = path_cost_parent + cost_from_parent_to_x, and rhs(x) = g(x') + cost_from_parent_to_x, where x' is the parent and should be the pas cost parent. In most situations rhs(x) = g(x) and there for is locally consistent, but if the path changes dynamically?
here is an example situation
x'--B-->x g(x') = A, g(x)= A + B, where B is the edge cost
say we change the edge cost B in between x' and x to be infinity, IE the path is blocked.
x'-INF->x
If we go back and compare the RHS value to g(x) we will find that g(x) != rhs(x) where g(x) = A+B and rhs(x)= A + INF, this means node is locally inconsistent.
There are two types of inconsistencies a node can take on. A node can be under consistent, where g(x) < rhs(x), this is the case described above, where a new obstacle as been placed in the way. A node can also be over consistent, where g(x) > rhs(x) which means that perhaps a path was cleared between a parent and child node.
The algorithm is as follows:
function calculatekey(vertex v)
key_pair.k1 = min(g(v), rhs(v)) + hueristic(v, goal)
key_pair.k2 = min(g(v), rhs(v))
return key_pair
function initialize()
rhs(start) = 0
g(start) = INF
key_pair.k1 = hueristic(start)
key_pair.k2 = 0
PQ.add(start, key_pair)
function updatevertex(vertex u)
if(u != start)
rhs(u) = INF
for(all predecesors of u, u')
temp = g(u') + cost_from(u', u)
if(rhs(u) > temp)
rhs(u) = temp
if(u in PQ)
PQ.remove(u)
if(g(u) != rhs(u))
PQ.add(u, calculatekey(u))
function computeshortestpath()
while(PQ.topkey < calculatekey(goal) or rhs(goal) != g(goal)) // top key < goal key, or goal not consistent
u = PQ.pop()
if(g(u) > rhs(u))
g(u) = rhs(u)
for all successors s' of u
updatevertex(s')
else
g(u) = INF
for all successors s' of u and u itself
updatevertex(s')
function main()
initialize()
while(true)
computeshortestpath()
if edge costs change
for all directed edges (u to v) which changed edge costs
update the edge cost cost_from(u, v)
updatevertex(v)
Psuedo code slightly modified from Sven Koeng, Maxim Likhachev, and David Furcy's Lifelong Planning A* paper(2002).
The basic overview of the algorithm is very similar, but there are a few key differences. One major difference is RHS value consideration of consistency (whether g(x) <, > or = to rhs(x)), another is the use of a key with two variables, this will be expanded upon later, but it is important to prioritize heuristic distances vs rhs value when deciding node priority. The steps are as follows
Step 1: initialize the starting rhs value and g value (distance so far to current point along path from start to goal) for starting index to 0. Initialize the priority queue and add the starting vertex with the appropriate key
Step 2: continuously, while the top key in the PQ is less than the goal, or the goal node is not consistent, pop off the top value in the priority queue set it equal to the current node. Otherwise continue to step 4.
Step 3: if the current node is over consistent set g(current node) = rhs(current node), which is to say set g(x) to the smaller calculated path value, rhs(current node), and update all subsequent vertices. Otherwise, if the node is under consistent or consistent reset g(current node) to infinity (unset or blocked) and update the vertex for all successors of the current node and the current node. return to step 2
Step 4: if there are changes to edge cost, for all edges update the edge cost from u to v, and update the vertex(v)
special note on how update vertex works
if the current node is not the start node, set the right hand side value to the smallest cost path from any predecessor (or parent) to the current node in order to reset the value, we need to do two things first remove the node from the priority queue through a remove function, since u is not guaranteed to be at the top, simply popping the value will not work, a special function for removing the value is required (similar to what was needed in the previous algorithms, though I didn't mention it before). Then we need to insert the value back in if the node back in with a new key if the value is not consistent.
LPA* however, while fixing some of the problems of Dijkstra's and A*, is not a very good algorithm for a robot on the move. It focuses on a start and end point, then accounting for changes afterwards when a path has been found. The next step is to modify this algorithm for use in travel changes.
#D* lite
Dstar lite (D* lite) is a direct modification of LPA*, created by Sven Koenig and Maxim Likhachev. There a few major differences Dstar lite implement on top of life long planning A star, first the start node becomes the current node as the robot or entity advances a long the path, instead of recalculating heuristics for each time edges change, heuristics are added to newly calculated keys. Additionally, g(x), the distance for a given node is no longer in terms of start distance, but instead from goal distance, in particular, from the goal to the current node, this causes virtually all references to predecessors, start, and goal nodes to be swapped with their appropriate opposites (goal to start, or predecessor to successor)
Below is the pseudo code for the algorithm
function calculatekey(vertex v)
key_pair.k1 = min(g(v), rhs(v)) + hueristic(start, v) + Km
key_pair.k2 = min(g(v), rhs(v))
return key_pair
function initialize()
Km = 0;
rhs(goal) = 0
g(goal) = INF
PQ.add(goal, calculatekey(goal))
function updatevertex(vertex u)
if(u != goal)
rhs(u) = INF
for(all successors of u, s')
temp = g(s') + cost_from(u, s')
if(rhs(u) > temp)
rhs(u) = temp
if(u in PQ)
PQ.remove(u)
if(g(u) != rhs(u))
PQ.add(u, calculatekey(u))
function computeshortestpath()
while(PQ.topkey < calculatekey(start) or rhs(start) != g(start)) // top key < goal key, or goal not consistent
Kold = PQ.topkey
u = PQ.pop()
if(Kold < calculatekey(u))
PQ.add(u, calculatekey(u))
else if(g(u) > rhs(u))
g(u) = rhs(u)
for all predecessors u' of u
updatevertex(u)
else
g(u) = INF
for all predecesors u' of u and u itself
updatevertex(u)
function main()
start_last = start
initialize()
computeshortestpath()
while(start != goal)
//if g(start) = INF, no known path
start = arg INF
for(all successors of start, s')
temp = g(s') + cost_from(start, s')
if(start > temp)
start = temp
move to start
scan graph for changed edge costs
if costs changed
Km = Km + hueristic(slast, start)
start_last = start
for all directed edges (u to v) which changed edge costs
update the edge cost cost_from(u, v)
updatevertex(v)
computeshortestpath()
Again, this code is slightly modified from Sven Koenig and Maxim Lkhachev's paper D*lite (Fast replanning for navigation in unknown terrain, 2005)
The steps of the pseudo code are as follows:
Step 1: initialize the last starting position, and initialize the Km value and rhs value to zero for the goal node, and g(goal) to infinity, then add the node to the priority queue.
Step 2: while the top key of the priority queue is less than the key for the starting position, or the starting node is not consistent,
old key = priority queue top key, pop the priority queue and set that to the current value if the old key is smaller than the new calculation for the current node, the node is inconsistent, and thus needs to be re-inserted else if the current node is over-consistent, set g(u) = rhs(u), and update the vertex of all parent nodes from current node otherwise reset g(u) and for all predecessors of u and u itself update the vertex.
Step 3, while our starting position is not the goal node,
start = the minimum of all (c(start, successors) + g(successors))
move to start
if graph now has changed edge cost
km += heuristic(last start, start), necessary to account for new edge changes and make sure that priority order is maintained for proper priorities when it is used in calculatekey() where it is used in the first part of the key.
start last = start
for all edges u,v, which changed edge costs, update the costs c(u,v), and update the vertex u.
compute the shortest path (step 2) and return to the beginning of step 3
Updatevertex has been changed to reflect the change in reference for heuristics (from goal to start instead of the opposite) updatevertex is now outlined below
if the current node is not the goal, RHS of the current node is now the minimum of (cost from current node to successor + g(successor) if the current node is in the priority queue, remove, and if it is inconsistent, re-insert the node in with the new calculated key.
Calculatekey itself has also changed, but only slightly, instead of key.k1 being the minimum of g(u) and rhs(U) plus the heuristic value for u, it is the same but with the Km constant value added along with the heuristic.
Finally we have a algorithm that we can actually use for robotics, instead of precomputing a path from the start, you make assumptions about the path from the goal to the robots current position, which informs the robot about the shortest distances from adjacent positions to the goal. an example of how this would work is first we get information of where we are and where the goal is. In a square grid, where we can move in all eight cardinal/diagonal distances we assume we can only see the adjacent 8 positions and our own, so we have no idea if anything blocks us beyond our short sighted vision around our robot and past traversal. We compute the shortest path based on where we know the goal is and move to the position adjacent to us with the smallest priority according to our priority queue. If we moved directly forward we would be able to see three new positions that we didn't have vision of before, this triggers a edge cost change and we update the corresponding edges, and calculate the shortest path again, rinse and repeat the process. With D* we can finally navigate our robot in unknown terrain. There is just one tiny problem.
#Field D* lite
Unfortunately our robot can only traverse graphs with discrete nodes, if we want to make our robot travel in the real world we will have to discretize our map, our world, and when actually traversing the world we will have to move and treat the world like it is a series of grid tiles or another conceptualization of a discrete unit movement system. We need an algorithm that can deal with non discrete terrain with better approximations, something that can work in the real world, this is where Field Dstar, which uses Dstar lite as its framework, comes in.
Field Dstar focuses on using the properties of linear interpolation to create a better approximation of paths along discrete grid interpretations of the world the robot lives in. It is a compromise between the accuracy of fully continuous navigation calculation and the speed at which approximating our world with grid nodes provides us.
Algorithm below Pseudo code, formulas and definitions slightly modified from Dave Ferguson and Anthony Stentz's Dstar: An Interpolation-based Path Planner and Replanner
First some definitions and calculations are needed to paste together the pseudo code
node s, is a node that lies on the cross-section of four square grid tiles, as do all nodes in this method
edges are defined as the real square edges connecting two corners, the costs of these edges are determined by the minimum traversal costs of the two adjacent cells.
sx->sy is the edge composed of two adjacent nodes neighboring s, ie if s is in the cross section of four squares, there are eight possible sn nodes, but any sx and sy pair would appear as sn, sn+1, for example, s1, s2 or s3, s4.
c is the center cell, or the cell in which the sx->sy pairs define an edge for. b is the bottom cell, or the cell opposite to the c cell, either bottom or top.
g(x), like defined previously is the minimum of (cost of path between(x to x predecessor) + g(x predecessor)
Sy is a point living on the edge between a sx and sy pair, for example s1 and s2
Y is the distance from s1 to Sy
UNIT = the length of a cell side, unit cells are presumed to be one long, typical for discrete terrain
g(Sy) = (Y)g(s2) + (UNIT - Y) g(s1) In other words the distance for Sy is the distance between s1 and s2, multiplied by the node distance for s2, adding (UNIT-Y) multiplied by the node distance for s1.
To calculate the cell costs given this interpolation we can use the function below x here is a number from 0 to UNIT which represents distance traveled along the x axis edge before cutting across the center node to reach y, a number from 0 to UNIT, which is where the line intersects on the y axis edge.
for all x and y, mininum of( bx + c * sqrt((1-x)^2 + y^2) + y*g(s2) + (1-y)g(s1))
with linear interpolation we force the minimum x and y that satisfy the above equation to be either 0 or 1 for at least one of the pair. cutting across diagonal in the center cell will result in a x = 0, and y = 1, crossing from corner to corner, and if nothing passes through the center y = 0 and the path will be along the x axis.
Because of our approximations with linear interpolation, where at least one of the x and y values needs to be zero or one, it forces the types of paths from s through edge s1,s2 or sx,sy to be either directly along the bottom edge (x = 1, y = 0), go straight and then to the top corner, s2 (x = n, y = 1), or straight diagonally across from s to s2 (x = 0, y = 1), x = 0 and y = 0 work, but don't make physical sense in our situation, similarly with x = 1 and y = 1, where a diagonal would make sense.
Choosing the right path is determined by the cost of traversal for c and b, and the difference between the path costs for sx and sy, in our examples, s1 and s2.
We define f = g(s1) - g(s2), if f < 0, where the top node (s2) has a higher cost then our path goes along the x axis to s1 the cost being minimum of (c, b)(due to the edge cost we discussed earlier) + g(s1) if f = b, or the difference between the cost of s1 and s2 is the same as the cost of traversal for b, the cost of using part of the bottom edge is the same as using none of the bottom edge since the traversal cost ends up being the same from node to node.
Ferguson and Stentz derive the following equation for that situation creating a new variable, k, which is equivalent to k = f = b the cost of the path through edge s1 to s2 is now (c)sqrt(1+y^2) + k(1-y) + g(s2)
the derivative with respect to y becomes sqrt(k2/(c^2 - k^2)) = y'
if f < b, then the cost is simply cheaper to traverse accross the edge s1 to s2, if f > b, then the bottom edge is used, resulting in the cost path through edge equation changing variables, k is swapped with b and the derivative becomes y' = 1-x'.
With these foundations we can finally construct our computecosts function
function computecost(s, sa, sb) //assuming unit cost of one, where one is here replace with
//your grid unit costs, and multiply the derivative accordingly.
if(sa is the diagonal neighbour of s)
s1 = sb
s2 = sa
else
s1 = sa
s2 = sb
c is the centre cell, ie the cell with corner nodes s, the current node, s1, s2
b is the opposite cell, ie the cell bellow or above with nodes s and s1 but not s2
if(min(c,b) = INF) // unset or blocked
cost_s = INF
else if( g(s1) <= g(s2))
cost_s = min(c,b) + g(s1) //no point in continuing, going in-between the nodes gives
//no useful advantage
else
f = g(s1) - g(s2)
if(f <= b) //following our discussion of f, k and b, if the path with b, the bottom/top cell
//is too much
if(c <= f) //if the cost of going through c is less than going through the edge itself
//we end up just going to s2
cost_s = (c)sqrt(2) + g(s2) //traversing across diagonal, hence multiplication of
//centre cell cost times the sqrt(2), heading to g(s2)
else
y = min(f/(sqrt(c^2 - f^2)), 1) //chose the derivative or one
cost_s = (c)sqrt(1+y^2) + (f)(1-y) + g(s2)
else
if(c <= b)
cost_s = (c)sqrt(s) + g(s2)
else
x = 1 - min(b/sqrt(c^2 - b^2), 1) //chose the b-swapped derivative or one
cost_s = c sqrt(1 + (1-z)^2) + (b)(x) + g(s2)
return cost_s;
To implement this, we need only to replace the cost function (g(x) + cost from (x to x')) with this new cost function, and then update the traversal costs on corners with update vertex instead of the centers which are no longer the nodes.
With this we will finally be able to create efficient paths with navigation code using Dstar lite, paths that tend to avoid Manhattan distance style traversal and inefficient turning in routes. There are other algorithms that further advance robotic path-finding beyond this, or are simply different than the end algorithm this page leads to, however field D* has been used in multiple autonomous vehicles in the past, vehicles you can see showcased in Dave Ferguson and Anthony Stentz's Dstar: An Interpolation-based Path Planner and Replanner.