Algorithms for an Autonomous Car

1
Algorithms for an Autonomous Car

• Edwin Olson
• CS and AI Lab
• Massachusetts Institute of Technology

Joint work with Matt Antone, David Barrett,
Mitch Berger, Ryan Buckley, Stefan Campbell,
Alexander Epstein, Gaston Fiore, Luke Fletcher,
Emilio Frazzoli, Jonathan How, Albert Huang, Troy
Jones, Sertac Karaman, Olivier Koch, Yoshi
Kuwata, John Leonard, Keoni Maheloni, David
Moore, Katy Moyer, Edwin Olson, Andrew
Patrikalakis, Steve Peters, Stephen Proulx,
Nicholas Roy, Daniela Rus, Chris Sanders, Seth
Teller, Justin Teo, Robert Truax, Matthew Walter,
and Jonathan Williams
2
Agenda

• What is the DARPA Urban Challenge?
• Meet Talos (our vehicle)
• How does the system work?
• Perception
• Obstacle tracking
• Lane detection
• Planning and control

Velocityposition estimation Model Fitting /
Linear Regression
Route PlanningSearch/A
3
DARPA Urban Challenge

• DARPA is driving autonomous technologies
• To save lives (40k deaths in US annually)
• To meet congressional mandate (2015, 33)
• DARPA Grand Challenges part of this push
• 2007 The 3rd Grand Challenge
• First for MIT
• Urban setting with traffic
• Planning and re-planning Routes
• Intersections, Parking
• GPS outages
• 60 miles in 6 hours
• More money! (2.5M prizes, 10M funding)

Source DARPA Participants Briefing, May 2006
4
Problem Formulation

• Route Network Definition File (RNDF)
• What the road network looks like
• Accurate, but Incomplete
• Mission Definition File (MDF)
• Waypoints we must hit
• (e.g., 3, 5, 4, 6)
• Given 5 minutes before mission

5
Related Work

• Fully Autonomous Driving Systems
• Limited domain (highways, traffic-free roads)
• Require human to stage control handoff,
  monitoroperation, and take over in emergency
  situations
• Munichs VaMoRs (1985-2004), VAMP (1993-2004)
  CMUs NAVLAB (1985) Penn (Southall Taylor
  2001)
• Assistive Driving Technologies
• Limited duty cycle (cruising, emergencies, staged
  parking) and actuation (e.g. none, or brakes
  only)
• Require human handoff and resumption of control
• Automakers ABS, cruise control, self-parking
  systems
• Lane departure warnings (Mobileye, Iteris, ANU)

6
Meet (some of) Team MIT
6
7
Meet Team MIT

• MIT faculty, postdocs, students, staff
• Operating software, sensor computer selection
  and configuration
• 8 full-time graduate student programmers
• Draper Laboratory IRD
• System Engineering, Vehicle Integration Test
  Support, Logistics Support
• Olin College of Engineering
• Vehicle Engineering (Mech. Elec.)
• System Testing support
• Other Team Sponsors
• Quanta Computer, BAE Systems, Ford, Land Rover,
  MIT Lincoln Laboratory,

7
8
Team MIT

• Excited by DARPAs mission
• Plus, civilian applications
• Our first time out
• Didnt know what would work
• Tried a little bit of everything
• Make sure we have a big sandbox to play in

9
Meet Talos

• Land Rover LR3
• EMC Drive-by-wire
• Sensors
• 5 cameras
• 12 SICK (lasers)
• Velodyne (3D laser)
• 16 Radars
• GPS/IMU
• 40 CPU cores
• 40GB RAM
• Multiple gigabit ethernet networks
• 6KW Generator (internally mounted)
• 2KW auxiliary air conditioner

10
Meet Talos Cameras

• 5 Firewire Cameras
• Point Grey Firefly MV
• 720x480 8bpp Bayer _at_ 22.8 fps
• 40 MB/s (2.5 GB/min)
• Lots of data!
• Separate network
• Purpose lane detection

11
Meet Talos SICK Lidars

• 12 SICKs
• 5 pushbrooms (ground-looking)
• 7 skirts (obstacle-looking)
• 180 measurements, 1 spacing _at_ 75 Hz (dithered)
• 70m range, 1cm accuracy
• Primary function
• Obstacles, vehicles, curbs, road hazards

12
Meet Talos SICK
False colored by height
13
Meet Talos Velodyne

• Tons of data
• 64 lasers
• 360 horizontal FOV
• 2 ? -24 vertical FOV
• 15 Hz
• 1M points/sec
• Good, but noisy!

