Sufficient Conditions for the Existence of Resolution Complete Planning Algorithms

Transcription

1 Sufficient Conditions for the Existence of Resolution Complete Planning Algorithms Dmitry Yershov and Steve LaValle Computer Science niversity of Illinois at rbana-champaign WAFR 2010 December 15, 2010

2 Motion planning problem with differential constraints Motion planning without differential constraints Incremental sampling of configuration space Connect samples Search for solution on graph Motion planning with differential constraints Difficult to connect samples in C-space Sample control space Prove resolution completeness

3 Strategy Sampling control space Design motion primitives: constant control motion capture optimal curves... Concatenate motion primitives to get control sequence se these discrete control sequences as sample points se brute-force algorithm to search for solution

4 Dispersion Consider X with metric ρ, and P X Define dispersion: [ ] δ(p, X ) = sup min ρ(x, p) x X p P

5 Sampling sequences on bounded sets Consider open unit interval X = (0, 1) R Let P = (1/2, 1/4, 3/4, 1/8, 3/8, 5/8, 7/8, 1/16,...) Let P N be first N elements of P, then lim δ(p N, X ) = δ( lim P N, X ) = 0 N N ( ) 0 1

6 Sampling sequences on unbounded sets Let X = R Let P be any enumeration of Q R Striking fact lim δ(p N, X ) = δ(q, X ) = 0 N R Lemma For any dense sequence P, dispersion of P N in X converges to zero if and only if X is precompact.

7 Technical Challenges Our function spaces are not classical, such as L 1, L 2, or L Dispersion needs metric on sampled space Some infinite dimensional spaces are not precompact Any bounded subset is not precompact in infinite dimensional space Proving resolution completeness becomes non trivial

8 Planning problem formulation Given Description of mechanical system Description of environment Initial state and set of goal configurations Find Collision-free feasible path from initial state to goal set, if exists (Resolution complete) Report if no path exists (Complete algorithm) Conditions under which resolution complete algorithm exists

9 Planning problem formulation Given Description of mechanical system Description of environment Initial state and set of goal configurations Find Collision-free feasible path from initial state to goal set, if exists (Resolution complete) Report if no path exists (Complete algorithm) Conditions under which resolution complete algorithm exists

10 Kinodynamic constraints and obstacles in C-space Assume given state space X X may represent configuration or velocity variables Collision detector implicitly defines X free X Differential constraints are given by ẋ = f (x, u) θ θ (x,y) (x,y)

11 Description of motion planning problem Given X, X free, X G, and x I Given ẋ = f (x, u) Find ũ : [0, T ] such that for corresponding x : [0, T ] X : 1) x(0) = x I, 2) x(t ) X G, and 3) for all t [0, T ] x(t) X free x I X G

12 Trajectory Space x I x x x I x x Definition (Trajectory space) X = { x : [0, T ] X T > 0 and x(t) x(t ) M t t } ρ X ( x, x ) = sup x(t) x (t) + M T T t min(t,t )

13 Trajectory Space x I x x x I x x Definition (Trajectory space) X = { x : [0, T ] X T > 0 and x(t) x(t ) M t t } ρ X ( x, x ) = sup x(t) x (t) + M T T t min(t,t )

14 Trajectory Space x I x x x I x x Definition (Trajectory space) X = { x : [0, T ] X T > 0 and x(t) x(t ) M t t } ρ X ( x, x ) = sup x(t) x (t) + M T T t min(t,t )

15 Control Space ũ 01 ũ x(x I,ũ) T T R x x(x I,ũ ) I Definition (Control space) = { ũ : [0, T ] T > 0 and ρ (ũ, ũ ) = T 0 T 0 } ũ(t) dt < ũ(t) ũ (t) dt + α T T

16 Control Space ũ 01 ũ x(x I,ũ) T T R x x(x I,ũ ) I Definition (Control space) = { ũ : [0, T ] T > 0 and ρ (ũ, ũ ) = T 0 T 0 } ũ(t) dt < ũ(t) ũ (t) dt + α T T

