Path planning

Mobile robot -Robot pose

The robot pose in a plane is defined by

its state vector [location and orientation]

The transformation between local frame m and

global frame g.
Wheel tangential velocities are vL(t) = rωL(t) and vR(t) = rωR(t), where
ωL(t) and ωR(t) are left and right angular velocities of the wheels around
their axes, respectively.
VR and VL are linear velocities
of left and right wheels
Considering the above relations the internal robot kinematics (in
local coordinates) can be expressed as
Forward
Kinematics
import numpy as np Code to simulate the robot motion with P controller
import matplotlib.pyplot as plt
# Physical constraints
Ts = 0.03 # Sampling time if abs(alpha) > np.pi/4:
t = np.arange(0, 30, Ts) # Simulation time alpha = np.pi/4 * np.sign(alpha)
d = 0.1 # Distance between axes if abs(v) > 0.8:
xyRef = np.array([4, 4]) # Reference v = 0.8 * np.sign(v)
position
q = np.array([1, 0, -np.pi]) # Initial
state
# Robot motion simulation
for k in range(len(t)): dq = np.array([v * np.cos(q[2]), v *
phi_ref = np.arctan2(xyRef[1] - q[1], np.sin(q[2]), v / d * np.tan(alpha)])
xyRef[0] - q[0]) # Reference orientation noise = 0.00 # Set to experiment with
qRef = np.array([xyRef[0], xyRef[1], noise (e.g. 0.001)
phi_ref]) q = q + Ts * dq + np.random.randn(3) *
e = qRef - q # Position and noise # Euler integration
orientation error
plt.plot(q[0], q[1], 'r*')
# Controller #plt.hold(True)
v = 0.3 * np.sqrt(e[0]**2 + e[1]**2)
alpha = 0.2 * e[2] plt.show()
 Single-Source Shortest Path Problem

Single-Source Shortest Path Problem - The problem of finding shortest

paths from a source vertex v to all other vertices in the graph.
 Dijkstra's algorithm
Dijkstra's algorithm - is a solution to the single-source shortest path problem in graph theory.

Works on both directed and undirected graphs. However, all edges must have nonnegative
weights.

It is a popular algorithm for solving many single-source shortest path problems that have non-
negative edge weights in graphs, i.e. it aims to find the shortest distance between two vertices in
a graph. It was invented by the Dutch computer scientist Edsger W. Dijkstra in 1956.

The algorithm maintains a set of visited vertices and a set of unvisited vertices. It starts from the
source vertex and recursively selects the unvisited vertex that has the smallest empirical
distance from the source.
It then visits the neighbors of that vertex and updates their empirical distances if a shortest path
is found. This process continues until the destination vertex is reached, or all reachable vertices
are visited.
Grids and Distance
Dijkstra’s Algorithm works by
visiting vertices in the graph
starting with the object’s starting
point.
‘It then repeatedly examines the
closest not-yet-examined vertex,
adding its vertices to the set of
vertices to be examined.
It expands outwards from the
starting point until it reaches the
goal.
Dijkstra’s Algorithm is guaranteed
The Greedy Best-First-Search
algorithm works in a similar way,
except that it has some estimate
(called a heuristic) of how far from the
goal any vertex is.
Instead of selecting the vertex closest
to the starting point, it selects the
vertex closest to the goal.
Greedy Best-First-Search
is not guaranteed to find a shortest
path. However, it runs much quicker
than Dijkstra’s Algorithm because it
uses the heuristic function to guide its
way towards the goal very quickly.
For example, if the goal is to the south
With obstacles Dijkstra vs Greedy
best search

The trouble is that Greedy Best-First-Search is “greedy” and tries

to move towards the goal even if it’s not the right path. Since it
only considers the cost to get to the goal and ignores the cost of
A star for the above 2 problems
 A star algorithm
A star it combines the pieces of information that Dijkstra’s Algorithm uses
(favoring vertices that are close to the starting point) and information that
Greedy Best-First-Search uses (favoring vertices that are close to the goal).
In the standard terminology used when talking about A*, g(n) represents the
exact cost of the path from the starting point to any vertex n, and h(n)
represents the heuristic estimated cost from vertex n to the goal.
In the above diagrams, the yellow (h) represents vertices far from the goal
and blue (g) represents vertices far from the starting point.
A* balances the two as it moves from the starting point to the goal. Each
time through the main loop, it examines the vertex n that has the lowest f(n)
= g(n) + h(n).
 Dijkstra's algorithm - Pseudocode

dist[s] ←0 (distance to source vertex is zero)

