Algorithms for indoor mobile robot. laser range finders data. Eng. Filippo Bonaccorso XXII Brain Storming Day

Transcription

1 Algorithms for indoor mobile robot self-localization through laser range finders data Eng. Filippo Bonaccorso XXII Brain Storming Day

2 OUTLINE INTRODUCTION SCAN-MATCHING FEATURES EXTRACTION FEATURE ASSOCIATION RESULTS

3 INTRODUCTION SELF-LOCALIZATION & SCAN-MATCHING θ T P 0 = O 0 n + R 0 n p n MSE N i= = 1 ' 2 ' ( P P ) + ( P P ) x x N y y 2

4 INTRODUCTION LOCAL MAP: is the representation of a part of the environment and, in this case, is built using the information from the laser scanner. GLOBAL MAP: is constructed combining each time existing information and adding new ones from the current local map. It represents the whole environment. LOCAL LOCALIZATION: is the problem to estimate the position of the robot with respect to the local map. GLOBAL LOCALIZATION: is the problem to estimate the absolute robot position in the environment. It can be made matching the local map with the global map.

5 INTRODUCTION PREVIOUS LOCAL MAP ACTUAL LOCAL MAP LASER RANGE FINDER ACTUAL y 1 Feature PREVIOUS SCAN-MATCHING SELF-LOCALIZATION θ x 1

6 SCAN-MATCHING Scan matching algorithms are usually categorized by the association method: Feature to feature line segments [Gutman AMR 1996] range extrema [Lingemann et al IROS 2004] 6000 Point to feature 5000 [Cox IEEE R&A 1991] 4000 Point to point 3000 Least dependent on environment. Y [mm] 2000 Result X [mm]

7 SCAN-MATCHING Point to point algoritms: Iterative Closest Point (ICP) [Besl and McKay IEEE PAMI 1992] Iterative Matching Range Point (IMRP) [Lu and Milios JIRS 1997] Iterative Dual Correpondence (IDC) Uses IMRP for rotation and ICP for translation in each iteration [Lu and Milios JIRS 1997].

8 SCAN-MATCHING While being very simple to be understood, ICP method has started great research activity as to overcome its drawbacks, mainly due to the iterative nature of the algorithm itself: Computational demand Convergence to local minima Slowness Outliers sensitivity As a matter of this, the IDC algorithm, introduced before, is an improvement of the ICP.

9 SCAN-MATCHING As sentenced in [Diosi and Kleeman IROS2005] PREVIOUS LOCAL MAP ACTUAL LOCAL MAP LASER RANGE FINDER Associations are the Key! A C T P R E V All point-to-point algorithms needs to associate each point in the actual scan with one in the previous one. Moreover each point can require all points in the other scan to be checked. Some search restrictions can reduce computational costs. In the pioneering [Lu and Milios JIRS 1997], the approach is to start with an approximate alignment of the two scans using odometry and then iteratively improve the alignment of the two scans.

10 FEATURE TO FEATURE LASER RANGE FINDER FEATURES EXTRACTION ODOMETRY-FREE LOCALIZATION SELF LOCALIZATION TECHNIQUE: Two successive local maps are analyzed in order to extract features of the environment than a matching of the common features is made to estimate the relative displacement between the starting point (1 st Local Map) and the new position (2 nd Local Map). The approach that will be shown is only based on LRF data. Odometry information are not used.

11 FEATURE TO FEATURE LASER RANGE FINDER FEATURES EXTRACTION ACTUAL LOCAL MAP PREVIOUS LOCAL MAP ACTUAL PREVIOUS [Lingermann et al IROS2004] deeply covered this topics introducing the HAYAI Algorithm LOCALIZATION

12 FEATURE TO FEATURE Two critical choices have to be done: The Feature Extraction Algoritm The Data Association scheme

13 LASER RANGE FINDER LRF FEATURES EXTRACTION 2-D SCAN

14 F.E. ALGORITHMS LASER SCANNER ODOMETRY-FREE LOCALIZATION SPLIT & MERGE RELATIVE ANGLES RELATIVE ANGLES (LEAST SQUARES) LINE TRACKING LINE TRACKING (LEAST SQUARES) RANSAC HOUGH TRANSFORM

15 F.E. ALGORITHMS Split & Merge algorithm consists of two phases: Split, which has to find all possible corners or edges, Merge, which has the task to eliminate false corners.

