Robot Motion Model

EKF based Localization

EKF SLAM

Graph SLAM

General Robot Motion Model

r

<xc, yc>

v • Robot state

• Control at time t

• State update model

Noise model of robot control • Noise model of control

• Robot motion model with noisy control

• Add a random rotation after arriving at xt

Typical noise

Large transition

Large rotation

State Update of a Non-sensing Robot

Variances increase with time

Adding Measurements

Monocular Camera

Stereo Camera RGB-D Camera

LiDAR

Sonar

Processing the measurements

1. Feature extraction • 2D range scan

• Lines • Corners • Local minima in range scans

• Camera • SIFT • SURF • ORB

The map consists a list of features The location of the ith feature is denoted by

, ,( , )i i x i ym m m=

We firstly assume feature extraction is well done.

1 2( , ,..., )Nm m m m=

Feature extraction and association is a key problem in SLAM

Measurements of Laser Range Scanner

Measure range and bearing angle to a feature in the environment

Distance

Angle

Feature state

EKF based Localization EKF 1. Prediction:

2. Correction:

),( 1−= ttt ug µµ

1T

t t t t tP A P A R−= +

1( )T Tt tt t t t tK P H H P H Q −= +

))(( ttttt hzK µµµ −+=

( ) tt t tP I K H P= −

EKF based Robot Localization

1. Prediction

2. Observation

3. State Correction

( )( )

1

ˆ

T Ti i i it t t t t t t

i i it t t t t

i it t t t

K H H H Q

K z z

I K H

µ µ

− = Σ Σ +

= + −

Σ = − Σ

Endfor

Observation to landmark j.

Observation matrix

Kalman Gain

Update state and covariance matrix

Simultaneously Locating and Mapping

Given: The robot’s controls Observations of nearby features

Estimate: Map of features Path of the robot

A robot is exploring an unknown, static environment.

Structure of the Landmark-based SLAM-Problem

SLAM Applications

Indoors

Space

Undersea

Underground

Robot，VR，AR

Why is SLAM a hard problem? SLAM: robot path and map are both unknown

Robot path error correlates errors in the map

17

Why is SLAM a hard problem?

In the real world, the mapping between observations and landmarks is unknown Picking wrong data associations can have catastrophic consequences Pose error correlates data associations

Robot pose uncertainty

18

SLAM: Simultaneous Localization And Mapping

Full SLAM:

Online SLAM:

Integrations typically done one at a time

),|,( :1:1:1 ttt uzmxp

121:1:1:1:1:1 ...),|,(),|,( −∫ ∫ ∫= ttttttt dxdxdxuzmxpuzmxp

Estimates most recent pose and map!

Estimates entire path and map!

Graphical Model of Online SLAM:

121:1:1:1:1:1 ...),|,(),|,( −∫ ∫ ∫= ttttttt dxdxdxuzmxpuzmxp

Graphical Model of Full SLAM:

),|,( :1:1:1 ttt uzmxp

SLAM Algorithms

Most of the SLAM algorithms are based on the following three different approaches:

• Extended Kalman Filter SLAM: (called EKF SLAM) • Particle Filter SLAM: (called FAST SLAM) • Graph-Based SLAM

EKF SLAM Using Known Correspondence • The EKF SLAM proceeds exactly like the EKF-based robot

localization • State Variables

• Initialization

( )1, 1, 1 , ,, , , , , , , , ,Tt

t x y N x N x N

xy x y m m s m m s

mθ

= =

0

0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

0 0 0 0

Σ = ∞

∞

3N+3

3N+3

(3N+3)*(3N+3)

Prediction

3N+3

3*(3N+3)

Full Motion model with noise

( )11

1

,t t Tt t x t x

t

g u yG I F g F

yµ−

−−

∂= = +

∂

3N+3

(3N+3)*(3N+3) 3N+3

(3N+3)*(3N+3) 3*(3N+3) (3N+3)*3

3*3

It is approximated by a linear function:

Measurement Model ( ) ( )2 2

, ,

, ,

,

0 0( , ) 0 0

0 0atan2

j x j y rit j y j x

sj s

m x m y

z m y m x Nm

φ

σθ σ

σ

− + − = − − − +

It is approximated by a linear function:

3*6 6*(3N+3)

3*6

6*(3N+3)

3j-3 3N-3j

For a Landmark Never Seen Before Initialize from (0,0,0) is a bad estimator. Initialize a newly seen landmark position by:

Works only when the measurement is bijective. Cannot do such kind of initialization in monocular camera based SLAM

Using centroid or triangulation for roughly location estimation instead.

The Overall EKF-SLAM Algorithm

Example of EKF-SLAM

Complexity Properties of EKF-SLAM

• Storage complexity O(N2) (3N+3)*(3N+3) • Computation complexity O(N2) (3N+3)*(3N+3) Becomes not tolerable when N>10000.

EKF SLAM with Unknown Correspondence • We actually don’t know the number of landmarks, nor the

correspondence between the measurements and the landmarks. • Using maximum likelihood method:

• In practice: Find the landmark with the minimum M-distance

• How to detect new landmark? When ( )( ); , , ,i i T

t t t t t t tN z h c m H H Qµ αΣ + < A threshold

EKF SLAM with Unknown Correspondence

Set number to the number of known landmarks

State prediction

EKF SLAM with Unknown Correspondence

A new landmark hypothesis

Generate observations from all Nt+1 landmarks

Check the M-distance between and i

tz ˆktz

EKF SLAM with Unknown Correspondence A distance threshold to detect new landmark

Nt+1, if all others have distances larger than alpha.

Update accordingly

EKF SLAM without correspondence

36

EKF-SLAM Summary • Quadratic in the number of landmarks: O(n2) • Convergence results for the linear case. • Can diverge if nonlinearities are large! • Have been applied successfully in large-scale

environments. • Approximations reduce the computational complexity.