1. Grand Challenge for Computer Science DARPA AGV Race 2005 Srini Vason srinivp@hotmail.com srini@datafluxsystems.com srini@eecs.berkeley.edu

2. Web Resources Http://home.pacbell.net/srinivp www.datafluxsystems.com www.cyberrider.org www.darpa.mil/grandchallenge

3. AGV Grand Challenge - Oct 8, 2005 Traverse a distance of 200 miles through rough terrain from Barstow, CA to Los Vegas, NV in 10 hrs or less using an autonomous ground vehicle No contact with the vehicle and a single failure in waypoint following results in disqualification Route is given only two hours before the start of race Best result so far is 7 miles by CMU in 20 min.

4. GC 2004 - March 13, 2004 Waypoint file - spreadsheet Video of waypoints created from aerial photographs and overlaying waypoints Video of vehicles at the starting line (15 vehicles started) Videos of cyberrider AGV under manual operation in the Barstow, CA OHA

5. T1 T2T4 T3 Front Back AGV T5 T6 T7 T8 T9T10P1 P2 P3 P4 R1 R2 S1 S2 S3 S4 Ti - Trinocular vision cameras Pi - Panorama cameras Si - Ultrasound sensors (Sonar) Ri - Radar OD1 OD2 ODi - Odor and dust sensor WGH1 WGH2 WGHi - Water depth and ground hardness sensor IR1 IR2 Iri - Infrared sensor Sensors and their Locations on Autonomous Ground Vehicle (AGV)

6. Real-time Integrated Sensors and GN&C Processing Image Stabilization Image to Road Model Correspondence Road Models Database Video image streams Path Planning & Path Tracking Stationary Obstacle Detection Moving Object Detection GPS Sensor Data & Map Data Radar & Sonar Data streams Obstacle Tracking Other Vehicle Tracking OD and WGH Sensors Data Steering, Brake, Cruise Control Correction Computation Vehicle Sensors Data Actuator Control Data Determination AGV Actuators ESTOP GPS Error updates Ladar Data Wind & Env. Data Compass Data RDDF Run/pause/stop

7. Ladar Radar GPS Digital compass Vision Wheel encoders Fusion Module Map Database Road model database Driver Module Pause/run/stop Waypoint list

8. Start Boot_failure Initialize E_pause E_stop AGV State Diagram Ready Running/ moving Pause Stopped Powerdown Diagnostics Fault Ignition_on Poweron Getset (start) Cruise(run) Pause Stop Shutdown E_stop E_pause

9. Challenges Low-cost navigation sensors (GPS, vision, hardness detector, compass, ladar, radar, sonar) construction, calibration, and maintenance Sensor processing in real-time (a few hundred TOPS) and stabilization of sensors Synchronization of diverse sensor processing systems and hierarchical data fusion Ambient (surrounding) awareness framework Automatic steering, throttle, and brake control

10. Stop and go adaptive cruise control Elimination of shadows fromvideo images in real- time after filtering, rectification, and LOG filtering 3D Terrain profile construction from diverse sensors in real-time Path finding between waypoints, path following, lane marker detection, and lane following Perceptive passing Stationary vegetation and solid obstacle detection on roads and trails Moving object detection and collision avoidance

11. Pot hole, gutter, washboard, barbed wire, and fence post detection on trails Scene analysis to adjust gazing angle for sensors Cliff, canyon, switchback, and hill detection Fault detection and recovery in real-time AGV surviving in harsh environments ( rough roads, stream crossing, blowing wind and sand) Experimental setup, testing, and measurement in harsh environments Navigation during switchbacks, hill climbing, and descending

12. Road / Trail Following - cases Path or trail following Trail center line following Road following (right side of road) Road following with yellow divider line Sharp turns in roads Switchbacks in roads Road following in rolling hills

13. W1 W2C4 C3 Front Back ROBOT T5 T6 T7 R1 S1 S2 T7 - Stereo vision cameras Si- Ultrasound sensors (Sonar) Ri- Radar Wi -wheel speed sensor IR1IR2 IRi -Ladar/Lasersensor Sensors and their Locations on the ROBOT DGPS – Differential GPS DC – Digital Compass DC DGPS T5, T6 - Edge detection cameras C3, C4 - Edge detection cameras

14. Experiments Robots with sensors attached to them - DREAMbot Campus level experiments using Robots

15. T1 T2T4 T3 Front Back AGV T5 T6 T7 T8 T9T10P1 P2 P3 P4 R1 R2 S1 S2 S3 S4 Ti - Trinocular vision cameras Pi - Panorama cameras Si - Ultrasound sensors (Sonar) Ri - Radar OD1 OD2 ODi - Odor and dust sensor WGH1 WGH2 WGHi - Water depth and ground hardness sensor IR1 IR2 Iri - Infrared sensor Sensors and their Locations on Autonomous Ground Vehicle (AGV)

16. Video Cameras Low-cost camera array Use it for edge detection Use it for depth determination using stereo cameras 3-D object recognition 3D Terrain profile construction Surrounding construction Predictive passing

