3D laser measurement system for large scale architecturesusing multiple mobile robots

Ryo Kurazume, Yukihiro Tobata, Yumi Iwashita, and Tsutomu HasegawaKyushu University

744, Motooka, Nishi-ku, Fukuoka, 819-0395, JAPANkurazume@is.kyushu-u.ac.jp

Abstract

In order to construct three dimensional shape models oflarge scale architectures by a laser range finder, a num-ber of range images are normally taken from various view-points and these images are aligned using post-processingprocedure such as ICP algorithm. However in general, be-fore applying ICP algorithm, these range images have to beregistered to correct positions roughly by a human opera-tor in order to converge to precise positions. In addition,range images must be overlapped sufficiently each other bytaking dense images from close viewpoints. On the otherhand, if poses of the laser range finder at viewpoints can beidentified precisely, local range images can be converted tothe world coordinate system directly with simple transfor-mation calculation. This paper proposes a new measure-ment system for large scale architectures using a group ofmultiple robots and an on-board laser range finder. Eachmeasurement position is identified by the highly precise po-sitioning technique named Cooperative Positioning Systemor CPS which utilizes the characteristics of multiple robotssystem. The proposed system can construct 3D shapes oflarge scale architectures without any post-processing pro-cedure such as ICP algorithm and dense range measure-ments. The measurement experiments in unknown and largeindoor/outdoor environments are successfully carried outusing the newly developed measurement system consistingof three mobile robots named CPS-V.

1. Introduction

Aiming at preserving 3D shapes or views of cultural her-itages using laser range finders or digital cameras, severalresearch projects are being promoted such as ”The DigitalMichelangelo Project”[8], Angkor[5] and Tzuchingch‘engPalace[1]. Generally, for constructing 3D models of theselarge scale architectures, since a laser range finder can-

not cover the whole area at once due to the limitation ofmeasurable distance and occlusion problems, a number ofrange images are normally taken from various viewpointsand these images are aligned using post-processing proce-dures such as ICP algorithm[2],[3]. After post-processingprocedures, range images described in the sensor coordi-nate system are transformed to the world coordinate systemand the whole shape of the architecture is obtained. How-ever in general, before applying ICP method, it is necessaryto register range images to correct positions roughly by ahuman operator in order to converge to proper positions.This procedure is quite laborious and time-consuming, andis considered as a significant obstacle for developing an au-tomatic 3D laser measurement system. In addition, in orderto register range images precisely by ICP algorithm, all theimages must contain a plenty of feature shapes and overlapsufficiently each other by scanning densely from a numberof positions.

On the other hand, another approach with no post-processing procedures can be considered, that is, the iden-tification of the pose of the range sensor precisely at eachmeasurement. Since this method can obtain the transforma-tion matrix from the sensor specific coordinate system tothe world coordinate system, local range images are con-verted to the world coordinate system directly with simpletransformation calculation, and no post-processing proce-dures such as the ICP algorithm are required. As an exam-ple of this approach, several systems have been proposedso far which utilizes the GPS [12],[10] for determining theposition of the range sensor. However, special instrumentsand techniques such as RTK (Real-time Kinematic) systemor VRS (Virtual Reference Station) method are requisite forachieving highly precise position identification by currentGPS. Moreover, GPS cannot be used if enough numbers ofsatellites cannot be seen, for example, in a narrow space inarchitectures, forest, or an indoor environment.

This paper proposes a new 3D measurement system forlarge scale architectures using a group of mobile robots andan on-board laser range finder. This system utilizes the Co-

operative Positioning System (CPS)[7] for multiple robots,which has been proposed as a highly precise position iden-tification technique for mobile robots. By combining highlyprecise position identification by CPS and an on-board laserrange finder, an automatic measurement system for largescale architectures is realized. This system can constructa 3D shape of large scale architecture without any post-processing procedures such as ICP algorithm. In addition,it is also possible to register range images even if numberof measurements is few and sparse range images are onlyavailable, or feature shapes or overlapped regions are notcontained sufficiently in range images. It is also possible toconstruct a 3D model in an environment where GPS is notavailable such as inside of architecture or an indoor environ-ment. The proposed system strongly relates to the SLAM(simultaneous localization and mapping) [9],[11],[4] whichis attracting much attention in robotics community. Wethink it is possible to utilize the system with the aim of thelocalization and mapping of mobile robots.

