Kinodynamic Path Planning: Aisha Walcott, Nathan Ickes, Stanislav Funiak

October 31, 2001
Kinodynamic Path Planning

Aisha Walcott, Nathan Ickes,
Stanislav Funiak
Where PRMs Fall Short
Using PRM
1. Construct roadmap
2. A* finds path in
roadmap
3. Must derive control
inputs from path
Path itself is not enough:
need control inputs
Cannot always execute
an arbitrary path
Path Planning in the Real World

Real World Robots
Have inertia
Have limited controlability
Have limited sensors
Face a dynamic environment
Face an unreliable environment
Static planners (ex. PRM) are not sufficient
Outline
Exploring the problem

Planning amidst moving obstacles
RRT-based planners
Demonstration
Conclusions
Outline

RRT-based planners
Demonstration
Conclusions
Two Approaches to Path Planning

Kinematic: only concerns the motion, without regard
to the forces that cause it
Performs well for systems where position can be

controlled directly
Does not work well 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
Example: Steerable car

Configuration space
(x, y, )
Representing Dynamic State

State space incorporates the dynamic state
of a robot
Example: Steerable car

State space . . .
X = (x, y, , x, y, )
Representing Dynamic State

Working in state space allows planner to
incorporate dynamic constraints on path
Examples: maximum velocity,
Examples: minimum turning radius
Working in state space doubles the

dimensionality of the planning problem
Math becomes exponentially harder
Incorporating Dynamic Constraints

Robot actuators can apply limited force
For some states, collision is unavoidable
Imminent
collision
region
Obstacle
Path planner should avoid these states
Regions in State Space

Collision regions: Xcoll
Clearly illegal
Region of Imminent Collision: Xric

Where robots actuators cannot prevent a collision
Free Space: Xfree = X (Xcoll + Xric)

Xfree
Xric Xcoll
Collision-free planning involves finding paths

that lie entirely in Xfree
Constraints on Maneuvering
Not all degrees of freedom are controllable
Consider a steerable car
System has 3 dof (x, y, ),
but only one controllable
dof (steering angle)
.
Equation. of Motion: G( s, s ) = 0
Constraint is a function of state and time derivative
of state
Constraints on Maneuvering
Nonholonomic: fewer controllable DOFs
than total DOFs
Nonholonomic systems cannot execute an
arbitrary path in configuration space
This is a problem for PRM and other
configuration space planners
Outline

RRT-based planners
Demonstration
Conclusions
Planning Amidst Moving Obstacles

Moving Obstacles Planner (MOP):
A PRM extension that accounts for both
kinematic and dynamic constraints of a
robot navigating amidst moving obstacles
Problem
Kinodynamic motion planning amidst
moving obstacles with known trajectories
Example: Asteroid avoidance problem
Asteroid Avoidance Problem

Autonomous Vehicle
Spacecraft
Known trajectories
Moving Obstacles
Asteroids
Path-planning among moving

obstacles with known trajectories
Docking station
http://antwrp.gsfc.nasa.gov/apod/astropix.html
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 time point and oriented along time
axis
MOP Overview
For each query, the planner incrementally
builds new roadmap in state time space
Why? The environment includes moving
obstacles that change location (state)

continuously with time
Each node in the roadmap must be indexed by
both its state and the time it is attained
Ex: node n is valid at time t, however at time
t+ node n collides with a moving obstacle
Building the Roadmap

1.
Randomly choose an
existing node
2.
Randomly select control

input u
Nodes
Select control
input u at random
(state, time)
3.
Randomly select integration

time interval [0, max ]
4.
Integrate equations of
motion
Randomly choose
existing node
Collision- free?
Integrate equations of motion from an existing

node with respect to u for some time interval
Building the Roadmap (cont.)

5.
6.
7.
If edge is collision-free then

Store control input with new edge
Add new node to roadmap
Nodes
(state, time)
Collision-free trajectory
u
Store control input u
with new edge
Add new node
Result: Any trajectory along tree satisfies

motion constraints and is collision-free!
Solution Trajectory
1.
2.
If goal is reached then

Proceed backwards from
the goal to the start
Start state and time

(sstart, tstart)
Goal state and time

