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

SLIDE 1

Scientifjc Computing Laboratory

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

SLIDE 2

Scientifjc Computing Laboratory

Autonomous back-in parking : Motivation

2

assisted by occupancy grid map and EKF-SLAM

SLIDE 3

Scientifjc Computing Laboratory

EKF SLAM : System model

3

zk = rk φk ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ = lkx − xk

( )

2 + lky − yk

( )

2

tan−1 lky − yk lkx − xk ⎛ ⎝ ⎜ ⎞ ⎠ ⎟ −θk − π 2 ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ + vk

vk ! vr,k vφ,k ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ∼ N 0,R

( )

Measurement equation

for multiple measurements : zk

(i)

y (imaginary) x (real) θk φk

(i)

rk

(i)

xk+1 = xk + Δsk,T cos θk + Δθk,T 2 + π 2 ⎛ ⎝ ⎜ ⎞ ⎠ ⎟ Δsk,T sin θk + Δθk,T 2 + π 2 ⎛ ⎝ ⎜ ⎞ ⎠ ⎟ Δθk,T ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ , where

xk ! xk yk θk ⎡ ⎣ ⎢ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎥

Δsk,T = Δsk + wΔs,k Δθk,T = Δθk + wΔθ ,k

wk ! wΔs,k wΔθ ,k ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ∼ N 0,Q

( )

State transition equation

Here, Δsk,Δθk are commanded (intended) transition of distance and

rientation variation from control system (erroneous!)

SLIDE 4

Scientifjc Computing Laboratory

EKF SLAM : State update

4

ˆ xk|k

sm

ˆ xk|k−1 ˆ xk−1|k−1

sm

ˆ xk+1|k

prediction

measurement update or mapping

prediction

measurement update or mapping

SLIDE 5

Scientifjc Computing Laboratory

EKF SLAM : Flow chart for the update process in a cycle

5

Find corresponding cell

zk

(i)

Measurement update

Count referring number & threshold test

Collecting measurements Choose i-th measurement and update i=i+1; i == N?

equal less NO YES

fnd corresponding cell

Count referring number & threshold test

Measurement update i == N’?

equal to threshold equal to threshold-1

YES

End

Mapping

NO

i == N’?

YES NO i=1;

Choose i-th measurement and update i=i+1;

Start the update procedure in a cycle

less

Count up

‘mapping’ is state augmentation procedure If the referring number matched to threshold-1, it is judged as a landmark and do mapping

SLIDE 6

Scientifjc Computing Laboratory

EKF SLAM : Demapping

6

If there are many landmarks, length of state vector increases excessively. It causes computation load and real-time processing can not guaranteed.  If a landmark is not used for measurement update times successively, then delete from state vector and state covariance matrix.

Ndemap_thres

radar position(x, y) & yaw angle position of landmark #1 position of landmark #2 position of landmark #3

SLIDE 7

Scientifjc Computing Laboratory

Occupancy grid map : Concept of grid-map

7

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

mi

mi = 0 1- p 1 p ⎧ ⎨ ⎩

p m

( ) =

p mi

( )

i

∏

m

1

m2 ex)

m2

: plot for p m

1

m2 ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎛ ⎝ ⎜ ⎞ ⎠ ⎟

1 1

This is not a map. Just a PMF

p1p2 p1(1− p2) (1− p1)(1− p2) (1− p1)p2

m

1

SLIDE 8

Scientifjc Computing Laboratory

Occupancy grid map : Probability of a cell (1/2)

8

p mi | z1:k,x1:k

( ) = p zk | mi,z1:k−1,x1:k ( ) p mi | z1:k−1,x1:k ( ) p z1:k−1,x1:k ( )

p zk | z1:k−1,x1:k

( ) p z1:k−1,x1:k ( )

= p zk | mi,xk

( ) p mi | z1:k−1,x1:k−1 ( )

p zk | z1:k−1,xk

( )

= p mi | zk,xk

( ) p zk | xk ( ) p mi | z1:k−1,x1:k−1 ( )

p mi

( ) p zk | z1:k−1,x1:k ( )

p mi | z1:k,x1:k

( )

xk : vehicle information (position, orientation) in k-th cycle

zk : radar measurement in k-th cycle

PMF of occupancy of grid map for a pixel

zk is determined by mi &xk mi is determined by combination of z &x p zk | mi,xk

