Algorithms for Sensor-Based Robotics: Kalman Filters for Mapping and Localization

Transcription

1 Algorihms for Sensor-Based Roboics: Kalman Filers for Mapping and Localizaion

2 Sensors! Laser Robos link o he eernal world (obsession wih deph) Sensors, sensors, sensors! and racking wha is sensed: world models Kinec IR rangefinder sonar rangefinder Ligh-field camera 6-735, Howie Chose wih slides from G.D. Hager and Z. Dodds

3 Sensors! Robos link o he eernal world... gyro Sensors, sensors, sensors! and racking wha is sensed: world models GPS Force/ Torque 6-735, Howie Chose wih slides from G.D. Hager and Z. Dodds Inerial measuremen uni (gyro + acceleromeer) compass

4 Infrared sensors Nonconac bump sensor () sensing is based on ligh inensiy. objec-sensing IR looks for changes a his disance diffuse disance-sensing IR (2) sensing is based on angle receved. IR emier/deecor pair 6-735, Howie Chose wih slides from G.D. Hager and Z. Dodds IR deecor

5 The response o whie copy paper (a dull, reflecive surface) Infrared calibraion raw values (pu ino 4 bis) inches 5º incremens in he dark 6-735, Howie Chose wih slides from G.D. fluorescen ligh incandescen ligh

6 Infrared calibraion energy vs. disance for various maerials ( he inciden angle is 0º, or head-on ) ( wih no ambien ligh ) 6-735, Howie Chose wih slides from G.D.

7 Sonar sensing single-ransducer sonar imeline 0 a chirp is emied ino he environmen 75ms ypically when reverberaions from he iniial chirp have sopped he ransducer goes ino receiving mode and awais a signal... limiing range sensing.5s afer a shor ime, he signal will be oo weak o be deeced ime response 6-735, Howie Chose wih slides from G.D. blanking ime Polaroid sonar emier/receivers No lower range limi for paired sonars...

8 Sonar effecs (a) Sonar providing an accurae range measuremen (b-c) Laeral resoluion is no very precise; he closes objec in he beam s cone provides he response (d) Specular reflecions cause walls o disappear (e) Open corners produce a weak spherical wavefron (f) Closed corners measure o he corner iself because of muliple reflecions --> sonar ray racing 6-735, Howie Chose wih slides from G.D. resoluion: ime / space

9 6-735, Howie Chose wih slides from G.D. Hager and Z. Dodds Sonar modeling iniial ime response accumulaed responses blanking ime cone widh spaial response

10 Laser Ranging LIDAR/Laser range finder 6-735, Howie Chose wih slides from G.D. Hager and Z. Dodds LIDAR map

11 Recen, Cool... The 3d ime-of-fligh camera () RF-modulaed opical radiaion field oupu (2) Reflecs off he environmen (3) Time-inerval inpu measuremens provides daa (no ime-inegraed!) How can we ge an image from his informaion? 4/2/205 CS 336, G.D. Hager hp:// /deailed/p_53_3d_cam.hm (w/slides from Z.

12 More recen, Cooler... Srucured ligh: Projec a known do paern wih an IR ransmier (invisible o humans) Infer deph from deformaion o ha paern deph from focus: Poins far away are blurry deph from sereo: Closer poins are shifed 4/2/205

13 The Laes, Cooles... Ligh field camera (passive) Capure he ligh going in every direcion a every 3D poin ieee.org 4/2/205 hp://ecelmahmike.blogspo.com

14 The Problem Mapping: Wha is he world around me (geomery, landmarks) sense from various posiions inegrae measuremens o produce map assumes perfec knowledge of posiion Localizaion: Where am I in he world (posiion wr landmarks) sense relae sensor readings o a world model compue locaion relaive o model assumes a perfec world model Togeher, hese are SLAM (Simulaneous Localizaion and Mapping) How can you localize wihou a map? How can you map wihou localizaion? All localizaion, mapping or SLAM mehods are based on updaing a sae: Wha makes a sae? Localizaion? Map? Boh? How cerain is he sae?

15 Represenaions for Bayesian Robo Localizaion Discree approaches ( 95) Topological represenaion ( 95) uncerainy handling (POMDPs) occas. global localizaion, recovery Grid-based, meric represenaion ( 96) global localizaion, recovery Paricle filers ( 99) sample-based represenaion global localizaion, recovery AI Kalman filers (lae-80s?) Gaussians approimaely linear models posiion racking Roboics Muli-hypohesis ( 00) muliple Kalman filers global localizaion, recovery

16 Background Gaussian (or Normal) Disribuion 2 p( ) ~ N( m, s ) : p( ) = e 2ps Univariae ( -m ) s 2 m p() ~ N(u,L) : -s s p() = (2p ) d /2 L 2 (-u) L - (-u) /2 e- Mulivariae

