33 2 Vol. 33, No. 2 2007 2 ACTA AUTOMATICA SINICA February, 2007 1 1 1. Hough, Hough. Hough Hausdorff. Fourier-Mellin Fourier Fourier-Mellin. Hausdorff (ICP).. Hough Fourier-Mellin Hausdorff TP24 Robot Self-Localization Based on Planar Laser Measurement Zeng Hui 1 Wu Fu-Chao 1 Hu Zhan-Yi 1 Abstract This paper presents two novel robot self-localization methods based on planar laser measurement. The first one is an improved Hough density spectrum based method, into which a novel Hough density spectrum is introduced, and by which the location accuracy and robustness can be both enhanced. The key advantageous aspect of our new spectrum is that its implementation does not involve any discretization error in the Hough space, which is the major source of location inaccuracy in the conventional method. The second one is an Fourier-Mellin transform based method. This method first converts the two measurement point sets into two binary images, then uses the Fourier-Mellin based image matching technique, a popular technique in image matching field, to determine the rotation parameter and finally invokes a standard ICP technique with the Hausdorff distance as its cost to estimate the two translation parameters. Experimental results show that both methods can perform robustly and accurately. Key words Robot self-localization, Hough density spectrum, Fourier-Mellin transform, Hausdorff distance 1. ( ),.. ( ) (R, T ). 2005-9-6 2006-5-11 Received September 6 2005; in revised form May 11 2006 (60303021), (863) (2005AA118020) Supported by National Natural Science Foundation of P. R. China (60303021), the National High-Tech Research and Development Plan of P. R. China (2005AA118020) 1. 100080 1. National Laboratory of Pattern Recognition, Institute of Automation, Chinese Academy of Sciences, Beijing 100080 DOI: 10.1360/aas-007-0138 [1, 2].. (Iterative closest point, ICP). [3, 4]. Andrea Censi [5] Hough.. Hough.. Hough
2 139 Hough (R, T ) [5] Hough. Fourier-Mellin. Fourier Fourier- Mellin (R, T ) Hausdorff ICP T. 2 3 Hough 4 Fourier-Mellin 5 6. 2 [2, 3]....... P 1, P 2,, P n P 1, P 2,, P n (R, T ). P i P i.... 3 Hough.. Hough. Hough.. 3 Hough 3.1 Hough P (R, T ) P, P = RP + T. P P Hough HT (θ, ρ) HT (θ, ρ) φ Hough [6] HT (θ, ρ) = HT (θ + φ, ρ + (cos θ, sin θ)t )] (1) (1) Hough P Hough θ ρ P Hough (θ + φ) ρ (cos θ, sin θ)t. Hough θ ρ Hough (θ + φ) ρ. g f (π) = f(π + a) g[f] = g[f ]. P 1, P 2,, P n Hough HS(θ) := g[ht (θ, )] (2) HT (θ, ) P 1, P 2,, P n Hough θ ρ ρ 1, ρ 2,, ρ n. 2π θ [0, 2π]. P 1, P 2,, P n P 1, P 2,, P n Hough HT (θ, ) = HT (θ + φ, ) Hough Hough HS(θ) HS (θ+φ) HS(θ) = HS (θ+ φ) Hough. 3.2 Hough [5] Hough Hough Hough. ρ θ Hough
140 33. A(ρ, θ) 0. Hough. P i ρ = (cos θ, sin θ)p i A(ρ, θ) 1. (R, T ) P 1, P 2,, P n P 1, P 2,, P n A(ρ, θ) A (ρ, θ) A θ e = [e 1, e 2,, e m ] T A θ + φ e = [e 1, e 2,, e m] T. 3.1 Hough e = e. [5] HT (θ, :) = e HT (θ, ) := HT (θ + φ, ). [5] g[f] = fi 2 Hough m HS(θ) := e 2 i (3) i=1 Hough π. Hough φ. Hough HS(θ) HS (θ) Corr(φ) = θ Θ HS(θ) HS (θ φ) (4) φ {φ 1, φ 2,, φ k }. Hough. φ i P φ 1, P φ 2,, P φ n Hough HT (θ, ρ) = HT φ (θ, ρ + (cos θ, sin θ)t ) (5) T. θ i (cos θ i, sin θ i )T = d(θ i ) i = 1,, n (6) n 2 T. d(θ i ). HT (θ i, ) HT φ (θ i, ) d(θ i ) θ i Hough. [5] Hough (cos θ, sin θ)t ρ Hough. [5]. 3.3 Hough 3.3.1 Hough Hough Hough ρ Hough., h = [ρ 1 ρ, ρ 2 ρ,, ρ n ρ] ρ = ( ρ i )/n ρ HT (θ, ) := h. Hough θ θ + φ ρ (cos θ, sin θ)t HT (θ, ) = HT (θ + φ, ). g[f] = fi 2 Hough HS(θ) := n (ρ i ρ) 2 (7) i=1 HS(θ) ρ i ρ ρ i. Hough HS(θ) HS (θ). [5] Hough Hough ρ. 3.3.2 φ T φ [5] φ {φ 1, φ 2,, φ k }. ( φ ) φ. I 1, I 2,, I k I i (i = 1, 2,, k) (α i 1, r i 1), (α i 2, r i 2),, (α i m, r i m) (α i j, r i j) I i α i j ri j (α i t, r i t)(t = 1, 2,, m) u i t = r i t / m t i j (8) j=1 (8) r t i. φ φ i m φ i = α i t u i t (9) t=1
2 141 k φ k {φ 1, φ 2,, φ k }. T [5] Hough θ i. φ φ i, Hough I 1, I 2,, I k (θ i, θ j ). Hough π. (6) T T. (9) T T i. (φ, T ). Hausdorff Hausdorff. (φ i, T i ) P 1, P 2,, P n P = RP + T P 1, P 2,, P n. P 1, P 2,, P n P 1, P 2,, P n Hausdorff. Hausdorff (φ, T ). 3.3.3 Hough 1) P 1, P 2,, P n P 1, P 2,, P n Hough Hough. 2) φ {φ 1, φ 2,, φ k }. 3) φ i a) P 1, P 2,, P n φ i P φ 1, P φ 2,, Pn φ. b) (θ i, θ j ) (6) T. c) b) T. d) T i. 4) 3) T T 1, T 2,, T k. 5) Hausdorff (φ, T ). 4 Fourier-Mellin 4.1 Fourier-Mellin. Fourier-Mellin [7 9]. Fourier Fourier-Mellin. f 2 (u, v) f 1 (u, v) u v u 0 v 0 f 2 (u, v) = f 1 (u u 0, v v 0 ) (10) Fourier F 2 (ξ, η) = e j ϕ(ξ,η) F 1 (ξ, η) (11) f 1 (u, v) f 2 (u, v) F 1 (ξ, η)f 2 (ξ, η) F 1 (ξ, η)f 2 (ξ, η) = eϕ(ξ,η) (12) F F. (12) Fourier (u 0, v 0 ) u 0 v 0 [10]. Fourier. f 1 (u, v) f 2 (u, v) f 2 (u, v) = f 1 (u cos φ + v sin φ + t u, u sin φ + v cos φ + t v ) (13) Fourier Fourier F 2 (ξ, η) = e j ϕ(ξ,η) F 1 (ξ cos φ + η sin φ, ξ sin φ + η cos φ) (14) M 1 M 2 F 1 F 2 M 2 (ξ, η) = M 1 (ξ cos φ + η sin φ, ξ sin φ + η cos φ) (15) (15) Fourier Fourier- Mellin [10]. 4.2 (R, T ) P 1, P 2,, P n P 1, P 2,, P n. f 1 (u, v) f 2 (u, v) ( 1 0)..
142 33 Table 1 1 Comparison of experimental results using four localization methods x(m) y(m) ( ) Hausdorff (m) HSM 0.0171 0.0185 0.3642 0.0657 IHSM 0.0047 0.0023 0.1031 0.0309 FM 0.0163 0.0189 0.0698 0.0497 FMH 0.0057 0.0021 0.0698 0.0284 HSM 0.0390 0.0230 0.6843 0.0846 IHSM 0.0226 0.0198 0.2485 0.0539 FM 0.0420 0.0215 0.1325 0.0971 FMH 0.0216 0.0167 0.1325 0.0567 HSM 0.0506 0.0470 2.3520 0.1089 IHSM 0.0369 0.0319 1.6751 0.0764 FM 0.0713 0.0364 0.8763 0.1215 FMH 0.0334 0.0328 0.8763 0.0734 (15) M 1 (ρ, θ) = M 2 (ρ, θ φ) (16) Fourier M 1 (ρ, θ) M 2 (ρ, θ φ) Fourier Fourier φ [10]. f 1 (u, v) φ f φ (u, v) f φ (u, v) f 2 (u, v) f 2 (u, v) = f φ (u + t 1, v + t 2 ). Fourier t 1 t 2 T. T. T Hausdorff ICP.. (ICP).. ICP. Hausdorff Hausdorff ICP T. φ P 1, P 2,, P n P φ 1, P φ 2,, P φ n P 1, P 2,, P n. P φ P Hausdorff H(P φ, P ) = max{h(p φ, P ), h(p, P φ )} (17) h(p φ, P ) = max a P φ min b P a b [11]. Hausdorff min Z = H(P φ + T, P ) + H(P φ, P T ) (18) T T. 5 [5]. 5.1 Radish [12] Stage Richard Vaughan [13] Texas Cicurina [14]. Sick PLS 5cm, 1 2.8m. [5] Hough (HSM) Hough (IHSM) Fourier-Mellin
2 143 (a) HSM (b) IHSM (c) FM (d) FMH Fig. 1 1 The matching results of the four localization methods (R, T ) (FM) Fourier-Mellin R Hausdorff ICP T (FMH) 1. 1. IHSM Hausdorff HSM FM HSM FMH HSM IHSM FMH. IHSM FMH. 5.2 irobot B21r. Sick PLS 101 312. 5cm 1 50m.. 1 HSM IHSM FM FMH. Hausdorff 0.0716m 0.0454m 0.0675m 0.0436m. 1 IHSM FMH. 6. Hough Fourier-Mellin. Andrea Censi [5]. References 1 Lingermann K, Surmann H, Nücher A, Hertzberg J. Indoor and outdoor localization for fast mobile robots. In: Proceedings of IEEE International Conference on Intelligrnt Robots and System. IEEE, Sendai, Japan, 2004. 2185 2190 2 Sooyong Lee, Iae-Bok Song. Mobile robot localization using
144 33 range sensors: Consecutive scanning and cooperative scanning. International Journal of Control, Automation, and Systems, 2005, 3(1): 1 14 3 Feng Lu, Evangelos Milios. Robot pose estimation in unknown environments by matching 2D range scans. Journal of Intelligent and Robotic Systems, 1997, 18(5): 249 275 4 Minguez J, Lamiraux F, Montesano L. Metric-based scan matching algorithms for mobile robot displacement estimation. In: Proceedings of IEEE International Conference on Robotics and Automation. IEEE, Barcelona,Spain, 2005. 3557 3563 5 Andrea Censi, Luca Iocchi, Giorgio Grisetti. Scan matching in the Hough domain. In: Proceedings of IEEE International Conference on Robotics and Automation. IEEE, Barcelona, Spain, 2005, 2750 2755 6 Illingworth J, Kittler J. A survey of the Hough transform. Computer Vision, Graphics, and Image Processing, 1998, 40(1): 87 116 7 Li Xiao-Ming, Zhao Xun-Po, Zheng Lian, Hu Zhan-Yi. An image registration technique based on Fourier-Mellin transform and its extended applications. Chinese Journal of Computers, 2006, 29(3): 466 472 (. Fourier-Mellin. 2006 29(3): 466 472) 8 Zhao Xun-Po. A Study on Robust Image Curve Matching Techniques[Ph.D. dissertation], Beijing: Institute of Automation, Chinese Academy of Sciences, 2004 (. [ ], 2004) 9 Chen Qin-Sheng, Michel Defrise, Deconinck F. A symmetric phase-only matched filtering of Fourier-Mellin transforms for image registration and recognition. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1994, 16(12): 1156 1168 10 Reddy B Srinivasa, Chatterji B N. An FFT-based technique for translation, rotation, and scale-invariant image registration. IEEE Transactions on Image Processing, 1996, 5(8): 1266 1271 11 Huttenlocher Daniel P, Klanderman Gregory A, Rucklidge William J. Comparing images using the Hausdorff distance. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1993, 15(9): 850 863 12 Andrew Howard, Nicholas Roy. The Robotics Data Set Repository[Online], available: http://radish.sourceforge. net/, 2003 13 Richard Vaughan. Hosptial Floorplan Fort Sam Houston and Cave Bitmap[Online], available: http://cres.usc.edu/ radish/view-all.php, May 06, 2003 14 The Texas Speleological Survey, Cicuinacave[Online], available: http: //www. utexas. edu/tmm/sponsored sites/tss/ CaveMaps/mapimages/cicurinacave.gif, May 22, 2005 2001 2004.. E-mail: hzeng@nlpr.ia.ac.cn (Zeng Hui Received her bachelor degree and master degree from Shandong University in 2001 and 2004 respectively. Currently, she is pursuing her Ph. D. degree at Institute of Automation, Chinese Academy of Science. Her research interests include robot localization and visual metrology. Corresponding author of this paper.) 1995 1999 2000. E-mail: fcwu@nlpr.ia.ac.cn (Wu Fu-Chao Acted as a professor in Anhui University from 1995 to 1999. Since 2000, he has been a professor at Institute of Automation, Chinese Academy of Science. His research interests include camera calibration, 3D reconstruction, and robot self-localization.) 1993 1993 Hough. E-mail: huzy@nlpr.ia.ac.cn (Hu Zhan-Yi Received his Ph.D. degree in computer science from University of Liege, Belgium in 1993. Since 1993, he has been a professor at Institute of Automation, Chinese Academy of Science. His research interests include camera calibration, 3D reconstruction, Hough transform, and vision guided robot navigation.)