( ) = p mi | zk,xk ( ) p zk | xk ( ) p xk ( )

p mi | xk

( ) p xk ( )

p mi | xk

( ) = p(mi) p mi | z1:k,x1:k

( )

p −mi | z1:k,x1:k

( )

= p mi | zk,xk

( ) p zk | xk ( ) p mi | z1:k−1,x1:k−1 ( )

p mi

( ) p zk | z1:k−1,x1:k ( )

p −mi | zk,xk

( ) p zk | xk ( ) p −mi | z1:k−1,x1:k−1 ( )

p −mi

( ) p zk | z1:k−1,x1:k ( )

= p mi | zk,xk

( ) p mi | z1:k−1,x1:k−1 ( )

p −mi | zk,xk

( ) p −mi | z1:k−1,x1:k−1 ( )

p −mi

( )

p mi

( )

= p mi | zk,xk

( )

1− p mi | zk,xk

( )

p mi | z1:k−1,x1:k−1

( )

1− p mi | z1:k−1,x1:k−1

( )

1− p mi

( )

p mi

( )

l(x) ! ln p(x) 1− p(x)

l(mi | z1:k,x1:k ) = l(mi | zk,xk ) + l(mi | z1:k−1,x1:k−1) − l(mi)

Log odds ratio :

inverse sensor model recursive term prior information

SLIDE 9

Scientifjc Computing Laboratory

Occupancy grid map : Probability of a cell (2/2)

9

l(mi | z1:k,x1:k ) = l(mi | zk,xk ) + l(mi | z1:k−1,x1:k−1) − l(mi)

inverse sensor model

l(mi = 1| zk

(1),zk (2),!,zk ( N ),xk ) = ln p(mi = 1| Zk,xk )

p(mi = 0 | Zk,xk ) = ln p(Zk xk,mi = 1)p(mi = 1 xk )p(xk ) p(Zk,xk ) p(Zk xk,mi = 0)p(mi = 0 xk )p(xk ) p(Zk,xk ) = ln p(Zk xk,mi = 1)p(mi = 1 xk ) p(Zk xk,mi = 0)p(mi = 0 xk ) = ln p(Zk xk,mi = 1)p(mi = 1) p(Zk xk,mi = 0)p(mi = 0) = ln p(Zk xk,mi = 1) p(Zk xk,mi = 0) = ln P

D

P

F

⎛ ⎝ ⎜ ⎞ ⎠ ⎟

nc

when there exists targets in the cell ln 1− P

D

1− P

F

⎛ ⎝ ⎜ ⎞ ⎠ ⎟ when there are no targets in the cell ⎧ ⎨ ⎪ ⎪ ⎩ ⎪ ⎪

Zk ! zk

(1),zk (2),!,zk ( N )

{ }

no prior information

( nc targets in the cell represented by mi )

SLIDE 10

Scientifjc Computing Laboratory

Vehicle motion control

10

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)

SLIDE 11

Scientifjc Computing Laboratory

Before simulation : Issue of scattering points

11

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.

y x z φ θ ! k = cosφsinθ ! x + sinφsinθ ! y + cosθ ! z

Es f ,θ,φ

( ) ≅

A

ie − j2π f 2 c xi,yi,zi

( )i −

! k

( )

i=1 N

∑

= A

ie j2π f 2 c xi cosφsinθ+yi sinφsinθ+zi cosθ

( )

i=1 N

∑

OMP can be applied for scattering point extraction!

If we assume there are N point scattering points, Then the electric field value is as below :

Body fixed frame

SLIDE 12

Scientifjc Computing Laboratory

Scattering points extraction : OMP

12

min

x

x subject to Ax − z

2 2 ≤ τ

NP-hard : Algorithmically solved Least-squares solution Algorithm : Orthogonal Matching Pursuit

1. find

2. 3. 4.

5. repeat 1. ~ 4. until terminal condition

aik = argmax

a

zHA

: i-th column for k-th iteration

aik

ˆ xT = argmin

xT

r − ATxT

2 2

AT = ai1,ai2,!,aik ⎡ ⎣ ⎤ ⎦

k : current iteration number

ˆ x(ij) = ˆ xT ( j)

r = z − AT ˆ xT

: Residue update for j=1,…,k

r = z , initially

SLIDE 13

Scientifjc Computing Laboratory

Scattering points extraction : Algorithm comparison

13