for all v ∈ V–{s}
do dist[v] ←∞ (set all other distances to infinity)
S←∅ (S, the set of visited vertices is initially empty)
Q←V (Q, the queue initially contains all vertices)

while Q ≠∅ (while the queue is not empty)

do u ← mindistance(Q,dist) (select the element of Q with the min.
distance)
S←S∪{u} (add u to list of visited vertices)
for all v ∈ neighbors[u]
do if dist[v] > dist[u] + w(u, v) (if new shortest path
found)
then d[v] ←d[u] + w(u, v) (set new value of shortest
path)
Dijkstra Example
# Python program for Dijkstra's single
# source shortest path algorithm. The program is
# for adjacency matrix representation of the
graph
class Graph(): def minDistance(self, dist, sptSet):

def __init__(self, vertices): # Initialize minimum distance for

self.V = vertices next node
self.graph = [[0 for column in min = 1e7
range(vertices)]
for row in # Search not nearest vertex not in
range(vertices)]
the
def printSolution(self, dist): # shortest path tree
print("Vertex \t Distance from Source") for v in range(self.V):
for node in range(self.V): if dist[v] < min and sptSet[v]
print(node, "\t\t", dist[node]) == False:
min = dist[v]
# A utility function to find the vertex with min_index = v
# minimum distance value, from the set of
return min_index
vertices
# not yet included in shortest path tree
Function that implements Dijkstra's single source
# shortest path algorithm for a graph represented
# using adjacency matrix representation
def dijkstra(self, src):

dist = [1e7] * self.V

dist[src] = 0 # Driver program
sptSet = [False] * self.V
g = Graph(9)
for cout in range(self.V): g.graph = [[0, 4, 0, 0, 0, 0, 0, 8, 0],
[4, 0, 8, 0, 0, 0, 0, 11, 0],
# Pick the minimum distance vertex from
# the set of vertices not yet processed.
[0, 8, 0, 7, 0, 4, 0, 0, 2],
# u is always equal to src in first iteration [0, 0, 7, 0, 9, 14, 0, 0, 0],
u = self.minDistance(dist, sptSet) [0, 0, 0, 9, 0, 10, 0, 0, 0],
[0, 0, 4, 14, 10, 0, 2, 0, 0],
# Put the minimum distance vertex in the
# shortest path tree [0, 0, 0, 0, 0, 2, 0, 1, 6],
sptSet[u] = True [8, 11, 0, 0, 0, 0, 1, 0, 7],
[0, 0, 2, 0, 0, 0, 6, 7, 0]
# Update dist value of the adjacent vertices
# of the picked vertex only if the current
]
# distance is greater than new distance and
# the vertex in not in the shortest path tree g.dijkstra(0)
for v in range(self.V):
if (self.graph[u][v] > 0 and
sptSet[v] == False and
dist[v] > dist[u] + self.graph[u][v]):
dist[v] = dist[u] + self.graph[u][v]

self.printSolution(dist)
Adjacency matrix Example
Exercise: Make adjacency matrix
and use it in Dijkstra code
DIJKSTRA GRID MAPPING
GRID MAP WTH CONSTANT COST
[Manhattan]
After 5 iterations After 9 iterations
WITH Constant COST [Manhattan]
Variable cost: diagonal lines have more friction
and the cost of travel is more along them
A* Algorithm – search based
algorithm

A* Algorithm is one of the best and popular techniques used for

path finding and graph traversals.
A lot of games and web-based maps use this algorithm for finding
the shortest path efficiently.
It is essentially a best first search algorithm. It is the combination
of Dijkstra and best first search algorithm
Working principle

A* Algorithm works as-

• It maintains a tree of paths originating at the start node.
• It extends those paths one edge at a time.
• It continues until its termination criterion is satisfied.

A* Algorithm extends the path that minimizes the following function-

f(n) = g(n) + h(n)

• Here, ‘n’ is the last node on the path

• g(n) is the cost of the path from start node to node ‘n’
• h(n) is a heuristic function that estimates cost of the cheapest path from node ‘n’ to
the goal node
Algorithm-
The algorithm is as follows-
• The implementation of A* Algorithm involves maintaining two lists-
OPEN and CLOSED.
• OPEN contains those nodes that have been evaluated by the
heuristic function but have not been expanded into successors
yet.
• CLOSED contains those nodes that have already been visited.
Step-01:

• Define a list OPEN.

• Initially, OPEN consists solely of a single node, the start node S.

• Step-02:

• If the list is empty, return failure and exit.

