Mapping and Navigation

Transcription

1 Mapping an Navigation Januar 6 th, 5 Ewin Olson

2 Wh buil a map? Time! Plaing fiel is big, robot is slow Driving aroun perimeter takes a minute! Scoring takes time often ~ secons to line up to a mouse hole. Eploration roun gives avantage to robots that can map

3 Attack Plan Motivation: wh buil a map? Terminolog, basic concepts Mapping approaches Metrical State Estimation Occupanc Gris Topological Data Association Hints an Tips

4 What is a feature? An object/structure in the environment that we will represent in our map Something that we can observe multiple times, from ifferent locations Bunker Hill Monument (Image courtes of H. Oestreich an stock.chng

5 What is an Observation? Where o we get observations from? Camera Range/bearing to ticks an lanmarks Corners etecte from camera, range finers For now, let s assume we get these observations plus some noise estimate. uncertaint ellipse maimum likelihoo estimate an observation robot

6 Data Association The problem of recognizing that an object ou see now is the same one ou saw before Har for simple features (points, lines Eas for high-fielit features (barcoes, bunker hill monuments With perfect ata association, most mapping problems become eas

7 Attack Plan Motivation: wh buil a map? Terminolog, basic concepts Mapping approaches Metrical State Estimation Occupanc Gris Topological Data Association Hints an Tips

8 Metrical Maps Tr to estimate actual locations of features an robot The robot is at (5,3 an feature is at (, Both occupanc gri an iscrete feature approaches. Relativel har to buil Much more complete representation of the worl Foer Dining Den Kitchen Beroom Bath

9 Metrical Maps State Estimation Estimate iscrete quantities: If we fit a line to the wall, what are its parameters mb? Often use probabilistic machiner, Kalman filters Occupanc Gri Discretize the worl. I on t know what a wall is, but gris 43, 44, an 45 are impassable.

10 Baesian Estimation Represent unknowns with probabilit ensities Often, we assume the ensities are Gaussian P( πσ ( µ / σ Or we represent arbitrar ensities with particles We won t cover this toa e Normal Distribution

11 Baesian Data Fusion Eample: Estimating where Jill is staning: Alice sas: We think σ ; she wears thick glasses Bob sas: We think σ ; he s prett reliable A B How o we combine these measurements?

12 Simple Kalman Filter Answer: algebra (an a little calculus! Compute mean b fining maima of the log probabilit of the prouct P A P B. Variance is mess; consier case when P A P B N(, Tr eriving these equations at home! A B both σ µ µ σ A σ B A σ σ B A µ σ σ B B A

13 Kalman Filter Eample We now think Jill is at:.66 σ A B both

14 Kalman Filter: Properties You incorporate sensor observations one at a time. Each successive observation is the same amount of work (in terms of CPU. Yet, the final estimate is the global optimal solution. The Kalman Filter is an optimal, recursive estimator.

15 Kalman Filter: Properties Observations alwas reuce the uncertaint.

16 Kalman Filter Now Jill steps forwar one step We think one of Jill s steps is about meter, σ.5 We estimate her position: before change σ σ before σ change Before After Uncertaint increases

17 State Vector We re going to estimate robot location an orientation ( r,, t, an feature locations (f n, f n. [ r t f f f f f n f n ] T We coul tr to estimate each of these variables inepenentl But the re correlate!

18 State Correlation/Covariance We observe features relative to the robot s current position Therefore, feature location estimates covar (or correlate with robot pose. Wh o we care? We nee to track covariance so we can correctl propagate new information: Re-observing one feature gives us information about robot position, an therefore also all other features.

19 Correlation/Covariance In multiimensional Gaussian problems, equal-probabilit contours are ellipsois. Shoe size oesn t affect graes: P(grae,shoesizeP(graeP(shoesize Stuing helps graes: P(grae,stutime!P(graeP(stutime We must consier P(, jointl, respecting the correlation! If I tell ou the grae, ou learn something about stu time. Eam score Time spent stuing Shoe Size

20 Kalman Filters an Multi-Gaussians We use a Kalman filter to estimate the whole state vector jointl. [ r t f f f f f n f n ] T State vector has N elements. We on t have a scalar variance σ, we have NN covariance matri Σ. Element (i,j tells us how the uncertainties in feature i an j are relate.

21 Kalman Filters an Multi-Gaussians Kalman equations tell us how to incorporate observations Propagating effects ue to correlation Kalman equations tell us how to a new uncertaint ue to robot moving We choose a Gaussian noise moel for this too.

22 Sstem Equations (EKF Consier range/bearing measurements, ifferentiall riven robot Let k f( k-,u k-, w k- ucontrol inputs, wnoise Let z k h( k,v k vnoise w u w w u w w u f ' sin( ( ' cos( ( ' v z v z h r f r f r f r f, arctan ( ] ( [( /

23 EKF Upate Equations Time upate: f(,u, PAPA T WQW T Observation KPH T (HPH T VRV T - K(z-h(, P(I-KHP P is our covariance matri The look scar, but once ou compute our Jacobians, it just works! Af/ Wf/w Hh/ Vh/v Staff can help (It s eas ecept for the atan! w u w w u w w u f ' sin( ( ' cos( ( ' v z v z h r f r f r f r f, arctan ( ] ( [( /

24 EKF Jacobians ' ' ' sin( ( ' cos( ( ' w u w w u w w u f r f r f r f r f / ] ( [( cos( sin( u u A cos( sin( sin( cos( u u W σ σ w w Q

25 EKF Jacobians v z v z h r f r f r f r f, arctan ( ] ( [( / / ( /( λ σ σ v v T VRV H / / / / / / / / λ λ λ λ

26 Kalman Filter: Properties In the limit, features become highl correlate Because observing one feature gives information about other features Kalman filter computes the posterior pose, but not the posterior trajector. If ou want to know the path that the robot travele, ou have to make an etra backwars pass.

27 Kalman Filter: a movie

28 Kalman Filters Nemesis With N features, upate time is O(N! For Maslab, N is small. Who cares? In the real worl, N can be 6. Current research: lowercost mapping methos

29 Non-Baesian Map Builing

30 Attack Plan Motivation: wh buil a map? Terminolog, basic concepts Mapping approaches Metrical State Estimation Occupanc Gris Topological Data Association Hints an Tips

31 Occupanc Gris Another wa of mapping: Divie the worl into a gri Each gri recors whether there s something there or not Use current robot position estimate to fill in squares accoring to sensor observations

32 Occupanc Gris Eas to generate, har to maintain accurac Basicall impossible to uno mistakes Occupanc gri resolution limite b the robot s position uncertaint Keep ea-reckoning error as small as possible When too much error has accumulate, save the map an start over. Use oler maps for reference?

33 Attack Plan Motivation: wh buil a map? Terminolog, basic concepts Mapping approaches Metrical State Estimation Occupanc Gris Topological Data Association Hints an Tips

34 Topological Maps Tr to estimate how locations are relate There s an eas (straight path between feature an Eas to buil, eas to plan paths Onl a partial representation of the worl Resulting paths are suboptimal Dining Foer Bath Kitchen Den Beroom

35 Topological Maps Much easier than this metrical map stuff. Don t even tr to keep track of where features are. Onl worr about connectivit.

36 Topological Map Eample G G 7 Note that the wa we raw (where we raw the noes oes not contain information.

37 Topological Map-Builing Algorithm Until eploration roun ens: Eplore until we fin a previousl unseen barcoe Travel to the barcoe Perform a 36 egree scan, noting the barcoes, balls, an goals which are visible. Buil a tree Noes barcoe features Eges connect features which are ajacent Ege weight is istance

38 Topological Maps: Planning Graph is eas to o process! If we re lost, go to nearest lanmark. Noes form a highwa Can fin nearest goal, fin areas of high ball ensit A* Search G.3 6 G

39 Attack Plan Motivation: wh buil a map? Terminolog, basic concepts Mapping approaches Metrical State Estimation Occupanc Gris Topological Data Association Hints an Tips

40 Data Association If we can t tell when we re reobserving a feature, we on t learn anthing! We nee to observe the same feature twice to generate a constraint

41 Data Association: Bar Coes Trivial! The Bar Coes have unique IDs; rea the ID.

42 Data Association: Tick Marks The blue tick marks can be use as features too. You onl nee to reobserve the same feature twice to benefit! If ou can track them over short intervals, ou can use them to improve our eareckoning.

43 Data Association: Tick Marks Ieal situation: Lots of tick marks, ranoml arrange Goo position estimates on all tick marks Then we search for a rigi-botransformation that best aligns the points.

44 Data Association: Tick Marks Fin a rotation that aligns the most tick marks Gives ou ata association for matche ticks Gives ou rigi bo transform for the robot! RotationTranslation

45 Attack Plan Motivation: wh buil a map? Terminolog, basic concepts Mapping approaches Metrical State Estimation Occupanc Gris Topological Data Association Hints an Tips

46 Using the eploration roun Contest a:. During eploration roun, buil a map.. Write map to a file. 3. During scoring roun, reloa the map. 4. Score lots of points. Use two separate applications for eplore/score rouns. Saving state to a file will ease testing: You can test our scoring coe without having to reeplore You can han-tweak the state file to create new test conitions or troubleshoot.

47 Debugging map-builing algorithms You can t ebug what ou can t see. Prouce a visualization of the map! Metrical map: eas to raw Topological map: raw the graph (using graphviz/ot? Displa the graph via BotClient Write movement/sensor observations to a file to test mapping inepenentl (an off-line

48 Course Announcements Gros: Forgot to mention that our first gro costs ZERO sensor points. Gro mounting issues: ais of rotation Lab checkoffs Onl a couple checkoffs estera

49 Toa s Lab Activities No structure activities toa Work towars tomorrow s check-off:. Robot place in plafiel. Fin an approach a re ball. 3. Stop. Keep it simple! Ranom walks are fine! Status messages must be isplae on OrcPa or BotClient