Robot Motion Planning

Rafael Murrieta Cid

CIMAT
Email: [email protected]
Web page:
http://www.cimat.mx/~murrieta/

Slides were borrowed from

Profs. J.C. Latombe and J.P. Laumond
 Goal of Motion Planning
• Compute motion strategies, e.g.:
– geometric paths
– time-parameterized trajectories
– sequence of sensor-based motion commands
• To achieve high-level goals, e.g.:
– go to A without colliding with obstacles
– assemble product P
– build map of environment E
– find object O
2
 Basic Motion Planning Problem
Compute a collision-free path for a rigid or
articulated object among static obstacles
§ Inputs:
― Geometry of moving object
and obstacles
― Kinematics of moving object
(degrees of freedom)
― Initial and goal configurations
(placements)
§ Output:
Continuous sequence of
collision-free robot configurations
connecting the initial and goal
configurations 3
 Extensions of Basic Problem
§ Moving obstacles § Optimal planning
§ Multiple robots § Uncertainty in model,
§ Movable objects control and sensing
§ Assembly planning § Exploiting task
§ Goal is to acquire mechanics (sensorless
motions, under-
information by sensing
actualted systems)
§ Model building
§ Object finding/tracking § Physical models and
§ Inspection deformable objects
§ Nonholonomic § Integration of planning
constraints and control
§ Dynamic constraints § Integration with higher-
level planning
§ Stability constraints
4
Some Applications

5
Lunar Vehicle (ATHLETE, NASA/JPL)

6
Dexterous Manipulation

7
Manipulation of Deformable Objects

Topologically
defined goal

8
Radiosurgical Planning

CyberKnife (Accuray) 9
Building Code Verification

10
24-inch turning radius
9-inch turning radius
Egress Simulation

Primary
escape route
Potential
congesting areas

Secondary
escape
route

11
 Transportation of A380
Fuselage through Small Villages

12
Kineo
Paths
 Disc Robot in 2-D Workspace
Workspace W Configuration space C
path

configuration = coordinates (x,y) of robot’s center

configuration space C = {(x,y)}
free space F = subset of collision-free configurations
 Translating Polygon in 2-D
Workspace

reference
point
Translating & Rotating Polygon in
2-D Workspace
Tool: Configuration Space (C-
Space C)
 Configuration Space
§ Space of all its possible configurations
§ But the topology of this space is usually
not that of a Cartesian space

q2
Configuration Space

Move
Co nfiguration Space
Configuration Space
 Every robot maps to a point in its
configuration space ...
15 D ~40 D

6D 11 D
q0 q1

qn

q3
~65-120 D q4
... and every robot path is a curve
in configuration space

q0 q1

qn

q3
q4
 But how do obstacles (and other
constraints) map in configuration space?
15 D ~40 D

6D 12 D
q0 q1

qn

q3
~65-120 D q4
 Probabilistic Roadmaps
(Sampling-Based Planning)

25
§ The cost of computing an exact representation
of the configuration space is often prohibitive.
§ But very fast algorithms exist to check if a
robot at a given configuration collides with
obstacles.
§ à Basic idea of Probabilistic Roadmaps (PRMs):
Compute a very simplified representation of the
free space by sampling configurations at random.
26
 Probabilistic Roadmap (PRM)
forbidden space free space
Space Ân

27
 Probabilistic Roadmap (PRM)
Configurations are sampled by picking coordinates at random

28
 Probabilistic Roadmap (PRM)
Configurations are sampled by picking coordinates at random

29
 Probabilistic Roadmap (PRM)
Sampled configurations are tested for collision (in workspace!)

30
 Probabilistic Roadmap (PRM)
The collision-free configurations are retained as “milestones”

31
 Probabilistic Roadmap (PRM)
Each milestone is linked by straight paths to its k-nearest neighbors

32
 Probabilistic Roadmap (PRM)
Each milestone is linked by straight paths to its k-nearest neighbors

33
 Probabilistic Roadmap (PRM)
The collision-free links are retained to form the PRM

34
 Probabilistic Roadmap (PRM)
The start and goal configurations are included as milestones

35
 Probabilistic Roadmap (PRM)
The PRM is searched for a path from s to g

36
Many degrees of freedom
 RRTs

[LaValle, Kuffner, IJRR02]

• RRT is a data structure and algorithm that is
designed for efficiently searching no convex high-
dimensional spaces.
• RRT can be considered as a Monte-Carlo way of biasing search
into largest Voronoi Regions.
Basic construction
RRT Algorithm
 Sampling-based
