Robot Building Lab: Off-board Map Building and Path Planning Demo: Find Goal, Build Map Record initial position Head toward goal light, avoiding and

Demo: Find Goal, Build Map

Record initial position Head toward goal light,

avoiding and reacting to obstacles

Take sonar sweeps and odometry readings and build map as you move

Fix odometry errors with landmarks (using ground sensors)

Keep track of landmarks and stop after counting 4 landmarks

Output map to host View graphically Graded based on quality of

map in successive regions

4ft

4ft

4ft

5ft

Lab 9

Off-board Map Building and Path Planning

http://plan.mcs.drexel.edu/courses/robotlab/labs/lab09.pdf



In-Class: Find Goal, Build Map

Record initial position Head toward goal light, avoiding

and reacting to obstacles Take sonar sweeps and

odometry readings and build map as you move – Send readings to PC (sweep of

readings plus x,y, theta)– Build map on PC



View map graphically on PC

4ft

4ft

4ft

5ft

Final Exam Demo: Build Map Dynamically and Use to Find Best Path

to Target

4ft

4ft

4ft

5ft

Record initial position Head toward target location Take sonar sweeps and odometry

readings and build map as you move – Send readings to PC (sweep of readings

plus x,y, theta)– Build map on PC


Use map on PC to continually send new movement vectors to robot

Head toward new movement direction, avoiding and reacting to obstacles (if needed)

Stop at target location View map graphically on PC (extra

credit for showing changes to map dynamically, in real-time)

Today

Review global sonar mapsReview kinematicsFull duplex serial communication for off-

board planning and controlDynamic path planning with global

maps

Grid-based Algorithm

Superimpose “grid” on robot field of view

Indicate some measure of “obstacleness” in each grid cell based on sonar readings

Using Sonar to Create Local Maps

What should we conclude if this sonar reads 10 feet?

there is something somewhere around here

there isn’t something here

10 f

eet

Local Mapunoccupied

occupied

or ...no information

(Courtesy of Dodds)

(From Borenstein et. Al.)

Building a Global Map

• The key to making accurate maps is combining lots of data.

• But combining these numbers means we have to know what they are !

What should our map contain ?

• small cells

• each represents a bit of the robot’s environment

• larger values => obstacle

• smaller values => free

(Courtesy of Dodds)

Building a Global Map with Vector Field Histogram

Faster than using probabilities Makes assumptions and approximations Uses certainty values in each grid cell rather

than occupancy probabilities Rather than update all cells in range, only

those on line of sight of sensor are updated with increment if near range reading and decrement below range reading

Cells bounded below and above (eg 0,10) Key observation: by taking rapid readings, this

method approximates a probability distribution by sampling the real world

Global Map Building Process

Re-evaluate position with odometry as robot movesDecay values over time

(From Borenstein et. Al.)

Displaying Global Map of Occupancy Values on Host

http://plan.mcs.drexel.edu/cgi-bin/legorobots/occgrid.cgi



Today



maps

Review: Forward Kinematics Given: Starting pose (x,y,theta), Motor commands Compute: Ending pose (x’,y’,theta’) Assume encoders mounted on drive motors Let

– Cm = encoder count to linear displacement conversion factor

– Dn = wheel diameter– Ce = encoder pulses per revolution– N = gear ratio

Cm = Dn / N Ce Incremental travel distance for left wheel

= Cm NL (NL = encoder counts on left wheel) Incremental travel distance for right wheel

= Cm NR (NR = encoder counts on right wheel) That’s all we need for determining horizontal

displacement and rotation from encoder counts

Differential Drive Odometry/Kinematics

DL = distance traveled by left wheel

DR = distance traveled by right wheel

Distance traveled by center point of robot is then D = (DR+DL)/2

Change in orientation Dtheta is then (DR – DL)/base

New orientation is then theta’=theta + Dtheta

