Path Planning

1 / 118
 Overview of the Lecture
□ Part 1 – Path Planning –
Introduction to Path
Planning
Notation and
Terminology Path
Planning Methods
□ Part 2 – Grid and Graph based Path Planning
Methods Grid-based Planning
DT for Path Planning
Graph Search
Algorithms D* Lite
Path Planning based 2 / 118
Introduction to Path Planning Notation Path Planning Methods

Part I
Part 1 – Path and Motion
Planning

3 / 118
Introduction to Path Planning Notation Path Planning Methods

Outlin
e

Introduction to Path
Planning

Notation and

Terminology Path

Planning Methods

4 / 118
Introduction to Path Planning Notation Path Planning Methods

Robot Motion Planning – Motivational

□ problem
How to transform high-level task specification (provided by humans) into
a low-level description suitable for controlling the actuators?
To develop algorithms for such a transformation.
The motion planning algorithms provide transformations how to move a
robot (object) considering all operational constraints.

It encompasses several disciples, e.g., mathematics, robotics, com-

puter science, control theory, artificial intelligence, computational
geometry, etc.
B4M36UIR – Lecture 03: Path Planning 5 / 118
Introduction to Path Planning Notation Path Planning Methods

Robot Motion Planning – Motivational

□ problem
How to transform high-level task specification (provided by humans) into
a low-level description suitable for controlling the actuators?
To develop algorithms for such a transformation.
The motion planning algorithms provide transformations how to move a
robot (object) considering all operational constraints.

It encompasses several disciples, e.g., mathematics, robotics, com-

puter science, control theory, artificial intelligence, computational
geometry, etc.
B4M36UIR – Lecture 03: Path Planning 5 / 118
Introduction to Path Planning Notation Path Planning Methods

Piano Mover’s Problem

A classical motion planning problem

Having a CAD model of the piano, model of the environment, the problem is how
to move the piano from one place to another without hitting anything.

Basic motion planning algorithms are focused primarily on rotations and translations.
□ We need notion of model representations and formal definition of
□ the problem. Moreover, we also need a context about the problem
and realistic assumptions.
6 / 118
Introduction to Path Planning Notation Path Planning Methods

Piano Mover’s Problem

A classical motion planning problem

Having a CAD model of the piano, model of the environment, the problem is how
to move the piano from one place to another without hitting anything.

Basic motion planning algorithms are focused primarily on rotations and translations.
□ We need notion of model representations and formal definition of
□ the problem. Moreover, we also need a context about the problem
and realistic assumptions.
6 / 118
Introduction to Path Planning Notation Path Planning Methods

Robotic Planning
Mission Planning Context
Tasks and Actions Plans
symbol level

Motion Planning
Problem Path Trajectory
Planning Planning

Models of
Path
robot and
workspace
"geometric" level

Trajectory

Robot Control Sensing and Acting

feedback control
controller − drives (motors) − sensors
"physical" level
7 / 118
Introduction to Path Planning Notation Path Planning Methods

Robotic Planning
Mission Planning Context
Tasks and Actions Plans
symbol level

Path (Motion) Planning / Trajectory Planning

Problem Path Planning Trajectory
Generation

Models of
Path
robot and
workspace
"geometric" level

Trajectory Open−loop control?

Robot Control Sensing and Acting

feedback control
controller − drives (motors) − sensors
"physical" level
7 / 118
Introduction to Path Planning Notation Path Planning Methods

Robotic Planning
Mission Planning Context
Tasks and Actions Plans
symbol level

Path (Motion) Planning / Trajectory Planning

Problem Path Planning Trajectory
Generation

Models of
Path
robot and
workspace
"geometric" level

Trajectory Open−loop control?

Robot Control Sensing and Acting

feedback control Sources of uncertainties
controller − drives (motors) − sensors because of real environment
"physical" level
7 / 118
Introduction to Path Planning Notation Path Planning Methods

Robotic Planning
Mission Planning Context
Tasks and Actions Plans
symbol level

Path (Motion) Planning / Trajectory Planning

Problem Path Planning Trajectory
Generation

Models of
Path
robot and
workspace
"geometric" level

Trajectory Open−loop control?

Robot Control Sensing and Acting

feedback control Sources of uncertainties
controller − drives (motors) − sensors because of real environment
"physical" level
7 / 118
Introduction to Path Planning Notation Path Planning Methods

Robotic Planning
Mission Planning Context
Tasks and Actions Plans
symbol level

Path (Motion) Planning / Trajectory Planning

Problem Path Planning Trajectory
Generation

Models of
Path
robot and
workspace
"geometric" level

Trajectory Open−loop control?

Robot Control Sensing and Acting

feedback control Sources of uncertainties
controller − drives (motors) − sensors because of real environment
"physical" level
7 / 118
Introduction to Path Planning Notation Path Planning Methods

Real Mobile
Robots
In a real deployment, the problem is more
complex.

□ The world is changing.

□ Robots update the knowledge
about the environment.
localization, mapping and
navigation
□New decisions have to made based
on the feedback from the
An example of robotic mission:
environment.
Josef Štrunc, Bachelor thesis, CTU, 2009.
Motion
Multi-robot planning is a part
exploration of of the mission re-
unknown
planning loop.
environment. How to deal with real-world
complexity? Relaxing constraints and considering realistic
assumptions. 8 / 118
Introduction to Path Planning Notation Path Planning Methods

Real Mobile
Robots
In a real deployment, the problem is more
complex.

□ The world is changing.

□ Robots update the knowledge
about the environment.
localization, mapping and
navigation
□New decisions have to made based
on the feedback from the
An example of robotic mission:
environment.
Josef Štrunc, Bachelor thesis, CTU, 2009.
Motion
Multi-robot planning is a part
exploration of of the mission re-
unknown
planning loop.
environment. How to deal with real-world complexity?
Relaxing constraints and considering realistic
assumptions. 8 / 118
Introduction to Path Planning Notation Path Planning Methods

Real Mobile
Robots
In a real deployment, the problem is more
complex.

□ The world is changing.

□ Robots update the knowledge
about the environment.
localization, mapping and
navigation
□New decisions have to made based
on the feedback from the
An example of robotic mission:
environment.
Josef Štrunc, Bachelor thesis, CTU, 2009.
Motion
Multi-robot planning is a part
exploration of of the mission re-
unknown
planning loop.
environment. How to deal with real-world complexity?
Relaxing constraints and considering realistic
assumptions.
Introduction to Path Planning Notation Path Planning Methods

Outlin
e

Introduction to Path
Planning

Notation and

Terminology Path

Planning Methods
Introduction to Path Planning Notation Path Planning Methods

Notation
□ W – World model describes the robot workspace and its boundary
determines the
obstacles O i .
2D world, W

= R2

□ A Robot is defined by its geometry, parameters (kinematics) and it is

controllable by the motion plan.
□ C – Configuration space (C-space)
A concept to describe possible configurations of the robot. The robot’s
configuration completely specify the robot location in W including
specification of all degrees of freedom.
E.g., a robot with rigid body in a plane C = {x, y, ϕ } = R ×
2
S 1.
□ Let A be a subset of W occupied by the robot, A = A(q).
□ A subset of C occupied by obstacles is
Cobs = {q ∈ C : A(q) ∩ O i , ∀i }.
 Introduction to Path Planning Notation Path Planning Methods

Notation
□ W – World model describes the robot workspace and its boundary
determines the
obstacles O i .
2D world, W

= R2

□ A Robot is defined by its geometry, parameters (kinematics) and it is

controllable by the motion plan.
□ C – Configuration space (C-space)
A concept to describe possible configurations of the robot. The robot’s
configuration completely specify the robot location in W including
specification of all degrees of freedom.
E.g., a robot with rigid body in a plane C = {x, y, ϕ } = R ×
2
S 1.
□ Let A be a subset of W occupied by the robot, A = A(q).
□ A subset of C occupied by obstacles is
Jan Faigl, 2020 Cobs = {q ∈ CB4M36UIR
: A(q)– ∩ O i , 03:
Lecture }. Planning
∀i Path 10 / 118
Introduction to Path Planning Notation Path Planning Methods

Notation
□ W – World model describes the robot workspace and its boundary
determines the
obstacles O i .
2D world, W

= R2

□ A Robot is defined by its geometry, parameters (kinematics) and it is

controllable by the motion plan.
□ C – Configuration space (C-space)
A concept to describe possible configurations of the robot. The robot’s
configuration completely specify the robot location in W including
specification of all degrees of freedom.
E.g., a robot with rigid body in a plane C = {x, y, ϕ } = R ×
2
S 1.
□ Let A be a subset of W occupied by the robot, A = A(q).
□ A subset of C occupied by obstacles is
Cobs = {q ∈ CB4M36UIR
: A(q)– ∩ O i , 03:
Lecture }. Planning
∀i Path 10 / 118
Introduction to Path Planning Notation Path Planning Methods