2 Precise positioning of mobile robots

Let’s consider the system in which a mobile robotequipped with an on-board laser range finder moves arounda measurement target, and scans the target from many po-sitions. If all the measurement positions are identified withhigh accuracy, range data acquired at each position can beconverted to the world coordinate system by simple coordi-nate transformation calculation.

Several position identification methods have been pro-posed so far, and these methods can be classified into threecategories.

1. Integrate sensor output from internal sensors such asan encoder at a wheel or an acceleration sensor.

2. Observe external landmarks by external sensors suchas a laser range finder or a camera.

3. Use Global Positioning System, GPS.

The method (1) is called as odometry or dead reckoningmethod, and quite popular positioning technique especiallyfor a wheeled vehicle. However, there are some drawbacksin this method, for example, the accuracy of position identi-fication in uneven terrain is quite low due to the slippage ofwheels, and 3D positioning including elevation is impossi-ble. The method (2) has high accuracy if landmarks placedon the moving path can be measured precisely. However,landmarks have to be placed beforehand along the movingpath and the precise map of these landmarks has to be avail-able. Therefore, the method (2) cannot be used in unknownenvironment. The method (3) can be considered as a spe-cial case of the method (2) and is becoming very popular

especially for field robots. However, this method also hasseveral drawbacks, for example, active fields of robots arelimited to an outdoor environment without obstacles towardsatellites, and the accuracy is not so high in current tech-nology except using some special techniques explained inSection I.

To overcome these limitations of position identificationproblems and realize accurate positioning for mobile robots,Kurazume et al. have proposed Cooperative PositioningSystem or CPS. In this system, multiple robots with highlyprecise measurement devices of mutual positions are con-trolled cooperatively, and incomparable accurate position-ing is realized even in unknown and uneven environmentsagainst conventional positioning techniques. This sectionintroduces the basic principle of CPS and the example ofSLAM (Simultaneous Localization And Mapping) usingCPS.

2.1 Cooperative Positioning System, CPS

The basic principle of CPS is as follows: We divide therobots into two groups, A and B. One group, A, remainsstationary and acts as a landmark while group B moves.Group B then stops and acts as a landmark for group A.This ”dance” is repeated until the target position is reached.By using the concept of ”portable landmarks”, CPS has a farlower accumulation of positioning error than dead reckon-ing, and can work in three-dimensions which are not pos-sible with dead reckoning. In addition, since there is noneed to place landmarks beforehand, CPS can be used inunknown environment.

An example of CPS is shown in Fig.1. This example isfor a robot system consisting of a parent robot with a sens-ing device such as a laser range finder and two child robots.The sensing device can measure the relative positions of thechild robots from the parent robot. Firstly, we assume thatthe initial position of the parent robot is measured or definedbeforehand.

(1) The child robots 1 and 2 are moved and stopped.

(2) The parent robot measures the distance, azimuth, andelevation angles to the child robot 1 and identifies theposition of the child robot 1.

(3) In the same as step 2, the position of the child robot 2is identified.

(4) The parent robot moves and stops. Then the distances,azimuth, and elevation angles to the child robots 1 and2 are measured and the position of the parent robot iscalculated using the triangular surveying technique.

(5) Repeat from steps 1 to 4 until the target position isreached.

An example of positioning experiments by CPS in anoutdoor environment is shown in Figs.2 and 3. The posi-tioning error after long distance movement of 323.9m in-cluding the difference of elevation of 10m was 0.97m (0.3%of the total distance). This positioning accuracy is quitehigh comparing with other conventional techniques.

02

1y

z

x

Parent robot

Child robots

(1) Robot 1 and 2 move. (2) Robot 0 measures the position of robot 1.

