Grand Challenge for

Computer ScienceDARPA AGV Race 2005

Srini Vasonsrinivp@hotmail.com

srini@datafluxsystems.comsrini@eecs.berkeley.edu

Web Resources

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

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.

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

T1

T2 T4

T3

Front Back

AGV

T5

T6

T7

T8

T9 T10P1

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)

Real-time Integrated Sensors and GN&C Processing

Image

StabilizationImage to Road Model Correspondence

Road Models

DatabaseVideo 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

Ladar

Radar

GPS Digital compass

Vision

Wheel encoders

Fusion Module

Map Database

Road model database

Driver ModulePause/run/stop

Waypoint list

Start

Boot_failure

Initialize

E_pause

E_stop

AGV State Diagram

Ready

Running/moving

Pause

StoppedPowerdown

Diagnostics

Fault

Ignition_on

Poweron

Getset (start)

Cruise(run)

Cruise(run)Pause

Stop

Shutdown

E_stop

E_pause

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

• 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

• 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

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

W1

W2 C4

C3

Front Back

ROBOT

T5

T6

T7R1

S1

S2

T7 - Stereo vision cameras

Si - Ultrasound sensors (Sonar)

Ri - Radar

Wi - wheel speed sensor

IR1 IR2

IRi - Ladar/ Lasersensor

Sensors and their Locations on the ROBOT

DGPS – Differential

GPS

DC – Digital

Compass

DCDGPS

T5, T6 - Edge detection cameras

C3, C4 - Edge detection cameras

Experiments

• Robots with sensors attached to them - DREAMbot

• Campus level experiments using Robots

T1

T2 T4

T3

Front Back

AGV

T5

T6

T7

T8

T9 T10P1

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)

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

Cameras used in Edge Detection

L-IBot cameraR-IBot

camera C-PtGrey camera

LadarX

Y

CenterCameraHeading

Left

camera

Right

camera

Front View of Robot

Road coverage by Cameras

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

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.

25m

75 cm

6.25cm X

6.25cm

squares1.25m

1 2 9 1212

280

3

10m

12.50cm X 12.50cm squares

15m

160

161

1 6

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

XX

X

road

Robot’s path

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)

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

Ima

ge

a

rea

Pa

rtial

ma

trix

Inn

er

ma

trix

Current heading

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

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

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:

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.

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.

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

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)

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.

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

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

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

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.

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

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

Y Y’

X X’

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.

X (X’)

YY’

RobotHeading

CameraHeading

Y Y’

X X’

Current heading

Desiredheading

Fusion Module Design (path following) (3)

3. Overlay grid boundary. (inner matrix)4. Overlay twice grid boundary. (partial matrix)

Y Y’

X X’

Current heading

Desiredheading

X (X’)

YY’

RobotHeading

CameraHeading

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.

Y Y’

X X’

Current heading

Desiredheading

X (X’)

YY’

RobotHeading

CameraHeading

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

X

Y

CenterCameraHeading

Fusion Module Design (path following) (5)

• Right camera:– Do the same sequence of steps as left

camera.– This is useful in doing right turn.