Intelligent RoboticsToday & Wednesday: Localization & Navigation

Workers Of The World, Meet Your Robot Replacements

“Where am I”?

• Localization– Given an initial estimate, q, of the robot’s location in

configuration space (C-space), maintain an ongoing estimate of the robot pose with respect to the map, P(q).

Overview of Location

Hybrid Robot Architecture

• Deliberative planner provides “advice” to the reactive controller

If your robot has a map, why is it difficult for it to know where it is?

• Sensors are the fundamental input for the process of perception, therefore the degree sensors can discriminate the world state is critical

• Sensor Aliasing– Many-to-one mapping between environmental states to

the robot’s perceptual inputs– Amount of information is generally insufficient to identify

the robot’s position from a single sensor reading

Why is it difficult for a robot to know where it is?

• Sensor Noise– Adds a limitation on the consistency of sensor readings– Often the source of noise problems is that some

environmental features are not captured by the robot’s representation.

• Dynamic Environments• Obstacle Avoidance

The Configuration Space (C-space)

• A set of “reachable” areas constructed from knowledge of both the robot and the world

• How to create it– First abstract the robot as a point object.

Then, enlarge the obstacles to account for the robot’s footprint and degrees of freedom

C-Space: the robot has...

• A Footprint– The amount of space a robot occupies

• Degrees of Freedom– The number of variables necessary to fully

describe a robot’s configuration in space– Six DoF– Most Use 2 DoF

• Why? When would you need more?

C-Space: Accommodate Robot Size

Obstacles

Free Space

Robot(treat as point object)x,y

The Cartographer: Spatial Memory

• Data structures and methods for interpreting and storing sensory input with relation to the robot’s world– Representation of the world– Sensory input interpretation– Path planning & evaluation– Collection of information about the current

environment

Map Representations• Quantitative (Metric Representations)

• Topological (Landmarks)

• Important considerations– Sufficiently represent the environment

• Enough detail to navigate potential problems

– Space and time complexity– Sufficiently represent the limitations of the

robot– Support for map changes and re-planning– Compatibility with reactive control layer

Using Dead Reckoning to Estimate Pose: [x,y,Θ]

• “ded reckoning” or deduced reckoning – Reckon: to determine by reference to a fixed

basis

• Keep track off the current position by noting how far the robot has traveled on a specific heading– Used for maritime navigation– Proprioceptive

Dead Reckoning with Shaft Encoders• How far has a wheel traveled?

distance = 2 * PI * radius * #revolutions

reflectance sensor

slot sensor

How far has the robot traveled and

how far has it turned?

Which way am I going?

• Heading– Percentage of the circumference of the circle

with radius d

How far have I gone?

• Half the distance of the arc at the end of the motion– Distance moved for center of the robot

Adding it up

• (x,y,Θ)

• Update the position information at each sensor update.

• How large can a single segment be?

• What does this assume?

Many Types of Mobile Bases

• Differential Drive– Two independently driven

wheels on opposite sides of the robot

– 3 DoF: pose = [x,y,Θ]– Holonomic Assumption:

robot can be treated as a mass-less point that can move in any direction

Some of the Various Types of Mobile Bases

Topological Representation• Use of Landmarks

– Natural or Artificial– Passive or Active– Based on a specific sensing modality

• Vision, laser, sonar, IR

Example (movies)

Errors• Systematic Errors vs. Random Errors

– Can they be managed?

(movie)

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?

Example of Using Landmarks in Mapp representation:

Distinctive Places

Distinctive Places

Another Use of Landmarks: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

• Decompose the space into regions around each point, such that all the points in the region around p_i are closer to p_i than any other point in S.

• Voronoi edges become equidistant from boundary points

• Voronoi vertex is created where Voronoi edges meet

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

Path Planning:Wave Front

Planners• Graph coloring algorithm.• Obstacles nodes and the Goal node are assigned their own

unique values.– Typically 1 and 2, respectively.

• From the goal, “color” each bordering region with a value indicating its proximity to the goal.

