Advanced path planning. ROS RoboCup Rescue Summer School PDF Free Download

Advanced path planning ROS RoboCup Rescue Summer School 2012 Simon Lacroix Toulouse, France

Where do I come from? Robotics at LAAS/CNRS, Toulouse, France Research topics Perception, planning and decision-making, control Plus: control architecture, interactions, ambient intelligence systems, learning Decision Perception Action A keyword: autonomy Research domains Cognitive and interactive Robotics Aerial and Terrestrial Field Robotics Human and anthropomorphic motion Bio-informatics, Molecular motion 3 research groups : 12 full time researchers 10 university researchers 4 visitors 50 PhD students 10 post-docs Considered applications: Planetary exploration, Service and personal robotics, virtual worlds and animation, biochemistry, embedded systems, transport, driver assistance, defense, civil safety

Robots @ LAAS Constructive and integrative approach UAVs UGVs Personal robotics Humanoids Field robotics 1979 1985 1990 1995 2000 2005 2011 2013 Open source software tools: www.openrobots.org

What am I working on? Field robotics Environment perception and modeling Localization and SLAM Autonomous rover navigation Multi-Robot cooperation

Advanced path planning (my) definition of a robot: a machine that moves and whose motions a controlled by a computer (plus: intelligent link between perception and action) Objectives of the lecture: Discover a bit the motion planning scientific corpus Have an overview of the various levels of motion planning Understand some practical means to generate/compute/plan mobile robot motions

But what is path planning? Given A current position A goal position Information on the environment Constraints to satisfy / criteria to optimize Find A trajectory that satisfies the constraints / optimizes the criteria A trajectory: a continuous function from time to space Decision Perception Action Decision in this talk = finding how to move

But what is path planning? With Google maps (???)

For a circular robot amidst obstacles But what is path planning?

But what is path planning? For a wheeled robot with a trailer inside a cluttered building [F. Lamiraux @ LAAS]

For a humanoid robot But what is path planning?

In a intervention or rescue context But what is path planning? Send fire extinguisher robots there

Outline Basic notions Configuration space, kinematic constraints, search algorithms Practical field solutions Potential field approaches Short-term ( reactive ) planning Long-term itineraries Other research problems

Consider a punctual (0 width) robot Configuration space Start Goal There are plenty of solution trajectories And there is a shortest one

Configuration space Now consider a round shaped robot (radius r) Cannot go this way! Start Goal

Configuration space Now consider a round shaped robot (radius r) Start Goal After growing the obstacles, we are back to the point problem

Now consider a rectangular robot Not OK! Configuration space Start Goal OK

Configuration space Now consider a rectangular robot with kinematic constraints (e.g. a car) Reachable Start Goal Unreachable Impossible to go through this way! The feasable paths are much more difficult to find...

Configuration space Now consider a rectangular robot with kinematic constraints (e.g. a car) Reachable Unreachable Shortest path and finding the shortest path is not so easy, even when there is no obstacle

Configuration of a robot: set of independent parameters that specify the position and orientation of every component of the robot Configuration space Robot configurations Associated (q 1,q 2 ) configuration space There is a mapping between the CS and the world space A path is a continuous set of configurations

Configuration space Case of a robot in the plane: 2D environment 3 configuration parameters: 3D configuration space Case of a humanoid robot 3D environment 3 position parameters (no a plane) + numerous internal degrees of freedm: very high dimension configuration space

Obstacles in the configuration space Case of a triangular robot on the plane:

Obstacles in the configuration space Case of a triangular robot on the plane: A rectangular obstacle in the configuration space

Obstacles in the configuration space

Searching for paths A complete algorithm: find a solution if one exists, reports if not General complete algorithms have been proposed, but are not tractable in practice Complexity: grows exponentially with n, the CS dimension Difficult geometric computations (requires infinite precision) Weaker notions of completeness: Resolution completeness: based on a systematic discretisation of the CS Probabilistic completeness: the probability of finding a solution converges to 1 with infinite time

Searching for paths Two search families: Graph search Build a graph that captures the topology of the CS Can handle multiple query Building a search tree No attempt to capture the topology of the CS Goal dependant (single query)

Approach 1: visibility graph Given a polygonal environment description 1. Build the graph (considering start and goal as nodes) 2. Shortest path search in the graph

Approach 2: Voronoi diagram

Approach 3: Cellular decomposition Exact triangular decomposition Approximate decomposition Overall principle of the search

Probabilistic roadmaps Principle: 1. Randomly sample the CS with configurations 2. Keep the collision-free configurations (milestones) 3. Connect pairs of milestones with feasable paths Two requirements: Allows to solve very difficult path planning problems Ability to check collisions in the working space Ability to find a path between close configurations

Complex geometric plans

Path planning techniques for biochemistry Simulation of conformation change of a protein Accessibility of a ligand to the active site of a protein

But wait Given A current position A goal position Information on the environment? Constraints to satisfy / criteria to optimize Find A trajectory that satisfies the constraints / optimizes the criteria

Potential fields Principle: the robot is influenced by external virtual forces Obstacles generate repulsive forces The goal generates an attractive force All the forces are applied to the robot: the resulting force is the direction of motion to execute

