ME 597/747- Lecture 7 Autonomous Mobile Robots

Instructor: Chris Clark Term: Fall 2004

Figures courtesy of Siegwart & Nourbakhsh

Navigation Control Loop

Prior Knowledge Localization Operator Commands Cognition

Perception

Motion Control

Localization: Outline
1.

The Kalman Filter

1. 2. 3.

KF for fusing multiple measurements KF for fusing measurements over time KF for Robot Motion Estimation

2. 3. 4.

Kalman Filter Localization SLAM Stochastic Maps

The Kalman Filter

The Kalman Filter is used by engineers to fuse different measurements optimally, assuming they are gaussian distributions. For localization, it is used to fuse the encoder measurements (Prediction Step) with range sensors (Correction Step) in an optimal manner.

The Kalman Filter: Multiple Measurement Fusion

Consider the fusion of two measurements

q1 with variance 12 q2 with variance 22

The fusion is framed as a weighted least squares optimization problem with cost S = i wi ( q qi )2 Minimize cost with S/ q = 0 to obtain:
q=q1+ 1 2 ( q2 q1 ) 1 2 + 2 2

The Kalman Filter: Multiple Measurement Fusion

This equation can be rewritten as a function of the Kalman Gain K:

q = q1 + K ( q2 q1 )

The Kalman gain in this case (1D) is:

K= 1 2 1 2 + 2 2

The Kalman Filter: Multiple Measurement Fusion

The variance of q can be calculated from two measurement variances 1 and 2

2 = 1 2 2 2 1 2 + 2 2

Note that the new variance is less than previous two measurement variances.

The Kalman Filter: Multiple Measurement Fusion

Localization: Outline
1.

The Kalman Filter

1. 2. 3.

KF for fusing multiple measurements KF for fusing measurements over time KF for Robot Motion Estimation

2. 3. 4.

Kalman Filter Localization SLAM Stochastic Maps

The Kalman Filter: Measurement Fusion over Time

The Kalman Filter can be used to fuse measurements over time. The measurements are used to estimate some state x The new measurement zk+1 at time step k+1 is fused with the previous state estimate xk at step k. The first measurement (q1, 1) is replaced by the previous state estimate measurement (xk, k) The second measurement (q2, 2) is replace by the new state measurement (zk+1 , z).

10

The Kalman Filter: Measurement Fusion over Time

The notation for state estimate update is:

xk+1 = xk + Kk+1 ( zk+1 xk ) k+1 2 = (1 Kk+1 )k 2

Where the Kalman Gain is updated as:

Kk+1 = k2 k2 + z2

11

Localization: Outline
1.

The Kalman Filter

1. 2. 3.

KF for fusing multiple measurements KF for fusing measurements over time KF for Robot Motion Estimation

2. 3. 4.

Kalman Filter Localization SLAM Stochastic Maps

12

The Kalman Filter: Robot Motion Estimation

For robot motion estimation, we dont just fuse measurements over time. At each time step k+1, we fuse the robots predicted motion estimate xk+1 with new measurements zk+1 to obtain an optimal estimate xk+1.

13

The Kalman Filter: Robot Motion Estimation

This is an iterative process.

zk

zk+1

xk k k

xk k

ut

xk+1 k+1 k+1

xk+1 k+1

14

The Kalman Filter: Robot Motion Estimation

Motion Prediction:

In predicting motion of a robot, consider ODE: dx = u + w u = velocity dt w = noise The state estimate can be updated based on the last robot position (xk ,k 2 ) xk+1 = xk + u [ tk+1 t ] k+1 2 = k 2 + w 2 [ tk+1 t ]

15

The Kalman Filter: Robot Motion Estimation

The straight addition of variances increases overall variance

16

The Kalman Filter: Robot Motion Estimation

Motion Correction

Can fuse sensor measurements zk+1 with motion prediction xk+1 : xk+1 = xk+1 + Kk+1 ( zk+1 xk+1 ) Where the Kalman Gain is updated as: Kk+1 = k+12 k+12 + z2

17

Localization: Outline
1. 2. 3. 4.

The Kalman Filter Kalman Filter Localization SLAM Stochastic Maps

18

Kalman Filter Localization

Algorithm:
1. 2. 3. 4. 5.

Robot Position Prediction Observation Measurement Prediction Matching Estimation: Applying the KF

19

Kalman Filter Localization: 1. Prediction

The robots position at time step k+1 is predicted based on its old location (time step k) and its movement due to the control input uk :
xk+1= f ( xk, uk) x,k+1=
xf

x,k

xf

uf

u,k

uf

Where f is the odometry function

20

Kalman Filter Localization: 1. Prediction

For example:
xk+1 = xk + uk

xk+1 xk

