Upload
others
View
5
Download
0
Embed Size (px)
Citation preview
1
Robotic Mapping
6.834 Student Lecture
Itamar Kahn, Thomas Lin, Yuval Mazor
Outline
• Introduction (Tom)
• Kalman Filtering (Itamar)J.J. Leonard and H.J.S. Feder. A computationally efficient method for large-scale concurrent mapping andlocalization. In J. Hollerbach and D. Koditschek, editors, Proceedings of the Ninth International Symposium onRobotics Research, Salt Lake City, Utah, 1999
• Hybrid Mapping Approaches (Yuval)S. Thrun, W. Burgard, and D. Fox. A real-time algorithm for mobile robot mapping with applications to multi-robot and 3D mapping. In Proceedings of the IEEE Internatinoal Conference on Robotics and Automation(ICRA), San Francisco, CA, 2000. IEEE
• Conclusion (Tom)
Vision / Steps
• Truly autonomous mobile robots
• Sense the environment• Acquiring models of the environment
• Reason• Act on environment
State of the Art
• 20 years of research
• Do well on static, structured, limited size
• Difficulty with dynamic, unstructured,
large scale
• Simulated versus Real-life
What is Robotic Mapping?
• Acquiring spatial models of physicalenvironments with robots
Paul Newman's mobile robot mapping MIT
What is Robotic Mapping?
• Sensors with different limitations
• Cameras, Sonar, Lasers, Radar,Compasses, GPS
2
Main Challenges
• Noise
• High Dimensionality
• Correspondence Problem
• Changing Environments
• Robotic Exploration Planning
Challenges - Noise
• Measurement errors accumulate over time
Odometry error will accumulate and throw off an entire map
Challenges - High Dimensionality
• 3-D visual maps can take millions ofnumbers
Challenges - Correspondence Problem
• Do these sensor readings from differenttimes correspond to the same object?
Is the blue object the same one it sensed earlier, or it a different object that seemslike it's in the same location because of accumulated sensor noise?
Challenges - Changing Environments
• Moving furniture, moving doors
• Even faster: Moving cars, moving people
• Hard to distinguish sensor noise and
moving items
Challenges - Robotic Exploration Planning
• How robots should explore usingincomplete maps
3
Today's Methods
• All Probabilistic
• Better models uncertainty, sensor noise
• Kalman Filtering (Itamar will present), Hybrid
Methods (Yuval will present)
• EM, Occupancy Grids, Multi-Planar Maps(not presenting)
Decoupled StochasticMapping
A Computationally Efficient Method for Large-ScaleConcurrent Mapping and Localization
John J. Leonard and Hands Jacob S. Feder, MIT, 2000
Introduction
• DSM: Feature based approach to CML
• Previous solutions are O(n2), where n is thenumber of features– Results from the number of correlations
between the vehicles and features
Overview
• Kalman and Extended Kalman Filters
• Conventional Stochastic Mapping
• Decoupled Stochastic Mapping
• Algorithm Testing
Kalman Filter Mini Tutorial
• The mini tutorial is an adaptation of a tutorialpresented at ACM SIGGRAPH 2001 by GregWelch and Gary Bishop (UNC).
– The slides of the tutorial are available athttp://www.cs.unc.edu/~tracker/ref/s2001/kalman/index.html
– More information (papers, software, links , etc) isavailable athttp://www.cs.unc.edu/~welch/kalman/index.html
Kalman Filter• KF operates by
– Predicting the new state and its uncertainty– Correcting with the new measurement
• IN: Noisy data --> OUT:less noisy
4
Kalman Filter Example2D Position-Only (e.g., 2D Tablet)
Process Model:
Measurement Model:†
xk
yk
È
Î Í
˘
˚ ˙ =
1 00 1
È
Î Í
˘
˚ ˙
xk-1
yk-1
È
Î Í
˘
˚ ˙ +
~ xk-1
~ yk-1
È
Î Í
˘
˚ ˙
†
uk
vk
È
Î Í
˘
˚ ˙ =
Hx 00 Hy
È
Î Í
˘
˚ ˙
xk
yk
È
Î Í
˘
˚ ˙ +
~ uk
~ vk
È
Î Í
˘
˚ ˙
state statetransition state noise
measurement measurementmatrix state noise
†
x k = Ax k-1 + w k-1
†
z k = Hx k + v k
Kalman Filter ExamplePreparation and Initialization
State transition:
Process Noise Covariance:
Measurement Noise Covariance:
Initialization:
†
A =1 00 1
È
Î Í
˘
˚ ˙
†
Q = E w * w T{ } =Qxx 00 Qyy
È
Î Í
˘
˚ ˙
†
R = E v *v T{ } =Rxx 00 Ryy
È
Î Í
˘
˚ ˙
†
x 0 = H-1z 0
P0 =e 00 e
È
Î Í
˘
˚ ˙
Kalman Filter ExamplePredict
Correct
†
x k- = Ax k-1
Pk- = APk-1A
T + Q
†
x k = x k- + K z k - Hx k
-( )Pk = I - KH( )P-
K = Pk-HT HPk
-HT + R( )-1
Kalman Filter Example
Predict Correct
†
x k- = Ax k-1
Pk- = APk-1A
T + Q
†
K = Pk-HT HPk
-HT + R( )-1
x k = x k- + K z k - Hx k
-( )Pk = I - KH( )P-
Kalman Filter ExampleExtend example to 2D Position-Velocity
Process model:
Measurement model:
†
state transition state
1 0 dt 00 1 0 dt0 0 1 00 0 0 1
È
Î
Í Í Í Í
˘
˚
˙ ˙ ˙ ˙
xy
dxdt
dydt
È
Î
Í Í Í Í Í
˘
˚
˙ ˙ ˙ ˙ ˙
†
measurement matrix state
Hx 0 0 00 Hy 0 0
È
Î Í
˘
˚ ˙
xy
dxdt
dydt
È
Î
Í Í Í Í Í
˘
˚
˙ ˙ ˙ ˙ ˙
Kalman Filter
• But, Kalman filter is not enough !!!
– Only matrix operations allowed (only works forlinear systems)
– Measurement is a linear function of state– Next state is linear function of previous state– Can’t estimate non-linear variables (e.g., gain,
rotation, projection, etc.)
5
Extended Kalman Filter
• Nonlinear Process (Model)– Process dynamics: A becomes a(x)– Measurement: H becomes h(x)
• Filter Reformulation– Use functions instead of matrices– Use Jacobians to project forward, and to relate
measurement to state
Stochastic Mapping
• Size-varying Kalman filter
• Add and Update of representation
• Build a map through spatial relationship
Stochastic Mapping• Estimated locations of the robot and the features
in the map
• Estimated error covariance†
x k[ ] = x r k[ ]T x f k[ ]T[ ]T
where x r = xr yr f v[ ]T
and x f k[ ]T= x 1 k[ ]T ...x N k[ ]T[ ], such that xi = xi yi[ ]T
†
P k[ ] =Prr k[ ] Prf k[ ]Pfr k[ ] Pff k[ ]
È
Î Í
˘
˚ ˙
Stochastic Mapping• The dynamic model of the robot is given by
• The observation model for the system is given by
†
x k +1[ ] = f x k[ ], u k[ ]( ) + dx u k[ ]( ) where u k[ ] = df du[ ]T
†
z k[ ] = h x k[ ]( ) + dz
Augmented Stochastic Mapping• Given these assumptions, an extended Kalman filter (EKF)
is employed to estimate the state and covariance .
†
x
†
P
Decoupled Stochastic Mapping
• Stochastic Mapping: complexity O(n2)• Solution: DSM
– Divide the environment into multiple submaps– Each submap has a vehicle position estimate
and a set of features estimates
6
How do we move from map tomap?
Cross-map relocationA B
Cross-map updatingA B
Single-pass vs. Multi-pass DSM
Decoupled Stochastic Mapping
• Vehicle travels to a previously visited area:Cross-map relocation
†
x B k[ ] ¨x r
A k[ ]x r
B j[ ]
È
Î Í
˘
˚ ˙ ,PB k[ ] ¨
PrrA k[ ] + Prr
B j[ ] PrfB j[ ]
PfrB j[ ] Pff
B j[ ]
È
Î Í
˘
˚ ˙
Decoupled Stochastic Mapping• Facilitate spatial convergence by bringing more accurate
vehicle estimates from lower to higher maps:Cross-map updating
Using EKF, estimate vehicle location in submap B: Usestate as measurement and covariance in A asprediction for state in B.
†
x B k-[ ] ¨f B
x fB k[ ]
È
Î Í
˘
˚ ˙ ,PB k-[ ] ¨
PrrB j[ ] + FB Prf
B j[ ]Pfr
B j[ ] 2PffB j[ ]
È
Î Í
˘
˚ ˙
†
x rA k[ ]
†
PrrA k[ ]
†
K = PB k-[ ]HT HPB k-[ ]HT + PrrA k[ ]( )
-1
x B k +[ ] ¨ x B k-[ ] + K z - Hx B k-[ ]( )PB k +[ ] ¨ I - KH( )PB k-[ ] I - KH( )T
+ KPrrA k[ ]KT
†
z
Methods Comparison
Full covariance ASM
Single-pass DSM
Multi-pass DSM
Testing
7
Limitations
• Sensor noise modeled by gaussian process
• Limited map dimensionality
Hybrid ApproachesA Real-Time Algorithm for Mobile Mapping with
Applications to Multi-Robot and 3D Mapping
Sebastian Thrun, Carnegie Mellon UniversityWolfram Burgard, University of FreibergDieter Fox, Carnegie Mellon University
Overview
• Concurrent mapping and localization using2D laser range finders
• Mapping: Fast scan-matching• Localization: Sample-based probabilities• Motivation: 3D-Maps and large cyclic
environments
Benefits
• Computation is all real-time• Builds 3D maps• Handles cycles• Accurate map generation in the absence of
odometric data
Background
• Incremental Localization
• Expectation Maximization
Incremental Localization
• Iterate localization for each new sensor scan• Can be done in real-time• Fail on cyclic environments as error grows
unbounded
8
Expectation Maximization (EM)
• Search most-likely map while consideringall past scans
• Probabilistically, iterate and refine the map• Can handle cyclic environments• Batch algorithms - not real-time
Goal
• Combine IL and EM in a real-timealgorithm that can handle cycles
• Use posterior estimation like in EM• Incremental map construction with
maximum likelihood estimators as in IL
Mapping
• A map is a collection of scans and poses
mt = { ot ,st }t =0,1,...,t
Map likelihood
P(m | dt ) = hP(m) L P(ott =0
t
’ÚÚ | m,st )
⋅ P(st +1 | at ,stt = 0
t -1
’ )ds1...dst
arg maxm
P(m | dt )dt ={so,ao ,s1,a1, ...,st}
•The most likely map:where:
Mapping
P(s | a, ¢ s )Posterior pose, s, after moving distance a from s’:
•The PDF has an elliptical/banana shape
PDF Intuition
• If a scan shows free space it is unlikely that futurescans will show obstacles in that space
• Darker regions indicate lower probability of anobstacle
9
Maximizing Map Likelihood
• Infeasible to maximize while robot ismoving in real-time
• In the past, the robot had to stop (EM) orrisk unbounded error (IL)
Conventional Incremental Map
• Given a scan and odometry reading,determine the most likely pose.
• Use that pose to increment the map. Nevergo back to change it.
ˆ s t = argmax P(st | ot , at -1, ˆ s t -1)
mt +1 = mt { ot , ˆ s tU }
Conventional Incremental Map
• This approach works in non-cyclicenvironments
• Pose errors necessarily grow• Past poses cannot be revised• Search algorithms cannot find solutions to
close loops
Incremental Map Problem
Posterior Incremental Mapping
• Basic premise:Use Markov localization to compute the fullposterior over robot poses
• Probability distribution over poses based onsensor data:
Bel(st ) = P(st | dt , mt -1)
Posterior Incremental Mapping
• Posterior is where the robot believes it is.• Can be incrementally updated over time
• Updated pose and maps:
Bel(st ) = hP(ot | st ,mt -1)
P(st | at -1, st -1)Bel(st -1 )dst-1Ú
s t = argmaxst
Bel(st ) mt +1 = mt { ot ,s tU }
10
Posterior Incremental Mapping
• Use the posterior belief to determine themost likely pose
• Uncertainty grows during a loop• The robot has a larger window to search to
close the loop
Implementation Details
• Take samples of posterior beliefs• Save computation and easier to generalize• Use gradient descent on each sample to find
globally maximum likelihood function.
Backwards Correction
• When a loops closes successfully, we cango back and correct our pose estimates
• Distribute the error ∆st among all poses inthe loop
• Use gradient descent for all poses in theloop to maximize likelihood
D st= s t - ˆ s t
Handling a Cycle
Multi-Robot Extensions
• Using posterior estimation extends naturallyto environments with multiple robots
• Each robot need not know any other robot’sinitial pose
• BUT every robot localize itself within themap of an initial Team Leader robot
Multi-Robot Extensions
• Use Monte Carlo Localization• Initially any location is likely• Posterior estimation localizes the robot in
the Team Leader’s map
11
Results - Cycle Mapping
• Groundrules:– Every scan used for localization– Scans appended to map every two meters
• Random odometric errors (30˚ or 1 meter)• Error generates large error during the cycle
but within acceptable range of “true” pose• Posterior estimation finds the true pose and
corrects prior beliefs
Results - Mapping w/out Odometry
• Same as before but with no odometric data• Traversing the cycles leads to very large
error growth• Once again, on cycle completion the errors
are found and fixed• Final map is virtually identical to map
generated with odometric data
Limitations
• Non-optimal• Nested cycles• Dynamic environments• Changing the map backwards in time can be
dangerous• Pseudo-Real Time
Brief Comparison
Kalman Filtering Hybrid MethodsRepresentation landmark locations point obstaclesSensor Noise Gaussian any
Map Dimensionality limited unlimitedDynamic Env's limited no
Scenario 1 - Infinite Corridor at Night
• Which algorithm is better for a robotmapping the infinite corridor late at night,when one janitor is walking around?
• Vote• Kalman Filtering• Hybrid Approaches• Don't Know
Scenario 1 - Infinite Corridor at Night
• Changing environment problem• Kalman - good! (Itamar will explain)
• Infinite corridor has few features• Can handle janitor (limited dynamics)
• Hybrid - bad! (Yuval will explain)
• Can't handle dynamic environments
12
Scenario 2 - Airport Parking Lot
• Which algorithm is better for a robotmapping an airport parking lot withhundreds of cars but no people?
• Vote• Kalman Filtering• Hybrid Approaches• Don't Know
Scenario 2 - Airport Parking Lot
• High dimensionality problem• Kalman - bad! (Itamar will explain)
• Only handles limited map dimensionality• Hybrid - good! (Yuval will explain)
• Nothing moving• Handles unlimited map dimensionality
Scenario 3 - Amusement Park
• Which algorithm is better for a robotmapping a busy amusement park duringChristmas?
• Vote• Kalman Filtering• Hybrid Approaches• Don't Know
Scenario 3 - Amusement Park
• Both fail• Kalman - bad! (Itamar will explain)
• Only does limited dynamics• Hybrid - bad! (Yuval will explain)
• Can't handle such a dynamic environment• Almost no algorithms learn meaningfulmaps in such a dynamic environment
Recap
• The Mapping Problem
• Main Challenges
• Kalman Filtering
• Hybrid Methods
• Comparison
Contributions
• Provided overview of robotic mapping
• Presented Kalman Filtering in depth
• Presented Hybrid Methods in depth