Am I Lost? Or Has the World Changed? Persistent Autonomous Navigation in Changing Environments

1

Am I Lost? Or Has the World Changed?Am I Lost? Or Has the World Changed?Persistent Autonomous Navigation in Persistent Autonomous Navigation in

Changing Environments Changing Environments

Aisha Walcott

PhD Candidate, EECS, Robotics

Advisor: Professor John Leonard

[draft of algorithm and framework]

2

SLAM in Changing EnvironmentsSLAM in Changing EnvironmentsGoal is to maintain an up-to-date map in a

changing environment

1: 1:( , | , )P X M , C Z UC t t t

Related Work [Montessano et al, 2005, Wang et. Al, 2007, Wolf, Sukhatme, 2005]

Change Indicator

1: 1: 1: 1

1: 1: 1 1: 1: 1

( | , , , )

( | , , ) ( | , )tP M X C Z U

P X C Z U P C Z Ut t t

C t t t t

Map is function of time

Subset of trajectory

1:X XC t

3

Talk OutlineTalk Outline

Introduction to SLAM

Traditional SLAM vs. Life-long SLAM

Mobile Robot Model

Probabilistic Problem Formulation

Pose Graph Approach

Dynamic Pose Graph SLAM (DPG-SLAM)

Simulated Data Analysis

Conclusion

4

Pose Graph SLAMPose Graph SLAM

5

Pose Graph SLAMPose Graph SLAM

Odometry

Scan Matching

Scan Matchingand

Loop Constraints

50m

6

Pose Graph FormulationPose Graph FormulationNetwork of spatial relations

Estimate relative geometric constraints between two poses

Translation in the x and y, and Rotation relative to pose

Odometry and scan matching (improved estimate)

x1

x7

Constraints

T1,2

T2,3

T3,4

7



Translation in the x and y, and Rotation relative to pose


x1

x7

Constraints

T1,2

T2,3

T3,4

Ground Truth

8



Translation in the x and y, and rotation ω relative to pose


x1

Constraints

T1,2

T2,3

Non-linear Motion Model

2, ,2

, , 12

, ( , )Tx x y x

i j y y i i i

Tx

Ty f x u w

Zero-mean Gaussian Noise

9

Pose Graph FormulationPose Graph Formulation“Closing the loop”

X1 and x7 are the same location

Similar to minimizing energy in spring network

x1

x7

T7,1

Loop Constraint

x1

x7

T7,1

Constraint Optimization

Ground TruthGround Truth

10

Pose Graph SLAMPose Graph SLAMScan matching (ICP) [Lu and Milios, 1997] Detect loopsPose graph optimization- iSAM “Incremental Smoothing

and Mapping” [Kaess et al., 2008]Returns estimate of robot trajectoryPoses and covariances are given in global frameComputes best configuration of poses according to a cost

function

11




Mobile Robot Model


Pose Graph Approach


Experiments and Results

Conclusion

12

Dynamic Pose Graph SLAM (DPG SLAM)Dynamic Pose Graph SLAM (DPG SLAM)Objective

Environment Model

Assumptions

Challenges

DPG-SLAM Algorithm

Analysis

13

ObjectiveObjectiveIf the world changes and ……Robot is certain about its position then not likely to be

lost

or

…Robot is not certain about its position then likely to become lost

Continuous map update and change detection to keep map current

14

Environment ModelEnvironment ModelStatic entities S = {s1…sj} Semi-static entities E = {e1…ek}Dynamic entities D = {d1…dm} Env = S E D

Static

Semi-Static

Dynamic

Sofa

Trashcan

Robot

Human

Walls

Door

15

DPG-SLAM ChallengesDPG-SLAM ChallengesTractability

Computationally feasible for constraint optimizationPose graph grows with time (storing matrices, matrix operations)

Maintain up-to-date mapIncorrect map ok for pair-wise scan matchingLoop constraints, insert bad constraint or never insert constraints

(reducing to scan matching )

A

B

C

A

B

C

A

C

A

C

DE

time

A

C

A

B

DE

Incorrect Map

A

C

DE

Correct Map

16

AssumptionsAssumptionsLaser range scanners (accurate)

Bounded 2D indoor environment

