An Iterative Graph Optimization Approach for 2D SLAM He Zhang, - - PowerPoint PPT Presentation

An Iterative Graph Optimization Approach for 2D SLAM

He Zhang, Guoliang Liu, and Zifeng Hou Lenovo Institution of Research and Development

Self‐Introduction

• English name: David, Graduate student in UCAS

(University of Chinese Academy of Sciences)

• Internship in Lenovo
• Start my first year Phd. Program in UALR (University of

Arkansas in Little Rock)

Index

• Problem Introduction
• Related Works
• iGO:

– Key Observation – Iterative Process – Algorithm explanation – Computational Time Analysis

• Other Strategies:

– Submap mechanism – Samples‐based motion estimation

• Experiments
• Conclusion & Future Extension

Problem Introduction

• Laser‐Odo have drift problem, that be corrected using the

state‐of‐the‐art graph SLAM method

• However,

– Graph optimization can be overfit [1] – Especially, Optimization cannot handle cases when it has biased edges (erroneous transformation but over‐confident information matrix)

The least χ2 error not guarantee the optimal solution Olson09[1] [1] Olson, E., & Kaess, M. (n.d.). Evaluating the Performance of Map Optimization Algorithms.

Related Works

• To make graph optimization robust, many previous work strive to remove

false loop edges:

– Front‐end validation

• Joint compatibility test, J. Neira and J. D. Tards. 2001
• SCGP(Single‐Cluster Spectral Graph Partitioning) validation, Olson et al. 2005
• RRR (Realizing, Reversing, Recovering) clique χ2 test, Latif et al. 2012

– Back‐end modeling

• Switchoff variants, N. Snderhauf and P. Protzel, 2012
• Max‐mixture, E. Olson and P. Agarwal, 2013
• However, these methods cannot reduce the errors propagated by the

biased edges because:

– These errors are not from the false loop edges – These errors originate from the front‐end and occurs when the vehicle slips in a corridor or frame alignment algorithm falls in a local minima

• In our work, we seek to minimize these errors by iteratively constructing the graph

structure in the front‐end by the aid of graph optimization in the back‐end

Index

• Problem Introduction
• Related Works
• iGO:

– Key Observation – Iterative Process – Algorithm explanation – Computational Time Analysis

• Other Strategies:

– Submap mechanism – Samples‐based motion estimation

• Experiment
• Conclusion & Future Extension

iGO‐ Key Observation

• Biased edges: motion estimation with poor prior initial

guess at corridor‐like environment

Motion estimation using Scan Alignment method in the two different scenes

• Good edges: even with small perturbation for the prior initial

guesses, the scan‐align algorithms can still fall into the same solution

Idea: try to iteratively improve the biased edges in the front‐end by the result from the back‐end.

iGO: An example for the process

2 4 3 1 5 2 4 3 1 5 2 4 3 1 5 2 4 3 1 5 2 4 3 1 5 2 4 3 1 5 (1) (2) (3) (4) (5) (6) Iterative Graph Reconstruction: (1) initial graph structure, (2) 1st graph

• ptimization, (3) 1st graph reconstruction, (4) 2nd graph optimization, (5)

2nd graph reconstruction, (6) final graph optimization. Green arrow stands for loop edge, blue for good edge and red dashed for biased edge

iGO: Algorithm

Graph Optimization Lchi2 Graph Reconstruction with new prior motion guesses Graph Optimization Cchi2 |lchi2 ‐ Cchi2| < ε Y N end start

Lchi2 =Cchi2

iGO: Computational Time Analysis

• Suppose optimization cost T(o) and each scan alignment

algorithm cost T(m), then the total iGO cost k(T(o) + E ∗ T(m)) + T(o) k is the iteration number, E is the edge numbers

• However, we can mark the edges with small changes before

and after the graph reconstruction.

• For example, we only recalculate the edges e(1,2),

e(2,3), e(4,5) and e(1,5) in the first iteration, and yet update edge e(3,4) in every iteration.

• Then, the computational time for iGO is

k(T(o) + b ∗ T(m)) + E∗ T(m) + T(o) b is the number of biased edges. If no biased edges exist, iGO costs 2∗T(o)+E ∗T(m),

Index

• Problem Introduction
• Related Works
• iGO:

– Key Observation – Iterative Process – Algorithm explanation – Computational Time Analysis

• Other Strategies:

– Submap mechanism – Samples‐based motion estimation

• Experiment
• Conclusion & Future Extension

Other Strategies: key‐node submap and interpolation match

• Key‐node : aggregate observations in

a local submap to enable robust loop detection

• Potential loop detection: First, distance

between key_node nk less than Tl, Second Mahalanobis distance between target node nl ϵ nk and current ni

• Interpolation prior motion guesses:

linear interpolation between current ni and key_node nk

Graph Structure

Other Strategies: samples‐based motion estimation

• Samples based on the motion noise, the current pose is the

weighted mean of the samples

• Covariance estimated from the scan alignment algorithm
• ften be over‐confident when the score is low, so we increase

it following:

*minScore is set as the number of beams that be aligned between two laser frames, in our experiment, it is 50% of the total beams Pi‐1 Pi P’i ∑odo Robust to large

• rientational motion

Index

• Problem Introduction
• Related Works
• iGO:

– Key Observation – Iterative Process – Algorithm explanation – Computational Time Analysis

• Other Strategies:

– Submap mechanism – Samples‐based motion estimation

• Experiment
• Conclusion & Future Extension

Experiment 1

In the first experiment, we use the uscsal data from the Radish. To simulate vehicle slippage or poor prior odometry, we intentionally increase the motion model covariance ∑odo with ∑tt = 1.6 and ∑rr = 0.8.

Top‐Down: Gmapping, GO, iGO Trajectory Comparison

• Gmapping: resampling mistakes
• GO: good loop but worse optimized trajectory
• iGO: the most resemble to the groundtruth

Experiment 2

Lenovo B2 office (17m width and 22m length, trajectory is about 80 meters) . Only laser‐odometry

• Gmapping: failed to close loop
• GO: succeed to detect loop but
• ptimizes into a worse trajectory
• iGO: succeed to detect loop and
• ptimizes into a better trajectory

Corridor

Conclusion & Future Extension

Contributions on two folds:

• An iterative Graph Optimization method to maintain the

well estimated edges, and improve the biased edges

• A 2D SLAM system which integrates modules such as the

submap mechanism, samples‐based motion estimation, graph structure and interpolation loop detection etc. Future Extension:

• 3D‐SLAM, e.g. icp motion estimation algorithms whose

convergence highly depends on the prior motion guess

• With submap mechanism, using large‐scale dataset

Thanks & Questions

Motivation

• Low‐price affordable Autonomous Vehicle
• Explore SLAM tech. mainly depends on cameras and lasers

2013 CES UK Unveils 'Affordable' Self‐Driving RobotCar, make a car for autonomous for $150 Localization mainly depends on cameras and lasers can also be applied into indoor autonomous robots: tele‐presence robots, inHouse robots etc.

Romo

Oddwerx iRobot