CS659: Autonomous Cyber-Physical Systems

Basics of Motion Planning

Indranil Saha

Department of Computer Science and Engineering

Indian Institute of Technology Kanpur

February 16, 2021

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 1/36

Discrete Feasible Planning Problem

Given:
1 A nonempty state space X , which is a finite or countably
infinite set of states.
2 For each state x ∈ X , a finite action space U(x).
3 A state transition function f that produces a state
f (x, u) ∈ X for every x ∈ X and u ∈ U(x). The state
transition equation is derived from f as x 0 = f (x, u).
4 An initial state xI ∈ X .
5 A goal set XG ⊂ X .

Find: A sequence of states (x1 , x2 , . . . , xn ) such that x1 = xI ,

xn ∈ XG , and for all 1 < i < n, there exists an u ∈ U(xi ) such
that xi+1 = f (xi , u)

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 2/36

Graph Formulation of the Planning Problem

Given: A graph G = (V , E) where

The set of vertices V is the state space X .

A directed edge e ∈ E from x ∈ X to x 0 ∈ X exists in the

graph if and only if there exists an action u ∈ U(x) such
that x 0 = f (x, u).

The initial state and goal set are designated as special

vertices vI and VG in the graph.
Find: A path from vertices vI to vG ∈ VG in the graph.

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 3/36

Optimal Path Finding Problem

Each control input u is associated with a cost c(u)

An edge e ∈ E corresponding to an action u, the weight of

the edge e, denoted by w(e) is equal to c(u)

Finding an optimal path is now equivalent to finding the

shortest path in the graph.

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 4/36

Example

Let X be the set of all integer pairs of the form (i, j), in which
i ∈ {1, . . . , 18} and j ∈ {1, . . . , 12}.
O is the set of obstacles.
U = {(0, 1), (0, −1), (1, 0), (−1, 0)}, U(x) = U for all x ∈ X .
The state transition equation is f (x, u) = x + u, in which x ∈ X
and u ∈ U are treated as two-dimensional vectors for the
purpose of addition.
For any x ∈ O, its corresponding vertex and associated edges
deleted from the state transition graph.
CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 5/36
Search Algorithm for Uniform Edge Cost

Apply Breadth-First Search (BFS) or Depth-First Search

(DFS) Algorithm.

Search start from vI

Search ends when a node vG ∈ VG is visited.

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 6/36

General Search Algorithms

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 7/36

Dijkstra’s Shortest Path Algorithm

Priority Basis: Cost to Come

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 8/36

Example: Dijkstra’s Shortest Path Algorithm

f (a) = 1.5, parent(a) = vI ,

f (d) = 2, parent(a) = vI
f (b) = 3.5, parent(b) = a
f (e) = 5, parent(e) = d
f (c) = 6.5, parent(f ) = b
f (g) = 7, parent(g) = e
f (g) = 7, parent(g) = e (tried vertex c)
CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 9/36
A* Algorithm
Priority Basis: Cost to Come (g) + heuristic cost to go (h).

Heuristic cost has to be an under-approximation of the optimal

cost to preserve optimality

Examples
Manhattan Distance
Euclidian Distance

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 10/36

Example: A* Algorithm

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 11/36

Example: A* Algorithm

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 12/36

Example: A* Algorithm

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 13/36

Example: A* Algorithm

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 14/36

Sampling Based Motion Planning Algorithm

Motion planning in the continuous configuration space.

Probabilistic Roadmap (PRM)

Rapidly Explored Random Tree (RRT)

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 15/36

Probabilistic Roadmap

Initialize set of points with xS and xG .

Randomly sample points in configuration space.

Connect nearby points if they can be reached from each

other.

Find path from xS to xG in the graph.

Alternatively: keep track of connected components

incrementally, and declare success when xS and xG are in
same connected component.

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 16/36

Example: Execution of the PRM Algorithm

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 17/36

Example: Execution of the PRM Algorithm
Configurations are sampled by picking coordinates at random.

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 18/36

Example: Execution of the PRM Algorithm
Configurations are sampled by picking coordinates at random

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 19/36

Example: Execution of the PRM Algorithm
Sampled configurations are tested for collision

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 20/36

Example: Execution of the PRM Algorithm
The collision-free configurations are retained as milestones

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 21/36

Example: Execution of the PRM Algorithm
Each milestone is linked by straight paths to its nearest
neighbors

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 22/36

Example: Execution of the PRM Algorithm
Each milestone is linked by straight paths to its nearest
neighbors

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 23/36

Example: Execution of the PRM Algorithm
Each milestone is linked by straight paths to its nearest
neighbors.

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 24/36

Example: Execution of the PRM Algorithm
The start and goal configurations are included as milestones.

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 25/36

Example: Execution of the PRM Algorithm
The PRM is searched for a path from s to g

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 26/36

PRM’s Pros and Cons

Strength:
Probabilistically complete: i.e., with probability one, if run
for long enough the graph will contain a solution path if one
exists.

Weakness:
Build graph over state space but no particular focus on
generating a path.

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 27/36

Rapidly Explored Random Tree
Basic Idea:
Build up a tree through generating “next states” in the tree
by executing random controls.
Apply measures to ensure good coverage

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 28/36

Illustration of the RRT Algorithm

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 29/36

RRT Exploration

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 30/36

RRT Exploration Strategies

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 31/36

RRT*

RRT is not optimal.

RRT* is the asymptotic optimal version of RRT

Main Idea:
Consider the nearby vertices of a given vertex u
For a nearby vertex v , introduce new parent w 0 through
which v is reachable from u in a less cost than through v ’s
original parent w.

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 32/36

RRT Exploration

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 33/36

RRT Vs. RRT*

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 34/36

References

S. M. LaValle. Planning Algorithms. Cambridge University

Press, 2006. (Chapter 2, Chapter 5)

Lecture Note by Peter Abbeel (UC Berkeley)

https://people.eecs.berkeley.edu/ pabbeel/cs287-
fa12/slides/SamplingBasedMotionPlanning.pdf

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 35/36

Next Class

Path Planning from Temporal Logic Specifications

CS659: Autonomous Cyber-Physical Systems Basics of Motion Planning 36/36