310 T. SICE Vol.51 No.5 May 2015 Konolige 7) Correlationbased Markov Localization Olson 8) Konolige Dellaert 9) Monte Carlo Localization (MCL) 10) 2 2

Size: px

Start display at page:

Download "310 T. SICE Vol.51 No.5 May 2015 Konolige 7) Correlationbased Markov Localization Olson 8) Konolige Dellaert 9) Monte Carlo Localization (MCL) 10) 2 2"

るるみしもかさ
7 years ago
Views:

1 19 I Vol.51, No.5, 309/ (CIF) Robust Global Scan Matching Method Using Congruence Transformation Invariant Feature Descriptors and a Geometric Constraint between Keypoints Takayuki Nakamura and Shohei Wakita This paper proposes a new global scan matching algorithm using the CIF descriptors and a geometric constraint between keypoints. The CIF descriptor was proposed in our previous work. It is a feature decriptor that is invariant against a congruence transformation. In our previous work, our method was able to perform robust local scan matching using CIF decriptors, but was apt to fail global scan mathching where a large map is used as the reference scan. In this paper, in order to resolve this problem, we propose to use a geometric constraint between keypoints in addtion to the CIF decriptors for the global scan mathching task. Our method can perform global scan matching in a cluttered environment without using an initial alignment. Through experiment in real environment, we confirm the validity of our method by comparing the performance of our method and that of our previous method. Key Words: CIF descriptor, geometric constraint, global scan matching, map updating, mobile robot 1. 1) 2) 3) 930 Faculty of Systems Engineering, Wakayama University, 930 Sakaedani, Wakayama Received June 20, 2014 Revised December 26, 2014 CIF (Congruence transformation Invariant Feature) 4), 5) CIF 2 4), 5) CIF 2. 1), 2), 6) 7) 10) TR 0005/15/ c 2014 SICE

This paper proposes a new global scan matching algorithm using the CIF descriptors and a geometric constraint between keypoints. The CIF descriptor was proposed in our previous work.

2 310 T. SICE Vol.51 No.5 May 2015 Konolige 7) Correlationbased Markov Localization Olson 8) Konolige Dellaert 9) Monte Carlo Localization (MCL) 10) FFT CIF (Congruence transformation Invariant Feature) CIF 3. Fig. 1 t Scan(t) Map ( ) Δ x, Δỹ, Δ θ (1) (3) Map 1 Fig. 1 Overview of our global scanmatching method CIF 4 (2) CIF

9) Monte Carlo Localization (MCL) 10) 2 2 2 2 FFT CIF (Congruence transformation

3 CIF 11) Fig. 1 (1) 1 δp max 2 δp 3 (Fig. 2 ) 11) ( ) yi+α y i η i =arctan x i+α x i η i ϑ i = η i+1 η i thresh CIF thresh Fig. 3 Definition of a keypoint Fig. 2 Rearranging scan data Fig. 1 (2) Fig. 3(a) p i(i =1, 2, N) p i,p i+α η i 4 η i 2 δp max =0.5m 3 δp =0.1m m 4 δp =0.1m α = CIF (Congruence transformation Invariant Feature) CIF 3. 2 p CIF Fig. 1 (3) CIF Fig. 3(b) p Σ p p i 1,p i+1 p 2 2 η p p Σ p p p p Σ p 2 1 SP p p

2 Rearranging scan data 3. 2 2 Fig. 1 (2) Fig. 3(a) p i(i =1, 2, N) p i,p i+α η i 4 η i 2 δp max =0.5m 3 δp =0.1m 0.

4 312 T. SICE Vol.51 No.5 May 2015 h =(H SP [1]...H SP [8]H SD [1]...H SD [8]) Fig. 4 Definition of segment SP and its histogram p M 2M CIF 32 CIF p M CIF h M 2M CIF h 2M h 2M h M 2 h 2M 1/2 h M h 2M 32 CIF h h = (h M[1], )..., h M[16], h2m [1],..., h2m [16] 2 2 Fig. 5 Definition of segment SD and its histogram p Fig. 4(a) SP 1 SD n 1 p Fig. 5(a) SD p d M neighbor() neighbor() index neighbor() SP λ SP i η Σ p θ SP i p θ SP i π π 8 H SP Fig. 4(b) H SP H SP H SP [k], (k =1, 2, 8) neighbor() SD λ SD i η Σ p θ SD i p θ SD i π π 8 H SD Fig. 5(b) H SD H SD H SD [k], (k =1, 2, 8) p H SP H SD CIF p, (i =1 N KC) q j, (j =1 N KR) (Fig. 1 (4) ) 3 3 (p s, p t, p u) CIF 3 r L 3 3 CIF 3 3 d min 3 d max r L d min d max r L =8.0m,d min =3.0m,d max =10.0m