Potential fields Advantage: very simple to implement. Can even work directly on the raw data R Resulting force Goal Difficulty: local minima ( potential wells ) R Goal Resulting force = 0 Everything relies on the analytic definition of the forces

Indoor environment (Raw 2D LRF data) Potential fields

Potential fields Illustration Attractive potential Repulsive potential

Potential fields Outdoor environment (probabilistic obstacle model) Classic potential Rotation potential Consideration of kinematic constraints: select on which point is applied the resulting force

Potential fields

Summary of a typical run Potential fields

Potential fields: summary Closer to automatic control than to autonomy : one stimulus, one response ( reactive motion mode) Decision Perception Action Well suited for easy environments with scarce obstacles Otherwise calls for a lot ot tuning See also «Vector field histograms» [Borenstein 91/98]

Short term path planning Elementary trajectory generation: evaluation of a set of elementary (v,ω) commands (circle arcs) Selection based on: Risk or cost of an arc (collision or terrain traversability) Interest of an arc (goal direction)

Illustration Short term path planning

Short term path planning Illustration with the Mars Exploration Rovers (2004)

But what is an obstacle? ~ OK for indoor environments Occupancy grid (P(Obst) on a regular Cartesian grid) But what about unstructured outdoor environments? Digital Terrain Model (z=f(x,y) on a regular Cartesian grid)

But what is an obstacle? Probabilistic obstacle detection (qualitative, conservative) 1. Discretisation of the perceived area (here with stereovision) 2. Probabilistic labelling Flat Obstacle Unknown Correlated pixels Labeling (top view) labeling (sensor view)

But what is an obstacle? Probabilistic obstacle detection (qualitative, conservative) Can be extended to «semantic» mapping

But what is an obstacle? Precise obstacle detection on a DTM «Convolution» of the terrain and the robot model For a given position (x,y,θ), the chassis state is determined, the safety of the position is assessed

But what is an obstacle? Illustration with the Mars Exploration Rovers (2004)

Short term path planning on a DTM Evaluation of the sequence of positions corresponding to an elementary command

Illustration at 1.5 m/s Short term path planning on a DTM

Extending the planning horizon Dead-end: local approaches can not find any solution (potential-based approaches don t do better) Goal Explore further

Extending the planning horizon Explore further: state lattices [Kelly-JFR-2009] Explicit and easy consideration of kinematic constraints

Extending the planning horizon Illustration during the Darpa Urban Challenge

But wait again Given A current position A goal position? Information on the environment Constraints to satisfy / criteria to optimize Find A trajectory that satisfies the constraints / optimizes the criteria

Navigation strategies Given A current position An objective (far away goal, area to map/explore) Information on the environment Constraints to satisfy / criteria to optimize Find A itinerary that satisfies the constraints / optimizes the criteria Where should the robot head? How should it head there? What for? An itinerary is akin to a coarse trajectory: sequence of waypoints, enriched with tasks and modalities

Navigation strategies

Navigation strategies For instance: How to reach the twin peaks?

Where to go and what for? Navigation strategies

Navigation strategies How to go there? A huge variety of situations Several motion modes, adapted to the situation at hand Potential Local arcs Route following Who selects, configures and controls the navigation modes?

Navigation strategies How to go there? A huge variety of situations Navigation strategies, a higher level instance of the perception / decision / action loop Decision Perception Action

Navigation strategies Now we are really into planning Decision Perception We need dedicated models of: Each available motion mode The environment Navigation mode applicability localisability Quantity/quality of information The perception tasks Localisation Environment modeling Action

Navigation strategies Models of the environment: Navigation mode applicability localisability Quantity/quality of information Example 1: exploit existing geographic data

Navigation strategies Models of the environment: Navigation mode applicability localisability Quantity/quality of information Example 2: exploit low altitude aerial imagery

Navigation strategies Models of the environment: Navigation mode applicability localisability Quantity/quality of information Example 3: build a dedicated model from rover data

Navigation strategies Spirit of the solutions Exploit graph search (A*, D*) Turn the goal-finding problem into an information acquisition problem Exploration case: analyse frontiers to assess relevant goals

Illustration with the Mars Exploration Rovers (2009) Navigation strategies

Actual results on Mars (2009) Navigation strategies

Coming soon with Curiosity? (MSL) Navigation strategies Target areas chosen on Aug. the 17th

Crossing obstacles The heavy way Crusher (Darpa / CMU)

Crossing obstacles A little bit less heavy way Rhex (Boston Dynamics)

Agile robots can be powerful Crossing obstacles

Crossing obstacles Smarter ways to cross obstacles? Advanced locomotion control On line evaluation of the masses repartition On line evaluation of the wheel torques

Crossing obstacles Smarter ways to cross obstacles? Advanced locomotion control On line evaluation of the masses repartition On line evaluation of the wheel torques On line evaluation of the chassis configuration evolution

Stepping into dynamics of motion Going fast

But wait one more time Given A current position? A goal position Information on the environment Constraints to satisfy / criteria to optimize Find A trajectory that satisfies the constraints / optimizes the criteria

Summary Basic notions Configuration space, kinematic constraints, search algorithms Practical field solutions Potential field approaches Short-term ( reactive ) planning Long-term itineraries Other research problems

http://homepages.laas.fr/jpl/book-toc.html Some readings

Advanced path planning. ROS RoboCup Rescue Summer School 2012