SLAM Using Single Laser Range Finder

1. SLAM Using Single Laser Range Finder AliAkbarAghamohammadi, Amir H. Tamjidi, Hamid D. Taghirad Advance Robotic and Automation Systems Lab (ARAS),Electrical and Computer Engineering DepartmentK. N. Toosi University of Technology, Iran

2. Outline 1-Motivation & Contributions 2-Probabilistic Framework 3-Feature Extraction 4-Error Modeling For Individual Features 5-Motion Prediction 6-Data Association 7-Adding new features 8-Filtering (IEKF) 9-Results 10-Conclusion 11-Refrences

3. Motivation • traditional encoder-base dynamic modeling are sensitive to: • slippage • surface type changing • imprecision in the parameters of robot's hardware. LSLAM is a significant step toward encoder-free SLAM and it is robust with respect to slippage and problems associated with encoder-base motion models.

4. Main Contributions • The key contributions of LSLAM include: • Robust feature extraction method • Accurate error modeling for individual extracted features • Uncertainty estimation in feature-based range scan matching • Achieving real-time drift-free solution for SLAM in restricted structured environments using a single laser range finder as the only data source

5. Probabilistic Framework • State Vector of the system comprises of robot pose and spatial features, represented in world coordinates • At system start-up, feature-based map is initialized; this map is updated dynamically by the Extended Kalman Filter until operation ends. The probabilistic state estimates of the robot and features are updated during robot motion and feature observation. When new features are observed the map is enlarged with new states. Robot Pose features

6. Outline 1-Motivation & Contributions 2-Probabilistic Framework 3-Feature Extraction • 4-Reliability Measure Calculation 5-Motion Prediction 6-Data Association 7-Adding new features 8-Filtering (IEKF) 9-Results 10-Conclusion 11-Refrences

7. Feature Extraction Point Features Line Features More Informative Features • invariant wrt displacement • robust wrt data association Features have to be

8. Feature Extraction Steps features

9. Omitting variant features • There exist two kind of variant features: • Those, appear due to occlusion • Those, appear due to low incidence angle

10. Feature Extraction Results Jump edge Extracted Features Low incidence angle Low incidence angle High Curvature High Curvature Occlusion Occlusion Jump edge

11. Outline 1-Motivation & Contributions 2-Probabilistic Framework 3-Feature Extraction • 4-Reliability Measure Calculation 5-Motion Prediction 6-Data Association 7-Adding new features 8-Filtering (IEKF) 9-Results 10-Conclusion 11-Refrences

12. Reliability Measure CalculationFor Individual Features pi • Feature uncertainty • Observation noise • Uncertainty due to quantization Fig. 5 er ri eө

13. Measurement noise pi er ri eө

14. Quantization Error This issue causes that the point pi, considered as a feature point, not necessarily be the same physical feature in the environment. qd fk (real feature in the environment) αi+1 μ Pi (selected edge feature) ri+1 ri ri-1 β β

15. Feature Covariance • Measurement and quantization errors are independent from each other

17. Outline 1-Motivation & Contributions 2-Probabilistic Framework 3-Feature Extraction • 4-Reliability Measure Calculation 5-Motion Prediction 6-Data Association 7-Adding new features 8-Filtering (IEKF) 9-Results 10-Conclusion 11-Refrences

18. Motion Prediction • Traditional models, based on encoders' data, suffer from some problems in motion modeling such as wheel slippage, unequal wheel diameters, unequal encoder scale factors, inaccuracy about the effective size of wheel base, surface irregularities, and other predominantly environmental effects Traditional Method

19. Motion Prediction • we use a prediction model, which does not merely rely on robot, but it uses environmental information too. Thus, method is robust with respect to wheel slippage, surface changing and other unsystematic effects and inaccurate information about robot's hardware. LSLAM Method

20. Motion Prediction • Matching: • Pose Shift Calculation ( Cost function based on weighted feature-based Range scan matching )

21. Motion Prediction – Uncertainty Calculation If there was an explicit relationship between features and pose shift: Indeed, Since T* and R* have to minimize the cost function E, we have an implicit relationship derived from: X contains the parameters of T and R. Thus there is an implicit relationshipbetween features and pose shift. But there is not !!!

22. Motion Prediction – Uncertainty Calculation • The implicit function theory can provide the desired Jacobian via below equation: complicated but a tractable matter of differentiation

23. Outline 1-Motivation & Contributions 2-Probabilistic Framework 3-Feature Extraction • 4-Reliability Measure Calculation 5-Motion Prediction 6-Data Association 7-Adding new features 8-Filtering (IEKF) 9-Results 10-Conclusion 11-Refrences

24. Data association • Batch data association methods greatly reduce the ambiguity in data association process. Thus, here JCBB method is adopted for data association. • After data association process, extracted features from new scan fall into two categories: • New features, which are not matched with any existent feature in the map • Existing features, (matched ones)

