Motion Planning Introduction to Optimization Techniques

Motion Planning Introduction to Optimization Techniques Martin Rufli IBM Research GmbH Margarita Chli, Paul Furgale, Marco Hutter, Davide Scaramuzza, Roland Siegwart Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 1

Introduction the see think act cycle knowledge, data base Localization Map Building position global map Cognition Path Planning mission commands environment model local map path Perception Information Extraction raw data Sensing see-think-act Path Execution actuator commands Acting Motion Control Real World Environment Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 2

Introduction definitions Object An object is something material [i.e. an element in ] that may be perceived by the senses 1. The union of objects forms the complement to the empty, or free-space. Agent Decision-making objects are agents. They adhere to a system description. [1]: Merriam-Webster. Object - Definition. Website, 2012 http://www.merriam-webster.com/dictionary/object Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 3

Introduction the motion planning problem Goal Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 4

Introduction origins and historical developments Geometric optimization: Dido s problem Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 5

Introduction origins and historical developments Functional optimization: Brachistochrone Problem (1696) 1 Posed as a riddle by Johan Bernoulli to prove his superiority over his brother Initially solved geometrically Functional Optimization Formulation [1]: J. Bernoulli. Problema Novum ad Cujus Solutionem Mathematici Invitantur. Acta Eroditorum, page 269, 1696. Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 6

Introduction origins and historical developments Pontryagin s minimum principle 1 Extension of variational calculus to problems where Closed-form solutions restricted to Linear systems with quadratic cost function Simple non-linear problems Cannot easily treat obstacles Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 7

Introduction origins and historical developments Potential Fields Special time-invariant case of variational calculus where no system model is specified Re-invented in the 1980s based on simple attractive and repulsive force analogy Courtesy O. Khatib Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 8

Introduction origins and historical developments Dynamic Programming (DP) 1 Bellman s principle of optimality Discretization entails curse of dimensionality Graph search Deterministic (forward searching) instances of DP Examples include Dijkstra, A*, D* Design of underlying graph Difficult to construct system com-pliant graph structures [1]: R. Bellman. Dynamic Programming. Princeton University Press, Princeton, NJ, 1957. Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 9

Introduction hierarchical decomposition 1. Motion control 2. Local collision avoidance 3. Global search-based planning Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 10

Introduction work-space versus configuration-space θ 2 θ 2 θ 1 θ 1 y y x Configuration-space Work-space x Work-space Configuration-space Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Introduction to Optimization Techniques 11

Motion Planning Collision Avoidance Martin Rufli IBM Research GmbH Margarita Chli, Paul Furgale, Marco Hutter, Davide Scaramuzza, Roland Siegwart Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 12

Classic collision avoidance overview Methods compute actuator commands based on local environment They are characterized by Being light on computational resources Being purely local and thus prone to local optima Incorporation of system models Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 13

Vector Field Histogramm (VFH) working principle Working Principle Environment represented as an evidence grid locally Reduction of the grid to a 1D histogram by tracing a dense set of rays emanating from the robot up to a maximal distance All histogram openings large enough for the robot to pass become candidates The direction with the lowest cost function G is selected Properties Does not respect vehicle kinematics Prone to local minima Courtesy Borenstein et al. Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart

Dynamic Window Approach (DWA) working principle v ω Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 15

Velocity Obstacles (VO) working principle The robot is assumed to move on piece-wise linear curves The Velocity Obstacle is composed of all robot velocities leading to a collision with an obstacle before a horizon time τ v y p RO +v R t < r + r R O VO τ RO U = 0 t τ D p t RO r, t RO v x Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 16

Velocity Obstacles (VO) working principle The robot is assumed to move on piece-wise linear curves The Velocity Obstacle is composed of all robot velocities leading to a collision with an obstacle before a horizon time τ v y v x Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 17

Reciprocal Velocity Obstacles working principle The robot is assumed to move on piece-wise linear curves Identical to the Velocity Obstacles method, except that collision avoidance is shared between interacting agents fairness property v y VO τ RO U = 0 t τ D p t RO r, t RO v x Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 18

Reciprocal Velocity Obstacles working principle The robot is assumed to move on piece-wise linear curves Identical to the Velocity Obstacles method, except that collision avoidance is shared between interacting agents fairness property v y v x Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Collision Avoidance 19

Motion Planning Potential Field Methods Martin Rufli IBM Research GmbH Margarita Chli, Paul Furgale, Marco Hutter, Davide Scaramuzza, Roland Siegwart Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 20

Harmonic Potential Fields working principle Robot follows solution to the Laplace Equation Boundary conditions, any mixture of U Neumann: Equipotential lines lie orthogonal to obstacle boundaries Dirichlet: Obstacle boundaries attain constant potential = 2 U 2 q i = 0 Neumann Dirichlet Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 21

Harmonic Potential Fields working principle Robot follows solution to the Laplace Equation Boundary conditions, any mixture of U Neumann: Equipotential lines lie orthogonal to obstacle boundaries Dirichlet: Obstacle boundaries attain constant potential = 2 U 2 q i = 0 Neumann Dirichlet Margarita Chli, Paul Furgale, Marco Hutter, Martin Rufli, Davide Scaramuzza, Roland Siegwart Potential Field Methods 22 Courtesy A. Masoud