21

Kalman Filter Localization: 2. Observation

The second step it to obtain the observation Zk+1 (measurements) from the robots sensors at the new location at time k+1 The observation usually consists of a set n0 of single observations zj,k+1 extracted from the different sensors signals. It can represent raw data scans as well as features like lines, doors or any kind of landmarks.

22

Kalman Filter Localization: 2. Observation

The parameters of the targets are usually observed in the sensor frame {S}.

Therefore the observations have to be transformed to the world frame {W} Or The measurement prediction have to be transformed to the sensor frame {S}.

23

Kalman Filter Localization: 2. Observation

zj,k+1 = R,k+1 =

24

Kalman Filter Localization: 3. Measurement Prediction

We use the predicted robot position and the map M(k) to generate multiple predicted observations zt. They are transformed into the sensor frame
zi,k+1 = hi (zt , xk+1 )

We can define the measurement prediction as the set containing all ni predicted observations
Zk+1 = { zi,k+1 | (1 i ni) }

25

Kalman Filter Localization: 3. Measurement Prediction

For prediction, only the walls that are in the field of view of the robot are selected.
xk+1

26

Kalman Filter Localization: 3. Measurement Prediction

The generated measurement predictions have to be transformed to the robot frame {R} According to the figure in previous slide the transformation is given by
zi,k+1 = t,i rt,i = hi (zt , xk+1) W W = t,i k+1 Wr Wx W Wy W t,i k+1cos( t,i) k+1sin( t,i)
R

27

Kalman Filter Localization: 4. Matching

Assignment from observations zj,k+1 (gained by sensors) to the targets zt (stored in map) For each measurement prediction for which an corresponding observation is found we calculate the innovation:
vij,k+1 = zj,k+1 zi,k+1 = zj,k+1 - hi (zt , xk+1)

28

Kalman Filter Localization: 4. Matching

The innovation covariance found by applying the error propagation law with the covariance of the measurement R,i :
IN ij,k+1 = hi x,k+1 hiT + R,i,k+1

The validity of the correspondence between measurement and prediction can e.g. be evaluated through the Mahalanobis distance:
vij,k+1T IN ij,k+1 -1 vij,k+1 g2

29

Kalman Filter Localization: 4. Matching

30

Kalman Filter Localization: 5. Estimation

Update of robots position estimate

xk+1 = xk+1 + Kk+1 vk+1

The associated variance

x,k+1 = x,k+1 Kk+1IN,K+1KTk+1

Kalman Gain:
Kk+1 = x,k+1 hT IN,k+1-1

31

Kalman Filter Localization: 5. Estimation

Kalman filter estimation of the new robot position :

By fusing the prediction of robot position (magenta) with the innovation gained by the measurements (green) we get the updated estimate of the robot position (red)

32

Localization: Outline
1. 2. 3. 4.

The Kalman Filter Kalman Filter Localization SLAM Stochastic Maps

33

SLAM: Simultaneous Localization And Mapping

Starting from an arbitrary initial point, a mobile robot should be able to autonomously explore the environment with its on board sensors, gain knowledge about it, interpret the scene, build an appropriate map and localize itself relative to this map.

34

SLAM: Problems

Trying to reduce uncertainty

35

SLAM: Problems

Cyclic Environment

Small local error accumulates to arbitrary large global errors. This is usually irrelevant for navigation, but global error does matter when closing loops.

36

SLAM: Problems

Dynamic Environments

37

SLAM: Localization Only

38

SLAM: Localization and Mapping

39

SLAM

Map Representation

M is a set n of probabilistic feature locations Each feature is represented by the covariance matrix t and an associated credibility factor ct M = { zt, t , ct | ( 1 t n)}

40

SLAM

Map representation

ct is between 0 and 1 and quantifies the belief in the existence of the feature in the environment ct(k) = 1 exp(- ns/a + nu/b) a and b define the learning and forgetting rate and ns and nu are the number of matched and unobserved predictions up to time k, respectively.

41

Localization: Outline
1. 2. 3. 4.

The Kalman Filter Kalman Filter Localization SLAM Stochastic Maps 1. Representation 2. Example 3. Spatial Relationships 4. Map Building

42

SLAM: Stochastic Maps

Use probability distributions to model all objects in the environment

Map features Robot states

Refer to:

43

Smith, Self and Cheesemans paper on Estimating Uncertain Spatial Relationships in Robotics

SLAM: Stochastic Maps

Representation

44

For single object (e.g. mobile robot), represent state x of object as first two moments of probability distribution Mean: x = E(x) . Variance: C(x) = E( ( x x )( x x )T ) For a mobile robot: x= x , C(x) = x2 xy x y xy y2 y x y 2

SLAM: Stochastic Maps

