advertisement

An Incremental Sampling-based Algorithm for Stochastic Optimal Control Martha Witick Department of Computer Science Rice University Vu Anh Huynh, Sertac Karamanm Emilio Frazzoli. ICRA 2012. Motion Planning! •Continuous time •Continuous state space •Continuous controls •Noisy. MDPs? S Motivation • Cnts-time, cnts space stochastic optimal control problem: no closed form, exact algorithmic solutions • Approximate cnts problem with discrete MDP and compute solution • ... but exponential with number of state and control spaces • Sampling-based methods: fast and effective, but... • RRT: not optimal • RRT*: no systems with uncertain dynamics Overview • Have a continuous time/space stochastic optimal control problem • Want an optimal cost function J* and ultimately an optimal policy μ* • Create a discrete-state Markov Decision Process and refine, iterating until the current cost-to-go Ji* is close enough to J* • What the iterative Markov Decision Process (iMDP) algorithm does. Outline • Continuous Stochastic Optimal Control Problem Definition • Discrete Markov Chain approximation • iMDP Algorithm • iMDP Results • Conclusion Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS S0 S state x(t) ∊ S control u(t) time t ≥ 0 Consider a stochastic dynamical system that looks like this: robot's dynamics Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS state x(t) ∊ S control u(t) time t ≥ 0 S0 S Consider a stochastic dynamical system that looks like this: robot's dynamics noise Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS state x(t) ∊ S control u(t) time t ≥ 0 S0 S Consider a stochastic dynamical system that looks like this: robot's dynamics noise Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS x ( 0 ) S0 S Solutions looks like this until x(t) hits δS: state x(t) ∊ S control u(t) time t ≥ 0 Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS x ( 0 ) S0 S Solutions looks like this until x(t) hits δS: state x(t) ∊ S control u(t) time t ≥ 0 Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS x ( 0 ) S0 S Solutions looks like this until x(t) hits δS: state x(t) ∊ S control u(t) time t ≥ 0 Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS x ( 0 ) S0 S state x(t) ∊ S policy μ(t) time t ≥ 0 A Markov control (or policy) μ(t): S→ℝ needs only state x(t). Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS x ( 0 ) S0 S The expected cost-to-go function under policy μ state x(t) ∊ S policy μ(t) time t ≥ 0 Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS x ( 0 ) S0 S The expected cost-to-go function under policy μ first exit time state x(t) ∊ S policy μ(t) time t ≥ 0 Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS x ( 0 ) S0 S The expected cost-to-go function under policy μ discount rate α ∊ [0,1) state x(t) ∊ S policy μ(t) time t ≥ 0 Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS x ( 0 ) S0 S The expected cost-to-go function under policy μ cost rate function state x(t) ∊ S policy μ(t) time t ≥ 0 Continuous Stochastic Dynamics S ⊂ ℝdx interior S0 smooth boundary δS δS x ( 0 ) S0 S The expected cost-to-go function under policy μ terminal cost function state x(t) ∊ S policy μ(t) time t ≥ 0 Continuous Stochastic Dynamics • We want the optimal cost-to-go function J*: • We want to compute J* so we can get its optimal policy μ* • But solving this continuous problem is hard. Solving this is hard • Lets make a discrete model! Outline • Continuous Stochastic Optimal Control Problem Definition • Discrete Markov Chain approximation • iMDP Algorithm • iMDP Results • Conclusion Markov Chain Approximation Approximate stochastic dynamics with a sequence of MDPs {Mn}∞n=0 Markov Chain Approximation Approximate stochastic dynamics with a sequence of MDPs {Mn}∞n=0 Markov Chain Approximation Approximate stochastic dynamics with a sequence of MDPs {Mn}∞n=0 Discretize states: grab finite set of states Sn from S assign transition probabilities Pn Discretize time: assign non-negative holding time Δtn(z) to each state z Don't need to discretize controls. Markov Chain Approximation Local Consistency Property: • For all states z ∊ S, • For all states z ∊ S and all controls v ∊ U, Markov Chain Approximation Local Consistency Property: • For all states z ∊ S, • For all states z ∊ S and all controls v ∊ U, Markov Chain Approximation Local Consistency Property: • For all states z ∊ S, • For all states z ∊ S and all controls v ∊ U, Markov Chain Approximation Local Consistency Property: • For all states z ∊ S, • For all states z ∊ S and all controls v ∊ U, Markov Chain Approximation Local Consistency Property: • For all states z ∊ S, • For all states z ∊ S and all controls v ∊ U, Markov Chain Approximation Approximate stochastic dynamics with a sequence of MDPs {Mn}∞n=0 Control problem is analogous, so let's define a discrete discounted cost: Continuous for comparison: Markov Chain Approximation Approximate stochastic dynamics with a sequence of MDPs {Mn}∞n=0 Discontinuity and Remarks • F, f, g, and h can be discontinuous and still work • While the controlled Markov chain has a discrete state structure and the stochastic dynamical system has a continuous model, they BOTH have a continuous control space. Outline • Continuous Stochastic Optimal Control Problem Definition • Discrete Markov Chain approximation • iMDP Algorithm • iMDP Results • Conclusion iMDP Algorithm Set 0th MDP M0 to empty S0 δS S iMDP Algorithm Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP S0 δS S iMDP Algorithm Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP Sample state from δS and add it to Mn S0 zs δS S iMDP Algorithm Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP Sample state from δS and add it to Mn S0 δS S iMDP Algorithm Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP Sample state from δS and add it to Mn Sample zs from S0 zs S0 δS S iMDP Algorithm Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP Sample state from δS and add it to Mn Sample zs from S0 Set znearest to nearest state in Sn zs S0 δS znearest S iMDP Algorithm zs x:[0,t] S0 δS znearest S Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP Sample state from δS and add it to Mn Sample zs from S0 Set znearest to nearest state in Sn Compute trajectory x:[0,t] with control u from znearest to zs iMDP Algorithm zs z x:[0,t] S0 δS znearest S Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP Sample state from δS and add it to Mn Sample zs from S0 Set znearest to nearest state in Sn Compute trajectory x:[0,t] with control u from znearest to zs Set z to x(0) iMDP Algorithm zs z x:[0,t] S0 δS znearest S Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP Sample state from δS and add it to Mn Sample zs from S0 Set znearest to nearest state in Sn Compute trajectory x:[0,t] with control u from znearest to zs Set z to x(0) and add to Sn iMDP Algorithm z S0 δS znearest S Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP Sample state from δS and add it to Mn Sample zs from S0 Set znearest to nearest state in Sn Compute trajectory x:[0,t] with control u from znearest to zs Set z to x(0) and add to Sn Compute cost and save Update() new z and Kn states in Sn iMDP Algorithm: Update Loop Update() new z and Kn states in Sn: iMDP Algorithm: Update Loop Update() new z and Kn states in Sn: iMDP Algorithm: Update Loop Update() new z and Kn states in Sn: iMDP Algorithm: Update() iMDP Algorithm: Update() iMDP Algorithm: Update() Uniformly sample or create Cn controls from z to nearest Cn states iMDP Algorithm: Update() For each control v in Un iMDP Algorithm: Update() Compute new transition probability to nearest log(Sn) states Znear iMDP Algorithm: Update() iMDP Algorithm: Update() holding time tau iMDP Algorithm: Update() cost rate function g iMDP Algorithm: Update() discount rate alpha iMDP Algorithm: Update() Expected cost-to-go to Znear under n-th policy iMDP Algorithm: Update() iMDP Algorithm: Update() If the new cost J is less then the old cost iMDP Algorithm: Update() Update z's cost, policy, holding time, and number of states in Sn iMDP Algorithm z S δS znearest S Set 0th MDP M0 to empty while n < N do nth MDP Mn <- (n-1)th MDP Sample state from δS and add it to Mn Sample zs from S0 Set znearest to nearest state in Sn Compute trajectory x:[0,t] with control u from znearest to zs Set z to x(0) and add to Sn Compute cost and save Update() new z and Kn states in Sn n=n+1 IMDP: Generating a Policy iMDP Time Complexity • |Sn|Ѳ states updated and log(|Sn|) controls processed per iteration • Time Complexity when Pn constructed with linear equations: O(n |Sn|Ѳ log |Sn| ) • Time Complexity when Pn constructed with Gaussian distribution: O(n |Sn|Ѳ (log |Sn|)2 ) • Space Complexity: O(|Sn|) • State Space Size: |Sn| = Θ(n) iMDP Analysis iMDP Analysis iMDP Analysis iMDP Analysis Outline • Continuous Stochastic Optimal Control Problem Definition • Discrete Markov Chain approximation • iMDP Algorithm • iMDP Results • Conclusion Results Experiment (a): convergence of iMDP applied to a stochastic LQR problem With dynamics dx = Results Experiment (a): convergence of iMDP applied to a stochastic LQR problem With dynamics dx = Results Experiment (b): A system with stochastic single integrator dynamics to a goal region (upper right) with free ending time in a cluttered environment. Results •Experiment (c): Noise-free versus noisy. σ = 0.37 Note that (b) failed to find a valid trajectory. Conclusions • The iMDP algorithm: • incrementally builds discrete MDPs to approximate continuous problems • Improves cost every iteration (value iteration), novel approach to computing Bellman updates • has a fast total processing time O(k^1+0 log k) • Remove necessity for point-to-point steering of RRT (e.g. RRT*)