5 Definition of segment SD and its histogram p Fig. 4(a) SP 1 SD n 1 p Fig. 5(a) SD p d M neighbor() neighbor() index neighbor() SP λ SP i η Σ p θ SP i p θ SP i π π 8 H SP Fig.

5 s, p o) <r L, d(p t, p o) <r L, d(p u, p o) <r L d(p d min <d(p s, p t) <d max d min <d(p t, p u) <d max d min <d(p u, p s) <d max d(p 1, p 2) 2 p 1, p 2 p o 3 p, (i =1 N KC) N α L C L C : { (p s1, p t1, p u1), (p s2, p t2, p u2), (p snα, p tnα, p unα )} q j, (j = 1 N KR) 3 3 (q l, q m, q n) d(p s, p t) δd < d(q l, q m) <d(p s, p t)+δd d(p t, p u) δd < d(q m, q n) <d(p t, p u)+δd d(p u, p s) δd < d(q n, q l ) <d(p u, p s)+δd 3 q j, (j =1 N KR) N β L R L R : { (q l1, q m1, q n1), (q l2, q m2, q n2), (q lnβ, q mnβ, q nnβ ) } 3 3 CIF L C α 3 CIF 3 96 H α L R β 3 CIF 3 96 H β H α = ( h s [1],..., h s [32], h t [1],..., h t [32], h u [1],..., h u [32] ) δd =0.1m ( ) H β = h l [1],..., h l [32], h m [1],..., h m [32], h n [1],..., h n [32] H α H α H α[k] = Hα[k] 96 H α[k] k=1 H α H β 3 (p sα, p tα, p uα) 3 (q lβ, q mβ, q nβ ) S (α, β) S (α, β) = 96 k=1 H α[k] H β [k] 3 (p sα, p tα, p uα) (q lβ, q mβ, q nβ ) α =1 N α,β =1 N β { } (α,β ) = arg max α arg max S(α, β) β (p sα, p tα, p uα ) (q lβ, q mβ, q nβ ) CIF CIF (Fig. 1 (5) ): 12), 13) 3 12), 13) 2 (Fig. 1 (6) ):

p s) δd < d(q n, q l ) <d(p u, p s)+δd 3 q j, (j =1 N KR) N β L R L R : { (q l1, q m1, q n1), (q l2, q m2, q n2), (q lnβ, q mnβ, q nnβ ) } 3 3 CIF 96 3 3 L C α 3 CIF 3 96 H α L R β 3 CIF 3 96 H β H α

6 314 T. SICE Vol.51 No.5 May ), 15) ICP o ICP [i,j (i )] w j (i) O o = w j (i) O Fig. 6 Map of environment I (reference scan) w j (i) w j () =1 / C j (), w j (i) = w j (i) 3 w j (i) j (i)=1 C j C j = 32 k=1 (h k hj k )2 ICP ICP ICP 4. SICK LMS Mobilerobots (P3-DX) A 1 5 PC(2.70 GHz Intel Corei7-2620M 8GB of RAM) 55 m 55 m (MRPT) 16) Fig. 6 A 1 ( I) 8 ID δp =0.1m 4. 1 CIF CIF thresh =40deg M =20 Fig. 7 CIF CIF 1 CIF Fig. 7 Example result of finding corresponding points based on only CIF descriptors Fig. 8 CIF ms ms 1.11 ms ms

SICK LMS100 270 0.25 Mobilerobots (P3-DX) A 1 5 PC(2.70 GHz Intel Corei7-2620M 8GB of RAM) 55 m 55 m (MRPT) 16) Fig. 6 A 1 ( I) 8 ID 26 8 21 3630 δp =0.

7 Table 1 Success /failure of finding corresponding points by our method at point [4] in case of changing M and thresh thresh =20 thresh =30 thresh =40 M =20 M =30 M =40 Fig. 8 Example result of finding corresponding points by our method CIF 26 7 ( 27%) CIF ( 73%) CIF CIF thresh M Fig. 8 Fig Table 1 thresh M ( ) M (CIF ) thresh thresh = 30 deg M =20 Fig Fig Fig. 9 Table 2 Example result of finding corresponding points by our method Success /failure of finding corresponding points by our method at point [5] in case of changing M and thresh thresh =20 thresh =30 thresh =40 M =20 M =30 M =40 Table 2 thresh M M thresh thresh =40deg M =20 thresh =40deg ms

6 4 43 Table 1 thresh M 20 40 ( ) M (CIF ) thresh thresh = 30 deg M =20 Fig. 9 12 Fig. 6 5 307 43 18 Fig.