17. Cameras used in Edge Detection L-IBot camera R-IBot camera C-PtGrey camera Ladar Left camera Right camera Front View of Robot Road coverage by Cameras

18. Edge Detection - input/output Edge detection is implemented in vision module. The input to the edge detection is an image grabbed by the camera. The color image (.ppm) is converted to a grayscale image (.pgm), and then the pgm image is processed with edge dection algorithm. The output is a binary image in which 1 stands for places containing edge and 0 stands for places containing no edge or a packet containing edge vectors represented using end points. The output image (compressed or vectorized) will be sent to Fusion module (cruise control node) in an UDP packet. Unix program outputing UDP packets 10 times a second

19. Traversal Grid (0.75 m X 25 m) Traversal grid is formed in front of the ROBOT for navigation. The traversal grid is divided into an array of 160 rows and 12 columns at the bottom and 120 rows and 6 columns at the top Each element in the lower part of the array represents a 6.25cm square. Each element in the upper part of the array represents a 12.5cm square. Each element in the array gets a 1 if an edge is found and a 0 if no edge is found in the binary image constructed from the compressed or vectored data given as output by an edge detection video camera.

20. 25m 75 cm 6.25cm X 6.25cm squares 1.25m 12912 1 2 280 3 10m 12.50cm X 12.50cm squares 15m 160 161 1 6

21. Navigation Scenario (waypoints outside trail) The robot should pass through each way point and stay within the lateral boundary (LB) specified by the circle. The waypoint may not be in the road and the robot should go along the road and stay within the LB area (which will be regarded as passing the way point). Generally the road is straight between every two neighboring way points, but the robot should still not go off road. X X X X road Robot’s path

22. Some tips on using Intermediate points Determine a road or trail near the two waypoints to be traversed. If the map database contain intermediate points to traverse the waypoints, use them. If no intermediate waypoints are found in the map database, follow the road and stay within the LB. Else traverse by staying within the LB (might have to go out of the road)

23. Navigation Algorithm using Road Edge The fusion module receives UDP packets from a vision node. It decompresses the packets and reconstruct the binary edge image matrix. Apply the traversal grid: – Select the partial matrix (pink rectangle) in the edge matrix based on the current heading. The partial matrix has double the width of traversal grid. –The inner matrix represents the traversal grid and has the same length as the partial matrix. It should be free of obstacles for the robot to move forward. Check if there is an 1 in the inner matrix. If so, that means there is an obstacle in front and the robot needs to go around it by using steering control. 0.75m 25m 1.5m Image areaPartial matrixInner matrix Current heading

24. Algorithm (contd.) If there is a ‘1’ in the inner matrix but none in the rest of the partial matrix, rotate the inner matrix (left or right) enough to avoid the grid containing the ‘1’. This is an obstacle detection step to see if there is room for avoiding the obstacle. (Ignore points outside partial matrix when rotating) If the inner matrix still resides in the partial matrix, robot will make a turn by the same angle we rotated the inner matrix. If not, slow the robot (for further detection and making new decisions). If there is no ‘1’ in the inner matrix, proceed to go straight without making changes to steering angle. 1 Current heading

25. Algorithm (contd.) If an edge is detected in the partial matrix, the robot should make a slight turn to keep away from the edge. edge Current heading

26. Case discussion The robot starts (WPT1) and goes forward to next way point (WPT2). The vision module needs to look ahead (WPT3) to check the condition of the trail (or check the intermediate point to the current target way point). The look ahead allows the calculation of expected turning angle when reaching current target waypoint. This also gives a hint that the road will turn. When the angle is calculated, the robot will be switched to one of the following cases: The front is clear and the next way point is far away: go straight. <1.7°: make a small correction to the direction. 1.7°~5°: slow down and make a medium correction to the direction. 5°~10°: slow down and prepare for turning in road. 10°~20°: really slow and prepare for a sharp turn. 20°~90°: stop and prepare for left and right turn. Switch back in road:

27. Special situations - preprocessing The edge detection algorithm should filter out small objects in the road by Wiener filtering the image. If there is a big object in the road, the robot may first check if it is a road turn. It may check if there is an edge in the partial area or from any hints contained in the waypoints. If this is a road turn, it will slow down and look at two sides to find the road. Otherwise, it should prepare to avoid the object.

28. Vision Module Design Steps(Edge Camera) Acquire the image. Pre process the image: filter small objects, shadows, and trail markings. Apply edge detection scheme (Canny or Sobel) Compress the binary image, packetize, and communicate to fusion module.

29. Vision Module Design Steps(Center Camera) Acquire the image. Enhance path edge. Enhance center line. Pre process the image: filter small objects, shadows, and trail marking (make sure that center line shows clearly.) Apply edge detection scheme (Canny or Sobel) Compress the binary image, packetize, and communicate to fusion module