14
Meet Talos Velodyne
False colored by height
15
Meet Talos Radars

• Delphi ACC radars
• Range bearing closing rate
• Narrow beam width
• Need 16 to cover 288 degrees
• Good far field car detectors

16
Meet Talos Radars
Raw range, bearing, range rate. Colored by radar
17
Meet Talos GPS/IMU

• GPS
• 3 antennas
• Absolute heading from standstill

• Odometry
• High precision external mount wheel encoder
• compare to 64 count for LR3 builtin
  odometry

• IMU
• Linear accelerations
• Angular velocities
• Fuse with
• GPS odometry

18
Agenda

• What is the DARPA Urban Challenge?
• Meet Team MIT
• Meet Talos (our vehicle)
• How does the system work?
• Perception
• Obstacle tracking
• Lane detection
• Planning and control
• Does the system work?

19
System Architecture
20
Perception
21
Perception Subsystems

• Obstacle Detection/Tracking
• Laser-based
• Radar-based
• Hazards and Road-Edge Detection
• Hazards bad but traversable
• Tend to appear at road-edges
• Lane Estimation
• Road paint detection
• Curve fitting
• Lane estimation

22
Obstacle Tracking

• Detect objects and their velocity
• Low latency
• Fusion strategy union of low false-positive
  subsystems
• Fault-tolerant
• Sensor fouling
• Can lose entire modalities and keep going

SICK
RIEGL
Velodyne
Radars
3D pointclouds
Points withclosing rates
planar pointclouds
Sick Front-end
Velodyne Front-end
Data association
Off-groundpoint detections
Least-squares trajectory estimation
Spatial clustering
Temporal Association
Stationary objects
Velocity tracks
23
SICK front-end

• Given planar scans, extract those that are not
  the ground.
• Problem Obstacles and hills look the same
• Solution Use multiple scanners at different
  heights
• Implemented using occupancy grid with sensor ids

Obstacle
Just a hill
Obstacle
Just a hill
Sensors agree ? sharp vertical feature
Sensors disagree ? not an obstacle
24
Velodyne front-end

• Explicitly fit a ground model to the data
• Biggest challenge Noise!
• Range errors and false positives, often function
  of environment
• (cant average them out)
• Laser-to-laser variation
• Separate auto-gain
• Intrinsic differences (sensitivity, internal
  reflections)
• High data rate (1M points/sec) limits algorithmic
  complexity

25
Velodyne front-end

• Data structure coarse occupancy grid storing
  individual returns
• Ground detector if cell has enough returns, use
  10th percentile height is considered ground
• Robust to outliers below ground plane
• Ground is linearly interpolated between ground
  detections along rays from sensor origin
• Above-ground detector
• Reject hits less than a threshold above the
  ground estimate
• Require multiple lasers to agree that an obstacle
  is present, require minimum number of hits

A tree, perhaps
Ground Estimate (10th percentile)
Erroneous long ranges
26
Velodyne Ground Estimation

• Downward slope towards underpass clearly visible
  (blue)
• Allows more aggressive obstacle thresholds
• Allows rejection of overhead objects
• Tree canopies
• Overpasses

Towards Underpass
Interpolated ground detections, colored by height
27
Obstacles chunks

• Internally, we track chunks
• A chunk has a bounded extent (25cm) and a
  velocity
• Chunks expire after unseen for 0.25 sec (in blind
  spot 5s)
• Incoming hits are associated to chunks
• Substantial data reduction typically 1k chunks
  versus 100k hits per second.

28
Obstacles clustering

• Chunks clustered based on physical proximity
• Connect any two chunks within d meters
• Color connected components
• Union-find algorithm

29
Obstacles flows
Associating chunks over time yields
instantaneous velocity estimates
30
Velocity Estimation

• Chunk tracking yields trajectory samples (t, xt,
  yt)
• Constant velocity model
• Using noisy trajectory samples, compute x0, y0,
  vx, vy
• Time-weighted linear regression

y
t5
t11
t6
t4
t8
t3
x
x(t) x0 t vx y(t) y0 t vy
31
Velocity Estimation

• (x0,vx) determined independently from (y0,vy)
• Leads to over-determined system of equations
• Find solution that minimizes the squared error

x(t) x0 t vx y(t) y0 t vy
x0 3 vx x3 x0 4 vx x4 x0 6 vx
x6
Ax b
x (ATA)-1ATb
32
Obstacles tracks

• Track clusters over time
• Motion of a cluster over time generates a flow
  field
• Flow fields filtered to achieve velocity
  estimates.

Track estimates, filtered and thresholded
velocities.
33
Obstacles
34
Radars