Notation
□ W – World model describes the robot workspace and its boundary
determines the
obstacles O i .
2D world, W

= R2

□ A Robot is defined by its geometry, parameters (kinematics) and it is

controllable by the motion plan.
□ C – Configuration space (C-space)
A concept to describe possible configurations of the robot. The robot’s
configuration completely specify the robot location in W including
specification of all degrees of freedom.
E.g., a robot with rigid body in a plane C = {x, y, ϕ } = R ×
2
S 1.
□ Let A be a subset of W occupied by the robot, A = A(q).
□ A subset of C occupied by obstacles is
Cobs = {q ∈ CB4M36UIR
: A(q)– ∩ O i , 03:
Lecture }. Planning
∀i Path 10 / 118
Introduction to Path Planning Notation Path Planning Methods

Notation
□ W – World model describes the robot workspace and its boundary
determines the
obstacles O i .
2D world, W

= R2

□ A Robot is defined by its geometry, parameters (kinematics) and it is

controllable by the motion plan.
□ C – Configuration space (C-space)
A concept to describe possible configurations of the robot. The robot’s
configuration completely specify the robot location in W including
specification of all degrees of freedom.
E.g., a robot with rigid body in a plane C = {x, y, ϕ } = R ×
2
S 1.
□ Let A be a subset of W occupied by the robot, A = A(q).
□ A subset of C occupied by obstacles is
Cobs = {q ∈ C : A(q) ∩ O i , ∀i }.
Introduction to Path Planning Notation Path Planning Methods

Path / Motion Planning Problem

□ Path is a continuous mapping in C-space such that
π : [0, 1] → Cfree, with π(0) = q0, and π(1) = qf .
□ Trajectory is a path with explicate parametrization of time, e.g.,
accompanied by a description of the motion laws (γ : [0, 1] → C,
where C is robot’s action space).
It includes dynamics.
[T0, Tf ] s t ~ τ ∈ [0, 1] : q(t) = π(τ ) ∈ Cfree
The path planning is the determination of the function π(·).
Additional requirements can be given:
□ Smoothness of the path;
□ Kinodynamic constraints, e.g., considering friction forces;

□ Optimality criterion – shortest vs fastest (length vs curvature).

□ Path planning – planning a collision-free path in C-space.

□ Motion planning – planning collision-free motion in the state space.
Introduction to Path Planning Notation Path Planning Methods

Path / Motion Planning Problem

□ Path is a continuous mapping in C-space such that
π : [0, 1] → Cfree, with π(0) = q0, and π(1) = qf .
□ Trajectory is a path with explicate parametrization of time, e.g.,
accompanied by a description of the motion laws (γ : [0, 1] → C,
where C is robot’s action space).
It includes dynamics.
[T0, Tf ] s t ~ τ ∈ [0, 1] : q(t) = π(τ ) ∈ Cfree
The path planning is the determination of the function π(·).
Additional requirements can be given:
□ Smoothness of the path;
□ Kinodynamic constraints, e.g., considering friction forces;

□ Optimality criterion – shortest vs fastest (length vs curvature).

□ Path planning – planning a collision-free path in C-space.

□ Motion planning – planning collision-free motion in the state space.
Introduction to Path Planning Notation Path Planning Methods

Planning in C-
space
Robot motion planning robot for a disk robot with a
Goal position

radius ρ. Goal configuration

Cfree

Cobst

Start position

Start configuration

Disk robot Point robot

