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
49
Embed
Robot Building Lab: Off-board Map Building and Path Planning Demo: Find Goal, Build Map Record initial position Head toward goal light, avoiding and.
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
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
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)
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)
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)