Hi everyone and welcome to

the first lesson of week three.

In this module, we'll be discussing

the mission planning problem in

autonomous driving and how to solve it.

If you recall from Module 1,

the autonomous driving mission

is the highest level portion of

our motion planning task and it's crucial

for navigating the autonomous car to its destination.

In this video, we'll introduce the mathematical concept

of a graph and how it can be used in our mission planner.

In addition, we will discuss how a graph can be used to

represent the road network that

we are required to navigate through.

Finally, we'll discuss

the Breadth-First Search algorithm and

how it can be applied to

mission planning. Let's get started.

First, let's recall the autonomous driving mission.

The objective of the autonomous

driving mission is to find

the optimal path for

the eagle vehicle from its current position to

a given destination by navigating

the road network while abstracting away

the lower-level details like the rules of

the road and other agents

present in the driving scenario.

In this module, we will think of optimality in terms of

the amount of time or distance it

takes for the car to reach its destination.

For autonomous driving, mission planning

is considered the highest level planning problem.

This is because the spatial planning scale

of the mission planner is on the order of kilometers

and the mission planner doesn't focus on

low-level planning constraints such

as obstacles or dynamics.

Instead, the mission planner will focus on

aspects of the road network when planning,

such as speed limits and road lengths,

traffic flow rates and road closures.

Based on these constraints posed to us by the map,

the mission planner needs to find

the optimal path to our required destination.

One thing to note about

the road network is that it is highly

structured which is something we can

exploit in our planning process to simplify the problem.

By exploiting the structure,

we can efficiently find the optimal path to

our destination based on the map given to us.

To do this, we will need to use

a mathematical structure known as a graph,

which we've overlaid onto our road network here.

So what is a graph?

A graph is a discrete structure

composed of a set of vertices denoted as

V and a set of edges denoted

as E. For the mission planner,

each vertex in V will

correspond to a given point on the road network,

and each edge E will correspond to

the road segment that connects

any two points in the road network.

In this sense, a sequence

of contiguous edges in the graph

corresponds to a path through

the road network from one point to another.

An example of a graph is shown here.

For now, we will assume

each road segment is of equal length,

so the edges of this graph are all unweighted.

However, in future lessons,

we will relax this restriction.

To generate a graph such as this,

the road network needs to be discreetly sampled,

which will give us the vertices of our graph.

The edges will then be

defined by the segments of the road

that connect each sample point

according to the rules of the road.

Note that in general,

just because point A is

adjacent to point B using a road segment,

it does not mean that point A can be

reached from point B from that same road segment.

This is because in many cases there is

only one direction that

a road segment can be legally traversed.

In this sense, the edges of our graph are directed

in that the edge is only traversable in one direction.

We've denoted this by using arrows for

our edges in the graph to display their directionality.

Now that we have are directed graph,

how do we find an optimal path to our destination?

First, we locate the vertices in the graph that

correspond to our current eagle vehicle position which we

will denote as S and

our desired destination which we will

denote as t. These two vertices

are shown on the graph here.

Once we have these two vertices,

we can use an efficient graph search algorithm to

find the optimal or shortest path to our destination.

Since our graph formulation is currently unweighted,

a good candidate algorithm is

the Breadth-First Search or BFS.

At a high level, BFS can be thought

of as iterating through all of

the vertices in the graph but

doing so in a manner such that

all adjacent vertices are evaluated

first before proceeding deeper into the graph.

In this sense, the graph search proceeds like

a moving wavefront through the graph or breadth-first.

Let's walk through the steps of the BFS algorithm.

We construct three data structures to aid in

our search and open queue

of vertices still to be assessed,

a closed set of vertices that

have been assessed by the search algorithm

and a dictionary of

predecessors which store the results of the search.

A queue is a first-in-first-out data structure,

such that the first vertex pushed or added to

the queue is the first one

popped off or returned from the queue.

A dictionary is an unordered set of

key-value pairs and for each node in the closed set,

stores a predecessor vertex

that will identify momentarily.

The algorithm starts by adding our

start vertex to the open queue.

Then, while the open queue contains vertices,

we take the first element from

the open queue and check if it is the goal location.

If so, we found our shortest path.

If not, we then add all adjacent vertices not

already in the open queue or

closed set to the open queue.

This prevents us from getting

stuck in cycles during the graph search.

Note that by adjacent,

we mean all vertices that can

be reached from the current vertex.

Because we use a queue to store open vertices,

we ensure that all adjacent vertices

at the current depth in

the search are processed

before proceeding deeper into the graph.

So all vertices that are

one step away from the start vertex

will be processed before moving

on to vertices that are two steps away.

As a vertex is added to the open queue,

we store its preceding vertex

in the predecessor dictionary.

This will help us reconstruct

the optimal path once the goal is found.

Finally, we add the currently active vertex to

the closed set and return to

the next element of the open queue to process.

Because of the breadth-first nature of the BFS algorithm,

by the time we reach the goal vertex,

we have processed all possible predecessor vertices of

the golden vertex that are closer to

the start vertex then the goal vertex.

This means that when we reach the goal vertex,

we have found the shortest path to the goal

vertex and we can terminate the algorithm.

To solidify our understanding of this algorithm,

let's work through a concrete example.

Suppose our mission planner needs to

find the optimal path from point s to

our destination t through the set of

vertices in our graph which are now labeled,

the first step would be to process the origin

s and add all adjacent vertices to

our queue and set their predecessors to

s. The outgoing edges that

lead to the adjacent vertices are highlighted in blue.

Once we've added these to our queue,

we then add s to our closed list.

Next, we pop off vertex a.

The outgoing edges of a lead to d and b,

but b is already in our queue, thus,

we only add d to the queue and mark a as its predecessor.

We've highlighted this duplicate path to be in

red to show that we do not add b to the queue twice.

We've now processed all adjacent edges

from a and move it to the closed set.

We repeat the same process for

b which adds E to the queue with b as

its predecessor and c which has

no new adjacent vertices so

does not add vertices to the open queue.

Next, we process d from the queue,

which only adds t to the queue with

d as its predecessor since e has already been added.

When e is popped off,

it doesn't add c or d to the queue because both of

these vertices have already been

processed and are present in the closed set.

Finally, we pop off t from the queue.

This is our goal vertex.

So we now reconstruct the path from s to t by following

the chain of predecessors from t

back to s. Once this is done,

we've found the optimal path to our destination,

which is highlighted in green.

The sequence of edges

corresponding to our optimal path in

the graph can be turned into

a root over the road network using our map,

which can then be used to govern

more detailed motion planning in

the subsequent layers of our planning hierarchy.

Before we wrap up this video,

we should note that there is also

the highly related Depth-First Search algorithm

among many others.

Depth-First Search uses a last-in,

first-out stack instead of a queue for the open set.

This change means that

the most recently added vertex is

evaluated instead of the oldest.

The result is a search that

quickly moves deeper in the graph

and then eventually backtracks

to vertices added much earlier.

From this video, you should have

an understanding of the mission planning problem and

how we construct and use graphs as

a map level representation of our planning domain.

In addition, you should now be comfortable with using

Breadth-First Search to navigate

an unweighted graph to find

the shortest path to a given destination.

In our next lesson,

we're going to make the graph more complex by

adding different weights to the edges in our graph,

to better reflect the different costs for

using different road segments and we'll

introduce Dykstra's algorithm a method for

handling this new complexity. See you next time.