17 Properies of Gaussians X Y 2 ~ N( m, s ) ü 2 2 ý Þ Y ~ N( am + b, a s ) = ax + b þ X ~ N(m,s 2 ) ü ý X 2 ~ N(m 2,s 2 2 ) þ Þ X + X 2 ~ N ( m + m 2,s s ) 2 We say in he Gaussian world as long as we sar wih Gaussians and perform only linear ransformaions. Same holds for mulivariae Gaussians

18 Kalman Filer Seminal paper published in 960 Grea web page a hp:// Recursive soluion for discree linear filering problems A sae R n A measuremen z R m Discree (i.e. for ime =, 2, 3, ) Recursive (i.e. = f ( - ) ) Linear sysem (i.e = A - ) The problem is defined by a linear process model = A - + B u - + w - sae conrol Gaussian ransiion Inpu whie (opional) noise and a measuremen linear model (wih whie Gaussian noise) z = H + v observaion model Gaussian whie noise

19 Iniial Predicion Correcion Predicion 6-735, Howie Chose ), ˆ ( N s Q A A Bu A s s ˆ ˆ ) ( ) ˆ ( ˆ ˆ H K H z K s s Q A A Bu A 2 2 ˆ ˆ s s

20 Eample If we are given all he ingrediens: -, z, A, H w -, v - (and B and u - ) wha is he opimal? Wha if = A - + B u - + w - z = H + v Wih w - N( 0, Q ) v N ( 0, R ) Q is iny and R is large? R is large and Q is iny? w - Q and R are large? Q and R are iny? v

21 Kalman Filer A priori esimae (predicion using process model) a sep A poseriori esimae (correcion using measuremen model ) a sep Compue a poseriori esimae as a linear combinaion of an a priori esimae and difference beween he acual measuremen and epeced measuremen Wha is K? ˆ ˆ K ( z Gain or blending facor ha adds a measuremen innovaion Hˆ )

22 Kalman Filer Define a priori error beween rue sae and a priori esimae and is covariance as Define a poseriori error beween rue sae and poserior esimae and is covariance as Then K ha minimizes he a poseriori covariance is defined by Noe ha K e If R 0 hen K = H - (increase residual weigh) If Σ 0 hen K = 0 (decrease residual weigh) ˆ E( e e e ˆ E( ee H T ( HH T T T ) ) R) residual

23 Kalman Filer Recipe: Given Time updae Measuremen updae Time updae (predic) Measuremen updae (correc) 0 0, ˆ Q A A B A T ˆ ˆ u T T K H H K R H H H K ) ( ) ˆ ( ˆ ˆ ) ( z

24 Some Eamples Poin moving on he line according o f = m a sae is posiion and velociy inpu is force sensing is posiion Poin in he plane under Newonian laws Non-holonomic kinemaic sysem (no dynamics) sae is workspace configuraion inpu is velociy command sensing could be direcion and/or disance o beacons All dynamic sysems are open-loop inegraion Force acceleraion velociy posiion Role of sensing is o close he loop and pin down sae

25 Iniial Predicion Correcion Predicion 6-735, Howie Chose ), ˆ ( N s Q A A Bu A s s ˆ ˆ ) ( ) ˆ ( ˆ ˆ H K H z K s s Q A A Bu A 2 2 ˆ ˆ s s

26 Fully Observable vs Parially Observable 6-735, Howie Chose

27 A concree eample Process Model Observaion Model

28 Kalman Filer for Dead Reckoning Robo moves along a sraigh line wih sae = [ p, v ] T p: posiion v: velociy u is he inpu force applied o he robo Newon s 2 nd law firs order finie difference: Inegrae velociy Robo has velociy sensor Inegrae acceleraion The robo posiion is no measured The measured velociy depends on he robo velociy (duh!)

29 Eample Le plug some numbers m = D = 0. predicion a = predicion covariance a = Sar a res from he curren posiion Uh oh! Mari is singular. The reason is ha an infinie number of saes can generae he same observaion

30 Observabiliy If H does no provide a one-o-one mapping beween he sae and he measuremen, hen he sysem is unobservable In his case H is singular, such ha many saes can generae he same observaion

31 Kalman Filer Limiaions Assumpions: Linear sae dynamics Observaions linear in sae Whie Gaussian noise Wha can we do if sysem is no linear? Non-linear sae dynamics Non-linear observaions Linearize i!

32 Eended Kalman Filer Where A, H, W and V are Jacobians defined by

33 Eended Kalman Filer Kalman Filer Recipe: Given Eended Kalman Filer Recipe: Given Predicion Predicion Measuremen correcion Measuremen correcion

34 EKF for Range-Bearing Localizaion Sae posiion and orienaion Inpu forward and roaional velociy Process model Given a map, he robo sees N landmarks wih coordinaes Se 2005

35 Linearize Process Model

36 Linearize Observaion Model

37 Daa Associaion From observaion model, we have an epeced So if we have N landmarks l,, l N and we are given a scan z, how do associae each landmark o a scan observaion? Given an observed landmark, we can do Neares neighbor Mahalanobis disance Probabilisic Daa Associaion Filer (PDAF) Xiaolei Hou Pick he bes landmark or, if i is oo differen creae a new landmark l N+

38 From Localizaion o Mapping For us, he landmarks have been a known quaniy (we have a map wih he coordinaes of he landmarks), bu landmarks are no par of he sae Two choices: Make he sae he locaion of he landmarks relaive o he robo (I also know eacly where I am ) - No noion of locaion relaive o pas hisory - No fied reference for landmarks Make he sae he robo locaion now (relaive o where we sared) plus landmark locaions + Landmarks now have fied locaion - Knowledge of my locaion slowly degrades (bu his is ineviable )

39 Kalman Filers and SLAM Localizaion: sae is he locaion of he robo Mapping: sae is he locaion of 2D landmarks SLAM: sae combines boh If he sae is hen we can wrie a linear observaion sysem noe ha if we don have some fied landmarks, our sysem is unobservable (we can fully deermine all unknown quaniies) Covariance is represened by hp://ais.informaik.uni-freiburg.de

40 Sae posiion and orienaion and landmarks Inpu forward and roaional velociy The process model for localizaion is This model is augmened for 2N+3 dimensions o accommodae landmarks. This resuls in he process equaion Sep : EKF Range Bearing SLAM Sae Updae D D D ' sin cos v v y s D D D,,,, sin cos N N v v l l y l l y

41 Sep 2: EKF Range Bearing SLAM Covariance Updae I y y y y y y A 0 0 Jacobian of he robo moion The moion of he robo does no affec he coordinaes of he landmarks Sep b: Updae he covariance mari. The funcion f(s,u,w) only affecs he robo s locaion and no he landmarks Jacobian of he process model 2N2N ideniy

42 Sep 3: EKF Range Bearing SLAM Correcion Gain Compue he Jacobian H i of each h i and hen sack hem ino one big mari H. Noe ha h i only depends on 5 variables:, y,, li, y li Need o be in he correc columns of H

43 Sep 4: EKF Range Bearing SLAM Measuremen Observe N landmarks Mus have daa associaion Which measured landmark corresponds o h i? If s conains he coordinaes of N landmarks in he map, h i predics he measuremen of each landmark Mus figure which measured landmark corresponds o h i. Neares neighbor Probabilisic Daa Associaion Filer (PDAF) If using visual landmarks use visual descripors o mach landmarks If he measuremen does no correspond o any prediced observaion, hen iniialize and add he landmark o he map l l y z i y r i i r i r i i cos( ) i sin( )

44 Sep 5: EKF Range Bearing SLAM Correcion Updae From K and H updae he poserior sae esimae Tada! And we are done!

45 Bearing-Only SLAM Les assume one landmark for now s h( s) an z h(s) y l l y l y l y T www-roboics.usc.edu Tully 2008 Ofen use omni-direcional sensor

46 Why Bearing-Only SLAM is Challenging We canno esimae he landmark locaion wih one measuremen We mus guess he range and iniialize wih a large covariance due o he lack of range informaion The locaion is very uncerain and difficul o resolve wih low paralla measuremens The measuremen model is very nonlinear, which breaks convenional filering echniques Tully 2008

47 Bearing-Only SLAM wih EKF EKF uses he sandard Kalman updae The Kalman gain is compued hrough a linearizaion abou he curren esimae The resul diverges Very dependen on he iniializaion guess of landmarks hp://

48 Mono SLAM A visual landmark wih a single camera does no provide range Daa associaion is given by racking or maching visual descripors/paches Robo Vision, Imperial College hp://homepages.inf.ed.ac.uk/

49 Eperimenal Resuls The Vicoria Park Daase A well sudied benchmark daase used in many oher SLAM publicaions We simply ignored all of he range values provided wih each landmark measuremen

50 Navigaion: RMS Tianic Leonard & Eusice EKF-based sysem 866 images 3494 camera consrains Pah lengh 3.km 2D / 3.4km 3D Conve hull > 300m min. daa / 39 min. ESDF* *ecludes image regisraion ime

51 Search of Fligh 370

52 Summary Basic sysem modeling ideas Kalman filer as an esimaion mehod from a sysem model Linearizaion as a way of aacking a wider variey of problems Mapping localizaion and mapping ino EKF Eensions for managing landmark maching and no-wellconsrained sysems.