17 Control Space ũ 01 ũ x(x I,ũ) T T R x x(x I,ũ ) I Definition (Control space) = { ũ : [0, T ] T > 0 and ρ (ũ, ũ ) = T 0 T 0 } ũ(t) dt < ũ(t) ũ (t) dt + α T T

18 Relating Controls and Trajectories ũ T R x I x(x I,ũ) Given ũ, x I X free, Assume f is Lipschitz and bounded Consider solution to initial value problem { ẋ(t) = f (x(t), ũ(t)) x(0) = x I Denote solution as x(x I, ũ) We have constructed function x(x I, ũ) : X

19 Relating Controls and Trajectories ũ T R x I x(x I,ũ) Given ũ, x I X free, Assume f is Lipschitz and bounded Consider solution to initial value problem { ẋ(t) = f (x(t), ũ(t)) x(0) = x I Denote solution as x(x I, ũ) We have constructed function x(x I, ũ) : X

20 Relating Controls and Trajectories ũ T R x I x(x I,ũ) Given ũ, x I X free, Assume f is Lipschitz and bounded Consider solution to initial value problem { ẋ(t) = f (x(t), ũ(t)) x(0) = x I Denote solution as x(x I, ũ) We have constructed function x(x I, ũ) : X

21 Relating Controls and Trajectories ũ T R x I x(x I,ũ) Given ũ, x I X free, Assume f is Lipschitz and bounded Consider solution to initial value problem { ẋ(t) = f (x(t), ũ(t)) x(0) = x I Denote solution as x(x I, ũ) We have constructed function x(x I, ũ) : X

22 Relating Controls and Trajectories ũ T R x I x(x I,ũ) Given ũ, x I X free, Assume f is Lipschitz and bounded Consider solution to initial value problem { ẋ(t) = f (x(t), ũ(t)) x(0) = x I Denote solution as x(x I, ũ) We have constructed function x(x I, ũ) : X

23 Relating Controls and Trajectories ũ T R x I x(x I,ũ) Given ũ, x I X free, Assume f is Lipschitz and bounded Consider solution to initial value problem { ẋ(t) = f (x(t), ũ(t)) x(0) = x I Denote solution as x(x I, ũ) We have constructed function x(x I, ũ) : X

24 Continuity Theorem Theorem (Continuity of x(x I, ũ)) Let f be Lipschitz in x and u, with constants L x and L u Then function x(x I, ũ) is continuous Moreover, for ũ, ũ, with T = max(t, T ) ( ρ X ( x(x I, ũ), x(x I, ũ )) max L u e Lx T, M ) ρ (ũ, ũ ) α ũ 01 ũ x(x I,ũ) T T R x x(x I,ũ ) I

25 Trajectory solution set Definition (Set of solutions to path planning problem instance) X sol = { x X x(0) = x I, x(t ) X G, t [0, T ] x(t) X free } x I X G

26 Control solution set Definition (Set of solutions to motion planning problem instance) sol = {ũ x(x I, ũ) X sol } Alternatively sol = x 1 (x I, X sol ) x I X G

27 Topology of X sol Theorem Let X free and X G be open in X Then X sol is an open subset of X Proof. For any x X sol Positive distance to boundary X free Positive distance to X G Minimum of two distances determines ɛ such that if ρ X ( x, x ) ɛ, then x X sol x I X G

28 Topology of sol Theorem Let X free and X G be open in X Let f (x, u) be Lipschitz and bounded Then sol is an open subset of Proof. X sol is open in X by previous theorem x(x I, ũ) is continuous by continuity theorem sol = x 1 (x I, X sol ) is open (preimage of open set under continuous map)

29 Finding an open needle in topological haystack? Sampling If solution exists, then sol Build dense sequence on Eventually, solution will be found

30 Motion primitives Consider finite or countable set of functions (motion primitives) Σ = {σ i : [0, t i ] i N t i > 0} Piecewise constant control Motion capture techniques Optimal curves Concatenation of two primitives { σi (t) if t [0, t (σ i σ j )(t) = i ) σ i (t t i ) if t [t i, t i + t j ] Strings of motion primitives Σ = {σ i1 σ i2 σ ik k 0 and each σ ij Σ}

31 Sufficient conditions Theorem (Sufficient conditions for resolution completeness) If Σ is dense in, then the brute-force algorithm is resolution complete Proof. sol is open subset of If sol is nonempty, then Σ sol The first element of Σ that is also in sol is returned If sol is empty, then solution does not exist and the algorithm runs forever

32 Discrete-time model ũ σ R Discretize with dense sequence d Build p/w constant motion primitives σ ij (t) u j defined on [0, 1/2 i ] Denote Σ dt = {σ ij i N j N} Construct Σ dt

33 Discrete-time model ũ σ R Discretize with dense sequence d Build p/w constant motion primitives σ ij (t) u j defined on [0, 1/2 i ] Denote Σ dt = {σ ij i N j N} Construct Σ dt

34 Discrete-time model ũ σ R Discretize with dense sequence d Build p/w constant motion primitives σ ij (t) u j defined on [0, 1/2 i ] Denote Σ dt = {σ ij i N j N} Construct Σ dt

35 Discrete-time model ũ σ R Discretize with dense sequence d Build p/w constant motion primitives σ ij (t) u j defined on [0, 1/2 i ] Denote Σ dt = {σ ij i N j N} Construct Σ dt

36 Discrete-time model ũ σ R Discretize with dense sequence d Build p/w constant motion primitives σ ij (t) u j defined on [0, 1/2 i ] Denote Σ dt = {σ ij i N j N} Construct Σ dt

37 Denseness theorem ũ σ R Theorem (Denseness theorem) Σ dt is dense in

38 Denseness theorem ũ σ R Theorem (Denseness theorem) Σ dt is dense in

39 Summary of the paper Σ X Σ x(x I,ũ) X

40 Summary of the paper Σ X Σ x(x I,ũ) X Mathematical description for mechanical systems and dynamics

41 Summary of the paper Σ X Σ x(x I,ũ) X Mathematical description for mechanical systems and dynamics Trajectory space with metric and associated topology

42 Summary of the paper Σ X Σ x(x I,ũ) X Mathematical description for mechanical systems and dynamics Trajectory space with metric and associated topology Control space with metric and associated topology

43 Summary of the paper Σ X Σ x(x I,ũ) X Mathematical description for mechanical systems and dynamics Trajectory space with metric and associated topology Control space with metric and associated topology Function from controls to trajectories, which is continuous

44 Summary of the paper Σ X Σ x(x I,ũ) X Mathematical description for mechanical systems and dynamics Trajectory space with metric and associated topology Control space with metric and associated topology Function from controls to trajectories, which is continuous Motion primitives

45 Summary of the paper Σ X Σ x(x I,ũ) X Mathematical description for mechanical systems and dynamics Trajectory space with metric and associated topology Control space with metric and associated topology Function from controls to trajectories, which is continuous Motion primitives Strings of motion primitives

46 Summary of the paper Σ X Σ x(x I,ũ) X Mathematical description for mechanical systems and dynamics Trajectory space with metric and associated topology Control space with metric and associated topology Function from controls to trajectories, which is continuous Motion primitives Strings of motion primitives Sampling property and sufficient conditions

47 Conclusions Results Functional analysis framework is quite general Analogous to: Introduction of value iteration (Bellman, 1950s) Lipschitz-based analysis of convergence (Bertsekas, 1975) Future analysis Practical for broader class of systems and algorithms Trajectory dispersion bound for Lie group symmetric systems Convergence of planning with pruned trajectories Function space sampling methods

48 The End Thank you! Many thanks to funding agencies 1 Questions, comments? 1 This work was supported in part by NSF grants (IIS Robotics), NSF grant (IIS Robotics) and (Cyberphysical Systems), DARPA SToMP grant HR , and MRI/ONR grant N

49 Planing Algorithm Input : set of motion primitives Σ the initial state x I, goal set X G collision detection module, an integration module Output : solution control σ n 0 loop σ select the nth string from Σ x(x I, σ) integrate equation ẋ = f (x, σ) C 1 (true or false) if x(x I, σ) contains in X free C 2 (true or false) if x(x I, σ) terminates in X G if C 1 C 2 return σ n n + 1 end loop