Robotics

Lecture 2.1
Navigation
● Navigation is the field of making a robot able to move from a start location to a
goal location in some environment
● The type of navigation we will be looking at is that of a mobile robot in a mapped
flat environment
● This means we’ll be operating in 2D and assuming we have a map of the
environment
Global vs Local Planning
● In navigation, the dynamic model of our robot can be quite complex, and the
environment through which we want to navigate can be large and dynamic.
● For this reason, we tend to plan the full path from start to end in simple terms,
and then use a more complex planner that continuously runs to try and follow the
path.
● The former is called the global path and the latter is called the local path
● For example, imagine a 4-wheeled robot that is trying to drive from one side of
the university to another.
○ The global planner would give you a chain of points to follow that describe the shortest path to get
there
○ The local planner would work out the velocity commands to send to the robot in order to follow
that path, identify moving obstacles, and work out how to get around them
Global Planning
● The global planning problem usually
assumes the following:
○ A 2D map
○ The current X-Y location of the robot is
known
○ The target X-Y location of the robot is known
○ The 2D shape of the robot is known
● The 2D map is usually either an
occupancy grid or a cost map
○ An occupancy grid divides the map into
squares, and labels each square as Occupied,
Free or Unknown
○ A costmap does the same, but can also add
costs to each cell, based on things such as
proximity to an obstacle or visibility
Global Planning: Graph Theory
● When a grid is used to describe the map,
this can also be represented as a graph
● Each free gridcell is considered a node and
the distance between each is given a cost 1
1,1 1,2 1,3 1,4 1,5
● This allows us to apply shortest path √2
1
solutions from Graph Theory to robotics √2
2,1 2,2 2,3 2,4 2,5
navigation, such as
○ Dijkstra's algorithm
○ A* algorithm 3,1 3,2

4,1 4,2 4,4 4,5

5,1 5,2 5,3 5,4 5,5

Dijkstra's Algorithm
● On a weighted undirected graph, Dijkstra’s algorithm tells you the lowest cost
path from a given start node, to all other nodes on the graph
● It takes advantage of the fact that if the optimal path from X to Z passes through
Y, then
○ the segment of that path from X to Y will also be the optimal path from X to Y
○ the segment of that path from Y to Z will also be the optimal path from Y to Z
● Example
○ If the shortest path to Kariba is through Chinhoyi taking the A1 and the Makuti turn off, then
■ The shortest path from Harare to Chinhoyi is taking the A1
■ The shortest path from Chinhoyi to Kariba is taking the A1 and the Makuti turn off
Dijkstra's Algorithm: Overview
● All nodes can be attributed the best known path from start to that node as well as
the cost to take that path
● Categorize the starting node as visited with zero cost
● Categorize all other nodes as univisited, with infinite cost
● Select the starting node, let’s call it Ns with cost Cs and lowest path cost Ps
● Repeat until all nodes are visited the following:
○ For each neighbour (Nn) of the selected node
■ Calculate the cost of getting to Nn through Ns: Cs + C(Ns, Nn)
■ If that cost is lower than the current cost for Nn
● Update the path: Pn = extend(Ps, Nn)
● Update the cost: Cn = Cs + C(Ns, Nn)
○ Mark the selected node as visited
○ Pick the lowest cost unvisited node as the next selected node
Dijkstra's Algorithm Example 1

Node Visited Cost Path

A 🗸 0 A

B 𐄂 ∞

C 𐄂 ∞

D 𐄂 ∞

E 𐄂 ∞
Dijkstra's Algorithm Example 1

Node Visited Cost Path

A 🗸 0 A

B 𐄂 3 AB

C 𐄂 1 AC

D 𐄂 ∞

E 𐄂 ∞
Dijkstra's Algorithm Example 1
We’re trying to see if getting to B is
cheaper through C:
● A>>C = 1 from table
● C->B = 7 from graph
● A>>C->B = 8
Node Visited Cost Path ● 8<3 so leave cost and path

A 🗸 0 A

B 𐄂 3 AB

C 🗸 1 AC

D 𐄂 3 ACD

E 𐄂 ∞

Similar calculation to above

Pick the lowest cost unvisited node
except 2+1 > ∞ so we update
and visit
Dijkstra's Algorithm Example 1

Node Visited Cost Path

A 🗸 0 A

B 🗸 3 AB

C 🗸 1 AC

D 𐄂 3 ACD

E 𐄂 4 ABE

No updates, all existing paths

cheapest
Dijkstra's Algorithm Example 1

