Autonomous Back-in Parking Based on Occupancy Grid Map and EKF SLAM - PowerPoint PPT Presentation

Autonomous Back-in Parking Based on Occupancy Grid Map and EKF SLAM with W-Band Radar Hyukjung Lee, Joohwan Chun, Kyeongjin Jeon E-mail : wooa@kaist.ac.kr Scienti fj c Computing Laboratory

Autonomous back-in parking : Motivation assisted by occupancy grid map and EKF-SLAM � 2 Scienti fj c Computing Laboratory

EKF SLAM : System model State transition equation Here, Δ s k , Δ θ k are commanded (intended) transition of distance and orientation variation from control system (erroneous!) ⎡ ⎤ ⎛ ⎞ Δ s k ,T cos θ k + Δ θ k ,T + π ⎢ ⎥ ⎜ ⎟ ⎢ ⎥ ⎡ ⎤ 2 2 ⎝ ⎠ x k Δ s k ,T = Δ s k + w Δ s , k ⎡ ⎤ w Δ s , k ⎢ ⎥ ⎢ ⎥ ( ) ⎢ ⎥ x k + 1 = x k + ⎛ ⎞ x k ! ⎢ Δ s k ,T sin θ k + Δ θ k ,T ⎥ y k w k ! ∼ N 0 , Q , where ⎢ ⎥ + π Δ θ k ,T = Δ θ k + w Δ θ , k ⎢ ⎥ w Δ θ , k ⎜ ⎟ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ θ k 2 2 ⎝ ⎠ ⎢ ⎥ ⎣ ⎦ ⎢ ⎥ Δ θ k , T ⎢ ⎥ ⎣ ⎦ y (imaginary) Measurement equation θ k φ k ( i ) ⎡ ⎤ 2 + l ky − y k ( ) ( i ) r k ( ) 2 ⎢ l kx − x k ⎥ ⎡ ⎤ ⎡ ⎤ v r , k z k = r k ⎢ ⎥ ( ) = + v k ⎢ ⎥ ⎢ ⎥ v k ! ∼ N 0 , R ⎢ ⎥ ⎛ ⎞ tan − 1 l ky − y k ⎟ − θ k − π φ k v φ , k ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎢ ⎥ ⎜ l kx − x k 2 ⎢ ⎝ ⎠ ⎥ ⎣ ⎦ ( i ) for multiple measurements : z k x (real) � 3 Scienti fj c Computing Laboratory

EKF SLAM : State update measurement prediction prediction update or mapping sm sm ˆ ˆ ˆ ˆ x k − 1| k − 1 x k | k − 1 x k | k x k + 1| k measurement update or mapping � 4 Scienti fj c Computing Laboratory

EKF SLAM : Flow chart for the update process in a cycle ‘mapping’ is state augmentation procedure Start the update Choose i-th measurement procedure in a cycle and update i=i+1; If the referring number matched to threshold-1, NO it is judged as a landmark and do mapping YES Choose i-th measurement i=1; i == N’? and update i=i+1; f nd corresponding cell ( i ) z k equal to threshold-1 Find corresponding cell Count referring Mapping number & threshold test less equal to threshold less Count referring Collecting number & threshold test measurements Count up Measurement update equal Measurement update NO i == N’? YES YES NO i == N? End � 5 Scienti fj c Computing Laboratory

EKF SLAM : Demapping If there are many landmarks, length of state vector increases excessively. It causes computation load and real-time processing can not guaranteed. 
 N demap_thres If a landmark is not used for measurement update times successively, then delete from state vector and state covariance matrix. radar position(x, y) & yaw angle position of landmark #1 position of landmark #2 position of landmark #3 � 6 Scienti fj c Computing Laboratory