• Step-03:

• Remove node n with the smallest value of f(n) from OPEN and move it to
list CLOSED.
• If node n is a goal state, return success and exit.
Step-04:

Expand node n.

Step-05:

•If any successor to n is the goal node, return success and the solution by
tracing the path from goal node to S.
•Otherwise, go to Step-06.

Step-06:

For each successor node,

•Apply the evaluation function f to the node.
•If the node has not been in either list, add it to OPEN.

Step-07:

Go back to Step-02.
 The purpose of the heuristic is to guide the search and a search
that receives accurate guidance will terminate faster than one
Problem-01: that receives poor guidance. There is, however, a trade-off. If
your heuristic is perfect, then it will guide the search so well that
Consider the following graph- the optimal route is the first one it examines

Heuristics is chosen by yourself, but cost is

attribute of the problem itself.

The numbers written on edges represent the distance between the nodes.
The numbers written on nodes represent the heuristic value.
Find the most cost-effective path to reach from start state A to final state J using A*
Solution-
Step-02:
Step-01:
Node G and Node H can be reached from
•We start with node A.
node F.
•Node B and Node F can be reached from node A.
A* Algorithm calculates f(G) and f(H).
•f(G) = (3+1) + 5 = 9
A* Algorithm calculates f(B) and f(F).
•f(H) = (3+7) + 3 = 13
•f(B) = 6 + 8 = 14
•f(F) = 3 + 6 = 9
Since f(G) < f(H), so it decides to go to node G.
Since f(F) < f(B), so it decides to go to node F.
Path- A → F → G
Path- A → F
Step-03: Step-04:

Node I can be reached from node G. Node E, Node H and Node J can be reached from node I.

A* Algorithm calculates f(I). A* Algorithm calculates f(E), f(H) and f(J).

f(I) = (3+1+3) + 1 = 8 f(E) = (3+1+3+5) + 3 = 15
It decides to go to node I. f(H) = (3+1+3+2) + 3 = 12
f(J) = (3+1+3+3) + 0 = 10
Path- A → F → G → I
Since f(J) is least, so it decides to go to node J.

Path- A → F → G → I → J
This is the required shortest path from node A to node J.
Important Note-

It is important to note that-

A* Algorithm is one of the best path finding algorithms.
But it does not produce the shortest path always.
This is because it heavily depends on heuristics.
A STAR PATH FINDING 3
In order to calculate heuristics,
Distance = abs(from.x - to.x) + abs(from.y - to.y)
This is known as the "Manhattan G
Distance" formula. 2

Let's calculate the "g" value for the blue square

immediately to the left of the green square: 1
abs(3 - 2) + abs(2 - 2) = 1
Now, let's try calculating the "h" value: S
abs(2 - 0) + abs(2 - 0) = 4 0
Perfect. Now, let's get the "f" value:
0 1 2 3
1+4=5
So, the final value for this node is "5".

1.The "g" value - This is how far away this node is from the green
square.
2.The "h" value - This is how far away this node is from the red square.
3.The "f" value - This is the sum of the "g" value and the "h" value. This
is the final number which tells us which node to move to.
 3

0 1 2 3
The big number in the center of each square is the "f" value, while the number on the top left is the "g" value,
and the number on the top right is the "h" value:

We've calculated the g, h, and f values for all of the blue nodes. Now, which do we pick?
Whichever one has the lowest f value.
However, in this case, we have 2 nodes with the same f value, 5. How do we pick between them?
Simply, either choose one at random, or have a priority set. Usually prefer to have a priority like so:
"Right > Up > Down > Left"
One of the nodes with the f value of 5 takes us in the
"Down" direction, and the other takes us "Left". Since
Down is at a higher priority than Left, we choose the
square which takes us "Down".

Now mark the nodes which we calculated the heuristics

for, but did not move to, as orange, and the node which
we chose as cyan.
A star path finding from A to B with
obstacles
Without obstacles
G + H =F
G- from Start; H – to the Goal which is heuristic
Distance between the grids =10 and
14 along diagonal =sqrt(2)x10
 Second iteration
G is increasing and H is decreasing towards the Goal

Select the shortest distance grid in the first iteration

 Consider the obstacles now

The path connecting smaller F is shown

 After 3 iterations

After 2 iterations whenever we have same F select the

grid with smaller H
Last iteration
Red nodes are the lowest in each iteration which is
explored further
Explore all the adjacent nodes of the red ones
each time
Whenever G and H are same for any two nodes
select randomly to proceed
The values of certain grids will change during
Iterations –’updated
Algorithm
 GRASSFIRE METHOD