8 316 T. SICE Vol.51 No.5 May 2015 thresh, M MCL Fig (MRPT) 16) MCL MCL ( ) Σ W Δ x, Δỹ, Δ θ (5), (6) Fig. 10 (6) 14 (MRPT) 16) ICP O =50 3 Table 3 Fig. 10 error ( ) Δ x, Δỹ, Δ θ Table 3 Result of global self-localization by our method in Fig. 9 Δx [m] Δy [m] Δθ [deg] ground truth our method error Fig. 11 A 5 ( II) 15 Fig Fig. 10 Result of precise matching by our method 13 MCL MCL 14 ( ICP) ms Fig. 11 Map of environment II (reference scan) δp =0.1m ms

9 Δx [m] Δy [m] Δθ [deg] ground truth 10.62 2.74 137.5 our method 10.54 2.71 137.4 error 0.08 0.03 0.1 4. 3 Fig. 11 A 5 ( II) 15 Fig.

9 Fig. 12 Result of finding corresponding points in environment II by our method ( ) 17 Fig. 13 Fig MCL ( ) Fig. 11 MCL MCL ( ) (MRPT) 16) 5000 Fig. 14 MCL 19 MCL Fig. 14 The result of precise alignment of reference and input (dark gray dots) scans using MCL method in the environment II Fig. 13 The result of precise alignment of reference (gray dots) and input (dark gray dots) scans based on pairs of the corresponding points in the environment II Table 4 Results of global self-localization by our method in Fig.12andMCLmethodinFig.13 Δx [m] Δy [m] Δθ [deg] ground truth our method error MCL error ms 0.90 ms 18 ( ICP) 1996 ms Table 4 II MCL 19 MCL ms

13 The result of precise alignment of reference (gray dots) and input (dark gray dots) scans based on pairs of the corresponding points in the environment II Table 4 Results of global

10 318 T. SICE Vol.51 No.5 May 2015 error MCL ( ) ( ) 5. CIF CIF ICP 3 ICP ( (C) No ) 1 J.S. Gutmann, T. Weigel and B. Nebel: Fast, Accurate, and Robust Self-Localization in Polygonal Environments, Proc. IROS 99, 1412/1419 (1999) 2 P. Jensfelt and S. Kristensen: Active Global Localization for a Mobile Robot Using Multiple Hypothesis Tracking, IEEE Trans. Robotics and Automation, 17-5, 748/760 (2001) , 66/77 (2007) 4 T. Nakamura and Y. Tashita: Congruence Transformation Invariant Feature Descriptor for Robust 2D Scan Matching, Proc IEEE International Conference on Systems, Man, and Cybernetics (SMC), SYS-10 (2013) 5 2D (CIF) 19 6C3, 592/598 (2014) 6 M. Tomono: A scan matching method using Euclidean invariant signature for global localization and map building, Proc. ICRA 04, 866/871 (2004) 7 K. Konolige and K. Chou: Markov Localization Using Correlation, Proc. IJCAI 1999, 1154/1159 (1999) 8 E. Olson: Real-time Correlative Scan Matching, Proc. ICRA 2009, 4387/4393 (2009) 9 F. Dellaert, D. Fox, W. Burgard and S. Thrun: Monte Carlo Localization for Mobile Robots, Proc. ICRA 1999, 1322/1328 (1999) 10 S. Bando, Y. Hara and T. Tsubouchi: Global Localization of a Mobile Robot in Indoor Environment Using Spatial Frequency Analysis of 2D Range Data, Proc. ICMA 2013, 488/493 (2013) 11 G.A. Borges and M.J. Aldon: Line extraction in 2D range images for mobile robotics, Journal of Intelligent and Robotic Systems, 40-3, 267/297 (2004) 12 K. Lingemann, H. Surmann, A. Nuchter and J. Hertzberg: Indoor and outdoor localization for fast mobile robots, Proc. IROS 04, 2185/2190 (2004) , 648/657 (2010) 14 P.J. Besl and N.D. McKay: A Method for Registration of 3-D Shapes, IEEE Trans. Pattern Analysis and Machine Intelligence, 14-2, 239/256 (1992) 15 F. Lu and E. Milios: Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans, Journal of Intelligent and Robotic Systems, 18-3, 249/275 (1997) 16 The Mobile Robot Programming Toolkit (MRPT):

Kristensen: Active Global Localization for a Mobile Robot Using Multiple Hypothesis Tracking, IEEE Trans. Robotics and Automation, 17-5, 748/760 (2001) 3 25-3, 66/77 (2007) 4 T. Nakamura and Y.

258 5) GPS 1 GPS 6) GPS DP 7) 8) 10) GPS GPS 2 3 4 5 2. 2.1 3 1) GPS Global Positioning System

258 5) GPS 1 GPS 6) GPS DP 7) 8) 10) GPS GPS 2 3 4 5 2. 2.1 3 1) GPS Global Positioning System Vol. 52 No. 1 257 268 (Jan. 2011) 1 2, 1 1 measurement. In this paper, a dynamic road map making system is proposed. The proposition system uses probe-cars which has an in-vehicle camera and a GPS receiver.