Gross Motion Planning...given a moving object, A, initially in an unoccupied region of freespace,s, a set of stationary objects, Bi, at known locations, and a legal goalposition, g,find a sequence of motions that take A from s to g without collidingwith any obstacles, Bi.sgAB1B2B31 Copyrightc2006 by Roderic GrupenPath Planning Configuration Spacesgsg2 Copyrightc2006 by Roderic GrupenPath Planning Configuration SpacesgSimplicial Decomposition:Schwartz and SharirLozano-PerezCannyVertex diagramsexponential in number of environmental verticessensitive to geometric error/revisionunsmooth pathsVisibility Graphssg3 Copyrightc2006 by Roderic GrupenPath Planning Configuration SpacesgVoronoi diagram4 Copyrightc2006 by Roderic GrupenPath Planning Configuration SpaceLozano-Perez - Mason - Taylorgeneralized damper, configuration space, backward chaining, preimage5 Copyrightc2006 by Roderic GrupenPath Planning Configuration SpacesgPotential Field Methods:KhatibRimon and KoditschekArkinLyons• obstacles are high potentials, goals are low potential• robot behaves as a marble rolling down potential gradient• physical analogs: gravitational, electrostatic• smoothness• analog implementations• incremental/nonstationary models• local minima6 Copyrightc2006 by Roderic GrupenManipulator Configuration Space...navigation functions are designed in the configuration space ofthe manipulator...xyeffectorORxy Cartesianoccupancy gridconfiguration space occupancy grid12127 Copyrightc2006 by Roderic GrupenNavigation FunctionsdefineV (x, t)a navigation function dependent on the state vector, x, and time, t.dVdt=dVdxdxdtwhere,dVdx= performance criteria -• obstacle avoidance• perspective control• grasp objectivedxdt= system dynamics8 Copyrightc2006 by Roderic GrupenNavigation FunctionsAnalytic Diffeomorphisms — Rimon and Koditsch ek• analytic — analytic functions can be represented by a series expan-sion and are continuously differentiable• diffeomorphisms — a diffeomorphism is a differentiable map f :S 7→ S′, which has a differentiable inverse f−1: S′7→ S, therefore,f is continuous and differentiable.• a diffeomorphism is called a conformal map if it is angle preserving(i.e., angles maps to the same angle, although lengths need not bepreserved).• transformations in sphere and star worlds which generate conver-gent control fields in the presence of obstacles.• environmental complexity and system dimensionality are challeng-ing practical issues.• does not easily incorporate incremental observation9 Copyrightc2006 by Roderic GrupenAdmissible Control Functionsconsider a candidate, multivariable control function, f(q0, q1, ..., qn),defined on domain, Q,the Hessian, ∂2f/∂Q2, can be used to establish several important ad-missibility criteria...∂2f∂Q2=∂2f∂q21∂2f∂q1∂q2· · ·∂2f∂q1∂qn......∂2f∂qn∂q1∂2f∂qn∂q2· · ·∂2f∂q2nConvex Control Functions: If the Hessian is positive semi-definiteover the domain Q, then the function f is convex over Q.Harmonic Control Functions: The harmonic constraint requiresthat the trace of the Hessian (or Laplacian) is identically zero:∇2f =∂2f∂q20+∂2f∂q21+ · · · +∂2f∂q2n= 0.Sub-Harmonic Control Functions: This class of admissible con-trol surfaces requires the Laplacian to be less than or equal to zero:∇2f =∂2f∂q20+∂2f∂q21+ · · · +∂2f∂q2n≤ 0.Properties: • complete up to model resolution• closed under linear composition10 Copyrightc2006 by Roderic GrupenMinima in Harmonic Potentialsconsider n dimensions:nXi=1∂2φ∂q2i= 0if some i, if∂2φ∂q2i> 0 (concave upward), then there is another dimension,j, where∂2φ∂q2j< 0 to satisfy Laplace’s constraint.therefore, if you’re not at a goal, there is always a way downhill...thereare no local minima...11 Copyrightc2006 by Roderic GrupenOther Properties∇2V =d2Vdx21+ . . . +d2Vdx2n= 0Min-Max Property -...in any compact neighborhood of freespace, the minimum andmaximum of the function must occur on the boundary.Mean-Value - up to truncation error, the value of the harmonic po-tential at a point in a lattice is the average of the values of its 2nmanhattan neighbors.∇2φ ≈ u(xi+1, yj) + u(xi−1, yj) + u(xi, yj+1) + u(xi, yj−1) − 4u(xi, yj))= 0Hitting Probabili ties - if we denote p(x) at state x as the prob-ability that starting from x, a random walk process will reach anobstacle before it reaches a goal—p(x) is known as the hitting prob-abilitygreedy descent on the harmonic function minimizes the hittingprobability.12 Copyrightc2006 by Roderic GrupenHarmonic Function Path Control∇2V =d2Vdx21+ . . . +d2Vdx2n= 0• obstacles fixed high potential, goals fixed low potential• analogs: laminar fluid flow, steady state temperature distribution,electromagnetic fields• computationally tractable — analog implementations13 Copyrightc2006 by Roderic GrupenHarmonic Functions — numerical propertiesJacobi Iterationu(k+1)(xi, yj) =14u(k)(xi+1, yj) + u(k)(xi−1, yj)+u(k)(xi, yj+1) + u(k)(xi, yj−1)• SIMD (Single Instruction, Multiple Data) machine, a parallel nu-merical relaxation using one computational element per node in thediscrete configuration space model• relatively good convergence times for small problems on sequentialmachines.14 Copyrightc2006 by Roderic GrupenHarmonic Functions — numerical propertiesGauss-SeidelIteration mixes values from different iterations:u(k+1)(xi, yj) =14u(k)(xi+1, yj) + u(k+1)(xi−1, yj)+u(k)(xi, yj+1) + u(k+1)(xi, yj−1)• In a single solution array replace each element by the average ofits neighbors. Half of these neighbors are from iteration k + 1, theother half are from iteration k.15 Copyrightc2006 by Roderic GrupenHarmonic Functions — numerical propertiesSuccessive Over Relaxation (SOR)u(k+1)(xi, yj) = u(k)(xi, yj) +ω4(u(k+1)(xi+1, yj) + u(k+1)(xi−1, yj) +u(k)(xi, yj+1) + u(k)(xi, yj−1) − 4u(k)(xi, yj))• k represents the i teration number,• ω is the acceleration constant• converges more rapidly than Gauss-Seidel or Jacobi iteration.16 Copyrightc2006 by Roderic GrupenReactive Potential FunctionsHeuristic “Bump” Avoidance:˙~qEQ= − [∇φ × (∇φ × ~w)]= −∇φ ×0 −φq3φq2φq30 −φq1−φq2φq10w1w2w3=(φ2q2+