Node Visited Cost Path

A 🗸 0 A

B 🗸 3 AB

C 🗸 1 AC

D 🗸 3 ACD

E 𐄂 4 ABE

No updates, all existing paths

cheapest
Dijkstra's Algorithm Example 1

Node Visited Cost Path

A 🗸 0 A

B 🗸 3 AB

C 🗸 1 AC

D 🗸 3 ACD

E 𐄂 4 ABE

No updates, all existing paths

cheapest
Dijkstra's Algorithm Example 2

Node Visited Cost Path

A 𐄂 ∞

B 𐄂 ∞

C 𐄂 ∞

D 🗸 0 D

E 𐄂 ∞
Dijkstra's Algorithm Example 2

Node Visited Cost Path

A 𐄂 ∞

B 𐄂 5 DB

C 𐄂 2 DC

D 🗸 0 D

E 𐄂 7 DE
Dijkstra's Algorithm Example 2

Node Visited Cost Path

A 𐄂 3 DCA

B 𐄂 5 DB

C 🗸 2 DC

D 🗸 0 D

E 𐄂 7 DE
Dijkstra's Algorithm Example 2

Node Visited Cost Path

A 🗸 3 DCA

B 𐄂 5 DB

C 🗸 2 DC

D 🗸 0 D

E 𐄂 7 DE
 Since B’s current cost is 5, any candidate

Dijkstra's Algorithm Example 2 cost will be at least 5. Since A, C and D

are all less than 5 already, we can skip
these.

Node Visited Cost Path

A 🗸 3 DCA

B 🗸 5 DB

C 🗸 2 DC

D 🗸 0 D

E 𐄂 6 DBE

5+1>7
Dijkstra's Algorithm Example 2

Node Visited Cost Path

A 🗸 3 DCA

B 🗸 5 DB

C 🗸 2 DC

D 🗸 0 D

E 𐄂 6 DBE
A* Algorithm
● The A* algorithm extends Dijkstra’s algorithm to be more computationally
efficient
● It works when we can come up with an admissible heuristic function that
estimates the shortest path cost between every node on the graph and the goal
○ Heuristic function means a function that uses some knowledge of the environment to make
estimates
○ Admissible means that it never overestimates the true shortest path cost, underestimating is fine
○ An example: For determining the driving distance between cities, the heuristic can be the distance
between cities. Some cities won’t have roads connecting them directly in a straight line, so
sometimes this heuristic will underestimate the cost, but you’ll never find a path between cities that
is shorter than the straight line distance between the two
● Unlike Dijkstra’s, A* only finds the shortest path between the start and a specific
goal, it does not find the shortest path between the start and all possible goals
A* Algorithm
● The A* algorithm works by building a tree from the start goal
○ Set the start as the root of the tree
○ Select the start node - we can call the selected node s
○ Until s is the goal
■ For each neighbour of the selected node
● Calculate the f-score: f(n) = g(n) + h(n)
○ Where g(n) is the cost from start to n along the path specified in the tree
○ Where h(n) is the heuristic cost from n to the goal
○ f(n) can be thought of as the lowest possible cost from start to goal if we go
through s and then n
● If n is not on the tree, or it is with a higher f-score than this f(n), add it to the tree with
s as it’s parent and f(n) as it’s branch weight
■ Select the leaf of the tree with the lowest f-score
A* Algorithm
Global Planning
Local Planning
● Local planning operates over a much smaller
workspace
● In general it only focuses on a small area
around the robot, and tries to move to or
through waypoints from the global path in
that area
● Local planning algorithms focus more on the
kinematics
○ What command velocities are feasible
○ What paths they result in
○ How those potential paths interact with obstacles
○ What cost those paths have, which is usually a
combination of time and closeness to the global path
Local Planning Example: Eband
Localization
● Localization aims to find out the position and orientation of the robot on the map
● Generally it is combining two key signals
○ Odometry: This is the location of the robot relative to a previous position based on how it moved
■ E.g:
● A robot is turned on, we say it’s odometry is (0, 0, 0)
● The robot moves at 1 m/s for 1 second, odometry is (1, 0, 0)
● The robot turns at pi/2 rad/s for 1 second, odometry is (1, 0, pi/2)
● The robot moves at 1 m/s for 2 seconds, odometry is (1, 2, pi/2)
○ Sensor data
■ This is usually a 2D lidar scan
■ A 2D lidar measures distances around the robot, each scan has a set of angle-distance pairs
Localization