SLAM U SING S INGLE L ASER R ANGE F INDER AliAkbar Aghamohammadi, Amir H. Tamjidi, Hamid D. Taghirad Advance Robotic and Automation Systems Lab (ARAS), Electrical and Computer Engineering Department K. N. Toosi University of Technology, Iran

O UTLINE 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 2

M OTIVATION traditional encoder-base dynamic modeling are sensitive to: a) slippage b) surface type changing c) imprecision in the parameters of robot's hardware. 3

M AIN C ONTRIBUTIONS The key contributions of LSLAM include: 4

P ROBABILISTIC F RAMEWORK 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. 5 Robot Pose features

O UTLINE 6 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

F EATURE E XTRACTION Point Features Line Features More Informative Features 7

F EATURE E XTRACTION 8 Scan Data Segmentation Detection of High Curvature points Discarding variant features Steps features

O MITTING VARIANT FEATURES There exist two kind of variant features: 1) Those, appear due to occlusion 2) Those, appear due to low incidence angle 9

F EATURE E XTRACTION R ESULTS 10 Jump edge Occlusion High Curvature High Curvature Low incidence angle Low incidence angle Extracted Features

O UTLINE 11 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

R ELIABILITY M EASURE C ALCULATION F OR I NDIVIDUAL F EATURES Feature uncertainty Observation noise Uncertainty due to quantization Fig eө eө erer pipi riri

M EASUREMENT NOISE 13 eө eө erer pipi riri

Q UANTIZATION E RROR 14 This issue causes that the point p i, considered as a feature point, not necessarily be the same physical feature in the environment. β β μ α i+1 qdqd f k (real feature in the environment) P i (selected edge feature) r i+1 riri r i-1

F EATURE C OVARIANCE Measurement and quantization errors are independent from each other 15

16

O UTLINE 17 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

M OTION P REDICTION 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 Traditional Method 18

M OTION P REDICTION 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. 19 LSLAM Method LSLAM Method

Matching : Pose Shift Calculation ( Cost function based on weighted feature-based Range scan matching ) 20 M OTION P REDICTION

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 relationship between features and pose shift. But there is not !!! 21 M OTION P REDICTION – Uncertainty Calculation

The implicit function theory can provide the desired Jacobian via below equation: 22 M OTION P REDICTION – Uncertainty Calculation complicated but a tractable matter of differentiation

O UTLINE 23 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

D ATA 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) 24

F ILTERING AND A DDING N EW 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 calculate kalman gain 3-calculate covariance update 2-calculate state vector update

O UTLINE 26 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

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

P URE L OCALIZATION 28 AdvantagesDisadvantages Prior Information is needed Can not be used in state-based frameworks Robust in Unstructured environments ICP Method

R ESULTS (P URE L OCALIZATION ) 29 AdvantagesDisadvantages all features have the same contribution in pose shift calculation. does not reject Variant Features features in different scales can not be extracted High Speed HAYAI Method

P URE L OCALIZATION 30 AdvantagesDisadvantages Compatible with EKF framework Robust WRT slippage Weighted feature- based matching Extract features in different scales Not Robust in unstructured environments Proposed motion model

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

LSLAM - S IMULATION 32 errors are considerably reduced at loop closures. Uncertainties remain bounded Estimated errors (blue curves) and estimated variances (red curves) in x, y and theta (robot heading) Error in y Error in x Error in θ

LSLAM ( REAL SCAN DATA ) 33 LSLAM Feature-based map resulted from LSLAM Error in x Error in θError in y Pure Localization Error and uncertainty are bounded < 2cm. Error growing unboundedly > 20cm Error and uncertainty are bounded < 1.5cm. Error growing unboundedly > 20cm Error and uncertainty are bounded < 1deg. Error growing unboundedly > 10deg

8-C ONCLUSION 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 34

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