16 F.E. ALGORITHMS First Two Points First segment Line Tracking Algorithm or Incremental The segment between the first and the last point in the scan is then built The difference in slope between these two segments is less than a given threshold? Add Point

17 F.E. ALGORITHMS TWO NEW VARIANTS RELATIVE ANGLES (LEAST SQUARES) LINE TRACKING (LEAST SQUARES) x m = y q = y i mx i m q = x i i x i y i N N y 2 ( x ) N ( x ) i i x 2 xi y ( x ) i i 2 ( x ) ( x ) = 2 i i 2 i y i This two new variants evaluate the line parameters using a different approach.

18 F.E. ALGORITHMS

19 FEATURE ASSOCIATION LASER SCANNER FEATURES EXTRACTION ODOMETRY-FREE LOCALIZATION Line extracted as features in two consecutive local maps are used to perform locale self localization. The features has to be in the two local maps. LINE BASED Corner extracted as feature in two consecutive local maps are used to perform self localization. The features has to be in the two local maps. POINT BASED

20 FEATURE ASSOCIATION

21 FEATURE ASSOCIATION [Bailey and Nebot 2001] describes a localization framework based on feature maps. The map is structured as a graph of nodes where each node defines a feature of the local map. The data association method uses simultaneous observation of multiple features to determine data associations without any information about the robot pose. Vehicle motion could be estimated without odometry and, moreover, without requiring a kinematic vehicle model.

22 FEATURE ASSOCIATION The proposed algorithm relies on the fact that a features map possess invariant inter-feature relationships, e.g.: distance between two points angle between two segments Features information are represented as a graph where: feature types properties are stored as graph nodes graph edges represent the inter-feature relationships Thus, data association between two feature maps can be expressed as the graph-theoretic problem of finding the maximum common subgraph (MCS) between two graphs.

23 FEATURE ASSOCIATION 1. An NxM matrix is created, each element is the different between the distance of two corner from two different scans; 2. The minimum of each row is detected; 3. If the minimum is below a threshold the two features could be considered as the same represented in two different local maps. Y [m] X [m]

24 FEATURE ASSOCIATION The use of only the distance between two features as inter-features relationship could leads, especially in large environments, to error in the data association and so in wrong scan-matching. To avoid this a new approach is proposed: 1. A first association is evaluated using only the distance as interfeatures relationship 2. For the other features, if the angle between two segment (connecting the two couple of features) is the same in the two maps than the two features are considered as the same represented in two different local maps.

25 FEATURE ASSOCIATION First Association Successive Association N The difference in slope between these two segments is less than a given threshold? Y Valid Association No association found

26 FEATURE ASSOCIATION Y [m] X [m]

27 S.M. ALGORITHMS Once that common features are found the problem of scan matching becomes a problem of optimization. Feature ROTO-TRANSLATION MATRIX x y cosθ = sinθ 0 sinθ Tx x1 cosθ Ty y y 1 PERFORMANCE INDEX θ MSE x 1 N i= = 1 2 ( x x ) + ( y y ) 2 1 N 2 1 2

28 S.M. ALGORITHMS TX= cm TY= cm θ= Y [mm m] X [mm]

29 RESULTS

30 F.E. RESULTS Features Extraction Algorithms N Features (361) Performances N Features (181) Split & Merge CHANGES WITH ANGULAR RESOLUTION!!! Relative Angles Relative Angles LS Line Tracking Line Tracking LS RANSAC 5 3 Hough Transform 4 3

31 F.E. RESULTS Features Extraction Algorithms Execution Time (ms) (361) Performances Execution Time (ms) (181) Split & Merge 6,8 3,5 ALL CHANGES WITH ANGULAR RESOLUTION!!! Relative Angles Relative Angles LS Line Tracking 5,2 3 Line Tracking LS 5,2 4,9 RANSAC Hough Transform

32 Y [mm m] S.M. RESULTS Result The average execution time is 50ms including the two S&M phases with an angular resolution of 1degree X [mm]

33 RESULTS 3MORDUC (3rd version of MObile Robot DIEES University of Catania) A robotic platform used to develop: Tele-operation software Navigation algorithms SLAM techniques