• Track estimate of position and velocity
• Incoming hits assigned to tracks
• Nearest neighbor with large weight on agreement
  of closing rate and velocity
• Closing rates help data association immensely
• Constant velocity model fit using hits (with
  timestamps)
• Recent hits weighted more strongly
• Least-squares
• Cant differentiate between stationary obstacles
  and cracks in pavement
• Report only moving tracks.

35
Merging into traffic
36
Radars
Track estimates, filtered and thresholded
velocities.
37
Hazards

• Obstacles module is tuned for very low false
  positives
• It can miss things that we shouldnt drive over,
  even if we could
• E.g., curbs, potholes, berms
• Hazards produces a cost map, as opposed to an
  infeasible map
• False positives are merely an annoyance wont
  prevent forward progress

• Look for height discontinuities
• ?z / distance badness
• Treat each velodyne laser independently
• Laser-to-laser error can be as big as a curb

Curb
38
Hazards

• Dont want to detect moving vehicles
• Obstacles will detect them we want to appraise
  the road
• A smooth detector runs in parallel
• Occupancy grid records minimum and maximum z
  height in cell
• If 3x3 area of cells all have small dz, then mark
  the center cell as smooth.
• Smooth overrides any other detected hazards.
• I.e., once we see ground as smooth, its always
  smooth.

39
Hazards
Hazard map red hazard, green smooth
40
Road Edge Detection

• Road Edge a physical boundary at the edge of the
  road
• Will be used by lane tracker
• Road Edge is a transition from non-hazard to
  hazard
• Algorithm
• Cast rays from vehicle position
• Assume vehicle is on the road
• Find first hazard, record a point
• Agglomeratively cluster points into contours
• Fit smoothed lines through clusters

41
Road Edge
42
Lane Detector

• Non-parametric lane model
• Consistency with RNDF

43
Road Paint Detector

• Matched filter for road paint
• Computationally affordable
• Runs vertically and horizontally
• Mask out self, obstacles, sky

44
Road Paint Detector

• Fit hermite splines to local maxima of filter
  response
• RANSAC
• Reject curves that point towards sun
• GPS/IMU used to compute ephemeris
• Used two similar implementations with different
  pro/cons!

45
Lane Estimation

• Accumulate lane center evidence in grid map
• Lane boundary detections imply lane center 2m
  away
• Bright likely to be center of lane

46
Lane Estimation

• Fit parabolas through filter response maxima
• Match parabolas to non-parametric lane model
• If match is good enough, snap lane model to
  parabola

47
Closed-Loop Tracking
Playback speed 2x
Prior estimate goes through trees and bushes!
48
When that doesnt work

• In absence of visually confirmed estimates, rely
  on curb detector to stay on road
• Extend whiskers emanating from the car
• Pick a whisker pointing towards the goal point
  that doesnt intersect a curb or obstacle
• Planner will try to drive to the best whisker
• Switch back to lane tracking when visually
  confirmed estimates return.

Playback speed 0.5x
49
Planning
50
Path Planning

• How do we plan a path through given waypoints?

51
Path Planning

• Full-resolution motion planning over entire
  trajectory is impractical (and silly!)

3
1
2
52
Path Planning

• Solution decouple path planning
• Route
• Use graphical world model
• Short-term trajectory
• Use detailed metrical world model

3
1
1
2
53
Route Planning

• Given a list of waypoints, how do we compute the
  best plan?
• A
• Edge cost expected travel time tcost distance
  / speed limit
• Node (intersection) cost
• tcost constant
• Learn blockages tcost inf
• Constantly re-evaluate best path
• So fast, its essentially free

3
1
2
54
Navigator

• Inputs
• RNDF Network
• MDF Mission
• Outputs
• Closest node
• Best path through network
• Short-term goal location
• Stoppage timer
• Relaxes constraints if no progress
• Passing across center line
• Allow close approaches
• Etc
• High-level behaviors
• Intersection precedence
• Passing

RNDF network
MDF mission
Navigator
55
Navigator
56
Motion Planning Formulation

• Sensor data navigator state motion planning
  problem
• Gridmap used to evaluate trajectories
• Do they hit something?
• Does it come close?
• Look-up table was the configuration space of a 1m
  circle
• Configuration space of car could be approximated
  rapidly
• Goal Find a path to the goal point

Goal Point
Curb seen by hazards, but not yet lane tracker
Car
Gray BadRed Infeasible
Lamp Post
57
Control

• Pure pursuit
• Control points define a poly line
• Look-ahead point lies on poly line
• Fixed distance from car
• Steering minimum curvature path that intersects
  look-ahead point
