Embed Size (px)
Citation preview
An Inexpensive Method for Evaluating the Localization Performance of a Mobile Robot Navigation System
Harsha Kikkeri, Gershon Parent, Mihai Jalobeanu, and Stan Birchfield
Microsoft Robotics
Motivation
Goal: Automatically measure performance of mobile robot navigation system
Purpose:• Internal comparison – how is my system improving over time?• External comparison – how does my system compare to others?
Requirements:• Repeatable – not just playback of recorded file, but run the system again (with
environment dynamics) • Reproducible – others should be able to measure the performance of their system
in their environment• Comparable – need to compare solutions with different hardware and sensors, in
different environments• Inexpensive – cost should not be a barrier to use
We focus only on localization performance here
Scalability
• System should scale • in space (large environments)• in time (long runs)• in variety (different types of environments)
• Simplicity is key to scalability:• Low setup time• Easy calibration• Inexpensive components• Non-intrusive
Previous work
• Datasets: Radish, New College, SLAM datasetsdo not always have ground truth
• SLAM with ground truth: Rawseeds, Freiburg, TUMuse prerecorded data, do not scale easily
• Qualitative evaluation: RoboCupRescue, RoboCupHomefocus is on achieving a particular task
• Benchmarking initiatives: EURON, RoSta, PerMIS, RTPhave not yet created definitive set of metrics / benchmarks for nav
• Comparison on small scale: Teleworkbenchsmall scale
• Retroreflective markers and laser: Tong-Barfoot ICRA 2011requires laser, subject to occlusion
Our approach
• Checkerboard pattern
• Yields 3D pose of camera relative to target
• Convert to 2D pose of robot on floor
Landmark
x
y
A useful instrument
Laser level:
• Upward facing laser provides plumb-up line
• Downward facing laser provides plumb-down line
• Horizontal laser (not used)
• Self-leveling, so plumb lines are parallel to gravity
• Used to determine point on ground directly below origin of target
Procedure
• Calibration• Internal camera parameters• External camera parameters w.r.t. robot (position, tilt)• Floor parameters under each landmark (tilt)
• Map-building• Build map• When under landmark, user presses button
• Pose estimation + calibration robot pose w.r.t. landmark• Store robot pose w.r.t. map*
• Runtime• Generate sequence of waypoints• When robot thinks it is under a landmark,*
• Pose estimation + calibration robot pose w.r.t. landmark• Error is difference between pose at runtime and pose at map-building
*Note: Any type of map can be used
internal cameraparameters
Coordinate systems
image
camera
robot
landmark
world2D/3D Euclidean
2D Euclidean (optional)
3D Euclidean(external camera parameters)
2D Euclidean(absolute metric)
2D/3D Euclidean
(relative metric){CALIBRATION
{ POSE ESTIMATION
{LOCALIZATION (what we want)
?
?
Camera-to-robot calibration
• Need to determine:• rotation between camera and robot 3• translation between camera and robot + 3
6 parameters
• If floor were completely flat, and camera were mounted perfectly upright, then
xr = x – drc cos qrc
yr = y – drc sin qrc
qr = q – qarobot
camera
drivingdirection
wheelbase
But floor is often not flat, and camera is never upright
camera poserobot pose
camera offset
camera rollrobotcenter
xr = x – drc cos qrc – z sin fc cos (qc+q) – z sin ff cos qf
yr = y – drc sin qrc – z sin fc cos (qc+q) – z sin ff cos qf
qr = q – qa
Camera-to-robot calibration
• When floor is not flat, and camera is not upright, then estimate• tilt of camera w.r.t. floor normal (fc)
• azimuth of camera tilt plane w.r.t. forward direction of robot (qc)
• tilt of floor w.r.t. gravity (ff)
• azimuth of floor tilt plane w.r.t. positive x axis of landmark (qf)
• Rotate robot incrementally 360 degrees• Rotation axis is perpendicular to floor• Optical axis traces cone
rc rf
} }floor
grav
ity
floor normaloptical
axisfc
ff
Calibration geometry
floor
landmark
grav
ity
floor
landmark
grav
ityrobot
cameracenter
Calibration geometry
robot
Calibration geometry
ff
floor
landmark
cameracenter
grav
ity
Calibration geometry
ff
floor
landmark
axis of rotation (= floor normal)
ff
cameracenter
grav
ity
Calibration geometry
ff
floor
landmark
axis ofrotation
ff
cameracenter
grav
ity
opticalaxis1
fc
Calibration geometry
ff
floor
landmark
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
fc
Calibration geometry
ff
floor
landmark
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
fc
x1
Calibration geometry
ff
floor
landmark
opticalaxis2
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
fc
x1
rotate robot
Calibration geometry
ff
floor
landmark
opticalaxis2
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
fc
x1
rotate robot
These are 180o apart
Calibration geometry
fc
ff
fc
floor
landmark
opticalaxis2
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
x1
x1
Calibration geometry
fc
ff
z2fc
floor
landmark
opticalaxis2
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
x1
Calibration geometry
fc
ff
z2
x2
fc
floor
landmark
opticalaxis2
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
x1
Calibration geometry
fc
ff
z2
x2
fc
x2 – x1
floor
landmark
opticalaxis2
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
(x1,z1), (x2,z2) are from pose estimation
sin fc = (x2-x1) / 2zsin ff = (x2+x1) / 2z
where
z = (z1+z2)/2
Note: x1 + (x2-x1) / 2 = (x2+x1) / 2
Calibration geometry
x1
fc
ff
z2
x2
fc
x2 – x1
floor
landmark
opticalaxis2
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
radius of circle:
distance from landmarkcenter to circle center:
(x1,z1), (x2,z2) are from pose estimation
sin fc = (x2-x1) / 2zsin ff = (x2+x1) / 2z
where
z = (z1+z2)/2
Calibration geometry
x1
fc
ff
z2
x2
fc
x2 – x1
floor
landmark
opticalaxis2
axis ofrotation
ff
cameracenter
grav
ityz1
opticalaxis1
radius of circle:
distance from landmarkcenter to circle center:
where
(x1,z1), (x2,z2) are from pose estimation
rc / zsin fc = (x2-x1) / 2zsin ff = (x2+x1) / 2z rf / z
where
z = (z1+z2)/2
Calibration geometry
(from real data)
Azimuth angles
Tilt angles
where
Top-down view of circle
Evaluating accuracy
• Mounted camera tocarriage of CNC machine
• Move to different known(x,y,q), measure pose
• Covered area 1.3 x 0.6 m
Position err: m=5 s=2 mm
max=11 mm
Angular err: m=0.3 s=0.2 deg max=1 deg
Evaluating accuracy
• Placed robotat 20 randompositions under one landmark
Position err usually < 20 mm Orient err usually < 1 deg
Evaluating accuracy
• 15 landmarksacross 2 bldgs.
• Placed robotat 5 canonicalpositions
Position err usually < 20 mm Orient err usually < 1 deg
Evaluating accuracy
• Our accuracy iscomparable to other systems
• Our system isscalable to largeenvironments
• GTvision/GTlaser from Ceriani et al. AR 2009 (Rawseeds)• mocap from Kummerle et al. AR 2009• retroreflective from Tong, Barfoot ICRA 2011
scales to arbitrarily large environmentsscales to very large single-floor environments (with additional step)
Evaluating accuracy
Two different buildings on the Microsoft campus
Evaluating accuracy
• Automated runs in 2 diff. environments
• Accuracy comparable
• Easy to setup
• Easy to maintain
Computing global coordinates
Theodolite:
• Horizontal laser • emanates from pan-tilt head• Reflects off mirror
• Measures (w.r.t. gravity)• horizontal distance to mirror• pan angle to mirror• tilt angle to mirror (not used)
Computing global coordinates
For target positions:
• Repeatedly measure distance and angle for each triplet of targets with line-of-sight
• 2D Euclidean coordinates of all targets in a common global coordinate system
• High accuracy of theodolite removes nearly all drift
• Drift can be checked by adding all angles in a loop, comparing with 360 degrees (optional)
q2
l12
l23
q1
q5
q3
q4
q6
q7
l34
l45
l15
l67
l78
theodolite
reflector
reflector
Computing global coordinates
Given l1, l2, a (from theodolite)and tlength (known), find q
Naïve solution is sensitive to noiseKey is to use only measured values
Better solutiontan q = (l1 - l2 cos a) / hwhere(l1 - l2 cos a)2 + h2 = l2
2
Naïve solutionsin q = (l1 - l2 cos a) / tlength
For target orientation:
• Place reflector under several positions within target
theodolitereflector
(multiple locations– only 2 needed) q
q
a
l1l2
t length
target
theodolite
h
Navigation contest
• Microsoft and Adept are organizing Kinect Autonomous Mobile Robot Contest at IROS 2014 in Chicago
http://www.iros2014.org/program/kinect-robot-navigation-contest
Conclusion
• System for evaluating localization accuracy of navigation• Inexpensive• Easy to setup• Easy to maintain• Highly accurate• Scalable to arbitrarily large environments• Scalable to arbitrarily run lengths (time or space)
• With theodolite, global coordinates are possible
• We have begun long-term, large-scale comparisons (results forthcoming)
• Mobile robot navigation contest at IROS 2014
Thanks!
