16.412/6.834J Cognitive Robotics February 7th, 2005

Probabilistic Methods for

Kinodynamic Path Planning

Based on Past Student Lectures by: Lecturer: Paul Elliott, Aisha Walcott,

Nathan Ickes and Stanislav Funiak Prof. Brian C. Williams

How do we maneuver or manipulate?

courtesy NASA JSC

courtesy NASA Ames

Outline

� Roadmap path planning

� Probabilistic roadmaps

� Planning in the real world

� Planning amidst moving obstacles

� RRT-based planners

� Conclusions

Outline

• Roadmap path planning

• Probabilistic roadmaps

• Planning in the real world

• Planning amidst moving obstacles

• RRT-based planners

• Conclusions

Brian Williams, Fall 03

Path Planning through Obstacles

Start

position

Goal Brian Williams, Fall 03 position

1. Create Configuration SpaceAssume: Vehicle

Start

position

translates, but no rotation

Idea: Transform to equivalent Goal

positionproblem of navigating a point. Brian Williams, Fall 03

2. Map From Continuous Problem to a

Roadmap: Create Visibility GraphStart

position

Goal Brian Williams, Fall 03 position

2. Map From Continuous Problem to a

Roadmap: Create Visibility GraphStart

position

Goal Brian Williams, Fall 03 position

3. Plan Shortest Path

Start

position

Goal Brian Williams, Fall 03 position

Resulting Solution

Start

position

Goal Brian Williams, Fall 03 position

A Visibility Graph is One Kind of Roadmap

Start

position

What are some other types of roadmaps?

Goal Brian Williams, Fall 03 position

Roadmaps: Voronoi Diagrams

Brian Williams, Fall 03

Lines equidistant from CSpace obstacles

Roadmaps: Approximate Fixed Cell

Brian Williams, Fall 03

Roadmaps: Approximate Fixed Cell

Brian Williams, Fall 03

Roadmaps: Exact Cell Decomposition

Brian Williams, Fall 03

Khatib 1986Potential Functions Latombe 1991

Koditschek 1998

Attractive Potential Repulsive Potential Combined Potential for goals for obstacles Field

Move along force: F(x) = �Uatt(x)-�Urep(x)

Brian Williams, Fall 03

Exploring Roadmaps

• Shortest path

– Dijkstra’s algorithm

– Bellman-Ford algorithm

– Floyd-Warshall algorithm

– Johnson’s algorithm

• Informed search

– Uniform cost search

– Greedy search

– A* search

– Beam search

– Hill climbing

Brian Williams, Fall 03

Robonaut Teamwork: Tele-robotic

•High dimensional state space

•Controllability and dynamics

•Safety and complianceBrian Williams, Fall 03

Outline

� Roadmap path planning

� Probabilistic roadmaps

� Planning in the real world

� Planning amidst moving obstacles

� RRT-based planners

� Conclusions

Applicability of Lazy

Probabilistic Road Maps

to Portable Satellite Assistant

By Paul Elliott

courtesy NASA Ames

Zvezda Service Module

Idea: Probabilistic Roadmaps

• Search randomly generated roadmap

• Probabilistically complete

• Trim infeasible edges and nodes lazily

Place Start and Goal

Goal

Start

Place Nodes Randomly

Select a Set of Neighbors

A* Search

16

18

18

2

1

2

A* Search

A* Search

Check Feasible Nodes

Check Feasible Nodes

Check Feasible Nodes

A* Search

Check Feasible Edges

A* Search

Lazy PRM Algorithm

�

�

�

�

Build Roadmap

Start and Goal Nodes

Uniform Dist Nodes

Nearest Neighbors

qinit, qgoal

Build Roadmap

Shortest

Path (A*)

Node

Enhancement

Remove Colliding

node/edge No path found

Check Nodes Collision

Check EdgesCollision

Lazy PRM Algorithm

�

�

the goal

�

Shortest Path (A*)

Heuristic = distance to

Path length = distance

between nodes

qinit, qgoal

Build Roadmap

Shortest

Path (A*)

Node

Enhancement

Remove Colliding

node/edge

Check Nodes

Check Edges

Collision

Collision

No path found

Lazy PRM Algorithm

� Check Nodes & Edges

� Search from Start and

End for collisions

� First check Nodes

then Edges

qinit, qgoal

Build Roadmap

Shortest

Path (A*)

Node

Enhancement

Check Nodes

Check Edges

Remove Colliding

node/edge

Collision

Collision

No path found

Lazy PRM Algorithm

�

�

edges

�

Remove Node/Edge

For Nodes, remove all

For Edges, just

remove the edge

qinit, qgoal

Build Roadmap

Shortest

Path (A*)

Node

Enhancement

Check Nodes

Check Edges

