1. Robot Building Lab: Off-board Map Building and Path Planning 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 5ft

2. Robot Building Lab: Off-board Map Building and Path Planning Lab 9 Off-board Map Building and Path Planning http://plan.mcs.drexel.edu/cours es/robotlab/labs/lab09.pdf

3. Robot Building Lab: Off-board Map Building and Path Planning 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 5ft

4. Robot Building Lab: Off-board Map Building and Path Planning Final Exam Demo: Build Map Dynamically and Use to Find Best Path to Target 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)

5. Robot Building Lab: Off-board Map Building and Path Planning Today  Review global sonar maps  Review kinematics  Full duplex serial communication for off- board planning and control  Dynamic path planning with global maps

6. Robot Building Lab: Off-board Map Building and Path Planning Grid-based Algorithm  Superimpose “grid” on robot field of view  Indicate some measure of “obstacleness” in each grid cell based on sonar readings

7. Robot Building Lab: Off-board Map Building and Path Planning 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 feet Local Map unoccupied occupied or... no information (Courtesy of Dodds) (From Borenstein et. Al.)

8. Robot Building Lab: Off-board Map Building and Path Planning 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)

9. Robot Building Lab: Off-board Map Building and Path Planning 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

10. Robot Building Lab: Off-board Map Building and Path Planning Global Map Building Process  Re-evaluate position with odometry as robot moves  Decay values over time (From Borenstein et. Al.)

11. Robot Building Lab: Off-board Map Building and Path Planning Displaying Global Map of Occupancy Values on Host http://plan.mcs.drexel.edu/cgi- bin/legorobots/occgrid.cgi http://plan.mcs.drexel.edu/cgi- bin/legorobots/occgrid.cgi

12. Robot Building Lab: Off-board Map Building and Path Planning Today  Review global sonar maps  Review kinematics  Full duplex serial communication for off- board planning and control  Dynamic path planning with global maps

13. Robot Building Lab: Off-board Map Building and Path Planning 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

14. Robot Building Lab: Off-board Map Building and Path Planning 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 now x’ = x + D cos theta’ y’ = y + D sin theta’

15. Robot Building Lab: Off-board Map Building and Path Planning 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

16. Robot Building Lab: Off-board Map Building and Path Planning Today  Review global sonar maps  Review kinematics  Full duplex serial communication for off- board planning and control  Dynamic path planning with global maps

17. Robot Building Lab: Off-board Map Building and Path Planning 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)

18. Robot Building Lab: Off-board Map Building and Path Planning Real-Time Interaction  No longer using terminal program  Replacing 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)

19. Robot Building Lab: Off-board Map Building and Path Planning Andrew’s Serial Comms API PC-side - Setup  Include “pcSerial.h” into your c++ program.  Constructor: pcSerial();  Destructor: ~pcSerial();

20. Robot Building Lab: Off-board Map Building and Path Planning Andrew’s Serial Comms API PC-side – Write to HandyBoard  Write 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);

21. Robot Building Lab: Off-board Map Building and Path Planning Andrew’s Serial Comms API PC-side – Read from HandyBoard  Read 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);

22. Robot Building Lab: Off-board Map Building and Path Planning Andrew’s Serial Comms API HandyBoard-side – Read from PC  Read 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)

23. Robot Building Lab: Off-board Map Building and Path Planning Andrew’s Serial Comms API HandyBoard-side – Write to PC  Write 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)

24. Robot Building Lab: Off-board Map Building and Path Planning 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;}

25. Robot Building Lab: Off-board Map Building and Path Planning To Read/Write Array of Ints This 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;}

26. Robot Building Lab: Off-board Map Building and Path Planning Running the code: sending data from the handyboard to the pc  Start 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

27. Robot Building Lab: Off-board Map Building and Path Planning 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.

28. Robot Building Lab: Off-board Map Building and Path Planning Today  Review global sonar maps  Review kinematics  Full duplex serial communication for off- board planning and control  Dynamic path planning with global maps

29. Robot Building Lab: Off-board Map Building and Path Planning Off-board Map Building and Path Planning  How does the robot use Global Map + Target location to choose current heading?  Closing the Loop

30. Robot Building Lab: Off-board Map Building and Path Planning 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)

31. Robot Building Lab: Off-board Map Building and Path Planning Host-HandyBoard Map Building, Path Planning and Control

32. Robot Building Lab: Off-board Map Building and Path Planning Path Planning Using Maps The motion planning problem consists of the following: InputOutput 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) q goal q robot Applications Robot-assisted surgery Automated assembly plans Drug-docking and analysis Moving pianos around... (Courtesy of Dodds)

33. Robot Building Lab: Off-board Map Building and Path Planning Roadmap approaches Visibility graphs In 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)

34. Robot Building Lab: Off-board Map Building and Path Planning Roadmap approaches Full visibility graph Reduced visibility graph, i.e., not including segments that extend into obstacles on either side. Visibility graphs (but keeping endpoints’ roads) (Courtesy of Dodds)

35. Robot Building Lab: Off-board Map Building and Path Planning 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)

36. Robot Building Lab: Off-board Map Building and Path Planning Local techniques Potential Field methods compute a repulsive force away from obstacles (Courtesy of Dodds)

37. Robot Building Lab: Off-board Map Building and Path Planning Local techniques Potential Field methods compute a repulsive force away from obstacles compute an attractive force toward the goal (Courtesy of Dodds)

38. Robot Building Lab: Off-board Map Building and Path Planning 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)

39. Robot Building Lab: Off-board Map Building and Path Planning 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)

40. Robot Building Lab: Off-board Map Building and Path Planning Another view of reactive control Direct mapping from the environment to a control signal goal-seeking behaviorobstacle-avoiding behavior (Courtesy of Dodds)

41. Robot Building Lab: Off-board Map Building and Path Planning Behavior Summer vector sum of the avoid and goal motor schemas path taken by a robot controlled by the resulting field (Courtesy of Dodds)

42. Robot Building Lab: Off-board Map Building and Path Planning Another primitive Direct mapping from the environment to a control signal larger composite task random motion schema (Courtesy of Dodds)

43. Robot Building Lab: Off-board Map Building and Path Planning Local minima Noise allows a system to “jump out” of local minima. the problem a solution (Courtesy of Dodds)

44. Robot Building Lab: Off-board Map Building and Path Planning 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)

45. Robot Building Lab: Off-board Map Building and Path Planning Lab 9 Off-board Map Building and Path Planning http://plan.mcs.drexel.edu/cours es/robotlab/labs/lab09.pdf

46. Robot Building Lab: Off-board Map Building and Path Planning 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 5ft

47. Robot Building Lab: Off-board Map Building and Path Planning Final Exam Demo: Build Map Dynamically and Use to Find Best Path to Target 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)

48. Robot Building Lab: Off-board Map Building and Path Planning Final Projects and Reports  Significant portion of grade (~3 labs or so)  Due Wednesday of Finals Week  Undergraduates: 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

49. Robot Building Lab: Off-board Map Building and Path Planning References  http://plan.mcs.drexel.edu/courses/robotl ab/labs/lab09.pdf http://plan.mcs.drexel.edu/courses/robotl ab/labs/lab09.pdf  http://plan.mcs.drexel.edu/courses/robotl ab/readings/hbmanual.pdf http://plan.mcs.drexel.edu/courses/robotl ab/readings/hbmanual.pdf  http://plan.mcs.drexel.edu/courses/readin gs http://plan.mcs.drexel.edu/courses/readin gs