ME 597- Lecture 1 Autonomous Mobile Robotsme597/ME597-Lecture7-LocalizationII.pdfSimultaneous Localization And Mapping Starting from an arbitrary initial point, a mobile robot should

1

ME 597/747- Lecture 7Autonomous Mobile Robots

Instructor: Chris ClarkTerm: Fall 2004

Figures courtesy of Siegwart & Nourbakhsh

2

Navigation Control Loop

Perception

Localization Cognition

Prior Knowledge Operator Commands

Motion Control

3

Localization: Outline

1. The Kalman Filter1. KF for fusing multiple measurements2. KF for fusing measurements over time3. KF for Robot Motion Estimation

2. Kalman Filter Localization3. SLAM4. Stochastic Maps

4

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.

5

The Kalman Filter:Multiple Measurement Fusion

Consider the fusion of two measurementsq1 with variance σ1

2

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 = q 1 + σ1

2 ( q2 – q1 )σ1

2 + σ2 2

6


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

7


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.

8


9




10

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 xThe 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).

11

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 = σk

2

σk2 + σz

2

12




13

The Kalman Filter:Robot Motion Estimation

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

14


This is an iterative process.

x’k xk

σ ’k σk

k

x’k+1 xk+1

σ ’k+1 σk+1

k+1

u∆t

zk+1zk

15


Motion Prediction:In predicting motion of a robot, consider ODE:

dx = u + w u = velocitydt w = noise

The state estimate can be updated based on the last robot position (xk ,σk

2 )x’k+1 = xk + u [ tk+1 – t ]

σ ’k+1 2 = σk

2 + σw 2 [ tk+1 – t ]

16


The straight addition of variances increases overall variance

17


Motion CorrectionCan fuse sensor measurements zk+1 with motion prediction x’k+1 :

xk+1 = x’k+1 + Kk+1 ( zk+1 – x’k+1 )

Where the Kalman Gain is updated as:

Kk+1 = σ ’k+12

σ ’k+12 + σz

2

18


1. The Kalman Filter2. Kalman Filter Localization3. SLAM4. Stochastic Maps

19

Kalman Filter Localization

Algorithm:1. Robot Position Prediction2. Observation3. Measurement Prediction4. Matching5. Estimation: Applying the KF

20

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:

x’k+1= f ( xk, uk)

Σ’x,k+1= x f Σx,k x f T + u f Σu,k u f T

Where f is the odometry function

21

Kalman Filter Localization:1. Prediction

For example:x’k+1 = xk + uk

xk+1

xk

22

Kalman Filter Localization:2. Observation

The second step it to obtain the observation Zk+1 (measurements) from the robot’s sensors at the new location at time k+1The 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.

23


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}

OrThe measurement prediction have to be transformed to the sensor frame {S}.

24


zj,k+1 =

ΣR,k+1 =

25

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 , x’k+1 )We can define the measurement prediction as the set containing all ni predicted observations

Zk+1 = { zi,k+1 | (1 ≤ i ≤ ni) }

26


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

xk+1

27


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 = R αt,i

rt,i

= hi (zt , x’k+1)= Wαt,i - Wθ’k+1

Wrt,i - Wx’k+1cos(Wαt,i) - Wy’k+1sin(Wαt,i)

28

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 , x’k+1)

29


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

30


31

Kalman Filter Localization:5. Estimation

Update of robot’s position estimatexk+1 = x’k+1 + Kk+1 vk+1

The associated varianceΣx,k+1 = Σ’x,k+1 – Kk+1ΣIN,K+1KT

k+1

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

-1

32

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)

33



34

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.

35

SLAM:Problems

Trying to reduce uncertainty

36

SLAM:Problems

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

37

SLAM:Problems

Dynamic Environments

38

SLAM:Localization Only

39

SLAM:Localization and Mapping

40

SLAM

Map RepresentationM is a set n of probabilistic feature locationsEach feature is represented by the covariance matrix Σt and an associated credibility factor ct

M = { zt, Σt , ct | ( 1 ≤ t ≤ n)}

41

SLAM

Map representationct 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.

42



1. Representation2. Example3. Spatial Relationships4. Map Building

43

SLAM:Stochastic Maps

Use probability distributions to model all objects in the environment

Map featuresRobot states

Refer to:Smith, Self and Cheeseman’s paper on “Estimating Uncertain Spatial Relationships in Robotics

44


RepresentationFor 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) = σx

2 σxy σxθ

y σxy σy2 σyθ

θ σxθ σyθ σθ2

45


RepresentationFor 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) …

46

SLAM:Example – 1. Sense Object 1

Obj1R

RobotR

Obj1I

RobotI

WORLDI WORLDR

47

SLAM:Example – 2. Move

Obj1RObj1I RobotI RobotR

WORLDI WORLDR

48

SLAM:Example – 3. Sense Object 2

Obj1IObj1RRobotI RobotR

Obj2RObj2W

WORLDI WORLDR

49

SLAM:Example – 4. Sense Object 1 again

Obj1IObj1RRobotI RobotR

Obj2RObj2W

WORLDI WORLDR

50

SLAM:Example – 5. Update Estimate

Obj1RObj1IRobotI RobotR

Obj2RObj2W

WORLDI WORLDR

51


We can use Spatial Relationships to update the stochastic map

CompoundingInverseComposites

52


Compound RelationshipGiven 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

53


Compound RelationshipThe 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)

54


Inverse RelationshipGiven 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

55


Inverse RelationshipThe first order estimate of the covariance is

C(xji) ≈ J C(xij) J T

J = δxji

δxij

56


Composite RelationsGiven the spatial relationship xij, we can compute the inverse relationship xji

xil = xij xjl = xij (xjk xkl)= xik xkl = (xij xjk) xkl

57

SLAM:Building Stochastic Maps

Iterate onMovementNew Spatial Information

Use Kalman Filter at each iteration

x’k xk

C(x’k) C(xk)

k

x’k+1 xk+1

C(x’k+1) C(xk+1)

k+1

58


MovementThe robot makes an uncertain move yR

yRob = uRob + wRob

This motion results in a new world statex’Rob = xRob yRob

This requires only modifying a small portion of the map (e.g. the element of X corresponding to the robot).

59


New Spatial InformationThere 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

60


New Spatial InformationLooking 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

61


Kalman FilterWe can use the KF to update the state estimates

xk = x’k + Kk [ zk – hk (x’k) ]

C(xk) = C(x’k) – Kk hxC(x’k)

62

SLAM:Example

Time t0Before 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 ]

63

SLAM:Example

Time t1The robot senses Object 1, and the object mean is added to the state vectors.

x = xR = 0x1 z1

C(x) = C(xR) C(xR,x1) = 0 0C(x1,xR) C(x1) 0 C(z1)

64

SLAM:Example

Time t2The 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(yR) 0C(x1,xR) C(x1) 0 C(z1)

65

SLAM:Example

Time t3The robot now senses a new object from the new location

x = xR = yR

x1 z1

x2 yR z2

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) 0C(x2,xR) C(x2,x1) C(x2) J1 C(yR) 0 C(x2)

66

SLAM:Example

Time t4The robot now senses the first object again from the new locationUse the Kalman Filter to update the estimate of Object 1

67

SLAM:Mining

Courtesy of S. Thrun

Documents

ME 597- Lecture 1 Autonomous Mobile Robotsme597/ME597-Lecture7-LocalizationII.pdfSimultaneous Localization And Mapping Starting from an arbitrary initial point, a mobile robot should