1

2

0 r1

1

1

ψφ

(3) Robot 0 measures the position of robot 2.

1

2

0

2

2

r2

ψ

φ

(4) Robot 0 moves and measures the position of robots 1 and 2.

1

2

0

1

r1

1ψφr2

φ22ψ

Figure 1. Cooperative Positioning System,CPS

Child robot 2

Child robot 1

Parent robot

Figure 2. Long distance measurement exper-iments

2.2 Positioning accuracy of CPS

We discuss analytically about the positioning accuracy ofCPS in this section. Let’s consider CPS consisting of threerobots as shown in Fig.1. Firstly we discuss the case that theposition of the parent robot 0 is identified by measuring thedistances, azimuth, and elevation angles to the child robots1 and 2 as shown in Fig.1(4). The positions of robots aredefined as Pi(xi, yi, zi, θi),(i = 0 ∼ 2). The distance fromthe robot 0 to the robots 1 and 2 are r1, r2, and the azimuthand elevation angles to the robots 1 and 2 are φ1,φ2,ψ1, andψ2, respectively. The observation equations in this case aregiven by the following equations.

(x0 − x1)2 + (y0 − y1)2 = r21 cos2 ψ1 (1)

Initial position (0,0,0)

(-7.5,31.3,-0.2)

(-14.5, 91.7,4.1)

(64.6,87.3,10.0)

(59.0,-3.6,10.0)

Final position (-0.4,1.1,0.5)

x

yz

[m]

90m

10m

80m

Figure 3. Results of long distance measure-ment experiments

(x0 − x2)2 + (y0 − y2)2 = r22 cos2 ψ2 (2)

z0 = z1 − r1 sinψ1 (3)

= z2 − r2 sinψ2 (4)

θ0 = −φ1 + tan−1 y1 − y0x1 − x0

(5)

= −φ2 + tan−1 y2 − y0x2 − x0

(6)

Here, since the variables to be determined are the positionof the robot 0, P0(x0, y0, z0, θ0), this system has redun-dancy. Therefore, by substituting xi = xi + dxi into theabove equations and using Taylor expansion, the followingequation is derived.

AX0 = L + K1X1,2 + K2Θ (7)

where X0 = (dx0 dy0 dz0 dθ0)T ,X1,2 = (dx1 dy1 dz1 dθ1 dx2 dy2 dz2 dθ2)T , Θ =(dr dφ dψ)T (dr,dφ, and dψ are measurement errors andwe assume dr = dri etc.),

L =

r1 cos ψ1 − d1

r2 cos ψ2 − d2

z1 − r1 sin ψ1 − z0z2 − r2 sin ψ2 − z0

φ1 + θ0 − tan−1 y1−y0x1−x0

φ2 + θ0 − tan−1 y2−y0x2−x0

(8)

and A ∈ R6×4, K1 ∈ R6×8, K2 ∈ R6×3 are all coefficientmatrices.

Next, by considering the case that all the elements of Lwhich indicates the observation error are 0, the square errorof the right part of Eq.(7), that is, the error variance matrixis calculated as follows:

ΣL = K1ΣKT1 + K2ΣΘKT

2 (9)

where Σ is a matrix consisting of the error variance ma-trix of the robots 1 and 2, and the error covariance matrixbetween the robots 1 and 2.

Σ =(

Σ11 Σ12

Σ21 Σ22

)(10)

ΣΘ isΣΘ = diag(σ2

r σ2φ σ

2ψ) (11)

and σ2r ,σ2

φ, and σ2ψ are the error variances of distance, az-

imuth, and elevation angles, respectively. Note that themeasurement error Θ is assumed to be obtained indepen-dently at each measurement, and thus it does not correlatewith the previous positioning error X1,2.

Now we want to solve Eq.(7) to obtain the solutionX0. Since the coefficient matrix A is not a square matrix,we solve Eq.(7) using the weighted least squares method.Firstly, we consider the residual equation.

V = L−AX0 (12)

Then, the error sum of squares weighted by Σ−1L shown in