Algorithms for Optimal
Motion Planning
 RRT*-Map Specs

( )
µ X free = 0.92 $µ X
( ) '1 d
1d free
d =2 γ RRT > (2(1+1 d)) & ) ≈ 0.9373
& ζd )
ζd = π % (

€ # log( n ) &1 d n = 10
€ rn = γ RRT % (
$ n ' r10 > 0.4497

€ €
RRT*-Tree after iteration 9

x init

€
 RRT*-New Sample

x init x rand

€ €
 RRT*-New Sample

x init x rand
x nearest

€ €
€
 RRT*-New Sample

η= 2
x init x new
x nearest
€
€ €
€
 RRT*-New Sample

x init x new
x min

€ €
€
RRT*-Connect Min Cost Path

X near

x init x new
€

€ €
RRT*-Connect Min Cost Path

X near

x init x new
x€min

€ €
€
RRT*-Connect Min Cost Path

x init x new
x min

€ €
€
 RRT*-Rewire

X near

x init x new
€

€ €
 RRT*-Rewire

Cost(x near ) = 4
Cost(x near ) = 4.5

€
€

x new x near
x init

€ €
€
 RRT*-Rewire

x new x near
x init

€ €
€
RRT algorithm
RRT* algorithm
 Collision Checking
§ Check if objects overlap
 Hierarchical Collision Checking
§ Enclose objects into bounding volumes (spheres or boxes)
§ Check the bounding volumes first
§ Decompose an object into two
BVH of a 3D Triangulated Cat
Nonholonomic
Motion Planning

59
Configuration Space

• Any admissible motion for the 3D

mechanical system appears a
collision-free path for a point in the
CSpace
Configuration Space
Algorithmic Path Planning

• Translating the continuous problem into a

combinatorial one

• Capturing the topology of CSfree

with graphs
Algorithmic Path Planning: few dates

• 80’s Configuration Space Approach, Decidability,

Deterministic Approaches
« Robot Motion Planning » Latombe,
Kluwer Academics Pub., 1991

• 90’s Nonholonomy and Probabilistic Approaches

« Robot Motion Planning and Control» Laumond,
Springer Verlag, 1998
http://www.laas.fr/~jpl (free of charge)

Choset et al, 2005

LaValle, 2006
Nonholonomic Path Planning

• Any admissible motion for the 3D

mechanical system appears a
collision-free path for a point
in the Cspace

• Holonomic systems: converse is true

• Nonholonomic ones: converse is not true

Differential geometry view point

x! = y!
x! cos q - y! sin q = 0

Integrable Not integrable

Dim(Reachable(q))=1 Dim(Reachable(q))=3
Differential geometry view point

The time that will

never happen!!!

x! = y!

Integrable
Dim(Reachable(q))=1
 Control view point

æ x! ö æ cos q ö æ0ö
ç ÷ ç ÷ ç ÷
ç y! ÷ = ç sin q ÷u1 + ç 0 ÷u2
çq! ÷ ç 0 ÷ ç1 ÷
è ø è ø è ø

Lie Bracket x! cos q - y! sin q = 0

Not integrable
Dim(Reachable(q))=3
Lie Bracket and Controlability

æ x! ö æ cos q ö æ cos q ö
ç ÷ ç ÷ ç ÷
ç y! ÷ = ç sin q ÷u + ç sin q ÷v
çq! ÷ ç 0 ÷ ç1 ÷
è ø è ø è ø
Small-time Controllability
Small-time Controllability
Small-time Controllability
Small-time Controllability
 Path Planning

• For small-time controllable

systems the existence of an
admissible motion is
characterized by the existence
of a path lying in an open
connected component of free-
CSpace
Two central questions

• Is my system nonholonomic ?

• Is my nonholonomic system small-time

controllable ?
Two central questions

• Is my system nonholonomic ?
Frobenius Theorem

• Is my nonholonomic system small-time

controllable ?
Lie Algebra Rank Condition
 Bibliography
• Robot motion planning, Kluwer,
Jean-Claude Latombe, 1991.
• Robot motion planning and control, Springer,
Jean-Paul Laumond, 1998.
• Planning algorithms, Cambridge, Steven M.
LaValle, 2006.
• Sampling-based algorithms for optimal motion
planning, Sertac Karaman and Emilio Frazzoli,
Int. J. Robotics Research, 2011.