Advanced Robotics Path Planning & Navigation 1
Agenda Motivation Basic Definitions Configuration Space Global Planning Local Planning Obstacle Avoidance ROS Navigation Stack 2
Literature Choset, Lynch, Hutchinson, Kantor, Burgard, Kavraki and Thrun. Principle of Robot Motion Theory, Algorithms and Implementation. MIT Press. 2005. 3
Motivation 4
given goal a set of obstacles Piano Mover s Problem the initial position of a robot the final position of a robot find a path that moves the robot from the initial to final position avoids the obstacles (at all times) 5
Challenges in the Real World physical Laws, e.g. inertia, acceleration uncertainty, e.g. maps, observations geometric constraints, e.g. shape of a robot dynamic environment, e.g. a moving crowd 6
completeness Properties of Algorithms does the algorithm find a solution in finite time if one exists resolution complete probabilistic complete optimality does the algorithm find the optimal solution(s) complexity what are the computational and memory demands off-line vs. on-line pre-computing? sensor-based does the algorithm integrate a sensing step 7
Classical Two-Layered Architecture Path Planning low frequency map sub-goal sensor data Obstacle Avoidance Reactive Navigation motion commands high frequency 8
Configuration Space to ensure that we have no collision at any time we have to represent the robot s position moreover we have to represent the position of each point of the robot we use the concept of configuration, e.g. necessary for robot arms the space of all possible configurations is the configuration space or C-space q single configuration Q entire configuration space 9
Example C-Space circular robot with radius r on the plane configuration can be represented as position (x,y) y x 10
Another Example C-Space Configuration Space Work Space 11
Obstacle in the Configuration Space let q be a point in the configuration space Q path planning revised: find a mapping c: [0,1] Q so that no configuration along the path intersects an obstacle a configuration space obstacle QO i is a set of configurations q at which the robot intersects WO i that is: QO i the free configuration space { q Q R( q) WO } a free path is a mapping: c: [0,1] Q free a semi-free path is a mapping: c: [0,1] cl(q free ) i Q Q \ ( free QO i i ) 12
Example Obstacle Space Ken Goldberg 13
Goal Building Blocks for Ground Robots Global Planner Path Trajectory Generation Intermediate Goals Local Planner Velocity Commands 14
given Global Planning a representation of the environment including known (static) obstacles, e.g. a map a current position (configuration) a goal position (configuration) goal find a free path from the current position (configuration) to the goal position (configuration) some assumptions holonomic motion point robot static environment no dynamic 15
Topological Representations represent the environment as graphs or roadmaps graph comprises nodes configurations edges (free) connections between configurations or roads either given or constructed (e.g., cell decomposition) planning = search for shortest path in graph 16
Geometrical Representations represent the environment by its geometry represents the workspace of the robot uses geometric primitives to represent the environment compact representation 17
Grid-Based Representations represent the environment as grid map grid cells are either free or occupied may represent some uncertainty represents the workspace of the robot memory demand depends on the resolution 18
Cell Decomposition basic Idea: convert representation into a graph and do motion planning on that graph divide the free configuration space Q free into simple connected regions named cells generate a connectivity graph with connected regions locate goal and start cells search for a path trough the graph follow the path 19
Trapezoid Cell Decomposition assumes polygonal environment decompose into trapezoids or triangles sort vertices in one direction iterate all vertices v i extend vertex v i up and/or down by two line segments until hitting an obstacle 20
Trapezoid Cell Decomposition Run Time for n vertices sorting the vertices needs O(n log(n)) time intersection done in a naïve way, we need O(n 2 ) time if we use a scan line we need only O(n log(n)) four different events and an edge list both edges of a vertex are on the left remove both from list both edges of a vertex are on the right add both to list if the upper edge is on the left and the lower edge on the right remove upper edge and add lower edge if the upper edge is on the right and the lower edge on the left remove lower edge and add upper edge perform intersection check only on edges in the list 21
Occupancy Grid occupancy grids are examples of an approximate cell decomposition regular grid of cells easy to implement widely used may require post planning smoothing 22
Occupancy Grid Alternative Representations resolution is a limiting issue too fine a grid leads to long search time too coarse a grid misses paths quad-tree adaptable cell size tree structure 23
A* Search most popular algorithm to find paths in graphs estimated costs function for a node n f(n) = g(n) + h(n) g(n) costs from start so far h(n) estimated costs to goal from n expand node n with lowest f(n) first two node lists: open (new), close (fully expand) if a successor node already closed ignore node if successor n is open and g (n) < g(n) then update f(n) and mark n as open continue until goal reached or no more open nodes h(n) has to underestimate the real costs 24
A* Search Example find the shortest path from Saabrücken to Würzburg h(n) Euclidian distance to goal 25
LPN Algorithm a method to find minimum cost paths based on linear programming work well on grid maps, extension of the wave-front algorithm given path as points in sample space P = {p 1, p 2,, p n } costs for traversing P navigation function represents the steepest gradient for a path P k starting at point k goal F( P) N a path P with minimum costs k k i 1 I( p i ) k 1 i 1 min F( P ) P k k A( p i, p i 1 ) 26
LPN Algorithm initially assign 0 to goal and to all other points obstacles contribute to I(p) goal point is active point expand path to neighbors p and calculate F (p) if F (p) < F(p) update F(p) and set p as active point continue for all active points until no one remains 27
A=10 A=10 A=10 A=10 A=10 A=10 A=10 A=10 I=0 C= LPN Example A=10 I=20 A=10 I=5 A=10 C= C= I=0 C= I=0 C=0 A=10 I=50 C= A=10 I=10 C= A=10 I=0 C= I=0 C= A=10 I=0 C= A=10 I=0 A=10 C= I=0 C= 28
Sampling-Based Approaches so far planner rely on explicit representations of Q free for complex and high-dimensional spaces this can be impractical basic idea: sample collision-free configuration and connect them to get a path multi-query planner: the planner once generate a general data structure by sampling which can be used for several path planning problems, e.g. probabilistic road maps single-query planner: sampling is done for every problem, e.g. rapidly-exploring random trees 29
Sampling-Based Approaches single-query planner multi-query planner 30
Rapidly-Exploring Random Trees (RRT) basic idea given a start configuration q init and a goal configuration q goal build a tree based on connected samples from Q free rooted in q init try to reach q goal what do we need configuration space Q start configuration q init and a goal configuration q goal collision detector: D: X true, false incremental simulator: given q(t) compute q(t + t) distance metric: ρ: Q Q [0, ) 31
Rapidly-Exploring Random Trees (RRT) build_rrt(q init ) T.init(q init ) for k=1 to K do q rand =random_config() extend(t, q rand ) return T extend(t,q) q near =nearest_config(q,t) if new_config(q,q near,q new,u new ) T.add_vertec(q new ) T.add_edge(q near, q new, u new ) if q new =q then return reached else return advanced return trapped 32
Rapidly-Exploring Random Trees (RRT) u new q new q rand q near q init 33
Rapidly-Exploring Random Trees (RRT) basically not goal-orientated apply some bias choose a new sample q rand randomly with probability 1 ε and as q goal randomly with probability ε grow two trees from q init and q goal and merge them the sampling strategy and the increment control the exploration of the configuration space, e.g. density of the tree RRT is probabilistic complete 34
OMPL: The Open Motion Planning Library open source sample-based motion planning library by Rice University provides a number of ready to use planner but also templates to implement new ones OMPL contains implementations of PRM, RRT, EST, SBL, KPIECE, OMPL supports various state (configuration) spaces OMPL is a stand-alone library but closely integrated in ROS OMPL is the standard library for arm motion planning in ROS 35
OMPL.app interactive GUI for motion planning problems to be solved by OMPL 36
performs local planning Obstacle Avoidance in general the environment is not fully modeled the environment changes dynamically obstacle avoidance may modify or replan a globally generated plan move the robot towards a goal without a global plan (e.g., Bug algorithm) it relies on information about the goal or sub-goal on the global path local context information (localization) recent sensor information (local map and obstacles) 37
Combining Local and Global Methods neither global nor local methods are enough global methods fail to address local variations, uncertainty, dynamics, local methods get trapped in local minima solution: combine both methods several methods come in a global X version such as global DWA 38
(Rough) Frequency Range large span in frequencies in navigation Path Planning (< 0.1 Hz) Path Deformation (~ 5 Hz) Obstacle Avoidance (~ 10 Hz) Actuator Control (~ 150 Hz) system architecture have to support this variety 39
ROS Navigation Stack ROS provides a full navigation stack global planning A* and LPN local planning Dynamic Window Approach or Trajectory Rollout supports differential drive and omni-directional robots works with and without global localization and map can work with 2d and/or 3d maps and sensors for usage and parameter look to the tutorial 40
ROS Navigation Stack Overview 41
Some Challenges the navigation stack is trimmed towards challenging indoor office-like environments narrow passages or doors difficult 3d structures like tables or wider bases rich variety of objects in the environment unexplored areas like blind corners 42
Cost Maps cost maps are used to generate a path or do achieve obstacle avoidance cost maps are 2d global cost map initialized by the global map updated by sensor information map coordinate frame local cost map rolling window centered around the robot cares about local obstacles updated by sensor information odometry coordinate frame 43
2d versus 3d the navigation stack is able to work with laser scans (2d) and point clouds (3d) laser scans are filtered for larger geometric objects like lines remaining points are classified as obstacle ego-hits are removed point clouds are converted into a 3d occupancy grid unknown or occluded areas are treaded as forbidden areas for navigation the 3d occupancy grid is projected to a 2d one 44
Thank you! 45