Autor: Carlos Roberto | Publicado: 2016-10-18T09:48:00-07:00




  

Design and Implementation of
  Autonomous Vehicles System – Part 3






This is the Part 3 of this project. If you are interested in
the others parts, you can take a look on my blog here.
In this section I will talk about Dijkstra’s algorithm that can be used in path
planning. 


What is an algorithm? Algorithm can be defined as a step by step procedure
designed to perform an operation, and which (like a map or flowchart) will lead
to the sought result if followed correctly.
So, based on Wikipedia, Dijkstra’s algorithm is an algorithm for finding the
shortest paths between nodes in a graph, which may represent, for example, road
networks. Putting in single words, suppose you want go from one city to another
where you have different routes that can be used, but you want to use the
shortest route. Well, Dijkstra’s algorithm can analyze all options and show you
the shortest route. In short words, this can be used by an autonomous vehicle
to go from one point to another. Let’s see how it works.


Pseudo
code


Suppose we have the graph of Figure 1, where each node might represent a
city, the lines are roads and the numbers are the distances between the cities.
Our goal is go from node A to the target node H, using the shortest path. Then
the Dijkstra’s algorithm pseudo code is




<!--[if !supportLists]-->1.&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;
<!--[endif]-->For each node n in the
graph

&nbsp;<!--[endif]-->n.distance = Infinity

<!--[if !supportLists]-->2.&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;
<!--[endif]-->Create an empty list

<!--[if !supportLists]-->3.&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;
<!--[endif]-->Make start.distance = 0 and add start to list

<!--[if !supportLists]-->4.&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;
<!--[endif]-->While list not empty

<!--[if !supportLists]-->·&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;
<!--[endif]-->let current = node in the list with the
smallest distance, remove current from list


<!--[if !supportLists]-->·&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;
<!--[endif]-->For each node n that
is adjacent to current
if n.distance &gt; current.distance +
length of edge from n to current
&nbsp;&nbsp; &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;n.distance
= current.distance + length of edge from n to current
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; n.parent = current
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; add n to list if it’s not there
yet










Dijkstra’s
algorithm is action


Based on the pseudo code above, lets mark in the graph
the starting point as A and distance = 0. Let’s use the red color to denote
that the node has been visited and will not be considered in next computations.
&nbsp;




Now we mark each neighbor node to node A with green
color, which means that green nodes are in the list to be considered (see the
pseudo code).




Then, we compute the shortest distance between the
current and its neighbors. So, from


A -&gt; B&nbsp; = 4, A -&gt; C = 2, A -&gt; D
= 1. Note that the graph below has the letters B, C and D associated with the
distances from node A.







In the next iteration, we compute the green nodes in
the list and, in this case, we start with node D that has the shortest path
from the start point. Then, for the neighbors of node D we have D -&gt; F = 9, D
-&gt; C = 4, D -&gt; G = 5. And we update the graph. 










Note that we do not update node C from 2 to 4 because 2
is the shortest distance to this node.
In the next iteration, we compute the neighbors of
node C. Then we have&nbsp;C -&gt; F = 7. And we update the graph.&nbsp;




In the next iteration, we compute node B that has only
node G as neighbor to consider and the distance is B -&gt; G = 9, that is
biggest than 5 and will not update the graph.
Then iterating until the list is empty, as showed in
the pseudo code, we get the final graph showed below.


So, the shortest path from A to H is 9 or A -&gt; C
-&gt; F -&gt; H or 2 + 5 + 2 = 9. 



Some tips to accelerate computation.


1.&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; Using a priority queue that
providces 3 basic operations: add_with_priority(), decrease_priority() and
extract_min(), can lead to faster computing times than using a basic queue.


Creating a Dijkstra’s algorithm code to the robot
Now that we know how the Dijkstra’s algorithm works,
lets create a function code and test with a simple example. To facilitate visualization,
I will use Matlab and some built-in Robotics functions. In addition, I have
ported the Matlab code to Python, that you can find in my Git repository, here.


Because
we are dealing with routes and paths, it’s interesting to use a map, where we
can draw the environment that we want to analyze.
Here, I will use another algorithm named Grid Mapping or Occupancy Grid
Mapping. The basic idea of the occupancy grid mapping is to represent a map of
the environment as an evenly spaced field (discretized) of binary random
variables, each representing the presence of an obstacle at that location in
the environment. So, assuming this discretized environment, we can setup some
colors that will help to display some conditions in this environment. Then, we
will have the following colors in the cells.


1 - white, clear cell
2 – black, obstacle 
3 – red, cell visited
4 – blue, on list
5 – green, start
6 – yellow, destination



Note that we have the start and destination points
marked in the grid mapping. 
In the code we initialize some variables map, start and destination.


occupancy_map = [
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 1,
1, 1, 1, 1, 1, 1];...
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 0,
0, 0, 0, 0, 0, 1];...
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 1,
1, 0, 0, 0, 0, 1];...
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 0,
0, 0, 0, 0, 0, 1];...
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 0,
0, 1, 1, 1, 1, 1];...
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 0,
0, 0, 0, 0, 0, 1];...
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 0,
0, 0, 0, 0, 0, 1];...
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 0,
0, 0, 0, 0, 0, 1];...
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 0,
0, 0, 0, 0, 0, 1];...
&nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp; &nbsp;&nbsp;[1, 1,
1, 1, 1, 1, 1, 1]];...


start_coords=[2,2];
dest_coords=[8,7];




Note that the map variable will store 0 and 1’s where
1 is a cell with an obstacle and zero is a free cell. We can change the
environment design, changing the values in the map matrix. Then, the Dijkstra’s
algorithm will take care of the obstacles and find the shortest path to target.


Create an array with the size of occupancy map, that will store the distances
while the algorithm progress. Initialize it with infinite values.


distanceFromStart = Inf(nrows,ncols);


Then in the main loop, for each iteration, we get the minimum and
current distances.




[min_dist, current] = min(distanceFromStart(:));


Then, inside the main loop, we iterate over all the 4 neighbors and check if the neighbor is an obstacle, or an empty cell, or
the target cell and store the distance to the current cell.


At the final of computation, we will have a list with all distances from start
to destination, as showed in Figure 7. 
You will find the complete code in the Git repository,
here.


The video below shows the Dijkstra’s algorithm in
action using the map matrix we discussed above.






















In the next section, I will talk about Motion Control algorithms, where we can use algorithms to provide functionalities to the robot to decide the direction and orientation to be taken to follow a path, for instance.&nbsp;
Stay tuned...