We placed 20 point scatterers on the random grid position. Scattering point extraction algorithm with OMP (proposed) and CLEAN (conventional) are compared.

−5 5 −1.5 −1 −0.5 0.5 1 1.5 −1.5 −1 −0.5 0.5 1 1.5 y [m] x [m] z [m] true CLEAN OMP

SLIDE 14

Scientifjc Computing Laboratory

Scattering points extraction : Scattering points for each pose

14

φc = 10° φc = −90°

θ = 0° φ = φc − 0.75°:0.015°:φc + 0.75° fc = 78:0.02:80 [GHz]

1

1 2

z [m]

2 3 4

Triangular Surface of car Model

4 2

y [m] x [m]

2
2
4

SLIDE 15

Scientifjc Computing Laboratory

Simulation result : Mode 1 start

15

cell size : 0.075m × 0.075m measurement error variance : σ r

2 = 0.052[m2] σ φ 2 = 0.0052[rad2]

command error variance : SNR Δs = 5dB, SNR Δθ = 15dB Detection probability : P

D = 0.8, False alarm rate : P F = 10 ×10−6

Nseen_thres = 3, Ndemap_thres = 5

: true position of the radar : estimated position of the radar : scattering points of the car : estimated positions of landmarks : landmark position by measurements  & predicted radar position

100 200 300 400 500 600 700

cycle number

20 40 60 80 100

length of state vector

SLIDE 16

Scientifjc Computing Laboratory

Simulation result : End of mode 1

16

: true position of the radar : estimated position of the radar : scattering points of the car : estimated positions of landmarks : landmark position by measurements  & predicted radar position

100 200 300 400 500 600 700

cycle number

20 40 60 80 100

length of state vector

cell size : 0.075m × 0.075m measurement error variance : σ r

2 = 0.052[m2] σ φ 2 = 0.0052[rad2]

command error variance : SNR Δs = 5dB, SNR Δθ = 15dB Detection probability : P

D = 0.8, False alarm rate : P F = 10 ×10−6

Nseen_thres = 3, Ndemap_thres = 5

SLIDE 17

Scientifjc Computing Laboratory

Simulation result : End of mode 2

17

: true position of the radar : estimated position of the radar : scattering points of the car : estimated positions of landmarks : landmark position by measurements  & predicted radar position

100 200 300 400 500 600 700

cycle number

20 40 60 80 100

length of state vector

cell size : 0.075m × 0.075m measurement error variance : σ r

2 = 0.052[m2] σ φ 2 = 0.0052[rad2]

command error variance : SNR Δs = 5dB, SNR Δθ = 15dB Detection probability : P

D = 0.8, False alarm rate : P F = 10 ×10−6

Nseen_thres = 3, Ndemap_thres = 5

SLIDE 18

Scientifjc Computing Laboratory

Simulation result : End of mode 3

18

: true position of the radar : estimated position of the radar : scattering points of the car : estimated positions of landmarks : landmark position by measurements  & predicted radar position

100 200 300 400 500 600 700

cycle number

20 40 60 80 100

length of state vector

cell size : 0.075m × 0.075m measurement error variance : σ r

2 = 0.052[m2] σ φ 2 = 0.0052[rad2]

command error variance : SNR Δs = 5dB, SNR Δθ = 15dB Detection probability : P

D = 0.8, False alarm rate : P F = 10 ×10−6

Nseen_thres = 3, Ndemap_thres = 5

SLIDE 19

Scientifjc Computing Laboratory

Simulation result : End of mode 4 and stop (mode 5)

19

: true position of the radar : estimated position of the radar : scattering points of the car : estimated positions of landmarks : landmark position by measurements  & predicted radar position

100 200 300 400 500 600 700

cycle number

20 40 60 80 100

length of state vector

cell size : 0.075m × 0.075m measurement error variance : σ r

2 = 0.052[m2] σ φ 2 = 0.0052[rad2]

command error variance : SNR Δs = 5dB, SNR Δθ = 15dB Detection probability : P

D = 0.8, False alarm rate : P F = 10 ×10−6

Nseen_thres = 3, Ndemap_thres = 5

SLIDE 20

Scientifjc Computing Laboratory

Simulation result : Control feedback only (without EKF SLAM)

20

s = 0 m = -2.4516e-06 deg sT = 0.0033226 m T = -0.058052 deg

5
4
3
2
1

1 2 3

x [m]