C−space
Motion planning problem in Motion planning problem in C-space
geometrical representation
representation
of W
C-space has been obtained by enlarging obstacles by the disk A with the radius
ρ.
By applying Minkowski sum: O ⊕ A = {x + y | x ∈ O , y ∈
Introduction to Path Planning Notation Path Planning Methods

Example of obs for a Robot with

 C y Robot
body
Rotation


Reference point x y

Cobs
x

 x

A simple 2D obstacle → has a complicated Cobs

□ Deterministic algorithms exist.
Requires exponential time in C dimension, J. Canny, PAMI, 8(2):200–209, 1986.
□ Explicit representation of Cfree is impractical to
compute. 13 / 118
Introduction to Path Planning Notation Path Planning Methods

Example of obs for a Robot with

 C y Robot
body
Rotation


Reference point x y

Cobs
x

 x

A simple 2D obstacle → has a complicated Cobs

□ Deterministic algorithms exist.
Requires exponential time in C dimension, J. Canny, PAMI, 8(2):200–209, 1986.
□ Explicit representation of Cfree is impractical to
compute. 13 / 118
Introduction to Path Planning Notation Path Planning Methods

Representation of C-space
How to deal with continuous representation of C-space?

Continuous Representation of C-space

↓
Discre tization
processing critical geometric events, (random)
sampling
roadmaps, cell decomposition, potential field

↓
Graph Search Techniques
BFS, Gradient Search, A∗

14 / 118
Introduction to Path Planning Notation Path Planning Methods

Representation of C-space
How to deal with continuous representation of C-space?

Continuous Representation of C-space

↓
Discre tization
processing critical geometric events, (random)
sampling
roadmaps, cell decomposition, potential field

↓
Graph Search Techniques
BFS, Gradient Search, A∗
Introduction to Path Planning Notation Path Planning Methods

Outlin
e

Introduction to Path
Planning

Notation and

Terminology Path

Planning Methods
Introduction to Path Planning Notation Path Planning Methods

Planning Methods -
(selected approaches)
Overview
□ Point-to-point path/motion planning. Multi-goal path/motion/trajectory planning later

□ Roadmap based methods – Create a connectivity graph of the free space.

□ Visibility graph (complete but impractical)
□ Cell decomposition
□ Voronoi graph
□ Discretization into a grid-based (or lattice-based) (resolution complete)
□ representation
Potential field (complete only for a “navigation function”, which is hard to compute
in general)
methods
Classic path planning algorithms
□ Randomized sampling-based
methods
□ Creates a roadmap from connected random samples
□ Probabilistic
in Cfree . roadmaps.
Samples are drawn from some distribution.
□ Very successful in
practice.
Introduction to Path Planning Notation Path Planning Methods

Planning Methods -
(selected approaches)
Overview
□ Point-to-point path/motion planning. Multi-goal path/motion/trajectory planning later

□ Roadmap based methods – Create a connectivity graph of the free space.

□ Visibility graph (complete but impractical)
□ Cell decomposition
□ Voronoi graph
□ Discretization into a grid-based (or lattice-based) (resolution complete)
□ representation
Potential field (complete only for a “navigation function”, which is hard to compute
in general)
methods
Classic path planning algorithms
□ Randomized sampling-based
methods
□ Creates a roadmap from connected random samples
□ Probabilistic
in Cfree . roadmaps.
Samples are drawn from some distribution.
□ Very successful in
practice.
Introduction to Path Planning Notation Path Planning Methods

1. Compute visibility
Visibility
graph
2. Find the shortest Graph E.g., by Dijkstra’s algorithm.
path

Proble Visibility Found shortest

m graph path
Constructions of the visibility graph:
□ Naïve – all segments between n vertices of the map
□ O(n3); Using rotation trees for a set of segments – M. H. Overmars and E. Welzl, 1988
O(n2). 17 / 118
Introduction to Path Planning Notation Path Planning Methods

1. Compute visibility
Visibility
graph
2. Find the shortest Graph E.g., by Dijkstra’s algorithm.
path

Proble Visibility Found shortest

m graph path
Constructions of the visibility graph:
□ Naïve – all segments between n vertices of the map
□ O(n3); Using rotation trees for a set of segments – M. H. Overmars and E. Welzl, 1988
O(n2). 17 / 118
Introduction to Path Planning Notation Path Planning Methods

Minimal Construct: Efficent Shortest Path in Polygonal Maps

□ Minimal Construct algorithm computes visibility graph during the A* search instead of first
computation of the complete visibility graph and then finding the shortest path using A* or
Dijkstra algorithm.
□ Based on A* search with line intersection tests are
delayed until they become necessary.
□ The intersection tests are further accelerated using
bounding boxes.

Full Visibility Graph Minimal Construct

Marcell Missura, Daniel D. Lee, and Maren Bennewitz (2018): Minimal Construct: Efficient Shortest Path Finding for Mobile Robots
in Polygonal Maps. IROS.
Introduction to Path Planning Notation Path Planning Methods

Voronoi Graph
1. Roadmap is Voronoi graph that maximizes clearance from the
obstacles.
2. Start and goal positions are connected to the graph.
3. Path is found using a graph search algorithm.

Voronoi Path in Found

graph graph path
Introduction to Path Planning Notation Path Planning Methods

Visibility Graph vs Voronoi Graph

Visibility graph
□ Shortest path, but it is close to obstacles. We have to
consider safety of the path.
An error in plan execution can lead to a
collision.
□ Complicated in higher dimensions

Voronoi graph
□ It maximize clearance, which can provide conservative
paths.
□ Small changes in obstacles can lead to large changes in
the graph.
□ Complicated in higher dimensions.

A combination is called Visibility-Voronoi – R. Wein, J. P. van den Berg,

D. Halperin, 2004.
Introduction to Path Planning Notation Path Planning Methods

Cell
1. Decompose free space into Decomposition
Any two points in a convex region can be directly connected by a
parts. segment.
2. Create an adjacency graph representing the connectivity of the
free space.
3. Find a path in the graph.
Trapezoidal decomposition

qg

q0

Centroids represent cells Connect Find path in the adjacency

adjacency cells graph
□ Other decomposition (e.g., triangulation) are
possible.
Introduction to Path Planning Notation Path Planning Methods

Shortest Path Map (SPM)

□ Speedup computation of the shortest path towards a particular goal location pg
for a polygonal domain P with n vertices.
□ A partitioning of the free space into cells with
respect to the particular location pg .
□ Each cell has a vertex on the shortest path to pg .
□ Shortest path from any point p is found by
determining the cell (in O(log n) using point
location alg.) and then travesing the shortest
path with up to k bends, i.e., it is found in O(log
n+k).
□ Determining the SPM using “wavefront” propagation
based on
continuous Dijkstra paradigm.
Joseph S. B. Mitchell: A new algorithm for shortest paths among obstacles in the
A similar
plane, Annals of Mathematics and structure can be found
Artificial Intelligence, for two-point
3(1):83–105, query, e.g., H. Guo, A. Maheshwari, J.-R. Sack, 2008.
1991.
□
Introduction to Path Planning Notation Path Planning Methods

Point Location Problem

□ For a given partitioning of the polygonal domain into a discrete set of cells,
determine the cell for a given point p.

Masato Edahiro, Iwao Kokubo and Takao Asano: A new point-location algorithm and its practical efficiency: comparison with
existing algorithms, ACM Trans. Graph., 3(2):86–109, 1984.
□ It can be implemented using interval trees – slabs and slices.

Point location problem, SPM and similarly problems are from the Computational Geometry field.
B4M36UIR – Lecture 03: Path Planning 23 / 118
Introduction to Path Planning Notation Path Planning Methods

Point Location Problem

□ For a given partitioning of the polygonal domain into a discrete set of cells,
determine the cell for a given point p.

Masato Edahiro, Iwao Kokubo and Takao Asano: A new point-location algorithm and its practical efficiency: comparison with
existing algorithms, ACM Trans. Graph., 3(2):86–109, 1984.
□ It can be implemented using interval trees – slabs and slices.

Point location problem, SPM and similarly problems are from the Computational Geometry field.
B4M36UIR – Lecture 03: Path Planning 23 / 118
Introduction to Path Planning Notation Path Planning Methods

Approximate Shortest Path and

□ We can useNavigation Mesh of the polygonal map to speed up shortest
any convex partitioning
path queries.
1. Precompute all shortest paths from map vertices to pg using visibility graph.
2. Then, an estimation of the shortest path from p to pg is the shortest path
among the one of the cell vertex.

□ The estimation can be further improved by “ray-shooting” technique combined

triangulation
with walking in(convex (Faigl, 2010)
partitioning).
24 / 118
Introduction to Path Planning Notation Path Planning Methods

Path Refinement
□Testing collision of the point p with particular vertices of the estimation of the
shortest path.
□ Let the initial path estimation from p to pg be a sequence of k vertices (p, v1, . . . ,
vk , pg ).
□ We can iteratively test if the segment (p, v ), 1 < i ≤ k is collision free up to
i
(p, pg ).

path over path over full

v0 v1
With precomputed structures, refinement
it allows to estimate the shortest path in units of
microseconds.
25 / 118
Introduction to Path Planning Notation Path Planning Methods

Navigation
□ Mesh
In addition to robotic approaches, fast shortest path queries are studied in
computer games.
□ There is a class of algorithms based on navigation mesh.
□ A supporting structure representing the free space.

It usually originated from the grid based maps, but it is represented as CDT – Constrained
DelaUnay triangUlation.

Grid mesh Merged grid mesh CDT mesh Merged C D T mesh

□ E.g., Polyanya algorithm based on navigation mesh and best-first search.

M. Cui, D. Harabor, A. Grastien: Compromise-free Pathfinding on a Navigation Mesh, IJCAI 2017, 496–502.
https://bitbucket.org/dharabor/pathfinding
Informative 26 / 118
Introduction to Path Planning Notation Path Planning Methods

Artificial Potential Field Method

□ The idea is to create a function f that will provide a direction towards the
goal for any configuration of the robot.
□ Such a function is called navigation function and −∇f (q) points to the
goal.
□ Create a potential field that will attract robot towards the goal qf while
obstacles will generate repulsive potential repelling the robot away from
the obstacles.
The navigation function is a sum of potentials.

Such a potential function can have several local minima.

Introduction to Path Planning Notation Path Planning Methods

Avoiding Local Minima in Artificial

Potential Field
□ Consider harmonic functions that have only one extremum

∇ 2 f (q) = 0.

□ Finite element method with defined Dirichlet and Neumann boundary

conditions.

J. Mačák, Master thesis, CTU, 2009

Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Part II
Part 2 – Grid and Graph based Path Planning
Methods
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Outlin
e
Grid-based Planning

DT for Path Planning

Graph Search

Algorithms D* Lite

Path Planning based

on Reaction-Diffusion
Process
Grid-based Planning D T for Path Planning Graph Search D* Lite RD-based Planning
Algorithms

Grid-based
□ A subdivision of Cfree Planning
into smaller cells.
□ Grow obstacles can be simplified by growing
bor- ders by a diameter of the
qstart qgoal
robot.
□ Construction of the planning graph G = (V ,
E ) for
V □as4-neighbors andand
a set of cells 8- E as the neighbor-
neighbors
relations.

□ A grid map can be constructed from the

so-called
occupancy grid E.g., using thresholding.
maps.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Grid-based Environment
□ Representations
Hiearchical planning with coarse resolution and re-planning on finer
resolution.
Holte, R. C. et al. (1996): Hierarchical A *: searching abstraction hierarchies
efficiently. AAAI.

□Octree can be used for the map representation.

□In addition to squared (or rectangular) grid a
hexagonal grid can be used.
□ 3D grid maps – https://octomap.github.io.
OctoMap
— Memory grows with the size of the
environment.
— Due to limited resolution it may fail in narrow
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Example of Simple Grid-based

Planning
□ Wave-front propagation using path
simplication
□ Initial map with a robot and goal.
□ Obstacle growing.
□ Wave-front propagation – “flood fill”.
□ Find a path using a navigation function.
□ Path simplification.
□ “Ray-shooting” technique combined with
Bresenham’s line algorithm.
□ The path is a sequence of “key” cells for
avoiding obstacles.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Example of Simple Grid-based

Planning
□ Wave-front propagation using path
simplication
□ Initial map with a robot and goal.
□ Obstacle growing.
□ Wave-front propagation – “flood fill”.
□ Find a path using a navigation function.
□ Path simplification.
□ “Ray-shooting” technique combined with
Bresenham’s line algorithm.
□ The path is a sequence of “key” cells for
avoiding obstacles.

B4M36UIR – Lecture 03: Path Planning 33 / 118

 Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Example of Simple Grid-based

Planning
□ Wave-front propagation using path
simplication
□ Initial map with a robot and goal.
□ Obstacle growing.
□ Wave-front propagation – “flood fill”.
□ Find a path using a navigation function.
□ Path simplification.
□ “Ray-shooting” technique combined with
Bresenham’s line algorithm.
□ The path is a sequence of “key” cells for
avoiding obstacles.

Jan Faigl, 2020 B4M36UIR – Lecture 03: Path Planning 33 / 118

Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Example of Simple Grid-based

Planning
□ Wave-front propagation using path
simplication
□ Initial map with a robot and goal.
□ Obstacle growing.
□ Wave-front propagation – “flood fill”.
□ Find a path using a navigation function.
□ Path simplification.
□ “Ray-shooting” technique combined with
Bresenham’s line algorithm.
□ The path is a sequence of “key” cells for
avoiding obstacles.

B4M36UIR – Lecture 03: Path Planning 33 / 118

Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Example of Simple Grid-based

Planning
□ Wave-front propagation using path
simplication
□ Initial map with a robot and goal.
□ Obstacle growing.
□ Wave-front propagation – “flood fill”.
□ Find a path using a navigation function.
□ Path simplification.
□ “Ray-shooting” technique combined with
Bresenham’s line algorithm.
□ The path is a sequence of “key” cells for
avoiding obstacles.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Example – Wave-Front Propagation

(Flood Fill)
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Path Simplification
□ The initial path is found in a grid using 8-neighborhood.
□ The rayshoot cast a line into a grid and possible collisions of the robot
with obstacles are checked.
□ The “farthest” cells without collisions are used as “turn” points.
□ The final path is a sequence of straight line segments.

Obtacle growing, wave-front

Initial and goal locations Ray-shooting Simplified path
propagation
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Bresenham’s Line
□ Algorithm
Filling a grid by a line with avoding float
y1 −y0
□ A line from (x
numbers. 0 , 0y ) to 1 (x , y ) is given by x1 −x0 (x − x 0) +
1 y=
CoordsVector& 1
bresenham(const Coords& p t 1 , const Coords& p t 2 , 26 y i n. t twoDy
0 = 2 * dy;
CoordsVector& l i n e ) 27 int twoDyTwoDx = twoDy - 2 * dx; //2*Dy - 2*Dx
2 { 28 int e = twoDy - dx; //2*Dy - Dx
3 // The pt2 point i s not added in to line int y = y0;
29 int xDraw, yDraw;
4 i n t x0 = p t 1 . c ; i n t y0 = p t 1 . r ; for ( i n t x = x0; x != x1; x += xstep)
30 { i f (steep) {
5 i n t x1 = p t 2 . c ; i n t y1 = p t 2 . r ; xDraw = y;
31 yDraw = x ;
6 Coords p; 32 } else {
7 i n t dx = x1 - x0; xDraw = x;
33 yDraw = y;
8 i n t dy = y1 - y0; }
34 p . c = xDraw;
9 i n t steep = (abs(dy) >= abs(dx)); p . r = yDraw;
35 line.push_back(p); // add to the line i f
10 i f (steep) { (e > 0) {
36 e += twoDyTwoDx; //E += 2*Dy - 2*Dx
11 SWAP(x0, y0); y = y + ystep;
37 } else {
12 SWAP(x1, y1); e += twoDy; //E += 2*Dy
38 }
13 dx = x1 - x0; // recompute Dx, Dy }
39 return l i n e ;
14 dy = y1 - y0; 50 }
40
15 }
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Outlin
e
Grid-based Planning

DT for Path Planning

Graph Search

Algorithms D* Lite

Path Planning based

on Reaction-Diffusion
Process
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Distance Transform based Path

□
Planning
For a given goal location and grid map compute a navigational function using
wave-front
algorithm, i.e., a kind of potential field.
□ The value of the goal cell is set to 0 and all other free cells are set to

some very high value.

It uses
□ For 8-neighbors
each and distance
free cell compute a number√isEuclidean
of cellsdistance
towardsof the
the centers
goal cell. of two
EV=1
the for orthogonal cells or EV =cells, i.e., 2 for
The values
□ diagonal are iteratively computed until the values are
cells.
changing.
□ The value of the cell c is computed as

cost(c) = min 8 (cost(ci ) +

i=1
EVci ,c ) ,
where ci is one of the neighboring cells from 8-neighborhood of the cell c.
□ The algorithm provides a cost map of the path distance from any free cell to
the goal cell.
□ The path is then used following the gradient of the cell cost.
Jarvis, R. (2004): Distance Transform Based Visibility Measures for Covert Path Planning in Known but Dynamic
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Distance Transform Path

Planning
Algorithm 1: Distance Transform for Path Planning
for y : = 0 to yMax do
for x : = 0 to xMax do
if goal [x,y] then
cell [x,y] := 0;
else
cell [x,y] :=
xMax *
yMax;
//initializati
on, e.g.,
pragmatic
of the use
longest
distance as
∞ ;

repeat
for y : = 1 to (yMax - 1)
do
for x : = 1 to (xMax - 1) do
if not blocked [x,y] then
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Distance Transform based Path Planning –

1
2
3
Grid& DT::compute(Grid& g r i d ) c onst
{ Impl. 1/2
s t a t i c c o ns t double DIAGONAL = s q r t ( 2 ) ;
35
36
37
for ( i nt r = H - 2; r > 0; - - r ) {
for ( i nt c = W - 2; c > 0; -- c ) {
i f ( m a p [ r ] [ c ] ! = FREESPACE) {
4 s t a t i c c o ns t double ORTOGONAL = 1; 38 c o nti nu e ;
5 c o ns t i n t H = map.H; 39 } / / o b s t ac le detected
6 c o ns t i n t W = map.W; 40 double t [ 4 ] ;
7 a s s e r t ( g r i d . H == H and grid.W == W, " s i z e " ) ; 41 t [ 1 ] = g r i d [ r + 1 ] [ c ] + ORTOGONAL;
8 bool anyChange = t r u e ; 42 t [ 0 ] = g r i d [ r + 1 ] [ c + 1 ] + DIAGONAL;
9 i n t counter = 0; 43 t [ 3 ] = g r i d [ r ] [ c + 1 ] + ORTOGONAL;
10 while (anyChange) { 44 t [ 2 ] = g r i d [ r + 1 ] [ c - 1 ] + DIAGONAL;
11 anyChange = f a l s e ; 45 double pom = g r i d [ r ] [ c ] ;
12 f o r ( i n t r = 1 ; r < H - 1 ; ++r) { 46 bool s = f a l s e ;
13 f o r ( i n t c = 1 ; c < W - 1 ; ++c) { 47 for ( i n t i = 0; i < 4; i++) {
14 i f ( m a p [ r ] [ c ] ! = FREESPACE) { 48 i f (pom > t [ i ] ) {
15 c o nti nu e ; 49 pom = t [ i ] ;
16 } / / o b s t ac le detected 50 s = true;
17 double t [ 4 ] ; 51 }
18 t [ 0 ] = g r i d [ r - 1 ] [ c - 1 ] + DIAGONAL; 52 }
19 t [ 1 ] = g r i d [ r - 1 ] [ c ] + ORTOGONAL; 53 i f (s) {
20 t [ 2 ] = g r i d [ r - 1 ] [ c + 1 ] + DIAGONAL; 54 anyChange = t r u e ;
21 t [ 3 ] = g r i d [ r ] [ c - 1 ] + ORTOGONAL; 55 g r i d [ r ] [ c ] = pom;
22 double pom = g r i d [ r ] [ c ] ; 56 }
23 for ( i n t i = 0; i < 4; i++) { 57 }
24 i f (pom > t [ i ] ) { 58 }
25 pom = t [ i ] ; 59 counter++;
26 anyChange = t r u e ; 60 } //end while any change
27 } 61 return g r i d ;
28 } 62 }
29 i f (anyChange) {
30 g r i d [ r ] [ c ] = pom; A boundary is assumed around the rectangular map
31 }
32 }
33 }
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Distance Transform based Path Planning –

□ Impl.
The path is retrived by following 2/2 value towards the
the minimal
goal using
1 min8Point().
Coords& min8Point(const Grid& g r i d , Coords& p) 22 CoordsVector& DT::findPath(const Coords& s t a r t , const Coords&
2 { g o a l , CoordsVector& path)
3 double min = std::numeric_limits<double>::max(); 23 {
4 const i n t H = grid.H; 24 s t a t i c const double DIAGONAL = sqrt(2);
5 const i n t W = grid.W; 25 s t a t i c const double ORTOGONAL = 1;
6 Coords t ; 26 const i n t H = map.H;
7 27 const i n t W = map.W;
8 f o r ( i n t r = p . r - 1 ; r <= p . r + 1 ; r++) 28 Grid grid(H, W, H*W); // H*W max grid value
9 { i f ( r < 0 or r >= H) { continue; } 29 g r i d [ g o a l . r ] [ g o a l . c ] = 0;
10 f o r ( i n t c = p . c - 1 ; c <= p . c + 1 ; c++) { 30 compute(grid);
11 i f ( c < 0 or c >= W) { continue; } i f 31
12 (min > g r i d [ r ] [ c ] ) { 32 i f ( g r i d [ s t a r t . r ] [ s t a r t . c ] >= H*W)
13 min = g r i d [ r ] [ c ] ; 33 { WARN("Path has not been found");
14 t . r = r ; t . c = c; 34 } else {
15 } 35 Coords pt = s t a r t ;
16 } 36 while ( p t . r != g o a l . r or p t . c != g o a l . c )
17 } 37 { path.push_back(pt);
18 p = t; 38 min8Point(grid, p t ) ;
19 return p; 39 }
20 } 40 path.push_back(goal);
41 }
42 return path;
43 }
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

DT Example
□ δ = 10 cm, L = 27.2 m

□ δ = 30 cm, L =
42.8 m
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Outlin
e
Grid-based Planning

DT for Path Planning

Graph Search

Algorithms D* Lite

Path Planning based

on Reaction-Diffusion
Process
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Graph Search
□ Algorithms
The grid can be considered as a graph and the path can be found using
graph search algorithms.
□ The search algorithms working on a graph are of general use, e.g.,
□ Breadth-first search (BFS);
□ Depth first search (DFS);
□ Dijsktra’s algorithm,;
□ A* algorithm and its variants.
□ There can be grid based speedups techniques, e.g.,
□ Jump Search Algorithm (JPS) and JPS+.
□ There are many search algorithms for on-line search, incremental
search and with any-time and real-time properties, e.g.,
□ Lifelong Planning A* (LPA*).
Koenig, S., Likhachev, M. and Furcy, D. (2004): Lifelong Planning A*. AIJ.
□ E-Graphs – Experience graphs
Phillips, M. et al. (2012): E-Graphs: Bootstrapping Planning with Experience Graphs. RSS.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* RD-based Planning
Lite

Examples of Graph/Grid Search

Algorithms

https://www.youtube.com/watch?v=U2XNjCoKZjM.mp4
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

A* Algorithm
□ A* uses a user-defined h-values (heuristic) to focus the search.
Peter Hart, Nils Nilsson, and Bertram Raphael,
1968
□ Prefer expansion of the node n with the lowest value
f (n) = g (n) + h(n),
where g (n) is the cost (path length) from the start to n and h(n) is the
estimated cost from n to the goal.
□ h-values approximate the goal distance from particular nodes.
□ Admissiblity condition – heuristic always underestimate the remaining cost
to reach the goal.
□ Let h∗(n) be the true cost of the optimal path from n to the goal.
□ Then h(n) is admissible if for all n: h(n) ≤ h∗(n).
□ E.g., Euclidean distance is admissible.
□ A straight line will always be the shortest path.
□ Dijkstra’s algorithm – h(n) = 0.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

A* Implementation
□ Notes
The most costly operations of A* are:
□ Insert and lookup an element in the closed list;
□ Insert element and get minimal element (according to f () value) from the
open list.
□ The closed list can be efficiently implemented as a hash set.
□ The open list is usually implemented as a priority queue, e.g.,
□ Fibonacii heap, binomial heap, k-level bucket;
□ binary heap is usually sufficient with O(logn).
□ Forward A*
1. Create a search tree and initiate it with the start location.
2. Select generated but not yet expanded state s with the smallest f -value,
f (s) = g (s) + h(s).
3. Stop if s is the goal.
4. Expand the state s.
5. Goto Step 2.
Similar to Dijsktra’s algorithm but it uses f (s) with the heuristic h(s) instead of
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Dijsktra’s vs A* vs Jump Point Search

(JPS)

https://www.youtube.com/watch?v=ROG4Ud08lLY
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Jump Point Search Algorithm for Grid-based Path

□ Planning
Jump Point Search (JPS) algorithm is based on a macro operator that
identifies and selectively expands only certain nodes (jump
points).
Harabor, D. and Grastien, A. (2011): Online Graph Pruning for Pathfinding on Grid Maps. AAAI.

□ Natural neighbors after neighbor

prunning with forced neighbors because
of obstacle.

□ Intermediate nodes on a path

connecting two jump points are never
expanded.

□ No preprocessing and no memory

overheads while it speeds up A*.
https://harablog.wordpress.com/2011/09/07/jump-po i nt -search/
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Theta* – Any-Angle Path Planning

□ Algorithm
Any-angle path planning algorithms simplify the path during the
search.
□ Theta* is an extension of A* with LineOfSight().
Nash, A., Daniel, K, Koenig, S. and Felner, A. (2007): Theta*: Any-Angle Path
Planning on Grids. AAAI.

Algorithm 2: Theta* Any-Angle Planning

if LineOfSight(parent(s), s’) then
/* Path 2 – any-angle path */
if g(parent(s))+ c(parent(s), s’) < g(s’) then
parent(s’) := parent(s);
g(s’) := g(parent(s)) + c(parent(s), s’);
else
/* Path 1 – A* path */
if g(s) + c(s,s’) < g(s’) then
parent(s’):= s;
g(s’) := g(s) + c(s,s’);

□ Path 2: considers path from start to parent(s) and from

parent(s) to s’ if s’ has line-of-sight to parent(s).
http://aigamedev.com/open/tutorials/theta-s t a r -any-angl e-paths/
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Theta* Any-Angle Path Planning

□ Examples
Example of found paths by the Theta* algorithm for the same problems as for
the DT-based examples on Slide 42.

δ = 10 cm, L = 26.3 m δ = 30 cm, L =

40.3 m
The same path planning problems solved by D T (without path smoothing) have
Lδ=10 =
27.2 m and Lδ=30 = 42.8 m, while D T seems to be significantly faster.
□ Lazy Theta* – reduces the number of line-of-sight checks.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

A* Variants – Online
□ Search
The state space (map) may not be known exactly in advance.
□ Environment can dynamically change.
□ True travel costs are experienced during the path execution.
□ Repeated A* searches can be computationally demanding.
□ Incremental heuristic search
□ Repeated planning of the path from the current state to the goal.
□ Planning under the free-space assumption.
□ Reuse information from the previous searches (closed list entries).
□ Focused Dynamic A* (D*) – h∗ is based on traversability, it has been used,
e.g., for the Mars rover “Opportunity”
Stentz, A. (1995): The Focussed D* Algorithm for Real-Time Replanning. IJCAI.
□ D* Lite – similar to D*
Koenig, S. and Likhachev, M. (2005): Fast Replanning for Navigation in Unknown
Terrain. T-RO.

□ Real-Time Heuristic Search

□ Repeated planning with limited look-ahead – suboptimal but fast
□ Learning Real-Time A* Korf, E. (1990): Real-time heuristic search. JAI.
□ (LRTA*)
Real-Time Adaptive A* (RTAA*) Koenig, S. and Likhachev, M. (2006): Real-time adaptive A*.
AAMAS.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Real-Time Adaptive A*
(RTAA*)
□ Execute A* with limited look-
while (scurr ∈/ GOAL) do
ahead. astar(lookahead);
□ Learns better informed heuristic if s’ = FAILURE then
from the experience, return FAILURE;
initially h(s), e.g., Eu- for all s ∈ CLOSED do
clidean distance. H(s) : = g(s’) + h(s’) -
g(s);
execute(plan); / / perform one
□ Look-ahead defines trade-off
stepSUCCESS;
between optimality and return
computational cost. s’ is the last state expanded during the
□ astar(lookahead) previous A* search.
A* expansion as far as ”lookahead”
nodes and it terminates with the
state s′.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Outlin
e
Grid-based Planning

DT for Path Planning

Graph Search

Algorithms D* Lite

Path Planning based

on Reaction-Diffusion
Process
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite –
Demo

https://www.youtube.com/watch?v=X5a149nSE9s
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite Overview
□ It is similar to D*, but it is based on Lifelong Planning A*.
Koenig, S. and Likhachev, M. (2002): D* Lite. AAAI.

□ It searches from the goal node to the start node, i.e., g -values estimate the
goal distance.
□ Store pending nodes in a priority queue.
□ Process nodes in order of increasing objective function value.
□ Incrementally repair solution paths when changes occur.
□ Maintains two estimates of costs per node:
□ g – the objective function value – based on what we know;
□ rhs – one-step lookahead of the objective function value – based on what we
know.
□ Consistency:
□ Consistent – g = rhs;
□ Inconsistent – g /= rhs.
□
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite: Cost
Estimates
□ rhs of the node u is computed based on g of its successors in the
graph and the transition costs of the edge to those
successors
0 if u = sstart
rhs(u) .
= mins′∈Succ(u)(g (s ) + c(s , u))
r r

otherwise
□ The key/priority of a node s on the open list is the minimum of g (s) and
rhs(s) plus a focusing heuristic h

[min(g (s), rhs(s)) + h(sstart, s); min(g (s), rhs(s))].

□ The first term is used as the primary key.

□ The second term is used as the secondary key for tie-breaking.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite
Algorithm
□ Main – repeat until the robot reaches the goal (or g (sstart ) = ∞ there is no path ).
Initialize(); Procedure Initialize
ComputeShortestPath U = 0;
();
while (sstart /= sgoal ) do foreach s ∈ S do
rhs(s) := g (s) := ∞;
sstart = argmins ′∈Succ (s start ) (c (sstart, sr) + g (sr));
rhs(sgoal ) := 0;
Move to
U.Insert(sgoal ,
sstart ; the graph for changed edge
Scan
CalculateKey(sgoal ));
costs;
foreach
if any directed
edge cost changed (u, v ) then
edgesperform with changed edge costs
do
Update the edge cost c
(u, v ); UpdateVertex(u);
foreach s ∈ U do
U.Update(s,
CalculateKey(s));
ComputeShortestPath();
U
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite Algorithm –
ComputeShortestPath()
Procedure ComputeShortestPath
while U.TopKey() < CalculateKey(sstart ) OR rhs(sstart ) /= g (sstart ) do
u := U.Pop();
if g (u) > rhs(u) then
g (u) := rhs(u);
foreach s ∈ Pred (u) do UpdateVertex(s);
else
g (u) := S
∞;
foreach s ∈ Pred (u) {u} do
UpdateVertex(s);

Procedure UpdateVertex
if u /= sgoal then rhs(u) := mins'∈Succ ( u ) (c(u, s′) + g (s
′));