the following equation is minimized.

minVTΣ−1L V (13)

By substituting Eqs.(9) and (12) into Eq.(13) and differenti-ating it with respect to X0, the residual position of the robot0, X0, which minimizes the error sum of squares is derivedas follows:

X0 = (ATΣ−1L A)−1ATΣ−1

L L = BL (14)

In addition, the error variance matrix of the position of therobot 0 is derived from Eq.(14) as

Σ0 = BΣLBT = (ATΣ−1L A)−1 (15)

and the covariance matrices between the robots 0 and 1, andthe robots 0 and 2 are obtained as

(Σ01,Σ02) = BK1Σ (16)

Next, in the case that the child robot i moves and stops,and its position is identified by the parent robot as shown inFig.1(2)(3), the position of the child robot is calculated as

xi = x0 + ri cosφi cosψi (17)

yi = y0 + ri sinφi cosψi (18)

zi = z0 + ri sinψi (19)

By substituting xi = xi + dxi into above equations and ap-plying Taylor expansion, the following equation is obtained.

Xi = Li + X0 + K2,iΘ (20)

where Xi = (dxi dyi dzi dθi)T and

Li =

x0 + ri cos φi cos ψi − xi

y0 + ri sin φi cos ψi − yiz0 + ri sin ψi − zi

(21)

Therefore, the residual position of the child robot i, the errorvariance matrix, and the error covariance matrix are derivedas the following equations.

Xi = Li (22)

Σii = Σ0 + K2,iΣΘiKT

2,i (23)

Σ0i = Σij = Σ0 (24)

Consequently, the positioning procedure is summarized asfollow: i) assume the proper initial position of robot i as Pi

and calculate Xi by solving Eqs.(14) and (22), ii) substi-tute Pi ← Pi + Xi, ii) repeat (i) and (ii) until Xi becomessmall enough. The positioning accuracy after repeating theposition identification by CPS can be estimated by calculat-ing the error variance and covariance matrices according toEqs.(15), (16), (23), and (24).

As an example, we substituted the actual measurementerror of the laser range finer explained in Section III (σr =3[mm], σφ = 5[sec.], and σψ = 5[sec.]), and calculatedthe positioning error after 1km movement. CPS motion inFig.1 is repeated 100 times and the robots moved 10m inone cycle, respectively. The obtained positioning accuracywas σ2

x + σ2y = 0.0203[m2] for the position elements of the

error variance matrix of the parent robot 0, Σ0. In actualscenario, due to the measurement error of the body incli-nation or the offset of the center position of corner cubes,such a high precision cannot be achieved. However we haveconfirmed that incomparable highly precise positioning ispossible against conventional dead reckoning method whichutilizes the rotation angle of wheels [6].

3 Construction of 3D environmental map bymultiple robots

This section proposes a new measurement system forprecise construction of a 3D environmental map by combin-ing CPS for multiple robots and a laser range finder. In thissystem, mobile robots move around a large scale target andscan the target by an on-board 3D laser range finder fromseveral viewpoints. Each measurement position is identifiedby CPS precisely using a parent and two child robots. Sincethere is no need to apply laborious post-processing proce-dures, the whole picture of the large scale architecture canbe obtained with ease and high accuracy. Firstly, we intro-duce the fifth CPS machine model named CPS-V, which isequipped with a 2D laser range finder and a scanning mech-anism, and show experimental results for the constructionof indoor and outdoor environmental maps by CPS-V.

3.1 The fifth CPS machine model, CPS-V

Figure 4 shows the fifth CPS machine model namedCPS-V. This system consists of a parent robot (P-cle, Par-ent mobile unit, Fig.6) and two child robots (HPI Japan,Fig.5). The parent robot is equipped with the on-board 2Dlaser range finder (LMS 200, Sick), a high precision 2-axisattitude sensor (MD900T, Applied Geomagnetics), and atotal station for surveying (AP-L1, TOPCON Ltd.)(Table1) which is used for measuring relative positions from thechild robots. Even if the body is tilted on a slope, bodyinclination can be compensated by the attitude sensor andprecise positions of the robots can be identified. The 2Dlaser range finder can acquire slit-like range data within therange of 80m and 180 degrees as shown in Table 2. Theparent robot has a rotating table on the upper body, and byrotating the table around the vertical axis while scanning bythe 2D laser range finder, all around 3D range images fromthe parent robot can be acquired in 37.8 seconds.