If old robot position is x,y new robot position is nowx’ = x + D cos theta’y’ = y + D sin theta’

Review: Localization using Kinematics

Need to know robot location during sonar sweep in order to know how to update Global Map

Issue: We can’t tell direction from encoders alone

Solution: Keep track of forward/backward motor command sent to each wheel

Localization program: Build new arrays into behavior/priority-based controller and use to continually update location

Today



maps

Review: Serial Communication Modes

Collect sensor data on Handy Board Upload to PC host, either

1. Batch mode: program stores data on Handy Board and later writes to host using serial line

2. Real-time data collection: program on Handy Board writes data directly to serial line during data collection activities

3. On-line interaction: programs active on both Handy Board and Host communicate with each other over serial line

(copyright Prentice Hall 2001)

Real-Time Interaction

No longer using terminal programReplacing terminal program with C++

code to monitor serial port and send/receive data

Adding functions to HandyBoard to implement communications protocol with new Host program

(We also have Java code available)

Andrew’s Serial Comms APIPC-side - Setup

Include “pcSerial.h” into your c++ program.

Constructor: pcSerial();Destructor: ~pcSerial();

Andrew’s Serial Comms APIPC-side – Write to

HandyBoardWrite byte (unsigned char) or integer to

Handyboard over serial line. – void writeByte(BYTE c);– void writeInt(int i); // not working

Write an Array to the Handyboard (not yet working)– void writeArray(int data[], int sizeOf);

Andrew’s Serial Comms APIPC-side – Read from

HandyBoardRead byte (unsigned char) or integer

from Handyboard over serial line. – BYTE readByte();– int readInt();

Read an Array of integers from the Handyboard

Needs array and size of array– void readArray(int data[], int sizeOf);

Andrew’s Serial Comms APIHandyBoard-side – Read from

PCRead a char or int from the PC

– char serial_readByte()– int serial_readInt() (not working yet!)

Read an Array from the PC (Not yet working)– void serial_readArray(int data[], int sizeOf)

Andrew’s Serial Comms APIHandyBoard-side – Write to PCWrite a char or int to the PC

– void serial_writeByte(int A)– void serial_writeInt(int A)

Write an Array to the PC– void serial_writeArray(int A[], int sizeOf)

To Read/Write One Int at a Time

This code goes into the handyboard main() int c = 500; start_press(); while(1){ serial_writeInt(c); msleep(5L);}This code goes into the pc main() int b; pcSerial S; while(1){ b = S.readInt(); cout << "b = " << b << endl;}

To Read/Write Array of IntsThis code goes into the handyboard main()

char c; int i; int a[90] ; start_press(); for(i=0;i<90;i++){ a[i] =1000+i;} printf("writeArray"); serial_writeArray(a, 90);This code goes into the pc main() int A[90]; pcSerial S; cout << "S.readArray " << endl; S.readArray(A,90); cout << "S.readArray done " << endl; for(int j =0; j<90; j++){ cout << "A[" <<j << "] = " << A[j] << endl;}

Running the code: sending data from the handyboard to

the pcStart the handyboard.

– Handyboard sends garbage over the serial line when it is first turned on.

Start the PC program.Press Start on the handyboard and the

data will be sent to the PC

Running the code: sending data from the pc to the

handyboard Start the Handyboard.Press the start button.

– Handyboard will wait for data to be sent to it.

Start the PC program and the data will be sent to the handyboard.

Today



maps


How does the robot use Global Map + Target location to choose current heading?

Closing the Loop

Review: Closed-loop Control

Drive parallel to wall Feedback from

proximity sensors (e.g. bump, IR, sonar)

Feedback loop, continuous monitoring and correction of motors -- adjusting distance to wall to maintain goal distance

(Courtesy of Bennet)

Host-HandyBoard Map Building, Path Planning and

Control

Path Planning Using Maps

The motion planning problem consists of the following:

