An Iterative Graph Optimization Approach for 2D SLAM. He Zhang, Guoliang Liu, and Zifeng Hou Lenovo Institution of Research and Development

Transcription

1 An Iterative Graph Optimization Approach for 2D SLAM He Zhang, Guoliang Liu, and Zifeng Hou Lenovo Institution of Research and Development

2 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)

3 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

4 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.

5 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 SCGP(Single Cluster Spectral Graph Partitioning) validation, Olson et al RRR (Realizing, Reversing, Recovering) clique χ2 test, Latif et al 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

6 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

7 igo Key Observation 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. Motion estimation using Scan Alignment method in the two different scenes Biased edges: motion estimation with poor prior initial guess at corridor like environment

8 igo: An example for the process (1) (2) (3) (4) (5) (6) Iterative Graph Reconstruction: (1) initial graph structure, (2) 1st graph optimization, (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

9 igo: Algorithm start Graph Optimization Lchi2 Graph Reconstruction with new prior motion guesses Graph Optimization Cchi2 lchi2 Cchi2 < ε Y end N Lchi2 =Cchi2

10 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),

11 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

12 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 Graph Structure Interpolation prior motion guesses: linear interpolation between current ni and key_node nk

13 Other Strategies: samples based motion estimation Samples based on the motion noise, the current pose is the weighted mean of the samples P i Pi 1 Pi odo Robust to large orientational motion Covariance estimated from the scan alignment algorithm often 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

14 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

15 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. Gmapping: resampling mistakes GO: good loop but worse optimized trajectory igo: the most resemble to the groundtruth Trajectory Comparison Top Down: Gmapping, GO, igo

16 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 optimizes into a worse trajectory igo: succeed to detect loop and optimizes into a better trajectory Corridor

17 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

18 Thanks & Questions

19 Motivation Low price affordable Autonomous Vehicle Localization mainly depends on cameras and lasers can also be applied into indoor autonomous robots: tele presence robots, inhouse robots etc CES UK Unveils 'Affordable' Self Driving RobotCar, make a car for autonomous for $150 Romo Oddwerx irobot Explore SLAM tech. mainly depends on cameras and lasers