if u ∈ U then U.Remove(u);
if g (u) /= rhs(u) then U.Insert(u, CalculateKey(u));

Procedure CalculateKey
return [min(g (s), rhs(s)) + h(s start , s); min(g (s), rhs(s))]
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite –
Demo

https://github.com/mdeyo/d-s t a r -l i t e
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example
3,0 3,1 3,2 3,3 3,4 Legend
Free node Obstacle
node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start □ A grid map of the
environment (what is
actually known).
1,0 1,1 1,2 1,3 1,4
□ 8-connected graph
superimposed on the grid
(bidirectional).
□ Focusing heuristic is not
0,0 goal 0,1 0,2 0,3 0,4 used (h = 0).

□ Transition costs
□ Free space – Free space: 1.0 and 1.4 (for diagonal
edge).
□ From/to obstacle: ∞.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(1)3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start Initialization
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
□ Set rhs = 0 for the goal.
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
□ Set rhs = g = ∞ for all other
1,0 1,1 1,2 1,3 1,4 nodes.
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 goal 0,1 0,2 0,3 0,4
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
rhs: 0 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(2)3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start Initialization
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
□ Put the goal to the open list.
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ It is inconsistent.
1,0 1,1 1,2 1,3 1,4
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 goal 0,1 0,2 0,3 0,4
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
rhs: 0 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning (3–init)

