am i lost? or has the world changed? persistent autonomous navigation in changing environments
DESCRIPTION
Am I Lost? Or Has the World Changed? Persistent Autonomous Navigation in Changing Environments. Aisha Walcott PhD Candidate, EECS, Robotics Advisor: Professor John Leonard [draft of algorithm and framework]. SLAM in Changing Environments. - PowerPoint PPT PresentationTRANSCRIPT
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
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
Ground Truth
8
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
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
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)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
. . .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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
. . .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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
. . .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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
. . .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
*Check for map overlap, remove additional nodes
1: 1: 1( | , , )P X C Z UC t t
1: 1: 1( | , , )P X C Z UC t t
26
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
. . .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
*Check for map overlap, remove additional nodes
T3,9 T9,5
. . .Compare scans Repair pose graphUpdate map*Invalidate node and arcs
27
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
. . .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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
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
Add new edges
x8
T7,12
T12,9T10,3
36
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
x1 x2 x3 x5
T2,3
x6 x7
time
T7,2
x9
T3,9
x10 x11 x12
T11,12
Update map and repair pose graphRemove x8
Add new edgesRemove x3 corresponding node of x8
T7,12
T12,9T10,3
37
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
x1 x2 x3 x5
T2,3
x6 x7
time
T7,2
x9
T3,9
x10 x11 x12
T11,12
Update map and repair pose graphRemove x8
Add new edgesRemove x3 corresponding node of x8
T7,12
T2,12
T12,9T10,3
T10,12
38
DPG-SLAM Example (cont.)DPG-SLAM Example (cont.)
x1 x2 x5
x6 x7
time
T7,2
x9
x10 x11 x12
T11,12
Update map and repair pose graphRemove x8
Add new edgesRemove x3 corresponding node of x8
T7,12
T2,12
T12,9
T10,12
39
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
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
DPG-SLAM on Simulated Data SetDPG-SLAM on Simulated Data Set
43
time
DPG-SLAM on Simulated Data SetDPG-SLAM on Simulated Data Set
44
DPG-SLAM on Simulated Data SetDPG-SLAM on Simulated Data Set
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
DPG-SLAM on Simulated Data SetDPG-SLAM on Simulated Data Set
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
DPG-SLAM on Simulated Data SetDPG-SLAM on Simulated Data Set
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
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
51
ConclusionConclusionOverview of mobile robots
Traditional SLAM vs. Life-long SLAM
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