Input Output• geometric descriptions of a robot and its environment (obstacles)

• initial and goal configurations

• a path from start to finish (or the recognition that none exists)

qgoalqrobot Applications

Robot-assisted surgery

Automated assembly plans

Drug-docking and analysis

Moving pianos around...

(Courtesy of Dodds)

Roadmap approaches

Visibility graphsIn a polygonal (or polyhedral) configuration space, construct all of the line segments that connect vertices to one another (and that do not intersect the obstacles themselves).

Converts the problem into one of graph search.

Dijkstra’s algorithm

O(N^2)N = the number of vertices in C. Space

(Courtesy of Dodds)

Roadmap approaches

Full visibility graphReduced visibility graph, i.e., not including segments that extend into obstacles on either side.

Visibility graphs

(but keeping endpoints’ roads)

(Courtesy of Dodds)

Visibility graph drawbacks

Visibility graphs do not preserve their optimality in higher dimensions:

In addition, the paths they find are “semi-free,” i.e. in contact with obstacles.

shortest path

shortest path within the visibility graph

(Courtesy of Dodds)

Local techniques Potential Field methods

• compute a repulsive force away from obstacles

(Courtesy of Dodds)



• compute an attractive force toward the goal

(Courtesy of Dodds)



• compute an attractive force toward the goal

let the sum of the forces control the robot

To a large extent, this is computable from sensor readings

(Courtesy of Dodds)

Local planning• Usually assumes some knowledge at the global level

The goal is known; the obstacles sensed

Each contributes forces, and the robot follows the resulting gradient.

(Courtesy of Dodds)

Another view of reactive control

Direct mapping from the environment to a control signal

goal-seeking behaviorobstacle-avoiding behavior(Courtesy of Dodds)

Behavior Summer

vector sum of the avoid and goal motor schemas

path taken by a robot controlled by the resulting

field

(Courtesy of Dodds)

Another primitive

Direct mapping from the environment to a control signal

larger composite task random motion schema(Courtesy of Dodds)

Local minima

Noise allows a system to “jump out” of local minima.

the problem a solution(Courtesy of Dodds)

Path Planning in Evidence Grids

Reduces to a search problem within the graph of cells, and the graph is created on the fly.

Search for a minimum-cost path, depending on

•length

•probability of collision

(Courtesy of Dodds)

•Can use many different path planning algorithms•We will use potential-field-like force summing•Note: need to compute vectors for sequence of moves around robot’s current location (policy)

Lab 9





In-Class: Find Goal, Build Map

Record initial position Head toward goal light, avoiding

and reacting to obstacles Take sonar sweeps and

odometry readings and build map as you move – Send readings to PC (sweep of

readings plus x,y, theta)– Build map on PC



View map graphically on PC

4ft

4ft

4ft

5ft

Final Exam Demo: Build Map Dynamically and Use to Find Best Path

to Target

4ft

4ft

4ft

5ft

Record initial position Head toward target location Take sonar sweeps and odometry readings

and build map as you move – Send readings to PC (sweep of readings plus

x,y, theta)– Build map on PC


Use map on PC to continually send new movement vectors to robot – using local path planning method

Head toward new movement direction, avoiding and reacting to obstacles (if needed)

Stop at target location View map graphically on PC (extra credit for

showing changes to map dynamically, in real-time)

Final Projects and Reports

Significant portion of grade (~3 labs or so)

Due Wednesday of Finals WeekUndergraduates: Report on Final Exam

Robot, including:– Code – … to be filled in soon

Graduate Students: Demo of Final Project, Report on Final Project, including:– Code– … to be filled in soon

References


http://plan.mcs.drexel.edu/courses/robotlab/readings/hbmanual.pdf

http://plan.mcs.drexel.edu/courses/readings







Documents

Robot Building Lab: Off-board Map Building and Path Planning Demo: Find Goal, Build Map Record initial position Head toward goal light, avoiding and