3,0 3,1 3,2 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ the open list (goal).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs).
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 goal 0,1 0,2 0,3 0,4
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
rhs: 0 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(3)3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ the open list (goal).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g >
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ rhs) therefore set g = rhs.
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 goal 0,1 0,2 0,3 0,4

g: 0 g: ∞ g: ∞ g: ∞ g: ∞
rhs: 0 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(4) 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
□ Expand popped node
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ (UpdateVertex() on all its
1,0 1,1 1,2 1,3 1,4 predecessors).
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ □ This computes the rhs values for
the predecessors.
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
□ Nodes that become inconsistent
0,0go al 0,1 0,2 0,3 0,4 are added to the open list.
g: 0 g: ∞ g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞

Small black arrows denote the node used for computing the rhs value, i.e., using the
respective transition cost.
□ The rhs value of (1,1) is ∞ because the transition to obstacle has cost ∞.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning (5–init)

3,0 3,1 3,2 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ the open list (1,0).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs).
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: ∞ g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(5) 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ the open list (1,0).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) set g
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ =
rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: ∞ g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(6) 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
□ Expand the popped
rhs: 2 rhs: 2.4 rhs: ∞ rhs: ∞ rhs: ∞ node (UpdateVertex() on all
1,0 1,1 1,2 1,3 1,4 predecessors in the graph).
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ □ Compute rhs values of the
predecessors accordingly.
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
□ Put them to the open list if they
0,0go al 0,1 0,2 0,3 0,4 be- come inconsistent.
g: 0 g: ∞ g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞

