CS 326A: Motion Planning
Kynodynamic Planning
+ Dealing with Moving Obstacles
+ Dealing with Uncertainty
+ Dealing with Real-Time Issues
Kinodynamic Planning Problem
Plan a robot’s trajectory that
 satisfies a dynamic equation of motion of the
form:
where:
s’ = f(s,u)
– s is robot’s state configuration x velocity at time t
– u is control input at time t
 avoids collision with moving obstacles
Outcome: (Piecewise-constant) control function u(t)
2
Problem
Plan a robot’s trajectory that
 satisfies a dynamic motion constraint of the
form:
where:
s’ = f(s,u)
Note similarity with
non-holonomic constraint
q’ = f(q,u)
– s is robot’s state configuration x velocity at time t
– u is control input at time t
 avoids collision with moving obstacles
Outcome: (Piecewise-constant) control function u(t)
3
Nonholonomic vs. Dynamic
Constraints
 Nonholonomic constraint: f(q,q’) = 0
 Dynamic constraints: f(q,q’,q’’) = 0
s = (q,q’)

g(s,s’) = 0
 Kinodynamic planning is done in state
(configuration x velocity) space rather than in
configuration space
 The dimensionality of state space is twice that of
configuration space
 Similarity of Method
 Construction of a tree,
by integrating equation
of motion with selected
control inputs
 However, planning
occurs in state space,
instead of configuration
space
 Hence, PRM-like planner
rather than
deterministic one, to
deal with greater
dimensionality of the
space
5
Example: Space Robot
Robot created to study
issues in robot control
and planning in no-gravity
space environment
air thrusters
gas tank
air bearing
6
Navigation Among Moving
Obstacles
7
Model, Sensing, and Control
 The robot and the
obstacles are represented
as disks moving in the plane
 The position and velocity of
each disc are measured by
an overhead camera every
1/30 sec
y
robot
obstacles
x
8
Model, Sensing, and Control
 The robot and the
obstacles are represented
as disks moving in the plane
 The position and velocity of
each disc are measured by
an overhead camera within
1/30 sec
 The robot controls the
magnitude f and the
orientation a of the total
pushing force exerted by
the thrusters
y
robot
f
a
q=(x,y)
s=(q,q')
u=(f,a )
obstacles
1
x"= fcosa
m
1
y"= fsina
m
f  fM
x
9
Moving Obstacles in
ConfigurationxTime Space
y
t
x
Traditional PRM
straight line
segments
Kinodynamic Planning with PRM
in State x Time Space
endgame region
t=3
t=4
s
t=1
t=2
g
t=4
t=2
t=1
t=5
t=2
t=3
t=3
Bi-directional search?
The roadmap is a tree oriented along the time axis
(because of the moving obstacles)
12
PRM in State x Time Space
endgame region
g
Compare!
s
The roadmap is a tree oriented along the time axis
13
Sampling Strategy
1
1
2
endgame region
2
1
1
1
1
1
Bi-Directional Search:
Forward & Backward Integration
Goal Region
Example Run
t
y
x
Obstacle map to cylinders in
configurationtime space
16
Other Examples
17
But executing this trajectory
is likely to fail ...
1) The measured velocities of the obstacles are inaccurate
2) Tiny particles of dust on the table affect trajectories and
contribute further to deviation
 Obstacles are likely to deviate from their expected
trajectories
3) Planning takes time, and during this time, obstacles keep
moving
 The computed robot trajectory is not properly
synchronized with those of the obstacles
 The robot may hit an obstacle before reaching its goal
[Robot control is not perfect but “good” enough for the task]
18
But executing this trajectory
is likely to fail ...
1) The measured velocities of the obstacles are inaccurate
2) Tiny particles of dust on the table affect trajectories and
contribute further to deviation
 Obstacles are likely to deviate from their expected
trajectories
3) Planning takes time, and during this time, obstacles are
moving
 The computed robot trajectory is not properly
synchronized with those of the obstacles
 The robot may hit an obstacle before reaching its goal
Planning must take both uncertainty in world
[Robot control
is not
perfect
but “good”
enough
for the task]
state and
time
constraints
into
account
19
Dealing with Uncertainty
 The robot can handle uncertainty in an obstacle
position by representing the set of all positions of the
obstacle that the robot think possible at each time
 For example, this set can be a disc whose radius grows
linearly with time
Initial set of
possible positions
t=0
Set of possible
positions at time T
t=T
Set of possible
positions at time 2T
t = 2T
20
Dealing with Uncertainty
 The robot can handle uncertainty in an obstacle
position by representing the set of all positions of the
obstacle that the robot think possible at each time
 For example, this set can be a disc whose radius grows
linearly with time
The robot must plan to be
outside this disc at time t = T
t=0
t=T
t = 2T
21
Dealing with Uncertainty
 The robot can handle uncertainty in an obstacle
position by representing the set of all positions of the
obstacle that the robot think possible at each time
(belief state)
 For example, this set can be a disc whose radius grows
linearly with time
 The forbidden regions in configurationtime space are
cones, instead of cylinders
 The trajectory planning method remains essentially
unchanged
22
Dealing with Planning Time
 Let t=0 the time when planning starts. A time limit d is
given to the planner
 The planner computes the states that will be possible
at t = d and use them as the possible initial states
 It returns a trajectory at some t < d, whose execution
will start at t = d
 Since the PRM planner isn’t absolutely guaranteed to
find a solution within d, it computes two trajectories
using the same roadmap: one to the goal, the other to
any position where the robot will be safe for at least
an additional d. Since there are usually many such
positions, the second trajectory is at least one order
of magnitude faster to compute
23
Planning an Escape
Trajectory
endgame region
Safe
Region
Are we done?
 Not quite !
 The uncertainty model may be itself incorrect, e.g.:
• There may be more dust on the table than anticipated
• Some obstacles have the ability to change trajectories
 But if we are too careful, we will end up with forbidden
regions so big that no solution trajectory will exist any more
 So, it might be better to take some “risk”
The robot must monitor the execution of the
planned trajectory and be prepared to re-plan
a new trajectory
25
Are we done?
Note tradeoff between uncertainty
model and number of re-plans
Execution monitoring consists
of using the camera (at 30Hz)
to verify that all obstacles are
at positions allowed by the
robot’s uncertainty model
If an obstacle has an unexpected
position, the planner is called back
to compute a new trajectory.
The robot must monitor the execution of the
planned trajectory and be prepared to re-plan
a new trajectory
26
Experimental Run with
Re-Planning
1
2
3
4
5
6
7
27
Experimental Run with
Re-Planning
1
2
3
4
5
6
7
28
Another Experimental Run
Total duration : 40 sec
29
Another Experimental Run
30
Example in Simulation
8 replanning operations
31
Is this guaranteed to work?
Of course not :
 Thrusters might get clogged
 The robot may run out of air or battery
 The granite table may suddenly break
into pieces
 Etc ...
[Unbounded uncertainty]
32