2
1

1 2 3 4 5 6 7 8 9

y [m]

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1

Autonomous Back-in Parking Based on Occupancy Grid Map and EKF SLAM with W-Band Radar

Autonomous back-in parking : Motivation

EKF SLAM : System model

zk = rk φk ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ = lkx − xk

( )

( )

tan−1 lky − yk lkx − xk ⎛ ⎝ ⎜ ⎞ ⎠ ⎟ −θk − π 2 ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ + vk

vk ! vr,k vφ,k ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ∼ N 0,R

( )

for multiple measurements : zk

xk+1 = xk + Δsk,T cos θk + Δθk,T 2 + π 2 ⎛ ⎝ ⎜ ⎞ ⎠ ⎟ Δsk,T sin θk + Δθk,T 2 + π 2 ⎛ ⎝ ⎜ ⎞ ⎠ ⎟ Δθk,T ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ , where

xk ! xk yk θk ⎡ ⎣ ⎢ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎥

Δsk,T = Δsk + wΔs,k Δθk,T = Δθk + wΔθ ,k

wk ! wΔs,k wΔθ ,k ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ∼ N 0,Q

( )

Here, Δsk,Δθk are commanded (intended) transition of distance and

EKF SLAM : State update

ˆ xk|k

ˆ xk|k−1 ˆ xk−1|k−1

ˆ xk+1|k

prediction

prediction

EKF SLAM : Flow chart for the update process in a cycle

zk

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. If a landmark is not used for measurement update times successively, then delete from state vector and state covariance matrix.

Ndemap_thres

Occupancy grid map : Concept of grid-map

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

mi

mi = 0 1- p 1 p ⎧ ⎨ ⎩

p m

( ) =

p mi

( )

∏

m

m2 ex)

m2

: plot for p m

m2 ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎛ ⎝ ⎜ ⎞ ⎠ ⎟

1 1

This is not a map. Just a PMF

p1p2 p1(1− p2) (1− p1)(1− p2) (1− p1)p2

m

Occupancy grid map : Probability of a cell (1/2)

( ) = p zk | mi,z1:k−1,x1:k ( ) p mi | z1:k−1,x1:k ( ) p z1:k−1,x1:k ( )

( ) p z1:k−1,x1:k ( )

( ) p mi | z1:k−1,x1:k−1 ( )

( )

( ) p zk | xk ( ) p mi | z1:k−1,x1:k−1 ( )

( ) p zk | z1:k−1,x1:k ( )

p mi | z1:k,x1:k

( )

zk : radar measurement in k-th cycle

PMF of occupancy of grid map for a pixel

( )

( )

( ) p zk | xk ( ) p mi | z1:k−1,x1:k−1 ( )

( ) p zk | z1:k−1,x1:k ( )

( ) p zk | xk ( ) p −mi | z1:k−1,x1:k−1 ( )

( ) p zk | z1:k−1,x1:k ( )

( ) p mi | z1:k−1,x1:k−1 ( )

( ) p −mi | z1:k−1,x1:k−1 ( )

( )

( )

( )

( )

( )

( )

( )

( )

l(x) ! ln p(x) 1− p(x)

l(mi | z1:k,x1:k ) = l(mi | zk,xk ) + l(mi | z1:k−1,x1:k−1) − l(mi)

Occupancy grid map : Probability of a cell (2/2)

l(mi | z1:k,x1:k ) = l(mi | zk,xk ) + l(mi | z1:k−1,x1:k−1) − l(mi)

l(mi = 1| zk

p(mi = 0 | Zk,xk ) = ln p(Zk xk,mi = 1)p(mi = 1 xk )p(xk ) p(Zk,xk ) p(Zk xk,mi = 0)p(mi = 0 xk )p(xk ) p(Zk,xk ) = ln p(Zk xk,mi = 1)p(mi = 1 xk ) p(Zk xk,mi = 0)p(mi = 0 xk ) = ln p(Zk xk,mi = 1)p(mi = 1) p(Zk xk,mi = 0)p(mi = 0) = ln p(Zk xk,mi = 1) p(Zk xk,mi = 0) = ln P

P

⎛ ⎝ ⎜ ⎞ ⎠ ⎟

If there are many landmarks, length of state vector increases excessively. It causes computation load and real-time processing can not guaranteed.  If a landmark is not used for measurement update times successively, then delete from state vector and state covariance matrix.