□ The rhs value of (0,0), (1,1) does not change.

□ They do not become inconsistent and thus they are not put on the
open list.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(7) 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: ∞ rhs: ∞ rhs: ∞ the open list (0,1).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
□ Expand the popped element,
e.g., call
0,0 go al 0,1 0,2 0,3 0,4 UpdateVertex().
g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(8) 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: ∞ g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: ∞ rhs: ∞ rhs: ∞ the open list (2,0).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(9) 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: ∞ g: ∞ g: ∞ g: ∞
□ Expand the popped element and
rhs: 2 rhs: 2.4 rhs: ∞ rhs: ∞ rhs: ∞ put the predecessors that become
1,0 1,1 1,2 1,3 1,4 inconsistent onto the open list.
g: 1 g: ∞ g: ∞ g: ∞ g: ∞
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning (10–

3,0 3,1 3,2 3,3 3,4
init) Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: ∞ g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: ∞ rhs: ∞ rhs: ∞ the open list (2,1).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs).
g: 1 g: ∞ g: ∞ g: ∞ g: ∞
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(10) 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: ∞ rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: ∞ rhs: ∞ rhs: ∞ the open list (2,1).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g >
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ rhs) and thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(11) 3,3 3,4 Legend
g: ∞ g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: ∞
□ Expand the popped element and
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: ∞ rhs: ∞ put the predecessors that become
1,0 1,1 1,2 1,3 1,4 inconsistent onto the open list.
g: 1 g: ∞ g: ∞ g: ∞ g: ∞
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(12) 3,3 3,4 Legend
g: 3 g: ∞ g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: ∞ rhs: ∞ the open list (3,0).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ □ Expand the popped element and
put the predecessors that become
0,0 go al 0,1 0,2 0,3 0,4 inconsistent onto the open list.
g: 0 g: 1 g: ∞ g: ∞ g: ∞ □ In this cases, none of the
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ become
predecessors
inconsistent.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(13) 3,3 3,4 Legend
g: 3 g: 3.4 g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: ∞ rhs: ∞ the open list (3,0).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞ □ Expand the popped element and
put the predecessors that become
0,0 go al 0,1 0,2 0,3 0,4 inconsistent onto the open list.
g: 0 g: 1 g: ∞ g: ∞ g: ∞ □ In this cases, none of the
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ become
predecessors
inconsistent.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(14) 3,3 3,4 Legend
g: 3 g: 3.4 g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: ∞ rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: 3.4 g: ∞ g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: ∞ rhs: ∞ the open list (2,2).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(15) 3,3 3,4 Legend
g: 3 g: 3.4 g: ∞ g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: 3.4 g: ∞ g: ∞
□ Expand the popped element and
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: ∞ put the predecessors that become
1,0 1,1 1,2 1,3 1,4 inconsis- tent onto the open list,
g: ∞ g: ∞ g: ∞ g: ∞ i.e., (3,2), (3,3), (2,3).
g: 1
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(16) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: 3.4 g: ∞ g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: ∞ the open list (3,2).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: ∞ □ Expand the popped element and
put the predecessors that become
0,0 go al 0,1 0,2 0,3 0,4 inconsistent onto the open list.
g: 0 g: 1 g: ∞ g: ∞ g: ∞ □ In this cases, none of the
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ predecessors become inconsistent.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(17) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: ∞ node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: 3.4 g: 4.4 g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: ∞ the open list (2,3).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: ∞
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(18) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: ∞ g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: 3.4 g: 4.4 g: ∞
□ Expand the popped element and
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: 5.4 put the predecessors that become
1,0 1,1 1,2 1,3 1,4 inconsis- tent onto the open list,
g: ∞ g: ∞ g: ∞ g: ∞ i.e., (3,4), (2,4), (1,4).
g: 1
□ The start node is on the open list.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8
□ However, the search does not
0,0 go al 0,1 0,2 0,3 0,4 finish at this stage.
g: 0 g: 1 g: ∞ g: ∞ g: ∞ □ There are still inconsistent nodes
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ (on the open list) with a lower
value of rhs.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(19) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: 3.4 g: 4.4 g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: 5.4 the open list (3,2).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8 □ Expand the popped element and
put the predecessors that become
0,0 go al 0,1 0,2 0,3 0,4 inconsistent onto the open list.
g: 0 g: 1 g: ∞ g: ∞ g: ∞ □ In this cases, none of the
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ predecessors become inconsistent.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(20) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: 3.4 g: 4.4 g: ∞
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: 5.4 the open list (1,3).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(21) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: 3.4 g: 4.4 g: ∞
□ Expand the popped element and
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: 5.4 put the predecessors that become
1,0 1,1 1,2 1,3 1,4 inconsistent onto the open list,
g: ∞ g: ∞ g: ∞ i.e., (0,3) and (0,4).
g: 1 g: 4.8
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(22) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
ComputeShortestPath
g: 2 g: 2.4 g: 3.4 g: 4.4 g: 5.4
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: 5.4 the open list (2,4).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs) and
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ thus set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8 □ Expand the popped element and
put the predecessors that become
0,0go al 0,1 0,2 0,3 0,4 inconsistent (none in this case)
g: 0 g: 1 g: ∞ g: ∞ g: ∞ onto the open list.
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