34 Pubblications PATENT about a control method for arc welding deposition process 1. A Modular Software Interface to Develop an Arc Welding Robot Control ; F. Bonaccorso G. Muscato, L. Cantelli; TIE (accepted) 2. A Feedback Control System For A Rapid Production Process Based On Robotic Welding Deposition ; F. Bonaccorso, C. Bruno, L. Cantelli, D. Longo, G. Muscato, S. Rapisarda; Proceedings of 41 International Symposium on Robotics ISR 2010; Munich, Germany, 7-9 June "A Modular Software Interface for the Control System of an Arc Welding Robot ; F. Bonaccorso, C. Bruno, L. Cantelli, D. Longo, G. Muscato; Proceedings of the 2nd International Conference on Human System Interaction HSI'09, pp , May , Catania, Italy. ISBN Control of a Shaped Metal Deposition Process ; F. Bonaccorso, C. Bruno, L. Cantelli, D. Longo, G. Muscato; Proceedings of 4th International Scientific Conference on Physics and Control, PHYSCON 09, Catania, Italy, 1-4 September Closed Loop Welding Controller for Manufacturing Process ; F. Bonaccorso, C. Bruno, L. Cantelli, D. Longo, G. Muscato, S. Rapisarda; TRAM09 6. Control of Welding for Rapid Manufacturing: Two Cases Study F. Bonaccorso, C. Bruno, L. Cantelli, D. Longo, G. Muscato, G. Spampinato, SIDRA08

35 Pubblications 7. The U-Go Robot, a multifunction rough terrain outdoor tracked vehicle for R&D on autonomous navigation algorithms ; F. Bonaccorso, L. Cantelli, D. Longo, D. Melita, G. Muscato, M. Prestifilippo; 5th International IARP Workshop RISE 2011 Leuven June Evaluation Of Algorithms For Indoor Mobile Robot Self-localization Through Laser Range Finders Data ; F. Bonaccorso, G. Muscato, F. Catania; Proceedings of IAV2010: 7th IFAC Symposium on Intelligent Autonomous Vehicles; Lecce, Italy, 6-8 September An Innovative Autonomous Outdoor Vehicle Based On Microsoft Robotic Studio ; P. Aranzulla, F. Bonaccorso, C. Bruno, L. Cantelli, G. Lanteri, D. Longo, D. Melita, G. Muscato, A. Pennisi, M. Prestifilippo; Proceedings of the 13th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines CLAWAR 2010; Nagoya, Japan, 31 August - 03 September 2010; p. 97, 104; ISBN-10: ; ISBN-13: ; World Scientific Publishing Co Pte Ltd (24 Aug 2010) 10. Modeling Of An Innovative Actuator For Climbing Robot Adhesion ; F. Bonaccorso, D. Longo, G. Muscato; Proceedings of the 12th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines CLAWAR 09, Istanbul, Turkey, September 2009, pp Structure And Model Identification Of A Vortex-based Suction Cup ; F. Bonaccorso, C. Bruno, D. Longo G. Muscato; Proceedings of the11th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines CLAWAR 08; 8-10 September 2008, Coimbra, Portugall

36 More SCHOOLS 1. Samcef robot modeling and optimization training Course ; SAMTECH Headquarters, Liège, Belgium, Febrary Microchip 16 bit curriculum ; Catania, Italy, May 2009; 3. 4th European Summer University in Surgical Robotics ; LIRMM, CNRS-Université Montpellier 2, Montpellier, France, 9-16 September Filtering, data analysis and optimization of uncertain systems ; PhD course of the series on Advanced methods of identification and data analysis ; Dipartimento di Elettronica e Informazione, Politecnico di Milano, September 2009 INVITED LECTURES 1. NATO 2010: Advanced All-Terrain Autonomous Systems; Çeşme, Turkey: Altin Yunus Resort, August Topic Area Leader on Laser Rangefinders, Inertial and Magnetic Sensors for Navigation. 2. Robocup Mediterranean Open: Algorithms for indoor mobile robot self-localization through laser range finders data. Rome March 2011 COMPETITIONS 1. M-ELROB th European Land Robot Trials - 3rd Military ELROB; Hammelburg, Germany, May 2010

37 Algorithms for indoor mobile robot self-localization through laser range finders data Thanks Eng. Filippo Bonaccorso XXII Brain Storming Day

38