25. Filtering and Adding New features • Existing features, (matched ones) are used to update the system state vector • Each newly seen feature is first transformed to the map reference coordinate and then the transformed feature is augmented with the system state vector. 1-calculate kalman gain 2-calculate state vector update 3-calculate covariance update

26. Outline 1-Motivation & Contributions 2-Probabilistic Framework 3-Feature Extraction • 4-Reliability Measure Calculation 5-Motion Prediction 6-Data Association 7-Adding new features 8-Filtering (IEKF) 9-Results 10-Conclusion 11-Refrences

27. Results • Melon: a tracked mobile robot equipped with two low range Hokuyo URG_X002 laser range scanners (High Slippage) An Structured Environment

28. Pure Localization ICP Method ICP method is a popular point-wise method. It is a powerful method, but it needs prior information about displacement.

29. Results(Pure Localization) HAYAI Method HAYAI method produces impressive results in term of processing speed. But it suffers from some disadvantages.

30. Pure Localization Proposed motion model

31. LSLAM Simulation Results • The environment consists of many features. • Ground truth is available • Loop closing effects can be investigated in a large loop

32. LSLAM - Simulation Error in x Error in θ Estimated errors (blue curves) and estimated variances (red curves) in x, y and theta (robot heading) Error in y

33. LSLAM (real scan data) LSLAM Feature-based map resulted from LSLAM Pure Localization Error in y Error in θ Error in x

34. 8-Conclusion • introducing robust motion model with respect to robot slippage and inaccuracy in hardware-related measures • calculating reliability measure for robot’s displacement derived through the feature-based laser scan matching • Extract features in different scales • construct an IEKF framework merely based on laser range finder information

35. 9-References • [1] Robot pose estimation in unknown environments by matching 2D range scans. Lu, F. and Milios, E. 1997. 1997, Journal of Intelligent and Robotic Systems, Vol. 18, pp. 249-275. • [2] Metric-based scan matching algorithms for mobile robot displacement estimation. Minguez, J., Lamiraux, F. and Montesano, L. 2005. Barcelona, Spain. : s.n., 2005. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). • [3] Scan alignment with probabilistic distance metric. AJensen, B. and Siegwart, R. 2004. 2004. Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. • [4] Weighted range sensor matching algorithms for mobile robot displacement estimation. Pfister, S., et al. 2002. s.l. : Proceedings of the IEEE International Conference on Robotics and Automation (ICRA’02), 2002. pp. 1667-1674. • [5] Feature-Based Laser Scan Matching For Accurate and High Speed Mobile Robot Localization. Aghamohammadi, A.A., et al. 2007. s.l. : European Conference on Mobile Robots (ECMR’07), 2007. • [6] High-speed laser localization for mobile robots. Lingemann, K., et al. 2005. 4, s.l. : Journal of Robotics and Autonomous Systems, 2005, Vol. 51, pp. 275–296. • [7] Natural landmark-based autonomous vehicle navigation. Madhavan, R. and Durrant-Whyte, H. F. 2004. s.l. : Robotics and Autonomous Systems, 2004, Vol. 46, pp. 79-95. • [8] Mobile robot positioning with natural landmark. Santiso, E., et al. 2003. Coimbra, Portugal : s.n., 2003. Proceedings of the 11th IEEE International Conference on Advanced Robotics (ICAR’03). pp. 47-52. • [9] Recursive Scan-Matching SLAM. Nieto, J., Bailey, T. and Nebot, E. 2007. 1, s.l. : Journal of Robotics and Autonomous Systems, January 2007, Vol. 55, pp. 39-49. • [10] Nieto, J. 2005. Detailed environment representation for the slam problem. Ph.D. Thesis. s.l. : University of Sydney, Australian Centre for Field Robotics, 2005. • [11] Globally consistent range scan alignment for environment mapping. Lu, F. and Milios, E. 1997. : Autonomous Robots, 1997, Vol. 4, pp. 333–349. • [12] An Interior, Trust Region Approach for Nonlinear. Coleman, T.F. and Y., Li. 1996. s.l. : SIAM Journal on Optimization, 1996, Vol. 6, pp. 418-445. • [13] Data association in stochastic mapping using the joint compatibility test. Neira, J. and Tardos, J.D. 2001. 6, s.l. : IEEE Transactions on Robotics and Automation, 2001, Vol. 17, pp. 890–897. • [14] Gelb, A. 1984. Applied Optimal Estimation. s.l. : M.I.T. Press, 1984. • [15] A real-time algorithm for mobile robot mapping with applications to multi-robot and 3D mapping. Thrun, S., Bugard, W. and Fox, D. 2000. s.l. : International Conference on Robotics and Automation, 2000. pp. 321–328.