First, we mark our start cell with “0” (see figure
above, top left). Then, we look at the cells above,
below, to the right, and to the left of the start cell.
We mark each of the cells in those directions with
a “1”, assuming they’re not obstacles or outside
the boundaries of the grid, because they’re 1 step
away from the start cell. Since there’s only one cell
with a depth of “0”, the start cell, that’s it for the
first iteration.
On the next iteration, we consider each cell
marked with depth “1” and look at the cells
adjacent to those, then mark them with “2” to
indicate that they’re 2 steps away from the start
cell.
GRASSFIRE is the breadth-first search, We continue in this manner, only marking adjacent
which explores evenly out from the start cells with the next depth if they’re not already
cell and is guaranteed to find the shortest marked with a smaller number, which would
path. Grassfire algorithm, so named indicate that they’re already closer to the start cell
because the search path looks like a than the current cell.
spreading shockwave or brush fire. The This process continues until either a) the
idea behind the algorithm is to mark each destination is found or b) all reachable cells have
cell (node) with its distance from the start been explored and no path to the destination
cell, exploring the cells adjacent to all cells
GRASSFIRE METHOD [Breadth first
search]
G

BFS calculates the shortest paths in unweighted

graphs. On the other hand, Dijkstra's algorithm calculates
the same thing in weighted graphs ALSO. When the
weights are same Grassfire = Dijkstra
Green – Start; Red - Goal
 The robot is in cell (3, 1) and the
target is in cell (3, 5). Suppose that
D star the value of stepping horizontally and
vertically are indicated with number
1, and diagonally 1.4
5
.

1 2 3 4 5
5 5

4 4

3 3

2 2

1 1

1 2 3 4 5 1 2 3 4 5
the neighbors of the selected cells (4, 5),
the cells (4, 5), (2, 5) and (3, 4) have the lowest
and (3, 4) are valued with the same patte
total value, and determine the direction towards
the target
This process will continue until the cell hosting
the robot also has a pointer. Fig. shows the
completed process for the example grid. The
cells containing a pointer, show the minimum
value direction towards the target. Tracing any
indicator-containing path, creates a minimum
value direction towards the target. Once
followed any indicator-containing path, creates a
minimum value.
 1 2 3 4 5

With obstacle
5

3
The method algorithm D* uses to avoid and
move away from the barriers, is increasing 2
stepping
value towards the cells containing obstacles.
This work of adding value is only done when 1
moving towards barrier-containing cells and are
not seen during leaving the cell. For example,
if there is an obstacle in cell (3,3) in Fig. a, the
value of movement increases from the
neighbors upstream of the barrier (close to the
starting point) towards the cell (3,3) (cells (2,2),
(3,2), and (4,2)), But lateral cells and the cells
downstream it (close to the target) will not
change. To clarify the issue, we will continue the
previous example in this section, this time with
a barrier in the cell (3,3).
Step A: First one must modify the three cells upstream the barrier, namely, cells (2,2), (3,2),
and (4,2). As seen in Fig.B, despite the fact that among the neighbor cells, cell (3,3) is
the closest one to the target cell, because of the existence of a barrier, it cannot be a standard
practice and should be removed from neighboring cells. This way, the suitable neighbors for
the cell (2,2) is cell (2,3) and the appropriate cell neighboring cells (3,2) and (4,2) would be
cell (4,3). This value-setting process is shown in Table, and its result is shown in Fig-C. According to
the results of this table, both of the cells (2,2) and (4,2) have the least value
and will form the basis to continue the process at next step.

Step B: At this step, the cells neighboring both cells (2,2) and (4,2) must be identified and
valuated. It has been done in Table 2.2b, base on which, three cells (1,2), (3,2) and (5,2) attain
the lowest value, and will form the basis to continue the process at a later step.

Step C: In this step, continuing the process (Table ) and evaluation the cells neighboring
both cells (2,1) and (4,1) have the lowest value and will make the basis of process for the next
step.
Step D: At this step, valuation of neighboring cells shows that no cell has a value less than
the others (the value of all the cells are the same) and thus, the valuation process end
• It should be noted that the marker located in cell (3,2) will not make
the robot return to the its starting point, because D* algorithm has left
behind the optimal path and in another hand will not get trapped in
local minima (Choset 2005; LaValle 2006; Latombe 2012). In addition,
value correction at each time, will result in dynamically making
decisions for the robot, such that when facing with visible obstacles,
there will be the possibility to produce optimal paths.
•