Child robot 1Child robot 2

Parent robot P-cle

Figure 4. Fifth CPS machine model, CPS-V

Celeron 650 MHz

Corner cubes

DC servomotor (36 W)

Battery

Figure 5. Child robot

3.2 Construction experiment of indoorenvironmental map

Experiments for constructing 3D maps are carried outusing CPS-V in an indoor environment.

2D scanninglaser range finderLMS200

2-axis attitude sendor

Yaw

Total stationAP-L1

Parent mobileunit, P-cle

Figure 6. 3D laser measurement using rota-tion table around Yaw axis

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 1. Specification of total station, AP-L1

In this experiment, each robot moves 4m at a time andthe parent robot captures 3D range images at each static po-sition by rotating the on-board 2D laser range finder aroundthe vertical axis (Fig.7). Obtained range images are trans-formed to the world coordinate system by simple coordinatetransformation calculation using the measured position byCPS. No post-processing procedure such as ICP algorithmis applied.

Path of the parent robot is shown in Fig.8. The par-ent robot moved up to 39m in x-direction and 10m in y-direction in the hall. Obtained 3D environmental maps areshown in Figs.9 and 10. Circles in the 3D range images in-dicate the measurement positions of the parent robot. Theparent robot captured the range images 23 times during themovements in the hall. Total distance traveled by the parentrobot is 86.21m. From these results, it is confirmed that,without post-processing procedures such as ICP, 3D envi-ronmental maps in unknown environment can be easily ob-tained by applying simple coordinate transformation usingposition information measured by CPS. The difference ofmeasured positions of the corner indicated in Fig.8 beforeand after the movements is 1.17m (1.36% of the distancetraveled). This error is fairly larger than the results obtainedby CPS so far. One of the reasons is that relative distancesbetween robots were about 4m, and this value is the mea-surement limit of the laser range finder as shown in Table

LMS 200 (SICK Corp.)Range 80[m]Field of view 180◦

Resolution (distance) 10[mm]Resolution (angle) 0.5◦

Table 2. Specification of laser range finder,LMS200

Figure 7. 3D laser measurement using multi-ple robots

2. From error measurement experiments of this sensor, it isknown that measurements for short distances tend to con-tain large distance errors, and the distance error is almostuniform for the measurement from 10m to 70m. Thus, ifthe robots keep larger relative distances between them, andmoves longer distance at one cycle, the accuracy is expectedto be higher than the results obtained by this experiments.In addition, after the Step (3) in Fig.1, the parent robot canmove, stop, and measure the environment repeatedly whilethe child robots keep stationary. This strategy can reducethe number of CPS cycle and the measurement accuracymay be greatly improved when the robots work in a largearea.

3.3 Construction experiment of largescale outdoor environmental map

Next, we performed the construction experiment of 3Denvironmental maps of large scale architecture. We mea-sured outward walls of a building from 13 points in out-door environment and constructed 3D models of the build-ing. Path of the parent robot and obtained maps are shownin Figs.8 and 13. In this experiment, we used small num-ber of measurements from sparse view points, and neitherdense range data nor sufficient overlapped regions, whichare indispensable for applying ICP algorithm, were avail-able. From this experiment, we can conclude that it is possi-

Upstairs

Downstairs

Hall

Cor

rido

r

Initial position (0, 0, 0)

: Scanning positions of parent robot

Path of parent robot

10 m

(32.1, -0.6, -0.1)

(14.5, -6.8, -0.3)

x

y

Final position(0.5, 1.1, -0.4)

Corner

Figure 8. Path of parent robot

ble to construct 3D maps of simple but large scale architec-ture by the proposed system with few scanning times. Thepositioning error in this experiment is 0.63m against the to-tal travel distance of the parent robot of 147.7m, which is0.43% of the distance traveled.

4 Conclusions

This paper proposed a new 3D measurement system forlarge scale architectures using the precise positioning sys-tem by multiple robots named CPS and an on-board laserrange finder. This system needs no post-processing proce-dures such as ICP algorithm, and can be applied to sparserange measurements. Therefore, there is a possibility to re-alize a fully automatic 3D measurement system for largescale architecture such as large cultural heritages with theproposed system. The measurement experiments in un-known and large indoor/outdoor environments are success-fully carried out using the newly developed multiple robotssystem named CPS-V.

Our future works include the development of the au-tomatic 3D laser measurement system which selects opti-mum measurement points automatically instead of opera-tor’s command. The positioning accuracy of CPS can alsobe improved by the back-projection technique, that is, theobservation of some stable landmarks before and after the

40m

Figure 9. Obtained 3D map of indoor environ-ment

movements and the correction of each measurement posi-tion by back-projecting errors. Combination of CPS withICP method depending on the situations is also worth con-sidering in order to improve the accuracy of both position-ing and obtained 3D environmental maps.

References

[1] Toppan vr system. http://biz.toppan.co.jp/vr/.[2] P. J. Besl and N. D. McKay. A method for registration of 3-d

shapes. IEEE Transactions on Pattern Analysis and MachineIntelligence, 2(14):239–256, 1992.

[3] Y. Chen and G. Medioni. Object modelling by registrationof multiple range images. Image and Vision Computing,3(10):145–155, 1992.

[4] D. M. Cole and P. M. Newman. Using laser range data for3d slam in outoor environment. Proc. IEEE InternationalConference on Robotics and Automation, pages 1556–1563,2006.

[5] K. Ikeuchi, K. Hasegawa, A. Nakazawa, J. Takamatsu,T. Oishi, and T. Masuda. Bayon digital archival project.In Proceedings of the Tenth International Conference onVirtual System and Multimedia, pages 334–343, November2004.

[6] R. Kurazume and S. Hirose. An experimental study of a co-operative positioning system. Autonomous Robots, 8(1):43–52, 2000.

[7] R. Kurazume, S. Nagata, and S. Hirose. Cooperative po-sitioning with multiple robots. Proc. IEEE Int. Conf. onRobotics and Automation, 2:1250–1257, 1994.

[8] 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. Proc. ACM SIGGRAPH 2000,pages 131–144, July 2000.

(a1) (a2)

(b1) (b2)

(c1) (c2)

Figure 10. Obtained 3D map of indoor envi-ronment

[9] A. Nuchter and H. Surmann. 6d slam with an applicationin autonomous mine mapping. Proc. IEEE InternationalConference on Robotics and Automation, pages 1998–2003,2004.

[10] K. Ohno, T. Tsubouchi, and S. Yuta. Outdoor map buildingbased on odometory and rtk-gps positioning fusion. Proc.IEEE International Conference on Robotics and Automa-tion, pages 684–690, April 2004.

[11] J. Weingarten and R. Siegwart. Ekf-based 3d slam for struc-tured environment reconstruction. Proc. IEEE/RSJ Interna-tional Conference on Intelligent Robots and System,, pages2089–2094, 2005.

[12] H. Zhao and R. Shibasaki. Reconstructing a textured cadmodel of an urban environment using vehicle-borne laserange scanners and line cameras. Machine Vision and Ap-plications, 14:35–41, 2003.

Benches

Side of thebuilding

Rocks

Initial position(0.0, 0.0, 0.0)

(36.4, -2.5, -0.03)

(4.3, -3.0, -0.03)

x

y

Final position(-1.4, 0.7, -0.03)

: P-cle scanning position [m]

Figure 11. Path of parent robot

Figure 12. 3D map of buildings in outdoor en-vironment

(a1) (a2)

(b1) (b2)

(c1) (c2)

Figure 13. 3D map of buildings in outdoor en-vironment