Advanced path planning. ROS RoboCup Rescue Summer School 2012

Transcription

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

2 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

3 LAAS Constructive and integrative approach UAVs UGVs Personal robotics Humanoids Field robotics Open source software tools:

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

5 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

6 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

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

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

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

10 For a humanoid robot But what is path planning?

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

12 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

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

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

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

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

17 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...

18 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

19 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

20 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

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

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

23 Obstacles in the configuration space

24 Obstacles in the configuration space

25 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

26 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)

27 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

28 Approach 2: Voronoi diagram

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

30 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

31 Complex geometric plans

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

33 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

34 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

35 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

36 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

37 Indoor environment (Raw 2D LRF data) Potential fields

38 Potential fields Illustration Attractive potential Repulsive potential

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

40 Potential fields

41 Summary of a typical run Potential fields

42 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]

43 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

44 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)

45 Illustration Short term path planning

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

47 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)

48 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)

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

50 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

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

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

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

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

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

56 Extending the planning horizon Illustration during the Darpa Urban Challenge

57 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

58 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

59 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

60 Navigation strategies

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

62 Where to go and what for? Navigation strategies

63 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?

64 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

65 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

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

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

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

69 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

70 Illustration with the Mars Exploration Rovers (2009) Navigation strategies

71 Actual results on Mars (2009) Navigation strategies

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

73 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

74 Crossing obstacles The heavy way Crusher (Darpa / CMU)

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

76 Agile robots can be powerful Crossing obstacles

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

78 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

79 Stepping into dynamics of motion Going fast

80 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

81 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

82 Some readings