• Feedback controller
• Look-ahead point constantly recomputed
• Yields smooth trajectories

Steering command
d
Control point Look-ahead Point
58
Motion Planner

• Rapidly-exploring Random Tree (RRT)
• Algorithm
• while (time remaining)
• Generate a randomized pure-pursuit control point
• Attach it to the closest node in the tree
• Simulate vehicle trajectory along edge
• Cull edge if infeasible
• Return lowest-cost sequence of control points
• One planner for all car behaviors
• But sampling strategy was varied

Obstacle
Oncoming lane
Off-road
Current Position
59
Whew!

• Thats nice. But does it work?

60
Test site

• South Weymouth Naval Air Station
• About 40 min. from MIT
• Usually 10K/day free to us when no paying
  customer
• Large tarmac area
• Can create arbitrary (flat) road networks
• Environmentally sensitive
• Obstacles traffic cones
• Lane markings flour
• Traffic team members cars
• Not realistic for perception testing

61
Talos in action
62
Intersection Precedence
Precedence determined by examining trigger
zones in intersection for obstacles
63
Collision - Cornell
64
Delay along easy roads

• Lane estimate jumped, putting vehicle into bad
  area

65
Results

• Brutal Eliminations
• Experienced and savvy competition

66
Results

• Pleased with our performance
• Came in 4th overall!
• Qualified with Ford IVS vehicle
• Before race day, had never
• Driven gt20miles in a day
• We kept meaning to
• Driven on a dirt hill
• Interacted with other robots
• We drove safely
• No program crashes
• Our chase driver your vehicle was always safe
• Room for improvement
• Indecisive pausing
• Potentially avoidable accidents

MIT vehicle finishes 4th
Ford/IVS team qualifies but doesnt
finish(Running one month old MIT code!)
67
Questions?

• See also
• http//www.darpa.mil/grandchallenge
• http//dgc.mit.edu

• More Questions?
• Edwin Olson
• eolson_at_mit.edu

68

• BACKUP

69
Evolution of Challenge Emphasis
Competition Stage CapabilityRequirement BAAMay2006 Testing Jan-Oct.2007 NQE (Quals) Oct. 2007 UCE (Final) Nov. 2007
No prior access or observations YES YES YES Allowed toscout course
Sparse RNDF YES YES Some minor
Dense traffic YES Some Some rare
Degraded GPS YES, Active jamming? YES, Passive denial? No no
70
Coordinate Systems

• Global position estimate can jump
• Even with 75k GPS/INS system!
• GPS outages and re-acquisitions
• Obstacles, Lane Markings, Path Planning
• None require GPS position!
• Smoothness and local consistency more important
  than global registration
• Local frame (or sensor fusion frame)
• Vehicle position defined by dead-reckoning
  (odometryIMU)
• Accurate over short time scales, no
  discontinuities
• Can project GPS coordinates into local frame as
  necessary.

71
Find another route, or drive around?

• If were stuck, there are two possibilities
• The road is blocked ? Find another route
• The road isnt where we think it is ? Relax lane
  constraint
• These two cases hard to distinguish!
• Which fail-safe timer expires first?

U-Turn
1st run of Area C (drove around blockage)
2nd run of Area C (found another route)
72
Delay on Dirt Road

• Road Edges appearing across road. Traversed FOUR
  times!

73
Collision - CarOLO
74
On Talos

• In Greek mythology, Talos was a powerful
  artificial man of bronze forged by the god
  Hephaestus to guard the island of Crete.
  According to myth, Talos would hurl boulders at
  passing ships, and would destroy offenders by
  clutching them to his breast and then jumping
  into a fire.
• http//spin.niddk.nih.gov/NMRPipe/talos/
• Its definitely not the planet on which Captain
  Pike was tortured by big-headed psychic aliens.
  That was Talos IV.

75

• STOP

76
To Users of this slide deck

• Codec hell?
• Im running with
• XVID 1.1.3
• ffdshow-tryout

77
Agenda

• Contest Overview
• Meet the team
• System architecture
• Perception
• The sensors
• Lidar, radar, cameras, GPS/INS
• Object detection and tracking
• Methods
• Obstacles
• Curbs
• Radars
• Lane and road detection
• Line finder
• Lane tracker

• Planning and Control
• Navigator (high-level planner)
• Unified planner (RRT)
• Cost map
• Pure-pursuit
• Infrastructure
• Compute
• LCM/Logging
• Viewer
• Simulation
• Tricky bits
• Planner pauses
• Timid stopping
• Dirt road (4 traversals)
• Cornell/Corolo