Occupancy grid map : Concept of grid-map ⎧ m i = 0 1- p ( ) = ( ) ∏ m i ⎨ p m p m i 1 p ⎩ i 1.Map is constructed by cells 2.Each cell has binary random variable 3.Something is there (m=1) with probability p 4.Cells are independent of each other ex ) m 2 m m 2 1 (1 − p 1 ) p 2 p 1 p 2 1 ⎛ ⎞ ⎡ ⎤ m 1 : plot for p ⎢ ⎥ ⎜ ⎟ This is not a map. Just a PMF m 2 ⎢ ⎥ ⎝ ⎠ ⎣ ⎦ (1 − p 1 )(1 − p 2 ) p 1 (1 − p 2 ) m 1 1 � 7 Scienti fj c Computing Laboratory

Occupancy grid map : Probability of a cell (1/2) x k : vehicle information (position, orientation) in k-th cycle PMF of occupancy of grid map for a pixel ( ) p m i | z 1: k , x 1: k z k : radar measurement in k-th cycle ( ) p z k | x k ( ) p m i | z 1: k − 1 , x 1: k − 1 ( ) p m i | z k , x k ( ) p m i | z 1: k − 1 , x 1: k ( ) p z 1: k − 1 , x 1: k ( ) ) = p z k | m i , z 1: k − 1 , x 1: k ( ) ( ) p z k | z 1: k − 1 , x 1: k ( ) ( p m i | z 1: k , x 1: k p m i p m i | z 1: k , x 1: k ( ) p z 1: k − 1 , x 1: k ( ) = ( ) ( ) p z k | x k ( ) p − m i | z 1: k − 1 , x 1: k − 1 ( ) p z k | z 1: k − 1 , x 1: k p − m i | z 1: k , x 1: k p − m i | z k , x k ( ) p z k | z 1: k − 1 , x 1: k ( ) p − m i ( ) p m i | z 1: k − 1 , x 1: k − 1 ( ) = p z k | m i , x k z k is determined by m i & x k ( ) ( ) p m i | z 1: k − 1 , x 1: k − 1 ( ) ( ) p z k | z 1: k − 1 , x k m i is determined by combination of z & x p − m i p m i | z k , x k = ( ) p − m i | z 1: k − 1 , x 1: k − 1 ( ) ( ) p − m i | z k , x k p m i ( ) p z k | x k ( ) p m i | z 1: k − 1 , x 1: k − 1 ( ) ( ) p z k | x k ( ) p x k ( ) ) = p m i | z k , x k ( = p m i | z k , x k p z k | m i , x k ( ) p x k ( ) ( ) ( ) ( ) ( ) p z k | z 1: k − 1 , x 1: k ( ) p m i | x k 1 − p m i p m i | z k , x k p m i | z 1: k − 1 , x 1: k − 1 p m i = ( ) ( ) ( ) ( ) = p ( m i ) p m i | x k 1 − p m i | z k , x k 1 − p m i | z 1: k − 1 , x 1: k − 1 p m i p ( x ) l ( m i | z 1: k , x 1: k ) = l ( m i | z k , x k ) + l ( m i | z 1: k − 1 , x 1: k − 1 ) − l ( m i ) l ( x ) ! ln Log odds ratio : 1 − p ( x ) inverse sensor model recursive term prior information � 8 Scienti fj c Computing Laboratory

Occupancy grid map : Probability of a cell (2/2) l ( m i | z 1: k , x 1: k ) = l ( m i | z k , x k ) + l ( m i | z 1: k − 1 , x 1: k − 1 ) − l ( m i ) inverse sensor model ( N ) , x k ) = ln p ( m i = 1| Z k , x k ) l ( m i = 1| z k (1) , z k (2) , ! , z k p ( m i = 0 | Z k , x k ) { } (1) , z k (2) , ! , z k ( N ) Z k ! z k p ( Z k x k , m i = 1) p ( m i = 1 x k ) p ( x k ) = ln p ( Z k x k , m i = 1) p ( m i = 1 x k ) p ( Z k , x k ) = ln p ( Z k x k , m i = 0) p ( m i = 0 x k ) p ( x k ) p ( Z k x k , m i = 0) p ( m i = 0 x k ) p ( Z k , x k ) ⎧ n c ⎛ ⎞ ln P ⎪ D when there exists targets in the cell ⎜ ⎟ = ln p ( Z k x k , m i = 1) p ( m i = 1) p ( Z k x k , m i = 0) p ( m i = 0) = ln p ( Z k x k , m i = 1) ⎪ ⎝ P ⎠ ( n c targets in the cell represented by m i ) p ( Z k x k , m i = 0) = F ⎨ ⎛ ⎞ ln 1 − P ⎪ D ⎟ when there are no targets in the cell ⎜ ⎪ 1 − P ⎝ ⎠ ⎩ F no prior information � 9 Scienti fj c Computing Laboratory

Vehicle motion control mode1 : go straight until terminal condition (y-axis position condition) mode2 : back straight until terminal condition (y-axis position condition) mode3 : back and rotate until terminal condition (degree condition) mode4 : back until terminal condition (x-axis position condition) mode5 : stop All modes are controlled by P(proportional) control (for every 1s) � 10 Scienti fj c Computing Laboratory

Before simulation : Issue of scattering points Although parked cars are stationary targets, measurements are varying when our vehicle is moving because location of scattering points are varying according to aspect angle. ! k = cos φ sin θ ! x + sin φ sin θ ! y + cos θ ! z z θ Body fixed frame If we assume there are N point scattering points, Then the electric field value is as below : ! ( ) − j 2 π f 2 j 2 π f 2 ( ) i − ( ) N N ( ) ≅ ∑ ∑ c x i cos φ sin θ + y i sin φ sin θ + z i cos θ c x i , y i , z i k E s f , θ , φ = A i e A i e y i = 1 i = 1 φ x OMP can be applied for scattering point extraction! � 11 Scienti fj c Computing Laboratory

Scattering points extraction : OMP NP-hard : Algorithmically solved min x 0 x 2 ≤ τ subject to Ax − z 2 Algorithm : Orthogonal Matching Pursuit a i k a i k = argmax z H A 1. find : i-th column for k-th iteration a 2 x T = argmin r − A T x T ⎡ ⎤ ˆ 2. A T = a i 1 , a i 2 , ! , a i k r = z , initially ⎣ ⎦ 2 x T x ( i j ) = ˆ 3. ˆ k : current iteration number x T ( j ) for j=1,…,k Least-squares solution r = z − A T ˆ x T 4. : Residue update 5. repeat 1. ~ 4. until terminal condition � 12 Scienti fj c Computing Laboratory

Scattering points extraction : Algorithm comparison We placed 20 point scatterers on the random grid position. Scattering point extraction algorithm with OMP (proposed) and CLEAN (conventional) are compared. true CLEAN 1.5 OMP 1 0.5 z [m] 0 − 0.5 − 1 − 1.5 − 5 1.5 1 0.5 0 0 − 0.5 − 1 − 1.5 5 y [m] x [m] � 13 Scienti fj c Computing Laboratory

Scattering points extraction : Scattering points for each pose Triangular Surface of car Model 4 θ = 0 ° 3 2 φ = φ c − 0.75 ° :0.015 ° : φ c + 0.75 ° z [m] 1 f c = 78:0.02:80 [GHz] 0 -1 2 4 2 0 0 -2 -2 -4 y [m] x [m] φ c = − 90 ° φ c = 10 ° � 14 Scienti fj c Computing Laboratory

Simulation result : Mode 1 start 100 : true position of the radar 80 : estimated position of the radar length of state vector 60 : scattering points of the car 40 : landmark position by measurements 
 20 & predicted radar position : estimated positions of landmarks 0 0 100 200 300 400 500 600 700 cycle number cell size : 0.075m × 0.075m 2 = 0.05 2 [m 2 ] σ φ 2 = 0.005 2 [rad 2 ] measurement error variance : σ r command error variance : SNR Δ s = 5dB, SNR Δ θ = 15dB F = 10 × 10 − 6 D = 0.8, False alarm rate : P Detection probability : P N seen_thres = 3, N demap_thres = 5 � 15 Scienti fj c Computing Laboratory