11/27/2006 Massachusetts Institute of Technology Context Optimal - PDF document

11/27/2006 Massachusetts Institute of Technology Context • Optimal path planning for dynamic systems Optimal, Robust Path Planning – “What is best sequence of control inputs that takes system state from the start to goal?” – a Probabilistic Approach • Non-convex Lars Blackmore, Hui Li feasible region and Brian Williams • Non-holonomic dynamics November 27, 2006 • Discrete-time • Prior work has solved problem using Disjunctive LP 2 Optimal Paths are not Robust Representing Uncertainty • Uncertainty arises due to: • Two principal ways to represent uncertainty: – Disturbances – Uncertain state estimation 1. Set-bounded uncertainty – Inaccurate modeling y • Optimal paths are not robust to uncertainty ∈ S x S 0 True path x 2. Probabilistic uncertainty p(x,y) Goal p ( ) = N ( ˆ , ) x x P Planned path 0 0 0 y x 3 4 Representing Uncertainty Robust Control under Probabilistic Uncertainty • Probabilistic representations much richer • Robustness formulated using chance constraints: – Set-bounded representation subsumed by p.d.f. – “Ensure that failure occurs with probability at most δ ” • Probabilistic representations often more realistic • Prior work developed chance-constrained MPC – What is the absolute maximum possible wind velocity? – Stochastic problem converted to deterministic problem – Deterministic problem solved using LP or QP • Probabilistic representations readily available in many cases – Restricted to control within convex feasible region – Disturbances e.g. Dryden turbulence model – Uncertain state estimation e.g. Particle filter, Kalman filter, SLAM • We extend this work to control within non-convex regions – Inaccurate modeling e.g. Parameter estimation � robust path planning with obstacles – Resulting problem solved using Disjunctive Linear Programming • We deal with probabilistic uncertainty (DLP) with same complexity as problem without uncertainty 5 6 1

11/27/2006 Problem Statement Problem Statement Expected final position Expected path • Design a finite, optimal sequence of control inputs u 0 … u k-1 such that the expected final vehicle position is the goal p (failure) ≤ δ – Take into account uncertainty such that collision with any obstacle at a given time step occurs with probability at most δ 7 8 Technical Approach: Assumptions Technical Approach: Summary • Assume discrete-time linear stochastic system: = + + + ν x Ax Bu w + 1 t t t t t 1. Convert stochastic problem into deterministic one Noise due to disturbances Noise due to uncertain model 2. Show that deterministic problem can be solved p ν p ( w ) ( ) • Assume that and are known. t t using Disjunctive Linear Programming • Initial state x 0 is unknown, but assume p(x 0 ) is known • Polytopic obstacles • All uncertainty is Gaussian 9 10 Linear Chance Constraints Linear Chance Constraints • Consider linear chance constraint on an uncertain • So for given covariance P , linear chance constraint is > b ≤ δ multivariate variable X : " p ( ) " equivalent to deterministic linear constraint on E[ X ] Ax Ax = Ax = b b Ensure mean at least distance ∆ ∫ > = p ( ) p ( X ) d Ax b x from constraint to guarantee Covariance > Ax b chance constraint satisfied ellipse of X E [ X ] ∆ • Probability of constraint violation depends on: 1. Covariance of X > ≤ δ ⇔ ≤ b − ∆ p ( ) E [ ] Ax b 2. Distance of E[ X ] from constraint A x 11 12 2

11/27/2006 Linear Chance Constraints Obstacle Chance Constraints • How is ∆ calculated? • Extension: use this to ensure that an obstacle is hit with probability at most δ ˆ Ax = • Find vector normal to constraint line n b Constraint j T ∫ ∫ ≤ ˆ S p ( X ) d p ( X ) d • The distribution of projected along is a x x x n Obstacle i S S ∪ T univariate Gaussian with variance ˆ T ˆ n P n = A x b ij ij • Notice that: p(collision with obstacle) ≤ p(constraint violated) ∆ • Simply need to ensure that expected state is ∆ away from at least one of obstacle’s constraints • So ∆ can be calculated using a simple lookup of erf , – Conservatism introduced the Gaussian c.d.f. function 13 14 Multiple Obstacles Robust Path Planning as DLP • Must ensure that probability of hitting any obstacle • Important analytic properties: is at most δ – Future state distribution is Gaussian – Future state mean is linear function of control inputs – Probabilities of collision are not mutually exclusive – Future state covariance is not a function of control inputs p(collision with obstacle 1 or 2) = p(collision w. obs. 1) + p(collision w. obs. 2) • Hence the constraint: – But can bound probability of collision with any obstacle “Ensure expected state is ∆ away from at least one of obstacle’s p(collision with obstacle 1 or 2) ≤ p(collision w. obs. 1) + p(collision w. obs. 2) constraints” – is a disjunctive linear constraint on control inputs u 1…t • Constrain probability of hitting each of N obstacles to be at most δ /N • Cost functions such as fuel use can be expressed as – Then probability of collision with any obstacle guaranteed piecewise linear functions of control inputs to be at most δ – Additional conservatism introduced � Problem can be posed as a Disjunctive Linear Program LB2 15 16 Robust Path Planning as DLP Results • Summary: • Trade off performance against plan conservatism – Calculate covariance P t of predicted state at each t in horizon 14 No uncertainty – Calculate required margin ∆ t for each t in horizon to ∆ = 0.1 110 ∆ = 0.001 12 ∆ = 0.0001 ensure probability of failure less than δ 105 100 10 – Pose disjunctive linear program to ensure margin satisfied 95 T 8 90 ∑ y(meters) = Fuel Use minimize J u 85 i 6 80 = i 1 75 ∨ ≤ − ∆ subject to E[X ] 4 A b 70 ij t ij t i 65 2 for each time step t and each obstacle j 60 −5 −4 −3 −2 −1 10 10 10 10 10 Maximum Probability of Collision ( ∆ ) 0 2 4 6 8 10 12 14 16 x(meters) – Solve using efficient, readily available solvers 17 18 3

Slide 15 LB2 Work out how to link into the next slide - see next slide Lars Blackmore, 6/10/2006

11/27/2006 Conclusion Questions? • Robust path planning problem can be solved as DLP – Essentially same complexity as DLP that does not take into account uncertainty (one lookup, matrix multiplication per constraint) • The catch? – The resulting plan is excessively conservative • We guarantee p(collision) is less than δ , but in practice, p(collision) is much less than δ • Hence there exists a better solution that still satisfies chance constraint • If we try to constrain the probability of collision at any time step, we get very conservative plans • Solution: Ongoing research 1. Particle Control approach approximates distributions using samples • Approximate approach instead of conservative approach 2. Ellipsoidal approximations have been used in the literature to solve analogous problems 19 20 Backup • Conservatism plot 21 4