□ The start node becomes consistent and the top key on the open list is not less than the key of the
start node.
□ An optimal path is found and the loop of the ComputeShortestPath is breaked.
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(23) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
g: 2 g: 2.4 g: 3.4 g: 4.4 g: 5.4 □ Follow the gradient of g values
from the start node.
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: 5.4
1,0 1,1 1,2 1,3 1,4

g: 1 g: ∞ g: ∞ g: 4.8 g: ∞
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(24) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
g: 2 g: 2.4 g: 3.4 g: 4.4 g: 5.4 □ Follow the gradient of g values
from the start node.
rhs: 2 rhs: 2.4 rhs: 3.4 rhs: 4.4 rhs: 5.4
1,0 1,1 1,2 1,3 1,4

g: 1 g: ∞ g: ∞ g: 4.8 g: ∞
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(25) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
g: 2 g: 2.4 g: ∞ g: 4.4 g: 5.4 □ A new obstacle is detected
rhs: ∞ during the movement from (2,3)
rhs: 2 rhs: 2.4 rhs: 4.4 rhs: 5.4
to (2,2).
1,0 1,1 1,2 1,3 1,4 □ Replanning is needed!
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning (25

update) Legend
3,0 3,1 3,2 3,3 3,4

g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle

rhs: 5.8
node
rhs: rhs: rhs: rhs:
3 3.4 3.8 4.8 On open list Active node
2,0 2,1 2,2 2,3 2,4 start
g: 2 g: 2.4 g: ∞ g: 4.4 g: 5.4 □ All directed edges with changed
edge, we need to call the
rhs: rhs: rhs: ∞ rhs: rhs: UpdateVertex().
2 2.4 4.4 5.4
1,0 1,1 1,2 1,3 1,4 □ All edges into and out of (2,2)
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ have to be considered.
rhs: rhs: ∞ rhs: rhs: rhs:
1 ∞ 4.8 5.8
0,2 0,3 0,4
0,0 goal 0,1
g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: rhs: rhs: ∞ rhs: rhs:
0 1 5.8 6.2
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning (26

update 1/2) Legend
3,0 3,1 3,2 3,3 3,4

g: 3 g: 3.4 g: g: 4.8 g: ∞ Free node Obstacle

3.8 rhs: 5.8
node
rhs: 3 rhs: 3.4 rhs: rhs: 4.8
3.8 On open list Active node
2,0 2,1 2,2 2,3 2,4 start Update Vertex
g: g: 2.4 g: ∞ g: 4.4 g: 5.4 □ Outgoing edges from (2,2).
2
rhs: rhs: 2.4 rhs ∞ rhs: 4.4 rhs: 5.4 □ Call UpdateVertex() on (2,2).
2 :
1,0 1,1 1,2 1,3 1,4 □ The transition costs are now ∞
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ because of obstacle.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8 □ Therefore the rhs = ∞ and (2,2)
be- comes inconsistent and it is
0,0 goal 0,1 0,2 0,3 0,4
put on the open list.
g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

91 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning (26

update 2/2) Legend
3,0 3,1 3,2 3,3 3,4

g: 3 g: 3.4 g: g: 4.8 g: ∞ Free node Obstacle

3.8 rhs: 5.8
node
rhs: 3 rhs: 3.4 rhs: rhs: 4.8
3.8 On open list Active node
2,0 2,1 2,2 2,3 2,4 start Update Vertex
g: g: 2.4 g: ∞ g: 4.4 g: 5.4 □ Incomming edges to (2,2).
2
rhs: rhs: 2.4 rhs: ∞ rhs: 4.4 rhs: 5.4 □ Call UpdateVertex() on the
2
1,0 1,1 1,2 1,3 1,4 neighbors (2,2).
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ □ The transition cost is ∞ , and
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8 therefore, the rhs value previously
computed using (2,2) is changed.
0,0 goal 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

92 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(27) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
Update Vertex
g: 2 g: 2.4 g: ∞ g: 4.4 g: 5.4
□ The neighbor of (2,2) is (3,3).
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 4.4 rhs: 5.4
□ The minimum possible rhs
1,0 1,1 1,2 1,3 1,4 value of (3,3) is 4.8 but it is
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ based on the g value of (3,2)
and not (2,2), which is the
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8
detected obstacle.
0,0 go al 0,1 0,2 0,3 0,4 □ The node (3,3) is still consistent
g: 0 g: 1 g: ∞ g: ∞ g: ∞ and thus it is not put on the open
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2 list.

93 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(28) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
Update Vertex
g: 2 g: 2.4 g: ∞ g: 4.4 g: 5.4
□ (2,3) is also a neighbor of (2,2).
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 5.4
□ The minimum possible rhs
1,0 1,1 1,2 1,3 1,4 value of (2,3) is 5.2 because of
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ (2,2) is obsta-
cle (using (3,2) with 3.8 + 1.4).
rhs: 1 rhs: ∞ rhs: ∞ rhs: 4.8 rhs: 5.8
□ The rhs value of (2,3) is different
0,0 go al 0,1 0,2 0,3 0,4 than
g: 0 g: 1 g: ∞ g: ∞ g: ∞ g thus (2,3) is put on the open list.
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

94 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(29) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
Update Vertex
g: 2 g: 2.4 g: ∞ g: 4.4 g: 5.4
□ Another neighbor of (2,2) is (1,3).
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 5.4
□ The minimum possible rhs
1,0 1,1 1,2 1,3 1,4 value of (1,3) is 5.4 computed
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ based on g of (2,3) with 4.4 + 1
= 5.4.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 5.4 rhs: 5.8
□ The rhs value is always computed
0,0 go al 0,1 0,2 0,3 0,4 using the g values of its
g: 0 g: 1 g: ∞ g: ∞ g: ∞ successors.
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

95 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning (29

update) Legend
3,0 3,1 3,2 3,3 3,4

g: 3 g: 3.4 g: g: 4.8 g: ∞ Free node Obstacle

3.8 rhs:
node
rhs: rhs: rhs: rhs:
3 3.4 3.8 4.8 5.8 On open list Active node
2,0 2,1 2,2 2,3 2,4 start Update Vertex
g: g: g: ∞ g: 4.4 g: 5.4
2 2.4
□ None of the other neighbor of
rhs: rhs: rhs: ∞ rhs: rhs: (2,2) end up being inconsistent.
2 2.4 5.2 5.4
1,0 1,1 1,2 1,3 1,4 □ We go back to
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ calling ComputeShortestPath()
until an optimal path is
rhs: rhs: ∞ rhs: ∞ rhs: rhs: determined.
1 5.4 5.8
0,2 0,3 0,4
0,0 goal 0,1
g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: rhs: rhs: ∞ rhs: rhs:
0 1 5.8 6.2
□ The node corresponding to the robot’s current position is inconsistent and its key is
greater than the minimum key on the open list.
□ Thus, the optimal path is not found yet.
96 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(30) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: 4.4 g: 5.4
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 5.4 the open list (2,2), which is
1,0 1,1 1,2 1,3 1,4 obstacle.
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ □ It is under-consistent (g < rhs),
there- fore set g = ∞.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 5.4 rhs: 5.8
□ Expand the popped element and
0,0go al 0,1 0,2 0,3 0,4 put the predecessors that become
g: 0 g: 1 g: ∞ g: ∞ g: ∞ inconsistent (none in this case)
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2 onto the open list.

□ Because (2,2) was under-consistent (when popped), UpdateVertex() has to be

called on it.
□ However, it has no effect as its rhs value is up to date and consistent. 97 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning (31–

3,0 3,1 3,2 3,3 3,4
init) Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: 4.4 g: 5.4
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 5.4 the open list (2,3).
1,0 1,1 1,2 1,3 1,4 □ It is under-consistent g < rhs.
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞
rhs: 1 rhs: ∞ rhs: ∞ rhs: 5.4 rhs: 5.8
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

98 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(31) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: 5.4
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 5.4 the open list (2,3).
1,0 1,1 1,2 1,3 1,4 □ It is under-consistent g < rhs
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ therefore set g = ∞.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 5.4 rhs: 5.8
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

99 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(32) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: 5.4
□ Expand the popped element and
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 6.2 update the predecessors.
1,0 1,1 1,2 1,3 1,4 □ (2,4) becomes inconsistent.
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ □ (1,3) gets updated and still
rhs: 1 rhs: ∞ rhs: ∞ rhs: 6.8 rhs: 5.8 inconsis- tent.
□ The rhs value (1,4) does not
0,0 go al 0,1 0,2 0,3 0,4
changed, but it is now computed
g: 0 g: 1 g: ∞ g: ∞ g: ∞ from the g value of (1,3).
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

