Localization Navigation II Topological Representations What makes a

Localization & Navigation II

Topological Representations: What makes a good landmark? • Perceivable from many different viewpoints • Persistence • Distinct features – Readily recognizable • Unique features – Avoid sensor aliasing

What would make good landmarks in the EB?

Navigation with a Relational Graph • Graph representation: G = (V, E) – Landmarks and paths connecting – DAG – Paths can contain additional information • Pathing – Graph search – Shortest Path Algorithm • Dijkstra’s Shortest Path

Example: Distinctive Places

Distinctive Places

Distinctive Places Hill-Climbing Algorithm to get position at the landmark

Triangulation • A single landmark can be used as a beacon • Given 2 landmarks the robot’s pose can be constrained on the arc of a circle • Given 3 landmarks the robot’s pose can be determined to a point

Metric Representations • Coordinate system representation • Path is decomposed into waypoints – As opposed to landmarks • Path planning is usually based on some measure of optimal or best path – Graph search or graph coloring methods

World Representations

Meadow Map • Extend boundaries to accommodate size of robot • Divide free space into convex polygons – Connecting corners • Create waypoints by determining the midpoints of lines that border two polygons • Use graph search method for path planning

Meadow Map What problems would the robot encounter using a Meadow Map? Are there other problems with using a Meadow Map?

Voronoi Graph • A Voronoi Graph is formed by paths equidistant from the two closest objects • This generates a very safe roadmap which avoids obstacles as much as possible

Voronoi Graph • Use a graph search method for path planning • Advantages and Disadvantages?

Regular Grid • Subdivide space into same size grid spaces • Use center of square or vertices as waypoints • Considered as 4 connected or 8 connected

Regular Grid • Advantages & Disadvantages?

Quadtree Representation • Recursively divides a square into four smaller squares until each square is either filled with free space or non-free space • Center of squares are used as waypoints

Wave Front Planners • Graph Coloring Algorithm • From the goal destination “color” each bordering region with a value indicating its proximity, continue from each region until the start region is colored. • Follow the gradient toward the goal

Wave Front Planner • World Representation – You could always use a large region and distances – However, a grid can be used for simplicity

Wave Front Planner: Connectivity • 8 -Point Connectivity • 4 -Point Connectivity

The Wavefront Planner: Setup

The Wavefront in Action (Part 1) • Starting with the goal, set all adjacent cells with “ 0” to the current cell + 1 – 4 -Point Connectivity or 8 -Point Connectivity?

The Wavefront in Action (Part 2) • Now repeat with the modified cells – This will be repeated until no 0’s are adjacent to cells with values >= 2 • 0’s will only remain when regions are unreachable

The Wavefront in Action (Part 3) • Repeat again. . .

The Wavefront in Action (Part 4) • And again. . .

The Wavefront in Action (Part 5) • And again until. . .

The Wavefront in Action (Done) • You’re done – Remember, 0’s should only remain if unreachable regions exist

The Wavefront, Now What? • To find the shortest path always move toward a cell with a lower number – The numbers generated by the Wavefront planner are roughly proportional to their distance from the goal Two possible shortest paths shown

Can difficult terrain be modeled?

Graph Search • The activity of looking for a sequence of actions that solves (achieves) the goal (goal state) • Plan: sequence of actions to achieve a goal • State-Space Search – Initial State – Set of Actions – Goal Test • Search Tree: root is the initial state, each successive level is a state expanded from its parent by the application of a valid action

8 -Tile Puzzle

Search Strategy: choosing which state to expand next.

Search Strategies • Uninformed or blind searches – Breadth First Search, Depth First Search Systematic, exhaustive search

Heuristic Search • Heuristic – “Rule of thumb” – a way to measure good a state is to get to your goal • Examples – Parking: what would be a good heuristic to find your car?

Search Strategies • Informed or heuristic searches – Use domain knowledge to determine which state looks most promising to expand next • Heuristic Function h(n) = estimate of the path cost from state n to the goal h(n) = 0 if and only if n is a goal state

Informed Search Strategies Best First or Greedy Search: Expand the state that is estimated to be closest to the goal

Informed Search Strategies Branch & Bound: Expand the state that is estimated to be closest to the goal f(n) = g(n) + h(n) g(n) = actual cost of path going through state n f(n) is estimated cost of the cheapest solution that goes through state n

Branch & Bound f(n) = g(n) + h(n)

A* Search • Admissible Heuristic: Always underestimates the actual cost of the path • A* search is Branch & Bound with an admissible heuristic • Why is it important for h(n) to “underestimate” the actual cost?

Search Space Comparison • Breadth First Search Space • Shaded region is A* Search Space • The better h(n) estimates the actual cost, the smaller the search space • A* and it variations used mostly for navigation

Search: A* • f(n) = g(n) + h(n) – g(n) Cost of going from the starting state to state n – h(n) heuristic estimate of the cost of going from state n to the goal state • Guaranteed to find a solution if one exists • Will find the optimal solution

Admissible Heuristics for Pathing Straight Line Distance h(A) = sqrt((A. x-goal. x)^2 + (A. y-goal. y)^2) Manhattan Distance h(A) = (abs(A. x-goal. x) + abs(A. y-goal. y)) Diagonal Distance h(A) = max(abs(A. x-goal. x), abs(A. y-goal. y)) Use a weighting factor to estimate the cost of traversing difficult terrain.

A* Primer Create a node containing the goal state node_goal Create a node containing the start state node_start Put node_start on the open list While the OPEN list is not empty { Get the node off the open list with the lowest f-value and call it node_current If node_current is the same state as node_goal we have found the solution; return solution Generate each state node_successor that can come after node_current For each node_successor of node_current { Set the cost of this node to be the cost of node_current plus the cost to get to node_successor If this node is on the OPEN list but the existing one is better then discard this successor; break If this node is on the CLOSED list but the existing one is better then discard this successor; break Remove occurrences of this state from OPEN and CLOSED Set the parent of node_successor to node_current Set h-value to be the estimated distance to node_goal Add this node to the OPEN list by f-value } Return failure Do you have to have an OPEN and CLOSED list?

A* Example: European Vacation

On the Road to Bucharest A* Pathing: Arad to Bucharest

Optimal (Shortest) Path • A* chooses which location to expand based on the value f(n) = g(n) + h(n) • If h(n) is admissible, A* will generate all paths that underestimate the actual path cost, thereby guaranteeing that the solution path found is the optimal one.

Additional Points • Cost of Travel – Additional costs associated with path or region. – Terrain – Uphill & down hill costs • An efficient data structure is important for storing the Open and Close lists. – Suggestions? • Variations A* Demo – IDA* (Iterative Deepening) – Bi-directional – D* (Dynamic Planning) – Hierarchical A*

Obstacles • What should the robot do if it cannot complete the plan? • Alternatives – D* – Dijkstra’s Shortest Path Algorithm