Remove Colliding

node/edge

Collision

Collision

No path found

Lazy PRM Algorithm

�

�

�

edges

Node Enhancement

Add ½ uniformly

Add ½ clustered around

midpoints of removed

qinit, qgoal

Build Roadmap

Shortest

Path (A*)

Node

Enhancement

Remove Colliding

node/edge No path found

Check Nodes Collision

Check EdgesCollision

PRMs Fall Short For Dynamical Systems

� Using PRM

1. Construct roadmap

2. A* finds path in

roadmap

3. Must derive control

inputs from path

� Cannot always find

inputs for an arbitrary

path

Outline

� Roadmap path planning

� Probabilistic roadmaps

� Planning in the real world

� Planning amidst moving obstacles

� RRT-based planners

� Conclusions

Path Planning in the Real World

Real World Robots

� Have inertia

� Have limited controllability

� Have limited sensors� Have limited sensors

� Face a dynamic environment

� Face an unreliable environment� Face an unreliable environment

Static planners (e.g. PRM) are not sufficientStatic planners (e.g. PRM) are not sufficient

Two Approaches to Path Planning

Kinematic: only concerned with motion, without regard to the forces that cause it

� Works well: when position controlled directly.

� Works poorly: for systems with significant inertia.

Kinodynamic: incorporates dynamic constraints

� Plans velocity as well as position

Representing Static State

� Configuration space represents the position

and orientation of a robot

� Sufficient for static planners like PRM

TExample: Steerable car

yConfiguration space

(x, y, T)

x

Representing Dynamic State

� State space incorporates robot dynamic state

� Allows expression of dynamic constraints

� Doubles dimensionality

Example: Steerable car

yState space . . .

X = (x, y, T, x, y, T)Constraints

•max velocity, min turn x

T

•car dynamics

Incorporating Dynamic Constraints

� For some states, collision is unavoidable

� Robot actuators can apply limited force

Obstacle

Imminent

collision

region

� Path planner should avoid these states

Regions in State Space

� Collision regions: Xcoll

� Clearly illegal

� Region of Imminent Collision: Xric

� Where robot’s actuators cannot prevent a collision

� Free Space: Xfree = X – (Xcoll + Xric)

Xfree XcollXric

� Collision-free planning involves finding paths that lie entirely in Xfree

Constraints on Maneuvering

� Nonholonomic: Fewer controllable degrees of freedom then total degrees of freedom

� Example: steerable car

� 3 dof (x, y, T), but only

� 1 controllable dof (steering angle)

. .� Equation of Motion: G( s, s ) = 0

� Constraint is a function of state and time derivative of state

Outline

� Roadmap path planning

� Probabilistic roadmaps

� Planning in the real world

� Planning amidst moving obstacles

� RRT-based planners

� Conclusions

Problem

� Kinodynamic motion planning amidst

moving obstacles with known trajectories

� Example: Asteroid avoidance problem

� Moving Obstacle Planner (MOP)

� Extension to PRM

MOP Overview

Similar to PRM, except

� Does not pre-compute the roadmap

� Incrementally constructs the roadmap by extending it from existing nodes

� Roadmap is a directed tree rooted at initial state u time point and oriented along time axis

Building the Roadmap

1. Randomly choose an

existing node

2.

input u

3.

4.

motion

Nodes (state, time)

Randomly choose

existing node

Select control

input u

Randomly select control

Randomly select integration

time interval G �[0, G max ]

Integrate equations of

Collision- free?

at random

Integrate equations of motion from an existing

node with respect to u for some time interval G

Building the Roadmap (cont.)

5. If edge is collision-free then

6. Store control input with new edge

7. Add new node to roadmap

Collision-free trajectory

Store control input u

with new edge

u Add new node

Result: Any trajectory along tree satisfies

motion constraints and is collision-free!

Nodes (state, time)

Solution Trajectory

1. If goal is reached then

2. Proceed backwards from

the goal to the start Goal state and time

(sstart, tstart)

Start state and time

(sgoal, tgoal)

MOP details: Inputs and Outputs

Planning Query:

� t

� Let (sstart , tstart ) denote the robot’s start point in the state u time space, and (sgoal , tgoal ) denote the goal

goal �Igoal , where I is some time interval in whichgoal

the goal should be reached

Solution Trajectory:

� Finite sequence of fixed control inputs applied over a specified duration of time

� Avoids moving obstacles by indexing each state with the time when it is attained

� Obeys the dynamic constraints

MOP details: Roadmap Construction

� Objective: obtain new node (s’, t’)

� s’ = the new state in the robot’s state space

� t’ = t + G, current time plus the integration time

Each iteration:

1. Select an existing node (s, t) in the roadmap at

random

2. Select control input u at random

