Mobile Robot Localization and Mapping by Scan Matching using Laser Reflection Intensity of the SOKUIKI Sensor

Transcription

1 Moble Robot Localzaton and Mappng by Scan Matchng usng Reflecton Intensty of the SOKUIKI Sensor *HARA Yoshtaka, KAWATA Hrohko, OHYA Akhsa, YUTA Shn ch Intellgent Robot Laboratory Unversty of Tsukuba 1-1-1, Tennouda, Tsukuba, , Japan {bluewnd, hro-kwt, ohya, Abstract Ths paper descrbes a new scan matchng method for moble robot localzaton and mappng. The proposed method "Intensty-ICP" uses Reflecton Intensty obtaned by a laser range scanner named "SOKUIKI sensor". Compared wth conventonal scan matchng methods whch are effectve just n geometrc featured envronments, Intensty-ICP s effectve n both geometrc featured and featureless envronments f there are some features lke colors or materals. Ths method can buld a map wth Reflecton Intensty data. The map wll be effectve to robust localzaton because t has abundant nformaton; not only geometrc data but also Reflecton Intensty. I. ITRODUCTIO Moble robot self-localzaton s ndspensable for navgaton and map buldng. Accordng to sensor characterstcs, they can be dvded nto two groups; self-localzaton usng nternal and external sensors. Self-localzaton usng nternal sensors s called dead reckonng. Odometry s a typcal example of dead reckonng. Other examples of dead reckonng use gyros and acceleraton sensors. On the contrary, self-localzaton wth external sensors uses ultrasonc sensors, cameras, laser range scanners such as "SOKUIKI sensors" [1], and so on. Recently, self-localzaton usng external sensors s popular, because they cancel accumulated error from nternal sensors. Usng ultrasonc sensors, fast processng s possble owng to the small amount of nformaton receved from sensors. However, the accuracy s not su for self-localzaton n geometrcally complex envronments. Wth cameras, we can obtan abundant nformaton of envronments, however, heavy computatonal loads are requred for mage processng. Also, llumnaton effects are bg problems n real applcatons. Compared wth these sensors, SOKUIKI sensors can get abundant nformaton, and are robust to llumnaton effects. Besdes, they furnsh geometrc data of envronments drectly, the computng process s smple and fast. Therefore, they are frequently used n selflocalzaton. Scan matchng s used extensvely n self-localzaton usng a SOKUIKI sensor. We can localze the current poston of a robot usng scan matchng whch s a method to algn current scan data to map of envronments. Almost all conventonal scan matchng methods use geometrc features of envronments. Therefore, they are effectve just n geometrc featured envronments and noneffectve n envronments wthout geometrc features. So they could not be appled to localzaton tasks, for example, n corrdors just havng flat walls and no pllars. To cope wth the above-mentoned problem, ths paper pro- Fg. 1. SOKUIKI Sensor "URG-4LX" [2] poses a new scan matchng method usng Reflecton Intensty. We named ths method "Intensty-ICP". Reflecton Intensty depends on surface colors and materals of scanned objects. Therefore, compared wth conventonal scan matchng methods usng only geometrc features, the proposed Intensty-ICP s effectve n both geometrc featured and featureless envronments f there are some features lke colors or materals. In ths paper, Reflecton Intensty s obtaned by a SOKUIKI sensor "URG-4LX" whch s made by HOKUYO AUTOMATIC CO., LTD. [2] and shown n Fg.1. Ths paper descrbes Intensty-ICP usng scan data of 2 dmensonal geometrc space, however t s also applcable to that of 3 dmensonal geometrc space. II. RELATED WORKS Scan matchng methods can be classfed nto two types; local matchng and global matchng. Local matchng needs ntal poston and s useful for poston trackng. Global matchng doesn t need ntal poston and s useful for global localzaton. Typcal examples of local matchng are ICP [3] and IDC [4]. Examples of global matchng are Sgnature-based Scan Matchng [5], CCF [6], LneMatch [7], APR [8], and so on. In moble robot localzaton, local matchng s frequently used because the ntal poston s suppled by odometry. Intensty-ICP s also a local matchng method. Intensty-ICP algns two scan data, same as almost all conventonal scan matchng methods. But there are smultaneous scan matchng methods [9] whch smultaneously algns three or more scan data at once. An advantage of smultaneous scan matchng s t has no accumulated error. However, t has huge processng burden. ICP, whch s the most basc scan matchng algorthm, uses

2 TABLE I SPECIFICATIOS OF URG-4LX USED I THIS STUDY Detec range.2 5.5m* Dstance resoluton 1mm Intensty range * Approx. 4, Scannng angle 24 degrees Angle resoluton Approx..7 degrees * Scannng tme 1 ms/scan Interface USB2. FS mode (12 Mbps) RS232C (Max. 75 kbps) (* : specally changed from normal URG-4LX) Scan Lne Scanned Objects SOKUIKI Sensor "URG" Moble Robot Fg. 2. Scan Envronment only geometrc coordnate values of scan ponts. There are other methods more robust than conventonal ICP, whch use not only coordnate values but also other feature quanttes. Color-ICP [1] s an extended ICP algorthm whch uses colors and textures of object surfaces as feature quanttes. A smlar method s proposed n [11]. However colors and textures obtaned by cameras are not nvarant feature quanttes because cameras are senstve to ambent lght. So Okatan et al. [12] proposed a method whch supports varance of reflect by lghtng condtons. These methods need two sensors n total; a SOKUIKI sensor and one camera, or two cameras. However, Intensty-ICP proposed n ths paper needs only one SOKUIKI sensor. Smultaneous 2D mages and 3D geometrc model regstraton [13] algns 2D camera mages to 3D geometrc model. 3D geometrc model s made from Reflecton Intensty edges of 3D range mage whch was got from 3D SOKUIKI sensor. Ths method and Intensty-ICP are smlar, because both use Reflecton Intensty. However, ths method algns camera mages, but Intensty-ICP algns scan data. As stated above, Intensty-ICP s a new scan matchng method, whch needs only one SOKUIKI sensor, and uses both geometrc coordnate values and Reflecton Intensty of scan data as feature quanttes. III. SCA MATCHIG USIG LASER REFLECTIO ITESITY A. The method to obtan Reflecton Intensty In ths paper, we use a SOKUIKI sensor "URG-4LX" (n the followng, spelled "URG") to get scan data. The frmware of URG s specally modfed to get Reflecton Intensty. The specfcatons of URG are shown n Table I. Angle resoluton of normal URG s approxmately.35 deg., but that of URG used n ths study s.7 deg. to obtan Reflecton Intensty. Then we descrbe an example of Reflecton Intensty data. We scanned wth URG n the envronment shown n Fg.2. URG s mounted on a moble robot, and there are black and whte boards as scanned objects n front of the robot. URG scanned a horzontal plane. As a result of the scan, Reflecton Intensty data shown n Fg.3 s obtaned. In Fg.3, the robot poston s at the orgn of the graph, and the rght scale bar shows Reflecton Intensty by colors. It s ndcated that scan ponts on the whte boards have hgh Reflecton Reflecton Intensty Fg. 3. Example of Reflecton Intensty data Intensty and the ones on the black boards have low Reflecton Intensty. B. Propertes of Reflecton Intensty Reflecton Intensty s not an nvarant feature quantty. It depends on not only surface colors and materals of scanned objects but also dstances and angles from URG to scanned objects. Therefore, the poston of URG also affects on Reflecton Intensty. Then n ths paper, we treat only cases n whch dstances between nput scan poston and reference scan poston are short enough not to affect Reflecton Intensty. From ths, we consder Reflecton Intensty as an nvarant feature quantty. C. ICP algorthm Intensty-ICP algorthm s desgned by expandng ICP algorthm. Therefore to ntroduce the proposed Intensty-ICP, we descrbe conventonal ICP algorthm at frst. Scan matchng usng ICP algorthm s a method to mnmze the matchng error usng the total squared geometrc dstance between the ponts n an nput scan and the correspondng ponts n a reference scan. ICP uses ntal relatve poston between an nput scan and a reference scan whch s suppled by odometry. In a lot of cases, map data s used as a reference scan. The defnton of evaluaton value s shown n Eq.(1), whch s the total squared geometrc dstance between the ponts n an nput scan and the correspondng ponts n a reference scan. The equaton of homogeneous coordnate transformaton s gven n Eq.(2).

3 e (m) 1 p (m) q (m) k 2 1 {(x pk (m) 1 {x 2 df f + y2 d f f } x q (m) ) 2 +(y pk (m) y q (m) ) 2 } (1) e : evaluaton value p : reference scan ponts q : nput scan ponts (m) : number of teratve calculaton : number of scan ponts k : pont number n a reference scan whch correspond to number n an nput scan x,y : geometrc coordnate values of scan ponts df f : dfference of parameters q (m+1) T (m) + {R (m) (q (m) c (m) )+c (m) } (2) T : translaton vector of homogeneous coordnate transformaton matrx R : rotatng matrx of homogeneous coordnate transformaton matrx c : mounted poston of a SOKUIKI sensor (center of scan), rotaton center of R If the SOKUIKI sensor mounted on the orgn of robot coordnate, c (1). The procedure for teratve calculatons of ICP algorthm s explaned as follows. In the frst step, the algorthm transforms an nput scan coordnate usng homogeneous coordnate transformaton matrx. The matrx s calculated from ntal relatve poston between an nput scan and a reference scan whch s suppled by odometry. In the second step, t searches correspondng ponts n a reference scan whch correspond to each nput scan pont. A reference scan pont whch has the shortest squared geometrc dstance to an nput scan pont of all reference scan ponts s corresponded. In the thrd step, t calculates homogeneous coordnate transformaton matrx whch mnmzes evaluaton value by nonlnear optmzaton method such as the steepest descent method or ewton s method. The evaluaton value s total squared geometrc dstance between the ponts n an nput scan and the correspondng ponts n a reference scan. After the thrd step, t returns to the frst step, and transforms an nput scan coordnate usng homogeneous coordnate transformaton matrx whch was calculated n the thrd step. And then, t teratvely calculates these steps. In these steps, the result of calculatng homogeneous coordnate transformaton matrx converges. And calculatng homogeneous coordnate transformaton matrx means localzng scan poston. The poston s self-localzed poston of the robot. Reference Scan Ponts Input Scan Ponts Conventonal ICP ncorrect correspondences : Correspondences Intensty-ICP correct correspondences : Scan Ponts wth Reflecton Intensty Fg. 4. Intensty-ICP algorthm D. Intensty-ICP algorthm As explaned above, Intensty-ICP algorthm s an extenson of ICP algorthm. An operaton mage of Intensty-ICP algorthm s shown n Fg.4. Each pont such as,, shows scan data ponts wth Reflecton Intensty.,, mean hgh, mddle, and low level of Reflecton Intensty, respectvely. In geometrc featureless scan data lke Fg.4, correspondng between scan ponts whch have the same Reflecton Intensty values s correct. That s to say, t s correct that correspond to, correspond to, and correspond to, respectvely. Conventonal ICP scan matchng algorthm doesn t consder Reflecton Intensty, so t can not correspond correctly. But Intensty-ICP consders Reflecton Intensty, so t can correspond correctly. The proposed Intensty-ICP s 3 dmensonal ICP, because of Reflecton Intensty. Specfcally, breakdown of 3 dmensons are 2 dmensons of geometrc coordnate values (x, y) and 1 dmenson of Reflecton Intensty. However, homogeneous coordnate transformaton s same as Eq.(2) because coordnate transformaton s n 2 dmensons. Evaluaton values on Intensty-ICP are consdered n 3 dmensons. Specfcally, ths method consders Reflecton Intensty, and searches correspondng ponts whch have the shortest geometrc dstances and the smallest dfferences of Reflecton Intensty between nput scan ponts and reference scan ponts. Ths s the key pont of the proposed Intensty-ICP. The defnton of evaluaton value s shown n Eq.(3), whch s the total squared geometrc dstance and dfference of Reflecton Intensty between the ponts n an nput scan and the correspondng ponts n a reference scan. It s extended from Eq.(1). e (m) 1 p (m) q (m) k 2 1 {(x pk (m) 1 {x 2 df f + y2 df f + w l2 df f } +w(l pk (m) x q (m) ) 2 +(y pk (m) y q (m) ) 2 l q (m) ) 2 } (3) l : Reflecton Intensty of each scan pont w : weghtng factor of Reflecton Intensty

4 Scan Lne Door SOKUIKI Sensor "URG" Moble Robot Poston for Input Scan Moble Robot 1 m Poston for Reference Scan y x 3 deg 1.5 m Whte Wall Black Door Whte Wall Reference Scan Ponts Input Scan Ponts Reflecton Intensty Fg. 5. Expermental envronment of localzaton Unts of x and y are mm. The value of w s decded from the expermental data. As shown n Table I, Reflecton Intensty range of URG s approxmately 4,. Consderng ths by some trals, the value of w s defned as.2. A dfference of Reflecton Intensty between black and whte objects scanned from the dstance of 1 m s approxmately 3,, and the value s nearly equvalent to 425 mm. However, the optmum value of w depends on scan data. IV. EXPERIMETS We expermentally evaluated the performance of the proposed Intensty-ICP. ewton s method s employed as nonlnear optmzaton method n Intensty-ICP algorthm. We expermented on localzaton and mappng. In localzaton experments, t s proved that Intensty-ICP s effectve even n geometrc featureless envronments f there are some features lke colors or materals. In mappng experment, we succeed to buld a map wth Reflecton Intensty data. A. Localzaton At frst, we expermented on localzaton n geometrc featureless envronments. In order to show the effectveness of Intensty-ICP n geometrc featureless envronments, we compared self-localzaton tasks by conventonal ICP scan matchng and Intensty-ICP scan matchng. Expermental envronment s shown n Fg.5. The left sde s a pcture of expermental envronment, and the rght s a top plan vew, respectvely. Ths s an envronment wthout geometrc features. There s only one feature, the color of the door s dfferent from that of the wall. And shown n Fg.5, URG s mounted on a moble robot. In ths envronment, URG scanned twce. Frst, URG scanned at the front of the door as a reference scan. And the poston of the robot at ths tme s defned as orgn of the world coordnate. ext, the robot was moved manually to (x, y, θ) ( mm, 1 mm, 3 deg.) on the world coordnate, and the scannng s performed agan to obtan an nput scan. Then, the robot localzed the poston where URG got an nput scan by scan matchng. The correct coordnate value of the poston s (x,y,θ)( mm, 1 mm, 3 deg.). In ths experment, we ddn t use odometry. The ntal relatve poston between an nput scan and a reference scan, whch s gven to scan matchng algorthm, s (x,y,θ) ( mm, mm, deg.). Fg.6 shows scan data before matchng. Fg.7 shows the result of conventonal ICP scan matchng and Fg.8 shows the result (a) scan classes Reference Scan Ponts Input Scan Ponts (a) scan classes Fg. 6. Scan data before matchng (b) ntensty data Fg. 7. Result of ICP scan matchng Reference Scan Ponts Input Scan Ponts (a) scan classes (b) ntensty data (b) ntensty data Fg. 8. Result of Intensty-ICP scan matchng Reflecton Intensty Reflecton Intensty of Intensty-ICP scan matchng, respectvely. In each fgure, red ponts n the left graph show a reference scan and blue ponts show an nput scan. The rght scale bar shows Reflecton Intensty by colors. Fg.7 shows what seems to be correct matchng. However, the result coordnate value of localzaton by ICP scan matchng s (x,y,θ) (4 mm, 567 mm, 29 deg.). It s ncorrect for the error approxmate 15 mm away from correct poston. In contrast, the result coordnate value of localzaton by Intensty-ICP scan matchng s (x, y, θ)(3 mm, 972 mm, 29 deg.), whch s correct value though there s an error approxmate 3 mm. So Fg.8 shows correct matchng. Therefore, t s proved that Intensty-ICP scan matchng can correctly algn scan data and correctly localze even n geometrc featureless envronments.

5 B. Mappng ext, we expermented on mappng by Intensty-ICP scan matchng. Two methods to remove outlers n scan matchng are mplemented to Intensty-ICP scan matchng [14]. A map wth Reflecton Intensty data was bult n ths experment. Expermental envronment s shown n Fg.9. The left sde s a top plan vew of expermental envronment, and the rght s a pcture of expermental envronment whch s taken at "Camera Poston" of the top plan vew. The robot ran around ths envronment by human control. The robot obtaned odometry data and scan data from URG at ntervals, and bult a map after runnng. Consderng propertes of Reflecton Intensty, Intensty-ICP scan matchng uses not all map ponts as reference scan but only scan ponts obtaned on the prevous poston, whch are prevous nput scan ponts, as reference scan. A map bult by Intensty-ICP scan matchng s shown n Fg.1. Reflecton Intensty s shown by colors n ths map. It s succeeded to buld a map wth Reflecton Intensty data. For comparson, a map bult by only odometry wthout scan matchng s shown n Fg.11. The map bult by odometry s dstorted because of accumulated error. However, the map bult by Intensty-ICP scan matchng s accurate compared wth the map bult by odometry. In addton, the map bult by Intensty- ICP has abundant nformaton; not only geometrc data but also Reflecton Intensty, and t s useful for robust localzaton. C. Consderatons It s proved that Intensty-ICP s effectve n both geometrc featured and featureless envronments. Conventonal scan matchng methods s not effectve n geometrc featureless envronment, but Intensty-ICP s effectve n also geometrc featureless envronments f there are some features lke colors or materals. We succeeded to buld a map wth Reflecton Intensty data. The followng advantages wll exst n localzaton usng a map wth Reflecton Intensty data. Even f scan ponts of a SOKUIKI sensor are sparse, the method can localze correctly because the map has abundant nformaton; not only geometrc data but also Reflecton Intensty. The method can remove outlers robustly usng Reflecton Intensty as a feature quantty. Reflecton Intensty data s useful to compute the ntal poston of global localzaton. converson s convenent because reflecton property values are nvarant feature quanttes. Alternatvely, another method usng dervatve values of Reflecton Intensty as nvarant feature quanttes s effectve explaned n [13]. ACKOWLEDGMET We are grateful to Dr. Jae Hoon Lee and Mr. Lus Yoch Morales Sak at Intellgent Robot Laboratory n Unversty of Tsukuba for ther assstance n wrtng ths paper. REFERECES [1] Hrohko Kawata, Akhsa Ohya, Shn ch Yuta, Wagle Santosh, and Toshhro Mor : "Development of ultra-small lghtweght optcal range sensor system", Proc. of IROS 5, pp , 25. [2] Hokuyo Automatc Co., Ltd. [3] Paul J. Besl, and el D. McKay : "A Method for Regstraton of 3-D Shapes", IEEE Trans. on PAMI, Vol.14, o.2, pp , [4] F. Lu, and E. Mlos : "Robot Pose Estmaton n Unknown Envronments by Matchng 2D Range Scans", J. of Intellgent and Robotc Systems, Vol.18, pp , [5] Masahro Tomono : "A Scan Matchng Method usng Eucldean Invarant Sgnature for Global Localzaton and Map Buldng", Proc. of ICRA 4, 24. [6] G. Wess, G. Wetzler, and E. V. Puttkamer : "Keepng Track of Poston and Orentaton of Movng Indoor Systems by Correlaton of Range-Fnder Scans", Proc. of IROS 94, pp , [7] J. S. Gutmann, T. Wegel, and B. ebel : "Fast, Accurate, and Robust Self- Localzaton n Polygonal Envronments", Proc. of IROS 99, [8] J. Weber, K. W. Jorg, and E. V. Puttkamer : "APR - Global Scan Matchng Usng Anchor Pont Relatonshps", Proc. of IAS-6, 2. [9] P. J. eugebauer : "Reconstructon of real-world objects va smultaneous regstraton and robust combnaton of multple range mages", Int. J. of Shape Modelng, Vol.3, o.1&2, pp.71-9, [1] A. E. Johnson, and S. B. Kang : "Regstraton and Integraton of Textured 3-D Data", Proc. of the Int. Conf. on Recent Advances n 3-D Dgtal Imagng and Modelng, [11] G. Godn, M. Roux, and R. Barbeau : "Three-dmensonal regstraton usng range and ntensty nformaton", Proc. SPIE Vdeometrcs III, Vol.235, pp , [12] I. (Shmzu) Okatan, and A. Sugmoto : "Regstraton of range mages that preserves local surface structures and color", Proc. of the 2nd Int. Symposum on 3D Data Proc., Vsualzaton, and Transmsson (3DPVT 24), pp , 24. [13] R. Kurazume, K. shno, Z. Zhang, and K. Ikeuch : "Smultaneous 2D mages and 3D geometrc model regstraton for texture mappng utlzng reflectance attrbute", Proc. of the 5th Asan Conf. on Computer Vson, Vol.1, pp.99-16, 22. [14] Yoshtaka Hara, Hrohko Kawata, Akhsa Ohya, and Shn ch Yuta : "Map Buldng for Moble Robots usng a SOKUIKI Sensor -Robust Scan Matchng usng Reflecton Intensty-", Proc. of SICE-ICCAS 6, 26. V. COCLUSIOS AD FUTURE WORKS We proposed a new scan matchng method, Intensty-ICP, consderng Reflecton Intensty as an nvarant feature quantty n ths paper. Then we proved effectveness of the method, and succeeded to buld a map wth Reflecton Intensty data whch wll be effectve to robust localzaton. But ndeed, Reflecton Intensty s not an nvarant feature quantty. Therefore, t s convenent to convert Reflecton Intensty nto reflecton property values of scanned objects. These values change dependng on only object colors and materals. The values are calculated by convertng Reflecton Intensty usng a prevously defned converson. Ths

6 cabnet cabnet rack rack frdge letter box letter box rack rack Camera Poston y parts box x parts box cabnet cabnet whte board sofa sofa whte board rack 7 m 15 m Fg. 9. Expermental envronment of mappng Reflecton Intensty Fg. 1. Map bult by Intensty-ICP scan matchng Fg. 11. Map bult by odometry