Safe robot motion planning in dynamic, uncertain environments
RSS 2011 Workshop: Guaranteeing Motion Safety for Robots
June 27, 2011
Noel du Toit and Joel Burdick California Institute of Technology
Dynamic, Cluttered, Uncertain Env’s
6/27/2011 Noel du Toit 2
Conceptual Problem
Components:
Localization Process Noise Mapping
Detection/
tracking Prediction
Characterization Occlusions/
drop-outs
Problem
Formulation?
Computation?
Probabilistic uncertainty => conservative
Conservatism: incorporate anticipated measurements
Previous works: static environments [Prentice 09], [Censi 08], [Yan 05]
Enabling capability: complicated agent behaviors, more clutter, agent information gathering
Safety: chance constraint conditioning
Safety vs Conservatism
6/27/2011 Noel du Toit 4
History I-state:
Belief state: probability distribution
State:
Transition function:
Cost Function
Encodes noise properties &
and dynamics
Stochastic Dynamic Programming (SDP)
state control
measurement noise process noise
=
An
i A i
R i
i
x x x
x
1
Terminal cost
Stage cost
6/27/2011 Noel du Toit 6
Control Policy:
Problem:
Stochastic Dynamic Programming (SDP)
Stochastic Dynamic Programming (SDP)
Feedback over all possible future measurements and the resulting belief states
Stochastic Dynamic Programming (SDP)
Issues:
Computationally intensive [Thrun et al. 05], [Bertsekas 05]
No hard constraints
Solution:
for Linear, quadratic cost, Gaussian noise [Bertsekas 05], [Bar-Shalom 81]
Approximations
Value & policy iteration [Thrun et al. 05],
Roll-out algorithm [Bertsekas 05]
Restricted structure approximations [Bertsekas 05]
Formulation: Stochastic RHC (SRHC)
Stochastic system:
Expected cost, chance constraints
Belief state:
Transition function:
Disturbance:
6/27/2011 Noel du Toit 8
Approximation: Most Likely Measurement
Most likely measurement:
Same computational benefits of OLRHC approach
Approximation: relies on RHC formulation to correct for assumed information to reduce conservatism in solutions
Effect of measurements
Update covariance
Update mean
Theorem [duToit ‘09] : The Most Likely Measurement Approx.
introduces no new information Least Informative Approximation
Chance Constraints
Constrain uncertain state
Probabilistic Collision Avoidance
Robot and obstacle uncertainty
Joint distribution and indicator function
6/27/2011 Noel du Toit 11
Collision Constraints: Evaluation
Monte-Carlo Simulation
Small level of confidence: requires ~10000 samples
~5ms per evaluation
Approximate: small disk/ellipse objects
Independent, Gaussian distributions:
Dependent, Gaussian distributions
6/27/2011 Noel du Toit 12
Safety: Reaction Horizon
Quantify time (# of stages) to react to changes in environment
Robot dynamics
Environment
Reaction horizon,
r
: react to modeled disturbancesChance constraint conditioning:
Use
r
-stage open-loop predicted distributionAnticipated information:
leverage PCL reduction in conservatism
6/27/2011 Noel du Toit 13
Safety vs Conservatism
Uncertainty grows over reaction horizon
Next stage: new information + re-solve problem (RHC)
PCL: leverage new information
OL: uncertainty growth results in conservative solutions
1-D example:
6/27/2011 Noel du Toit 15
Car following:
Collision constraint (maintain some separation distance)
Velocity controlled random-walk model
Reaction horizon:
r=2
(to influence position)1-D Example (cont’d)
Reaction horizon = 1
Reaction horizon = 2
Plot separation distance
Dynamic Environment: Oncoming Agents
6/27/2011 Noel du Toit 17
OLRHC PCLRHC
Summary
Practical systems: trade off conservatism and safety
PCLRHC
Reduce conservatism through anticipated information
RHC: resolve problem to incorporate actual measurements
Chance constraint conditioning
Allow for modeled disturbances
Can still leverage anticipated information
See Noel’s thesis for various variations on this problem
Thank you
Questions?
Publications:
Du Toit, N.E. and Burdick, J.W., “Robot Motion Planning in Dynamic, Cluttered, Uncertain Environments”, accepted to IEEE Transactions on Robotics
Du Toit, N.E. and Burdick, J.W., “Probabilistic Collision Checking with Chance Constraints”, accepted to IEEE Transaction on Robotics
Du Toit, N.E., “Robot Motion Planning in Dynamic, Cluttered, Uncertain Environments: the Partially Closed-Loop Receding Horizon Control Approach”, Ph.D. thesis, Caltech, 2010
Conferences:
Workshop on Motion Planning: From Theory to Practice (RSS) 2010
Du Toit, N.E. and Burdick, J.W., “Robotic Motion Planning in Dynamic, Cluttered, Uncertain Environments”, ICRA 2010
6/27/2011 Noel du Toit 19
Robot:
Agent:
Constraints:
Objective function:
Problem Definition
Related Work
Deterministic Probabilistic
Static Dynamic Static Dynamic
Control OC [Friedland 05]
RHC [Mayne 00]
OC with augmented states
OC with separation [Friedland 05]
Stochastic RHC (later)
Robotics Graph search, roadmap methods, etc. [LaValle 06], [Choset et al. 07]
Dynamic window [Fox et al. 97]
Velocity obstacles [Fiorini & Shiller 98]
Graph search in extended state space [LaValle 06], [Censi 08]
Probabilistic velocity obstacles [Fulgenzi et al. 07]
AI
MDPs [LaValle 06] Extended state space (time x pose) [LaValle 06]
DP [Bertsekas 05]
MDPs, POMDPs [Thrun et al. 05]
6/27/2011 Noel du Toit 21
Stochastic systems: Probabilistic vs. non-deterministic
AI: Artificial Intelligence OC: Optimal Control
RHC: Receding Horizon Control
DP: Dynamic Programming MDP: Markov Decision Process POMDP: Partially Observable MDP
PCLRHC
Most likely measurement:
Restricted information:
Resulting belief state:
Deterministic in belief state:
Partially Closed-loop RHC
Robot: Linear model, linear measurements
Velocity constraints:
Control constraints:
Collision constraints:
Agent: Linear constant velocity model, linear measurements
Simulation Setup
6/27/2011 Noel du Toit 23
PCLRHC Approximation
Information gain: relative entropy
Baseline:
PCL distribution:
Executed distribution:
