1234 Vol. 25 No. 8, pp.1234 1242, 2007 CPS SLAM Study on CPS SLAM 3D Laser Measurement System for Large Scale Architectures Ryo Kurazume,Yukihiro Tobata,KoujiMurakami and Tsutomu Hasegawa In order to construct three-dimensional shape models of large-scale architectural structures using a laser range finder, a number of range images are normally taken from various viewpoints, and these images are aligned using post-processing procedures such as the ICP algorithm. However, in general, before applying the ICP algorithm, these range images must be registered to roughly correct positions by a human operator in order to converge to precise positions. In addition, range images must be made to sufficiently overlap each other by taking dense images from close viewpoints. On the other hand, if the positions of the laser range finder at viewpoints can be identified precisely, local range images can be directly converted to the global coordinate system through a simple transformation calculation. The present paper proposes a new measurement system for large-scale architectural structures using a group of multiple robots and an on-board laser range finder. Each measurement position is identified by a highly precise positioning technique called Cooperative Positioning System (CPS), which utilizes the characteristics of the multiple-robot system. The proposed system can construct 3D shapes of large-scale architectural structures without any post-processing procedure such as the ICP algorithm or dense range measurements. Measurement experiments in unknown and large indoor/outdoor environments are successfully carried out using the newly developed measurement system consisting of three mobile robots named CPS-V. Key Words: SLAM, 3D Map, Multiple Robots, Laser Range Finder, Digital Archive 1. SLAM Simultaneous Localization And Mapping [13] [15] SLAM SLAM 2 1 2 1 SLAM 2007 4 5 Kyushu University 2 [1] [2] 1 ICP [4] [5] ICP JRSJ Vol. 25 No. 8 90 Nov., 2007
CPS SLAM 1235 ICP SLAM ICP GPS [6] [7] GPS RTK VRS GPS Cooperative Positioning System CPS [8] CPS CPS SLAM SLAM ICP 2. 1 2 3 GPS 1 2 SLAM 3 2 CPS [8] CPS 2. 1 CPS A B A B A B A B CPS Fig. 1 1 2 1 1 2 2 1 1 3 2 2 4 1 2 CPS 10 [m] Fig. 2 3 323.9 [m] Fig. 1 Cooperative Positioning System, CPS 25 8 91 2007 11
1236 Fig. 2 Long distance measurement experiment (dr dφ dψ) T, d i = ( x 0 x i) 2 +(ỹ 0 ỹ i) 2 L = r 1 cos ψ 1 d 1 r 2 cos ψ 2 d 2 z 1 r 1 sin ψ 1 z 0 z 2 r 2 sin ψ 2 z 0 φ 1 + θ 0 tan 1 ỹ 1 ỹ 0 x 1 x 0 φ 2 + θ 0 tan 1 ỹ 2 ỹ 0 x 2 x 0 8 A R 6 4, K 1 R 6 8, K 2 R 6 3 L 0 7 Σ L = K 1ΣK T 1 + K 2Σ ΘK T 2 9 Fig. 3 Results of long distance measurement experiment Σ 1 2 1 2 ( ) Σ11 Σ 12 Σ = 10 Σ 21 Σ 22 0.97 [m] 0.3% [9] 2. 2 CPS CPS Fig. 1 3 CPS 2 1 2 0 Fig. 1 (4) 0 2 P i(x i,y i,z i,θ i) (i =0 2) 0 1 2 r 1 r 2 φ 1 φ 2 ψ 1 ψ 2 0 1 2 (x 0 x 1) 2 +(y 0 y 1) 2 = r 2 1 cos 2 ψ 1 1 (x 0 x 2) 2 +(y 0 y 2) 2 = r 2 2 cos 2 ψ 2 2 z 0 = z 1 r 1 sin ψ 1 3 = z 2 r 2 sin ψ 2 4 θ 0 = φ 1 +tan 1 y 1 y 0 x 1 x 0 5 = φ 2 +tan 1 y 2 y 0 6 x 2 x 0 0 (x 0,y 0,z 0,θ 0) x i = x i + dx i Taylor Σ Θ Σ Θ = diag(σ 2 r σ 2 φ σ 2 ψ) 11 σr σ 2 φ σ 2 ψ 2 X 1,2 Θ 7 X 0 A X 0 Σ 1 L V = L AX 0 12 min V T Σ 1 L V 13 X 0 13 9 12 X 0 0 X 0 X 0 =(A T Σ 1 L A) 1 A T Σ 1 L L = BL 14 0 14 Σ 0 = BΣ LB T =(A T Σ 1 L A) 1 15 0 1 0 2 AX 0 = L + K 1X 1,2 + K 2Θ 7 (Σ 01, Σ 02) =BK 1Σ 16 X 0 =(dx 0 dy 0 dz 0 dθ 0) T, X 1,2 = (dx 1 dy 1 dz 1 dθ 1 dx 2 dy 2 dz 2 dθ 2) T, Θ = i JRSJ Vol. 25 No. 8 92 Nov., 2007
CPS SLAM 1237 Fig. 1 (2) (3) x i = x 0 + r i cos φ i cos ψ i y i = y 0 + r i sin φ i cos ψ i z i = z 0 + r i sin ψ i 17 18 19 x i = x i + dx i Taylor X i = L i + X 0 + K 2,iΘ 20 X i =(dx i dy i dz i dθ i) T Fig. 4 CPS SLAM with activetouchrobot L i = x 0 + r i cos φ i cos ψ i x i ỹ 0 + r i sin φ i cos ψ i ỹ i z 0 + r i sin ψ i z i 21 i X i = L i Σ ii = Σ 0 + K 2,iΣ Θi K T 2,i Σ 0i = Σ ij = Σ 0 22 23 24 i P i 14 22 X i P i P i + X i CPS 15 16 σ r =3[mm] σ φ =5[s] σ ψ =5[s] 10 [m] 100 1[km] Σ 0 σx 2 + σy 2 =0.0203 [m 2 ] [12] 3. 3. 1 CPS SLAM CPS SLAM SLAM Simultaneous Localization And Mapping [13] [15] CPS SLAM Fig. 5 Obtained floor map CPS SLAM [10] Fig. 4 CPS Fig. 5 CPS SLAM CPS SLAM CPS ICP CPS CPS-V CPS-V 3. 2 5 CPS CPS-V Fig. 6 5 CPS CPS-V 1 P-cle Parent mobile unit Fig. 7 2 HPI Japan Fig. 8 LMS 200 Sick AP-L1 TOPCON Ltd. Table 1 25 8 93 2007 11
1238 Fig. 6 Fifth CPS machine model, CPS-V Fig. 9 Indoor experiment using CPS-V Fig. 7 3D laser measurement system using rotation table around yaw axis Fig. 8 Child robot Table 1 Specification of total station, AP-L1 AP-L1 (TOPCON Ltd.) Range 4 400[m] Resolution (distance) 0.2[mm] Resolution (angle) 5 Precision (distance) 3 2ppm[mm] Precision (angle) 5 Table 2 Specification of laser range finder, LMS200 LMS 200 (SICK Corp.) Range 80[m] Field of view 180 Resolution (distance) 10[mm] Resolution (angle) 0.5 2 MD900-TS Applied Geomechanics Inc. 80 [m] 180 Table 2 Fig. 10 Path of parent robot yaw Fig. 7 1 37.8 CPS 3. 3 5 CPS CPS-V 1 4[m] Fig. 9 JRSJ Vol. 25 No. 8 94 Nov., 2007
CPS SLAM 1239 (a1) (a) (a2) Fig. 11 Obtained 3D map of indoor environment Fig. 10 x 39 [m] y 10 [m] 2 Fig. 11 12 23 Fig. 12 (a2) (b2) Fig. 12 (c2) CPS ICP CPS 86.21 [m] 1.25 [m] 1.45% yaw 4.2 2.1 CPS [9] Fig. 10 Corner 1 23 Fig. 13 (a) 1.17 [m] 1 2 CPS 1 Table 1 5[m] [12] 4[m] 2 22 CPS CPS CPS CPS (b1) (b2) (b) (c1) (c2) (c) Fig. 12 Obtained 3D map of indoor environment CPS [8] Fig. 14 22 5 CPS 93.9 [m] 0.22 [m] 0.24% yaw 1.5 Fig. 14 Corner Fig. 13 (b) 0.22 [m] Fig. 13 (a) 3. 4 13 25 8 95 2007 11
1240 (a) Child robot moves 22 times (b) Child robot moves 5 times Fig. 13 Comparison of modeling errors Fig. 15 Total view of 3D map in outdoor environment (a1) (a) (a2) (b1) (b) (b2) Fig. 14 Path of parent robot (each child robot moves 5 times) Fig. 15 16 Fig. 17 147.7 [m] 0.63 [m] 0.43% yaw 0.9 ICP 3. 5 3[m] Fig. 18 Fig. 19 Fig. 20 2 (c1) (c2) (c) Fig. 16 3D map of buildings in outdoor environment 130.6 [m] 0.80 [m] 0.61% yaw 0.7 4. JRSJ Vol. 25 No. 8 96 Nov., 2007
CPS SLAM 1241 Fig. 17 Path of parent robot Fig. 19 3D map of outdoor environment with slopes Fig. 18 Outdoor environment with slopes CPS CPS SLAM CPS CPS-V CPS ICP CPS ICP Fig. 20 Path of parent robot ICP B 18360124 5 RT 19360119 25 8 97 2007 11
1242 [ 1] K. Ikeuchi, K. Hasegawa, A. Nakazawa, J. Takamatsu, T. Oishi and T.Masuda: Bayon Digital Archival Project, Proceedings of the Tenth International Conference on Virtual System and Multimedia, pp.334 343, 2004. [2] VR http://biz.toppan.co.jp/ vr/ [3] M. Levoy, K. Pulli, B. Curless, S. Rusinkiewicz, D. Koller, L. Pereira, M. Ginzton, S. Anderson, J. Davis, J. Ginsberg, J. Shade and D. Fulk: The Digital Michelangelo Project: 3D Scanning of Large Statues, Proceedings of ACM SIGGRAPH 2000, pp.131 144, 2000. [4] P.J. Besl and N.D. McKay: A method for registration of 3- Dshapes, IEEE Trans. Pattern Anal. Machine Intell., vol.14, no.2, pp.239 256, 1992. [5] Y.Chen and G. Medioni: Object modelling by registration of multiple range images, Image and Vision Computing, vol.10, no.3, pp.145 155, 1992. [6] H. Zhao and R. Shibasaki: Reconstructing a textured CAD model of an urban environment using vehicle-borne lase range scanners and line cameras, Machine Vision and Applications, vol.14, pp.35 41, 2003. [7] K.Ohno, T. Tsubouchi and S. Yuta: Outdoor Map Building Based on Odometory and RTK-GPS Positioning Fusion, Proc. IEEE International Conference on Robotics and Automation, pp.684 690, 2004. [8] vol.13, no.6, pp.838 845, 1995 [9] 8 CPS-III 16 pp.169 170, 1998. [10] CPS vol.17, no.1, pp.84 90, 1999. [11] CPS SLAM CPS- V SLAM 24 2N17, 2006. [12] CPS-II vol.15, no.5, pp.773 780, 1997. [13] A. Nuchter and H. Surmann: 6D SLAM with an Application in Autonomous Mine Mapping, Proc. IEEE International Conference on Robotics and Automation, pp.1998 2003, 2004. [14] J. Weingarten and R. Siegwart: EKF-based 3D SLAM for Structured Environment Reconstruction, Proc. IEEE/RSJ International Conference on Intelligent Robots and System, pp.2089 2094, 2005. [15] D.M. Cole and P.M. Newman: Using Laser Range Data for 3D SLAM in Outoor Environment, Proc. IEEE International Conference on Robotics and Automation, pp.1556 1563, 2006. Ryo Kurazume 1967 2 4 1991 1995 2000 2002 2007 Kouji Murakami 1975 4 25 2004 Yukihiro Tobata 1981 10 26 2005 CPS SLAM Tsutomu Hasegawa 1950 2 18 1973 1992 JRSJ Vol. 25 No. 8 98 Nov., 2007