Dynamic

Robot makes N passes through the environment

Change occurs after each pass

Pose graph representation

Known start (home) location

Focus on semi-static

Pair-wise (ICP) scan matching

17

DPG-SLAM FrameworkDPG-SLAM Framework1. Build a pose graph, run scan matcher on

sequential nodes to obtain relative constraints

2. Attempt to insert loop closing constraints using a nearby poses heuristic

3. Detect change on current pose and corresponding nodes from different passes, compute Ci

4. Repair the pose graph, compute Xc

5. Choose next robot pose

Traditional Pose Graph SLAM

18

DPG-SLAM ExampleDPG-SLAM Example

NodesPoses, xiScans, zi

EdgesSpatial Constraints, Tij

x1 x2 x3 x4 x5

T1,2 T2,3 T3,4T4,5

19

DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)

Generate new waypointDrive to waypointTake laser range scanAdd constraint

x1 x2 x3 x4 x5

T1,2 T2,3 T3,4T4,5

x6 x7 x8

T6,7 T7,8

time

T1,6 T7,2

20


Generate new waypointDrive to waypointTake laser range scanAdd constraint Get corresponding nodeCompare scans (Detect Change)

x1 x2 x3 x4 x5

T1,2 T2,3 T3,4T4,5

x6 x7 x8

T6,7 T7,8

time

T1,6 T7,2

21


Generate new waypointDrive to waypointTake laser range scanAdd constraint Get corresponding nodeCompare scans (Detect Change)Add constraint

x1 x2 x3 x4 x5

T1,2 T2,3 T3,4T4,5

x6 x7 x8

T6,7 T7,8

time

T1,6 T7,2 T8,3

22


. . .Compare scans (detect change)

x1 x2 x3 x4 x5

T1,2 T2,3 T3,4T4,5

x6 x7 x8

T6,7 T7,8

time

T1,6 T7,2 T8,3

T8,9

x9

23


. . .Compare scans by scan matchingRepair pose graphUpdate map

x1 x2 x3 x4 x5

T1,2 T2,3 T3,4T4,5

x6 x7 x8

T6,7 T7,8

time

T1,6 T7,2 T8,3

T8,9

x9

Change Detected

1,

0,

C

i

if percent changed

otherwise

24


. . .Compare scans Repair pose graphUpdate map*Invalidate node and arcs

x1 x2 x3 x4 x5

T1,2 T2,3 T3,4T4,5

x6 x7 x8

T6,7 T7,8

time

T1,6 T7,2 T8,3

T8,9

x9

*Check for map overlap, remove additional nodes

1: 1: 1( | , , )P X C Z UC t t

1:X XC t

25


. . .Compare scans Repair pose graphUpdate map*Invalidate node and arcsAdd constraints

x1 x2 x3 x4 x5

T1,2 T2,3 T3,4T4,5

x6 x7 x8

T6,7 T7,8

time

T16, T7,2 T83

T8,9

x9


1: 1: 1( | , , )P X C Z UC t t

1: 1: 1( | , , )P X C Z UC t t

26


. . .Compare scans Repair pose graphUpdate map*Invalidate node and arcsAdd constraints (repair pose graph by sewing in constraints)

x1 x2 x3 x5

T1,2 T2,3

x6 x7 x8

T6,7 T7,8

time

T1,6 T7,2 T8,3

T8,9

x9


T3,9 T9,5

. . .Compare scans Repair pose graphUpdate map*Invalidate node and arcs

27


. . .Compare scans Repair pose graphUpdate mapInvalidate node and arcsAdd constraints (repair pose graph by sewing in constraints)

x1 x2 x3 x5

T1,2 T2,3

x6 x7 x8

T6,7 T7,8

time

T1,6 T7,2 T8,3

T8,9

x9

T3,9 T9,5

28


Node Types: Normal, Change, CorrespondingEdge Types: Loop, Change, Sequential, Corresponding tim

e

Normal node: neither change nor corresponding

Change node: change detected at node

Corresponding node: paired with a later nodex2

x9

x12

29


Node Types: Normal, Change, CorrespondingEdge Types: Loop, Change, Sequential, Corresponding tim

e

Change edge: added during repair

