0:08

It's common to represent the free see space as a graph where the Nodes represent

configurations and the edges represent free pass between the configurations.

Once we have a graph we need to search it to find a path

between the start configuration and a goal configuration.

The graph shown here has six Nodes.

Let's imagine that the edges connecting them are

roads and the roads may be straight or they may be winding.

We'd like to find a path connecting the start and the goal that

minimizes the cost which is the total path length in this case.

We draw the roads as straight edges but we assign

each edge a weight indicating the length or cost of the road.

For example, going from Node one to Node three has a cost of 18.

We now have a weighted undirected graph, in this video,

I'll focus on an undirected graph but it's

easy to generalize Graph Search to directed graphs.

For this graph, the shortest path has a cost of 30 and

goes from Node one to Node four to Node five to Node six.

To find the lowest cost path we'll use

A* Graph Search one of the most useful graph search algorithms.

A* is used to find lowest cost paths on a graph where the path cost,

is the sum of the positive costs of the individual edges.

In addition to the graph,

A* Search requires a function that calculates an

Optimistic cost-to-go from any Node to a goal

Node an estimate of the cost-to-go is optimistic

if the actual cost-to-go can never be less than the optimistic estimate.

The function that calculates the optimistic cost-to-go should satisfy two criteria.

First, it should be fast to evaluate and second,

the estimated cost-to-go should be reasonably close to the actual cost-to-go.

A function that always estimate zero cost-to-go,

satisfies the fast to evaluate criterion,

but fails the criterion of providing a good estimate.

A function that exactly calculates the cost-to-go by

searching the graph fails the fast to evaluate criteria.

For our example, a good choice for the optimistic cost-to-go function is

one that calculates the straight line distance to the goal as the bird flies,

this is best to evaluate and guaranteed to be optimistic.

For Node one of our example,

the optimistic cost-to-go is 20 and for Nodes two,

three, five, the Optmistic cost-to-go is 10.

With this as background,

we can begin the search process.

Let's create a table to keep track of our progress,

the columns of the table correspond to the Nodes one through six.

First, the past cost refers to the cost of the best known path to this Node,

says Node one is the start Node the past cost zero.

It doesn't cost us anything to get to Node one.

The past costs for all the other Nodes is Infinity since we

don't know yet if there is a path to any of these Nodes.

Next the optimistic cost-to-go is 20 for Node one,

10 for Nodes two, three,

five and zero for Node six,

since it is the goal configuration.

Next to define the estimated total cost to be

the estimated cost of the best solution path that passes through that Node.

The estimated total cost is the sum of the past cost plus the optimistic cost-to-go.

The estimated total cost is 20 for Node one and infinity for all the other Nodes.

Finally, the parent Node of Node I is the previous Node on the best known path to Node

I. Node one does not have

a parent Nodes and we don't know of any paths to any of the other Nodes yet.

Now we define two list,

open which is a list of Nodes to explore from

and closed the list of Nodes we have already explored from.

We initialize the list open with Node one,

the start Node, we also indicate its estimated total cost which is 20.

Now we begin the search by exploring from the first Node in open,

we will explore all edges leading away from Node one,

the first edge goes to Node three,

currently Node three has no parent Nodes so we

update that to indicate that Node three can be reached from Node one.

We also see that the cost of each Node three is 18 so we update the past cost to 18.

This means that the new estimated total cost is 28.

Now we add Node three to the list open and we insert it in

the list in the sorted order according to its estimated total cost, 28.

Next we take the edge from Node one to Node four and we

update Node four to indicate its parent is Node one,

its past cost is 12 and its estimated total cost is 22.

We now insert Node four into open and it goes

before Node three because it's estimated total cost is lower.

Finally, we take the edge from Node one to Node five

and we update Node five to make its parent Node one,

its past cost 30 and its estimated total cost 40.

Then we insert Node five into the sorted list open.

We're done exploring from Node one so we moved Node one to the list closed,

this means we never have to revisit Node one.

Now the first Node on the open list,

the Node with the lowest estimated total cost is Node four,

so we mark it for exploration.

Node four connects the Nodes one, five,

and six but we don't have to consider Node one since it's closed,

so let's take the edge to Node five currently Node five indicates that its parent is

Node one and its past cost is 30 but the cost of the path to Node four is only 20,

the sum of the past cost to Node four plus the cost of the edge from Node four to five.

Therefore, the new best path to Node five

goes through Node four and has a past cost of 20.

We update Node five's information so the past cost is

20 the estimated total cost is 30 and the parent Node is four.

We also reflect that change in Node five's information and in the open list.

Next we take the edge from Node four to Node six,

we update Node six' as information to reflect the past cost an estimated cost

of 32 and the parent Node four and we add Node six to open,

even though Node six is the goal configuration,

our search is not done.

We might still find a lower cost path to Node six in the future.

We are now done exploring from Node four so we move it to closed.

Next we explore from Node three.

First, we take the edge to Node two,

we update Node two's information and we insert Node two in the sorted list open.

Now we take the edge to Node six and we see that the cost of the path

through Node three is 18 plus 15 or 33,

higher than the past cost of an already known path to Node six,

therefore, we can ignore this edge.

Now we're done with Node three so we move it to

closed and we mark Node five for exploration.

The only Node it is connected to that it's not enclosed is Node six,

so let's explore that edge.

The past cost to Node six by path passing through

Node five is the sum of Node five's past cost,

which is 20 plus the cost of the edge from Node five to Node six,

which is 10, the new past cost is 30,

which is less than 32 so we update Node six' information.

Its past cost and the estimated total cost is now 30 and its parent Node is five,

we also update Node six' information in the open.

We're done exploring from Node five, so we move it to closed.

Now the first Node in open is Node six,

which is the goal configuration.

Because of the additive nature of cost,

this goal configuration cannot be reached by

a lower cost path that we'll find in the future,

therefore, the search is done.

We can reconstruct the optimal path by going back to Node six' parent,

Node five then to Node five's parent,

Node four then to Node four's parent, Node 1.

The path shown in green is the optimal path of the graph.

To fully understand this algorithm,

you may need to spend some time

studying it in the book or better yet you should implement it.

The algorithm can be summarized in this pseudo code.

First, we initialize the algorithm then enter a while loop,

where we marked the first Node in the open list for exploration.

If this Node is in the goal region then the algorithm has

succeeded in finding an optimal solution and the algorithm exits.

If not then the algorithm explores

each neighbor in the graph that has not already closed.

For each neighbor Node, the algorithm checks to see if it has found

a new best path to that Node and if so it updates the Nodes past cost,

estimated total cost, parent,

and position in the open list.

If the open list ever becomes empty then

there is no solution to the motion planning problem.

A variation on this algorithm is to always choose the optimistic cost-to-go to be zero,

then the past cost and the estimated total cost are equal.

This algorithm is called Dijkstra's Algorithm which preceded A* historically.

Dijkstra's Algorithm also finds optimal paths but it's typically much slower than

A* because it doesn't use a heuristic cost-to-go to help guide the search.

So you should use a reasonable optimistic estimate if you can.

If you make a mistake and your optimistic cost-to-go function

actually returns an estimate greater than the actual cost-to-go,

A* may terminate with a solution that is not optimal.