Safe robot motion planning in dynamic, uncertain environments

Transcription

1 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

2 Dynamic, Cluttered, Uncertain Env s 6/27/2011 Noel du Toit 2

3 Conceptual Problem Components: Localization Process Noise Mapping Detection/ tracking Prediction Characterization Occlusions/ drop-outs Problem Formulation? Computation? 6/27/2011 Noel du Toit 3

4 Safety vs Conservatism 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 6/27/2011 Noel du Toit 4

5 Stochastic Dynamic Programming (SDP) History I-state: state control process noise measurement noise x i xi xi = xi R A 1 A n Belief state: probability distribution State: Transition function: Cost Function Terminal cost Encodes noise properties & and dynamics Stage cost 6/27/2011 Noel du Toit 5

6 Stochastic Dynamic Programming (SDP) Control Policy: Problem: Stochastic Dynamic Programming (SDP) Feedback over all possible future measurements and the resulting belief states 6/27/2011 Noel du Toit 6

7 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] 6/27/2011 Noel du Toit 7

8 Formulation: Stochastic RHC (SRHC) Stochastic system: Expected cost, chance constraints Belief state: Transition function: Disturbance: 6/27/2011 Noel du Toit 8

9 Approximation: Most Likely Measurement Effect of measurements Update covariance Update mean Most likely measurement: Same computational benefits of OLRHC approach Approximation: relies on RHC formulation to correct for assumed information to reduce conservatism in solutions Theorem [dutoit 09] : The Most Likely Measurement Approx. introduces no new information Least Informative Approximation 6/27/2011 Noel du Toit 10

10 Chance Constraints Constrain uncertain state Probabilistic Collision Avoidance Robot and obstacle uncertainty Joint distribution and indicator function 6/27/2011 Noel du Toit 11

11 Collision Constraints: Evaluation Monte-Carlo Simulation Small level of confidence: ~5ms per evaluation requires ~10000 samples Approximate: small disk/ellipse objects Independent, Gaussian distributions: Dependent, Gaussian distributions 6/27/2011 Noel du Toit 12

12 Safety: Reaction Horizon Quantify time (# of stages) to react to changes in environment Robot dynamics Environment Reaction horizon, r: react to modeled disturbances Chance constraint conditioning: Use r-stage open-loop predicted distribution Anticipated information: leverage PCL reduction in conservatism 6/27/2011 Noel du Toit 13

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 6/27/2011 Noel du Toit 14

14 1-D example: Car following: Collision constraint (maintain some separation distance) Velocity controlled random-walk model Reaction horizon: r=2 (to influence position) 6/27/2011 Noel du Toit 15

15 1-D Example (cont d) Reaction horizon = 1 Plot separation distance Reaction horizon = 2 6/27/2011 Noel du Toit 16

16 Dynamic Environment: Oncoming Agents OLRHC PCLRHC 6/27/2011 Noel du Toit 17

17 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 6/27/2011 Noel du Toit 18

18 Thank you Publications: Questions? 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 /27/2011 Noel du Toit 19

19 Problem Definition Robot: Agent: Constraints: Objective function: 6/27/2011 Noel du Toit 20

20 Related Work Stochastic systems: Probabilistic vs. non-deterministic 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) PCLRHC 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] AI: Artificial Intelligence OC: Optimal Control RHC: Receding Horizon Control DP: Dynamic Programming MDP: Markov Decision Process POMDP: Partially Observable MDP 6/27/2011 Noel du Toit 21

21 Partially Closed-loop RHC Most likely measurement: Restricted information: Resulting belief state: Deterministic in belief state: 6/27/2011 Noel du Toit 22

22 Simulation Setup Robot: Linear model, linear measurements Velocity constraints: Control constraints: Collision constraints: Agent: Linear constant velocity model, linear measurements 6/27/2011 Noel du Toit 23

23 PCLRHC Approximation Information gain: relative entropy Baseline: PCL distribution: Executed distribution: 6/27/2011 Noel du Toit 24