Loop edge: added during loop detection Sequential edge: added between consecutive node

Corresponding edge: added between corresponding node pairs

T11,12

T8,3

T10,3

T9,5

30


Change at x12

x1 x2 x3 x5

T2,3

x6 x7 x8

T7,8

time

T7,2 T8,3

T8,9

x9

T3,9

x10 x11 x12

T10,3

T11,12

T9,5

31


Robot takes measurement at x12

x1 x2 x3 x5

T2,3

x6 x7 x8

T7,8

time

T7,2 T8,3

T8,9

x9

T3,9

x10 x11 x12

T10,3

T11,12

32


x1 x2 x3 x5

T2,3

x6 x7 x8

T7,8

time

T7,2 T8,3

T8,9

x9

T3,9

x10 x11 x12

T11.12

Get corresponding node, x8

T10,3

33


Get corresponding node, x8

Compare measurements

x1 x2 x3 x5

T2,3

x6 x7 x8

T7,8

time

T7,2 T8,3

T8,9

x9

T3,9

x10 x11 x12

C11,12

Change Detected

T10,3

34


x1 x2 x3 x5

T2,3

x6 x7

T7,8

time

T7,2 T8,3

T8,9

x9

T3,9

x10 x11 x12

T11,12

Update map and repair pose graphRemove x8

x8

T10,3

35


x1 x2 x3 x5

T2,3

x6 x7

T7,8

time

T7,2 T8,3

T8,9

x9

T3,9

x10 x11 x12

T11,12


Add new edges

x8

T7,12

T12,9T10,3

36


x1 x2 x3 x5

T2,3

x6 x7

time

T7,2

x9

T3,9

x10 x11 x12

T11,12


Add new edgesRemove x3 corresponding node of x8

T7,12

T12,9T10,3

37


x1 x2 x3 x5

T2,3

x6 x7

time

T7,2

x9

T3,9

x10 x11 x12

T11,12



T7,12

T2,12

T12,9T10,3

T10,12

38


x1 x2 x5

x6 x7

time

T7,2

x9

x10 x11 x12

T11,12



T7,12

T2,12

T12,9

T10,12

39




Mobile Robot Model


Pose Graph Approach



Conclusion

40

DPG-SLAM on Simulated Data SetDPG-SLAM on Simulated Data Set

A

B

C

A

B

C

A

C

A

C

DE

time

time

41

42


43

time


44


0 20 40 60 80 100 120 140 160 1800

20

40

60

80

100

120

Time)

Tot

al N

odes

Total Nodes Over Time

45


0 20 40 60 80 100 120 140 160 1800

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

0.5

Node # (time)

Per

cent

Cha

nge

Percent Change Between Corresponding Poses

46


0 20 40 60 80 100 120 140 160 1800

5

10

15

20

25

30

35

40

Time

Tot

al C

hang

e N

odes

Total Change Nodes Over Time

47

DPG-SLAM Issues (cont.)DPG-SLAM Issues (cont.)Navigation strategy

Once achieve change threshold can recursively go back and fix previous pairs that overlap

This has problem based on different vantage points from different maps and different navigation strategy

48

DPG-SLAM IssuesDPG-SLAM IssuesIf there are loop closing constraints you might

remove a good one and then the estimate become worse

Blocking points that are static

49

DPG-SLAM Issues (cont.)DPG-SLAM Issues (cont.)Number of edges will not change when there is a

DPG repair

Number of nodes can stay constant (a lot of change detected) or increase

Repeated little change that turns huge

Do we need to add all the corresponding edges? (mutual information)

50




Mobile Robot Model


Pose Graph Approach



Conclusion

51

ConclusionConclusionOverview of mobile robots


Presented the DPG-SLAM algorithm to maintain an up-to-date map in a changing environment

Next Steps

Analysis of real world data sets

Temporal Dynamic Pose Graph

52

Conclusion (cont.)Conclusion (cont.)Towards life-long SLAM

Provide robots with the capability to operate robustly (live) for extended periods

Enable persistent localization

Capable of reasoning over long terms

Collect and manage environment information over time

Documents

Am I Lost? Or Has the World Changed? Persistent Autonomous Navigation in Changing Environments