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
Fix odometry errors with landmarks (using ground sensors)
Keep track of landmarks and stop after counting 4 landmarks
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
Fix odometry errors with landmarks (using ground sensors)
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
Review global sonar mapsReview kinematicsFull duplex serial communication for off-
board planning and controlDynamic path planning with global
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
Review global sonar mapsReview kinematicsFull duplex serial communication for off-
board planning and controlDynamic path planning with global
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
Review global sonar mapsReview kinematicsFull duplex serial communication for off-
board planning and controlDynamic path planning with global
maps
Off-board Map Building and Path Planning
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)
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)
Local techniques Potential Field methods
• compute a repulsive force away from obstacles
• compute an attractive force toward the goal
(Courtesy of Dodds)
Local techniques Potential Field methods
• compute a repulsive force away from obstacles
• 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
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
Fix odometry errors with landmarks (using ground sensors)
Keep track of landmarks and stop after counting 4 landmarks
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
Fix odometry errors with landmarks (using ground sensors)
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/labs/lab09.pdf
http://plan.mcs.drexel.edu/courses/robotlab/readings/hbmanual.pdf
http://plan.mcs.drexel.edu/courses/readings
Top Related