Representation

For many objects, represent states in the stacked vector X : X = x1 , C(X) = C(x1)2 C(x1,x2) x2 C(x1,x2) C(x2)2 xn C(x1,xn) C(x2,xn)

45

SLAM: Example 1. Sense Object 1

Obj1I

Obj1R

RobotI

RobotR WORLDR

46

WORLDI

SLAM: Example 2. Move

Obj1I

RobotI

Obj1R RobotR

47

WORLDI

WORLDR

SLAM: Example 3. Sense Object 2

Obj2W Obj2R RobotI Obj1R

Obj1I

RobotR

48

WORLDI

WORLDR

SLAM: Example 4. Sense Object 1 again

Obj2W Obj2R RobotI Obj1R

Obj1I

RobotR

49

WORLDI

WORLDR

SLAM: Example 5. Update Estimate

Obj2W Obj2R RobotI Obj1R

Obj1I

RobotR

50

WORLDI

WORLDR

SLAM: Stochastic Maps

We can use Spatial Relationships to update the stochastic map

Compounding Inverse Composites

51

SLAM: Stochastic Maps

Compound Relationship

Given two spatial relationships, xij and xjk, we can compute the resultant relationship xik xik = xij xjk = xjk cos ij yjk sin ij + xij xjk sin ij + yjk cos ij + yij ij + jk Use first order estimate of mean: xik xij xjk

52

SLAM: Stochastic Maps

Compound Relationship

The first order estimate of the covariance is C(xik) J C(xij) C(xij,xjk) J T C(xij,xjk) C(xjk) J = xik (xij,xjk)

53

SLAM: Stochastic Maps

Inverse Relationship

Given the spatial relationship xij, we can compute the inverse relationship xji xji = xij = -xij cos ij - yij sin ij xij sin ij - yij cos ij -ij Use first order estimate of mean: xji xij

54

SLAM: Stochastic Maps

Inverse Relationship

The first order estimate of the covariance is C(xji) J C(xij) J T J = xji xij

55

SLAM: Stochastic Maps

Composite Relations

Given the spatial relationship xij, we can compute the inverse relationship xji xil = xij xjl = xij (xjk xkl) = xik xkl = (xij xjk) xkl

56

SLAM: Building Stochastic Maps

Iterate on

Movement New Spatial Information

Use Kalman Filter at each iteration

xk C(xk) k xk C(xk) xk+1 C(xk+1) k+1 xk+1 C(xk+1)

57

SLAM: Building Stochastic Maps

Movement

The robot makes an uncertain move yR yRob = uRob + wRob This motion results in a new world state xRob = xRob yRob This requires only modifying a small portion of the map (e.g. the element of X corresponding to the robot).

58

SLAM: Building Stochastic Maps

New Spatial Information

There are two categories of new information: a new object or a new constraint on an existing object. If a new object is added to the map, the additional entries must be added to X and C(X) If a new measurement is gained for an existing object, we can model measurement z z = h(x) + v

59

SLAM: Building Stochastic Maps

New Spatial Information

Looking at the first moments z h( x ) C(z) = hx C(x) hx + C(v) A simple function h(x) might be: h(x) = xij = xi xj

60

SLAM: Building Stochastic Maps

Kalman Filter

We can use the KF to update the state estimates xk = xk + Kk [ zk hk (xk) ] C(xk) = C(xk) Kk hxC(xk)

61

SLAM: Example

Time t0

Before the robot does anything, the initial position in both frames of reference are equated at 0. The covariance is also 0 representing no uncertainty x = [ xR ] = [ 0 ] C(x) = [ C(xR) ] = [ 0 ]

62

SLAM: Example

Time t1

The robot senses Object 1, and the object mean is added to the state vectors. x = xR = 0 x1 z1 C(x) = C(xR) C(xR,x1) = C(x1,xR) C(x1) 0 0 0 C(z1)

63

SLAM: Example

Time t2

The robot moves to another location, where the relative motion is given by yR x = xR = yR x1 z1 C(x) = C(xR) C(xR,x1) C(x1,xR) C(x1) = C(yR) 0 0 C(z1)

64

SLAM: Example

Time t3

The robot now senses a new object from the new location x = xR = yR x1 z1 x2 yR z2

65

C(x) = C(xR) C(xR,x1) C(xR,x2) = C(yR) 0 C(yR)J1T C(x1,xR) C(x1) C(x1,x2) 0 C(z1) 0 C(x2,xR) C(x2,x1) C(x2) J1 C(yR) 0 C(x2)

SLAM: Example

Time t4

The robot now senses the first object again from the new location Use the Kalman Filter to update the estimate of Object 1

66

SLAM: Mining

Courtesy of S. Thrun

67