30. Data Fusion using CMV In the fusion module, we merge the data from different sensors by using the confidence measure vector (CMV) at each waypoint or intermediate point. –Initialize confidence measure vector. –Merge value for traversal grid. (edge detection, center line) –Follow center line. (center camera gets high confidence measure) –Follow path. (medium value for center camera and high value for edge detection)

31. Certainty Matrix (CM) Points in the traversal grid are covered by different sensors. The certainty of measurements at grid points change for each sensor. Certainty matrix specifies how certain each measurement at the grid points are Each sensor has a certainty matrix for the partial matrix The value for certainty can be from 0 to 255 Values 0 to 20 indicate low certainty and values 200 to 255 indicate high certainty on the measurements.

32. CM for T5 (left camera) Camera’s sweet spot is between rows 11 and 100 Rows 1 to 10 and cols. 1 to 6 : 150; cols. 7 to 12: 50 Rows 11 to 100 and cols. 1 to 6: 200; cols. 7 to 12: 70 Rows 101 to 130 and cols. 1 to6: 150; cols. 7 to 12: 50 Rows 131 to 160 and cols. 1 to 6: 20; cols. 7 to 12: 20 Rows 161 to 200 and cols. 1 to 6: 10; cols. 7 to 12: 0 Rows 201 to 280 and cols. 1 to 6: 5; cols. 7 to 12: 0 All rows and columns on the left side of column 1 in the partial matrix have the value 150

33. CM for T6 (right camera) Camera’s sweet spot is between rows 11 and 100 Rows 1 to 10 and cols. 1 to 6 : 50; cols. 7 to 12: 150 Rows 11 to 100 and cols. 1 to 6: 70; cols. 7 to 12: 200 Rows 101 to 130 and cols. 1 to6: 50; cols. 7 to 12: 150 Rows 131 to 160 and cols. 1 to 6: 20; cols. 7 to 12: 20 Rows 161 to 200 and cols. 1 to 6: 0; cols. 7 to 12: 10 Rows 201 to 280 and cols. 1 to 6: 0; cols. 7 to 12: 5 All rows and columns on the right side of column 12 in the partial matrix have the value 150

34. CM for T7 (center camera) Camera’s sweet spot is between rows 11 and 100 Stereo cameras allow depth map calculation also Rows 1 to 10 and cols. 3 to 10 : 150; rest of the cols.: 50 Rows 11 to 100 and cols. 3 to 10: 200; rest of cols. : 70 Rows 101 to 130 and cols. 3 to 10: 150; rest of cols. : 50 Rows 131 to 160 and cols. 3 to 10: 20; rest of cols. : 20 Rows 161 to 200 and cols. 3 to 10: 10; rest of cols. : 0 Rows 201 to 280 and cols. 3 to 6: 5; rest of cols. : 0 All rows and columns on the left side of column 1 and right side of column 12 in the partial matrix have the value 0

35. Merging Data from Sensors For each sensor use the CM to calculate values for each point in the traversal grid. Form the data structure, MTA, merged_traversal_array(k, i, j) that contains for the kth sensor the value for the square on the ith row and jth column of the 0.75m X 25m traversal grid. The value can be 0 to 255. Find the CMV for the current waypoint traversal by consulting the waypoint database and the threshold value (T). Calculate the goodness measure for the traversal grid using the CMV. This calculation is shown as NMTA.

36. Normalized MTA (NMTA) Let CMV = [v1 v2 v3 Vn], where n is the total number of sensors. MTA[l, i, j] = vl * MTA[l, i, j] NMTA[i, j] = (1/(255 *n)) * ( ΣMTA[l, i, j]) Use threshold T on each element of NMTA to come up with a binary version of NMTA

37. Fusion Module Design (path following) (1) Left camera gets camera data input: 1. Overlay camera axis. (XY for camera coordinate and X’Y’ for Robot coordinate) X (X’) YY’ Robot Heading Camera Heading YY’ X X’

38. Fusion Module Design (path following) (2) 2. Overlay path as determined by GPS coordinates. (Desired path) That is, given the current GPS coordinates and compass reading, locate the target waypoint position and calculate correct desired heading. YY’ X X’ Current heading Desired heading

39. Fusion Module Design (path following) (3) 3. Overlay grid boundary. (inner matrix) 4. Overlay twice grid boundary. (partial matrix) YY’ X X’ Current heading Desired heading

40. Fusion Module Design (path following) (4) 5. look for obstacles in grid. a. Ignore shadows, path marking (e.g. bikes, no bikes). b. Use Ladar data. c. Merge data. 6. Interference with road edge. Steering correction. 7. Turn. Slow the robot and then make the turn. Start path following again. 8. Repeat 1 to 7 for each sample. This is useful when doing left turn. YY’ X X’ Current heading Desired heading

41. Fusion Module Design (center line following) (1) Input image comes from center camera after edge detection. The center line is the path to be followed. Overlay Grid on the image. Look for interference between partial matrix and path edge. Steering correction – line up with center line. Y X

42. Fusion Module Design (path following) (5) Right camera: –Do the same sequence of steps as left camera. –This is useful in doing right turn.