(sgoal, tgoal)
MOP details: Inputs and Outputs

Planning Query:
Let (sstart , tstart ) denote the robots start point in the state
time space, and (sgoal , tgoal ) denote the goal
tgoal Igoal , where Igoal is some time interval in which
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 robots state space

t = t + , 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 at random from [0, 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

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
statetime space into bins
Achieving Uniform Node Distribution

1.
2.
Equally divide space

Denote each section
as a bin; number each
bin
Space
bin 1
bin 2 bin 3 bin 4
bin 8
bin 9 bin 10 bin 11 bin 12 bin 13 bin 14
...
*bins store roadmap nodes
that lie in their region
.
.
.
bin 5
bin 6
...
bin 7

3.
Array
Create an array of
bins
Equal-sized bins
Existing nodes
bin 1
bin 2
bin 3 .

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 .
Insert new node into its

corresponding bin
Demonstration of MOP
Pointmass robot moving
in a plane

State s = (x, y, x, y )
(sgoal , tgoal)
Moving obstacles
Point-mass robot
(sstart , tstart)
Static obstacle
Demonstration of MOP
Summary
MOP algorithm incrementally builds a roadmap in
the statetime 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

RRT-based planners
Demonstration
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
Different exploration strategy from MOP

Extends to more advanced planning
techniques
How it Works
Build a rapidly-exploring random tree 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
a
Building an RRT
To extend an RRT (cont.)
Apply control inputs u
for time , so robot
reaches c
If no collisions occur
in getting from a to c,
add c to RRT and
record u with new edge
u
c
b
a
Executing the Path

Once the RRT reaches sgoal
Backtrack along tree to identify edges that lead
from sstart to sgoal
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

Single RRT biased towards the goal
Bidirectional planners
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 a point near sgoal
Based on distance from
goal to the nearest v in G
Gradual bias towards sgoal
Also, can apply sampling
methods for PRMS
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
Demos
Holonomic
Removing an object from a cage
Nonholonomic/kinodynamic
Steerable car moving in forward direction
Car with limited controllability
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

Kinodynamic Path Planning: Aisha Walcott, Nathan Ickes, Stanislav Funiak

Uploaded by

Copyright:

Available Formats

Kinodynamic Path Planning: Aisha Walcott, Nathan Ickes, Stanislav Funiak

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Kinodynamic Path Planning: Aisha Walcott, Nathan Ickes, Stanislav Funiak

Uploaded by

Copyright:

Available Formats

October 31, 2001

Kinodynamic Path Planning

Where PRMs Fall Short

Path Planning in the Real World

Static planners (ex. PRM) are not sufficient

Exploring the problem

Exploring the problem

Two Approaches to Path Planning

Performs well for systems where position can be

Kinodynamic: incorporates dynamic constraints

Plans velocity as well as position

Representing Static State

Example: Steerable car

Representing Dynamic State

Example: Steerable car

Representing Dynamic State

Working in state space doubles the

Incorporating Dynamic Constraints

Path planner should avoid these states

Regions in State Space

Region of Imminent Collision: Xric

Free Space: Xfree = X (Xcoll + Xric)

Collision-free planning involves finding paths

Exploring the problem

Planning Amidst Moving Obstacles

Asteroid Avoidance Problem

Path-planning among moving

obstacles that change location (state)

Building the Roadmap

Randomly select control

Randomly select integration

Integrate equations of motion from an existing

Building the Roadmap (cont.)

If edge is collision-free then

Add new node

Result: Any trajectory along tree satisfies

If goal is reached then

Start state and time

Goal state and time

MOP details: Inputs and Outputs

MOP details: Roadmap Construction

Objective: obtain new node (s, t)

s = the new state in the robots state space

MOP details: Roadmap Construction

MOP details: Uniform Distribution

Achieving Uniform Node Distribution

Equally divide space

bin 2 bin 3 bin 4

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

Achieving Uniform Node Distribution

Achieving Uniform Node Distribution

At that bin, the planner

Achieving Uniform Node Distribution

Insert new node into its

Bin technique to ensure that the space is explored

Exploring the problem

Planning with RRTs

Different exploration strategy from MOP