• Continue until start node is reached.– Coloring whole grid may yield a more optimal solution.

• Follow the gradient from the start node toward the goal node.

MechanicsStarting Grid:

The Goal node is assigned ‘2’, Obstacle nodes with ‘1’ and all other nodes are set to ‘0’. The colored ‘0’ at node (7, 0) is the robot’s start position.

MechanicsEnding Grid (after complete propagation):

Note that this example assumes that the robot can move diagonally (typically 8 edges per node).

Question: How can difficult terrain be modeled?

Queue Approach

• An efficient means to propagate the wave (on a computer with sufficient memory and processing power) is to use a queue.

1. Note the robot Start node.

2. Assign the values of 1 to all Obstacles nodes and 2 to the Goal node. Set all others to 0.

3. Push the goal node into the queue.

4. Pop the first item from the queue and set it as the current node.

5. Examine all nodes connected to the current node. Push those nodes that have values equal to 0 into the queue and assign a value to them equal to one greater than the current node.

6. Repeat steps 4 and 5 until either the Start node is reached or all nodes are assigned a value greater than 0.

Gary Mayer, “Implementation of a Deliberative Robot Control Architecture on an Inexpensive Robot Platform”, SIUE MS Thesis, 2004

Loop Approach

• For computers without a lot of memory and processing power, a simple loop approach will work. Note that this restricts the dimensions of the world to a grid.

– Use a column traversal loop embedded in a row traversal loop.

1. Note the robot Start node.

2. Set a Change flag to FALSE.

3. Set the Current node to node (0, 0).

4. If Current node is not an Obstacle node and has been assigned a non-zero value, then examine the nodes connected to Current node.

5. If the examined node has a value equal to 0, assign a value to it equal to one greater than Current node and set the Changed flag to TRUE. Repeat for each connected node.

Gary Mayer, “Implementation of a Deliberative Robot Control Architecture on an Inexpensive Robot Platform”, SIUE MS Thesis, 2004

Loop Approach (cont’d)

6. If not the last column in the row, set Current node to the node in the next column and repeat steps 4 and 5.

7. If not the last row, set Current node to the node in the first column in the next row and repeat steps 4, 5, and 6.

8. If the Changed flag is TRUE, repeat steps 2 - 7.

• While wasteful in the fact that many nodes must be revisited, this approach ensures successful wave propagation around even a very complex environment.

• To ease processing, it may help to define a border of obstacles around the grid.

Pros to Using Wavefront• Simplistic and not very memory intensive.

– Forward moving wave is easy to understand.– Requires little data for each node.– Easy to implement a low memory / simple process approach.

• Benefit by storing goal location(s) in one array, obstacle location(s) in another, and robot position in a variable.– Each time the world grid is reset, traverse through each array

and reinsert goals, obstacles, and robot position into world representation array.

– Able to create plans to multiple goals (one at a time).– Allows easy modification of obstacle and goal locations in the

event the robot’s sensors detect a new one or one is not sensed where it was originally mapped to be.

Cons to Using Wavefront• Not very efficient in processing time.

– Every node must be visited for optimal path; many nodes more than once.

• Typically <10 sec to create a path on a 4 x 6 usable grid with 2 goals.

• Can only plan a path to one goal at a time.– When multiple goals are present, plan a path to each in turn

(set the other goals to empty). Go to the closest goal first. If you don’t necessarily want the closest goal (such as if there is goal priority), then it is best to set the other goals to obstacles so that the robot does not plot a path through them.

• More than one path may result.– Can use factors such as robot’s current heading, path with

least turns, and random choice to decide.

Path Planning: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

(link)

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

Use a weighting factor to

estimate the cost of traversing

difficult terrain.

A* PrimerCreate 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– IDA* (Iterative Deepening)– Bi-directional– D* (Dynamic Planning)– Hierarchical A*

A* Demo

Obstacles

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

Other Methods

• Potential Fields

• Occupancy Grids

• Monte Carlo Localization– http://www.cs.washington.edu/ai/Mobile_Robotics/mcl/

• SLAM

(movie)