Robot Motion Model EKF based Localization EKF SLAM Graph SLAM

Transcription

1

2 Robo Moion Model EKF based Localizaion EKF SLAM Graph SLAM

3 General Robo Moion Model Robo sae v r Conrol a ime <x c, y c > Sae updae model

4 Noise model of robo conrol Noise model of conrol Robo moion model wih noisy conrol Typical noise Large ransiion Add a random roaion afer arriving a x Large roaion

5 Sae Updae of a Non-sensing Robo Variances increase wih ime

6 Adding Measuremens Sonar LiDAR Sereo Camera RGB-D Camera Monocular Camera

7 Processing he measuremens 1. Feaure exracion 2D range scan Lines Corners Local minima in range scans Camera SIFT SURF ORB Feaure exracion and associaion is a key problem in SLAM We firsly assume feaure exracion is well done. The map consiss a lis of feaures m= m1 m2 m N (,,..., ) The locaion of he ih feaure is denoed by m = ( m, m ) i ix, iy,

8 Measuremens of Laser Range Scanner Measure range and bearing angle o a feaure in he environmen Disance Angle Feaure sae

9 EKF based Localizaion EKF 1. Predicion: µ 2. Correcion: g (, µ 1) = u P = AP A + R T 1 T T K = PH ( HPH + Q) µ = µ + K z h( µ )) ( P= ( I KH) P 1

10 EKF based Robo Localizaion 1. Predicion

11 2. Observaion

12 3. Sae Correcion Observaion o landmark j. Observaion marix i i T i i T K =Σ H H H Q Σ + i i i K ( z zˆ ) i i ( I KH) µ = µ + Σ = Σ 1 Kalman Gain Updae sae and covariance marix Endfor

13 Simulaneously Locaing and Mapping A robo is exploring an unknown, saic environmen. Given: The robo s conrols Observaions of nearby feaures Esimae: Map of feaures Pah of he robo

14 Srucure of he Landmark-based SLAM- Problem

15 SLAM Applicaions Indoors Robo,VR,AR Undersea Space Underground

16 Why is SLAM a hard problem? SLAM: robo pah and map are boh unknown Robo pah error correlaes errors in he map

17 Why is SLAM a hard problem? Robo pose uncerainy In he real world, he mapping beween observaions and landmarks is unknown Picking wrong daa associaions can have caasrophic consequences Pose error correlaes daa associaions 17

18 SLAM: Simulaneous Localizaion And Mapping Full SLAM: p( x :, m z1:, u1 : 1 Online SLAM: ) Esimaes enire pah and map! p ( x, m z1 :, u1: ) = p( x1:, m z1:, u1: ) dx1dx2... dx 1 Inegraions ypically done one a a ime Esimaes mos recen pose and map! 18

19 Graphical Model of Online SLAM: p ( x, m z1 :, u1: ) = p( x1:, m z1:, u1: ) dx1 dx2... dx 1

20 Graphical Model of Full SLAM: p( x :, m z1:, u1 : 1 )

21 SLAM Algorihms Mos of he SLAM algorihms are based on he following hree differen approaches: Exended Kalman Filer SLAM: (called EKF SLAM) Paricle Filer SLAM: (called FAST SLAM) Graph-Based SLAM

22 EKF SLAM Using Known Correspondence The EKF SLAM proceeds exacly like he EKF-based robo localizaion Sae Variables x y= = xy,, θ, m1,, m1,, s1,, m,, m,, s m Iniializaion ( ) x y Nx Nx N 3N+3 T 3N Σ 0 = (3N+3)*(3N+3)

23 Predicion 3N+3 3*(3N+3)

24 Full Moion model wih noise I is approximaed by a linear funcion: 3N+3 (, y ) 3N+3 g u G = µ = I+ F gf 1 T 1 x x y 1 (3N+3)*(3N+3) (3N+3)*3 3*(3N+3) (3N+3)*(3N+3) 3*3

25 Measuremen Model ( mjx, x) + ( mjy, y) 2 2 σ r 0 0 i z = aan2( mjy, ym, jx, x) θ + N 0 σφ 0 m 0 0 σ js, s I is approximaed by a linear funcion: 6*(3N+3) 3*6 6*(3N+3) 3j-3 3N-3j 3*6

26 For a Landmark Never Seen Before Iniialize from (0,0,0) is a bad esimaor. Iniialize a newly seen landmark posiion by: Works only when he measuremen is bijecive. Canno do such kind of iniializaion in monocular camera based SLAM Using cenroid or riangulaion for roughly locaion esimaion insead.

27 The Overall EKF-SLAM Algorihm

28

29 Example of EKF-SLAM

30 Complexiy Properies of EKF-SLAM Sorage complexiy O(N 2 ) (3N+3)*(3N+3) Compuaion complexiy O(N 2 ) (3N+3)*(3N+3) Becomes no olerable when N>10000.

31 EKF SLAM wih Unknown Correspondence We acually don know he number of landmarks, nor he correspondence beween he measuremens and he landmarks. Using maximum likelihood mehod: In pracice: Find he landmark wih he minimum M-disance How o deec new landmark? ( i ;, i,, T ) N z h µ c m H Σ H + Q < α A hreshold When ( )

32 EKF SLAM wih Unknown Correspondence Se number o he number of known landmarks Sae predicion

33 EKF SLAM wih Unknown Correspondence A new landmark hypohesis Generae observaions from all N +1 landmarks Check he M-disance beween and z i zˆk

34 EKF SLAM wih Unknown Correspondence A disance hreshold o deec new landmark N +1, if all ohers have disances larger han alpha. Updae accordingly

35 EKF SLAM wihou correspondence

36 EKF-SLAM Summary Quadraic in he number of landmarks: O(n 2 ) Convergence resuls for he linear case. Can diverge if nonlineariies are large! Have been applied successfully in large-scale environmens. Approximaions reduce he compuaional complexiy. 36

37

38