100 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(33) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: 5.4
□ Because (2,3) was under-
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 6.2 consistent (when popped), call
1,0 1,1 1,2 1,3 1,4 UpdateVertex() on it is needed.
g: 1 g: ∞ g: ∞ g: 4.8 g: ∞ □ As it is still inconsistent it is put
back onto the open list.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 5.4 rhs: 5.8
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

101 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(34) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: 5.4
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 6.2 the open list (1,3).
1,0 1,1 1,2 1,3 1,4 □ It is under-consistent (g < rhs),
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ there- fore set g = ∞.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 6.8 rhs: 5.8
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: 5.8 rhs: 6.2

102 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(35) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: 5.4
□ Expand the popped element and
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 6.2 update the predecessors.
1,0 1,1 1,2 1,3 1,4 □ (1,4) gets updated and still
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ inconsis- tent.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 6.8 rhs: 6.4 □ (0,3) and (0,4) get updated and
now consistent (both g and rhs are
0,0 go al 0,1 0,2 0,3 0,4 ∞).
g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞

103 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(36) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: ∞ g: 5.4
□ Because (1,3) was under-
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 6.2 consistent (when popped), call
1,0 1,1 1,2 1,3 1,4 UpdateVertex() on it is needed.
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ □ As it is still inconsistent it is put
back onto the open list.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 6.8 rhs: 6.4
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞

104 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(37) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: 5.2 g: 5.4
□ Pop the minimum element from
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 6.2 the open list (2,3).
1,0 1,1 1,2 1,3 1,4 □ It is over-consistent (g > rhs),
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ there- fore set g = rhs.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 6.8 rhs: 6.4
0,0 go al 0,1 0,2 0,3 0,4

g: 0 g: 1 g: ∞ g: ∞ g: ∞
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞

105 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(38) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
ComputeShortestPath
g: 2 g: 2.4 g: ∞ g: 5.2 g: 5.4
□ Expand the popped element and
rhs: 2 rhs: 2.4 rhs: ∞ rhs: 5.2 rhs: 6.2 update the predecessors.
1,0 1,1 1,2 1,3 1,4 □ (1,3) gets updated and still
g: 1 g: ∞ g: ∞ g: ∞ g: ∞ inconsis- tent.
rhs: 1 rhs: ∞ rhs: ∞ rhs: 6.2 rhs: 6.4 □ The node (2,3) corresponding to
the robot’s position is consistent.
0,0 go al 0,1 0,2 0,3 0,4
□ Besides, the top of the key on the
g: 0 g: 1 g: ∞ g: ∞ g: ∞ open list is not less than the key
rhs: 0 rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞ of (2,3).
□ The optimal path has been found
and we can break out of the loop.

106 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite – Example Planning

3,0 3,1 3,2
(39) 3,3 3,4 Legend
g: 3 g: 3.4 g: 3.8 g: 4.8 g: ∞ Free node Obstacle
rhs: 3 rhs: 3.4 rhs: 3.8 rhs: 4.8 rhs: 5.8 node
On open list Active node
2,0 2,1 2,2 2,3 2,4 start
g: 2 g: 2.4 g: ∞ g: 5.2 g: 5.4 □ Follow the gradient of g values
rhs: ∞ from the robot’s current position
rhs: 2 rhs: 2.4 rhs: 5.2 rhs: 6.2
(node).
1,0 1,1 1,2 1,3 1,4

g: 1 g: ∞ g: ∞ g: ∞ g: ∞
rhs 1 rhs: ∞ rhs: ∞ rhs: 6.2 rhs: 6.4
:
0,0 go al 0,1 0,2 0,3 0,4

g: g: 1 g: ∞ g: ∞ g: ∞
0
rhs: 1 rhs: ∞ rhs: ∞ rhs: ∞
rhs: 0

107 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

D* Lite –
Comments

□ D* Lite works with real valued costs, not only with binary costs
(free/obstacle).
□ The search can be focused with an admissible heuristic that would be
added to the g
and rhs values.
□ The final version of D* Lite includes further optimization (not shown in the
example).
□ Updating the rhs value without considering all successors every time.
□ Re-focusing the search as the robot moves without reordering the entire
open list.

108 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Outlin
e
Grid-based Planning

DT for Path Planning

Graph Search

Algorithms D* Lite

Path Planning based

on Reaction-Diffusion
Process

109 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Reaction-Diffusion Processes
Background
□ Reaction-Diffusion (RD) models – dynamical systems capable to reproduce
the au- towaves.
□ Autowaves - a class of nonlinear waves that propagate through an active
media.
At the expense of the energy stored in the medium, e.g., grass combustion.
□ RD model describes spatio-temporal evolution of two state variables u =
u(→x , t ) and
v = v (→x , t ) in space →x and time t
u˙ = f (u, v ) +
Du Δ u
,
v˙ = g (u, v ) +
Dv Δv
where Δ is the Laplacian. 110 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Reaction-Diffusion
Background
□ FitzHugh-Nagumo (FHN) FitzHugh R, Biophysical Journal (1961)
model
u˙ = ε u — u3 — v + φ +
Du Δ u
,
v˙ = (u — αv + β) + Dv Δu
where α, β, ϵ, and φ are parameters of the model.
□ Dynamics of RD system isεdetermined
u — u3 — vby
+ the
φ associated
= nullcline
configurations for u˙=0 and — αvin+the absence of diffusion, i.e.,
(uv˙=0
0,
β)
which have associated geometrical = 0,
shapes.

111 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* RD-based Planning
Lite

Nullcline Configurations and Steady

0.5
States
□ Nullclines intersections represent:
□ Stable States (SSs);
v
0.0 □ Unstable States.
□ Bistable regime
−0. The system (concentration levels of (u, v) for each grid cell)
5
tends to be in SSs.
−1.5 −1.0 0.0 0.5 1.0
−0.5 1.5
u
□ We can modulate relative stability of both SS. SS+

“preference” of SS + over SS − .
□ System moves from SS − to SS +, if a small
perturbation is intro- duced.
□ The SS s are separated by a mobile frontier – a kind of
traveling frontwave (autowaves). SS+

112 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

RD-based Path Planning – Computational

Model
□ Finite difference method on a Cartesian grid with Dirichlet
conditions
boundary discretization → grid based computation → grid map
□ (FTCS).
External forcing – introducing additional information
i.e., constraining concentration levels to some specific values.
□ Two-phase evolution of the underlying RD model.
1. Propagation phase
□ Freespace is set to SS − and the start location SS +.

□ Parallel propagation of the frontwave with non-annihilation

property.
Vázquez-Otero and Muñuzuri, CNNA (2010)
□ Terminate when the frontwave reaches the goal.
2. Contraction phase
□ Different nullclines configuration.
□ Start and goal positions are forced towards SS +.
□ SS − shrinks until only the path linking the forced points

remains. 113 / 118

Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Example of Found
Paths

700 × 700 700 × 700 1200 × 1200

□ The path clearance maybe adjusted by the wavelength and size of the
computational grid.
Control of the path distance from the obstacles (path safety).
114 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Comparison with Standard

Distance Transform Approaches
Voronoi Diagram Reaction-Diffusion

Jarvis R Beeson P, Jong N, Kuipers B Otero A, Faigl J, Muñuzuri A

Advanced Mobile Robots (1994) ICRA (2005) IROS (2012)

□ RD-based approach provides competitive paths regarding path length

and clearance, while they seem to be smooth.
115 / 118
Grid-based Planning D T for Path Planning Graph Search Algorithms D* Lite RD-based Planning

Robustness to Noisy
Data

Vázquez-Otero, A., Faigl, J., Duro, N. and Dormido, R. (2014): Reaction-Diffusion based Computational Model for Autonomous Mobile
Robot Exploration of Unknown Environments. International Journal of Unconventional Computing (IJUC).

116 / 118
Topics Discussed

Summary of the
Lecture

117 / 118
Topics Discussed

Topics Discussed
□ Motion and path planning problems
□ Path planning methods – overview
□ Notation of configuration space
□ Path planning methods for geometrical map representation
□ Shortest-Path Roadmaps
□ Voronoi diagram based planning
□ Cell decomposition method
□ Distance transform can be utilized for kind of navigational function
□ Front-Wave propagation and path simplification
□ Artificial potential field method
□ Graph search (planning) methods for grid-like representation
□ Dijsktra’s, A*, JPS, Theta*
□ Dedicated speed up techniques can be employed to decreasing computational burden, e.g.,
JPS
□ flrid-path can be smoothed, e.g., using path simplification or Theta* like algorithms
□ We can avoid demanding planning from scratch reusing the previous plan for
the updated environment map, e.g., using D * Lite
□ Unconventional reaction-diffusion based planning (informative)
□ Next: Robotic Information Gathering – Mobile Robot Exploration 118 / 118