3. Select integration time G at random from [0, G max]

MOP details: Roadmap Construction

3. Integrate control inputs over time interval

4. Edge between (s, t) and (s’, t’) is checked

for collision with static obstacles and

moving obstacles

5. If collision-free, store control input u with

the new edge

6. (s’, t’) is accepted as new node

MOP details: Uniform Distribution

Modify to Ensure

Uniform Distribution

of Space:

� Why? If existing roadmap

nodes were selected

uniformly, the planner

would pick a node in an

already densely sampled

region

� Avoid oversampling of

any region by dividing the

stateutime space into bins

Achieving Uniform Node Distribution

Space 1. Equally divide space

2. Denote each section

as a bin; number each

bin

*bins store roadmap nodes

that lie in their region

bin 1 bin 2 bin 3 bin 4 bin 5 bin 6 bin 7

bin 8 bin 9 bin 10 bin 11 bin 12 bin 13 bin 14

. . . . . .

.

.

.

Achieving Uniform Node Distribution

Array

Equal-sized bins

bin 1 bin 2 bin 3 ….Existing nodes

3. Create an array of

bins

Achieving Uniform Node Distribution

Planner randomly picks a

bin with at least one node

At that bin, the planner

picks a node at random

bin 1 bin 2 bin 3 ….

Achieving Uniform Node Distribution

into its

corresponding bin

Insert new node

Demonstration of MOP

•Point–mass robot moving in a planex x

•State s = (x, y, x, y )

(sstart , tstart)

(sgoal , tgoal)

Moving obstacles

Static obstacle

Point-mass robot

Demonstration of MOP

Summary

� MOP algorithm incrementally builds a roadmap in the stateutime space

� The roadmap is a directed tree oriented along the time axis

� By including time the planner is able to generate a solution trajectory that

� avoids moving and static obstacles

� obeys the dynamic constraints

� Bin technique to ensure that the space is explored somewhat uniformly

Outline

� Roadmap path planning

� Probabilistic roadmaps

� Planning in the real world

� Planning amidst moving obstacles

� RRT-based planners

� Conclusions

Planning with RRTs

� RRTs: Rapidly-exploring Random Trees

� Similar to MOP

� Incrementally builds the roadmap tree

� Integrates the control inputs to ensure that the

kinodynamic constraints are satisfied

� Informed exploration strategy from MOP

� Extends to more advanced planning

techniques

How it Works

� Build RRT in state space (X), starting at sstart

� Stop when tree gets sufficiently close to sgoal

Goal

Start

Building an RRT

� To extend an RRT:

� Pick a random point a

in X

� Find b, the node of the

tree closest to a

� Find control inputs u to

steer the robot from b

to a

b u

a

Building an RRT

� To extend an RRT (cont.)

� Apply control inputs

u for time G, so robot

reaches c

� If no collisions occur

in getting from a to c,

add c to RRT and

record u with new

edge

b u

c a

Executing the Path

Once the RRT reaches sgoal � Backtrack along tree to identify edges that

lead from s to sstart goal

� Drive robot using control inputs stored along

edges in the tree

Principle Advantage

� RRT quickly

explores the state

space:

� Nodes most likely

to be expanded are

those with largest

Voronoi regions

Advanced RRT Algorithms

1. Single RRT biased towards the goal

2. Bidirectional planners

3. RRT planning in dynamic environments

Example: Simple RRT Planner

� Problem: ordinary RRT explores X uniformly

ĺ slow convergence

� Solution: bias distribution towards the goal

Goal-biased RRT

BIASED_RANDOM_STATE()

1 toss ĸ COIN_TOSS()

2 if toss = heads then

3 Return sgoal

4 else

5 Return RANDOM_STATE()

Goal-biased RRT

The world is full of…local minima

� If too much bias, the

planner may get trapped in

a local minimum

A different strategy:

� Pick RRT point near sgoal

� Based on distance from

goal to the nearest v in G

� Gradual bias towards sgoal

Rather slow convergence

Bidirectional Planners

� Build two RRTs, from start and goal state

� Complication: need to connect two RRTs

� local planner will not work (dynamic constraints)

� bias the distribution, so that the trees meet

Bidirectional Planner Algorithm

Bidirectional Planner Example

Bidirectional Planner Example

Conclusions

� Path planners for real-world robots must account for dynamic constraints

� Building the roadmap tree incrementally

� ensures that the kinodynamic constraints are satisfied

� avoids the need to reconstruct control inputs from the path

� allows extensions to moving obstacles problem

Conclusions

� MOP and RRT planners are similar

� Well-suited for single-query problems

� RRTs benefit from the ability to steer a

robot toward a point

� RRTs explore the state more uniformly

� RRTs can be biased towards a goal or to grow

into another RRT