3D laser measurement system for large scale architecturesusing multiple mobile robots
Ryo Kurazume, Yukihiro Tobata, Yumi Iwashita, and Tsutomu Hasegawa
Kyushu 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 number of range images are normally taken from various viewpoints and these images are aligned using post-processing
procedure such as ICP algorithm. However in general, before applying ICP algorithm, these range images have to beregistered to correct positions roughly by a human operator 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 other
hand, if poses of the laser range finder at viewpoints can be
identified precisely, local range images can be converted to
the world coordinate system directly with simple transformation calculation. This paper proposes a new measurement 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 positioning technique named Cooperative Positioning System
or CPS which utilizes the characteristics of multiple robots
system. The proposed system can construct 3D shapes oflarge scale architectures without any post-processing procedure such as ICP algorithm and dense range measurements. The measurement experiments in unknown and large
indoor/outdoor environments are successfully carried out
using the newly developed measurement system consistingof three mobile robots named CPS-V.

1. Introduction
Aiming at preserving 3D shapes or views of cultural heritages using laser range finders or digital cameras, several
research projects are being promoted such as ”The Digital
Michelangelo Project”[8], Angkor[5] and Tzuchingch‘eng
Palace[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 of
measurable distance and occlusion problems, a number ofrange images are normally taken from various viewpoints
and these images are aligned using post-processing procedures such as ICP algorithm[2],[3]. After post-processing
procedures, range images described in the sensor coordinate system are transformed to the world coordinate systemand the whole shape of the architecture is obtained. However in general, before applying ICP method, it is necessary
to 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 automatic 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 overlap
sufficiently each other by scanning densely from a numberof positions.
On the other hand, another approach with no postprocessing procedures can be considered, that is, the identification of the pose of the range sensor precisely at eachmeasurement. Since this method can obtain the transformation matrix from the sensor specific coordinate system tothe world coordinate system, local range images are converted to the world coordinate system directly with simple
transformation calculation, and no post-processing procedures such as the ICP algorithm are required. As an example of this approach, several systems have been proposedso far which utilizes the GPS [12],[10] for determining theposition of the range sensor. However, special instruments
and techniques such as RTK (Real-time Kinematic) systemor VRS (Virtual Reference Station) method are requisite forachieving highly precise position identification by current
GPS. 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 identification technique for mobile robots. By combining highly
precise position identification by CPS and an on-board laser
range finder, an automatic measurement system for large
scale architectures is realized. This system can construct
a 3D shape of large scale architecture without any postprocessing 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 only
available, 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 environment. The proposed system strongly relates to the SLAM
(simultaneous localization and mapping) [9],[11],[4] which
is attracting much attention in robotics community. We
think it is possible to utilize the system with the aim of thelocalization and mapping of mobile robots.

especially for field robots. However, this method also has
several drawbacks, for example, active fields of robots arelimited to an outdoor environment without obstacles toward
satellites, and the accuracy is not so high in current technology except using some special techniques explained in
Section I.
To overcome these limitations of position identification
problems and realize accurate positioning for mobile robots,
Kurazume et al. have proposed Cooperative Positioning
System or CPS. In this system, multiple robots with highly
precise measurement devices of mutual positions are controlled cooperatively, and incomparable accurate positioning is realized even in unknown and uneven environments
against conventional positioning techniques. This section
introduces the basic principle of CPS and the example of
SLAM (Simultaneous Localization And Mapping) using
CPS.

2

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

The basic principle of CPS is as follows: We divide therobots into two groups, A and B. One group, A, remains
stationary 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 far
lower accumulation of positioning error than dead reckoning, and can work in three-dimensions which are not possible with dead reckoning. In addition, since there is noneed to place landmarks beforehand, CPS can be used in
unknown environment.
An example of CPS is shown in Fig.1. This example isfor a robot system consisting of a parent robot with a sensing 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 defined
beforehand.

3. Use Global Positioning System, GPS.

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

Precise positioning of mobile robots

Let’s consider the system in which a mobile robot
equipped with an on-board laser range finder moves arounda measurement target, and scans the target from many positions. If all the measurement positions are identified withhigh accuracy, range data acquired at each position can beconverted to the world coordinate system by simple coordinate transformation calculation.
Several position identification methods have been proposed so far, and these methods can be classified into three
categories.
1. Integrate sensor output from internal sensors such as
an encoder at a wheel or an acceleration sensor.

The method (1) is called as odometry or dead reckoning
method, and quite popular positioning technique especiallyfor a wheeled vehicle. However, there are some drawbacks
in this method, for example, the accuracy of position identification in uneven terrain is quite low due to the slippage ofwheels, and 3D positioning including elevation is impossible. 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 moving
path and the precise map of these landmarks has to be available. Therefore, the method (2) cannot be used in unknown
environment. The method (3) can be considered as a special case of the method (2) and is becoming very popular2.1

Cooperative Positioning System, CPS(2) The parent robot measures the distance, azimuth, andelevation angles to the child robot 1 and identifies the
position of the child robot 1.
(3) In the same as step 2, the position of the child robot 2
is identified.
(4) The parent robot moves and stops. Then the distances,azimuth, and elevation angles to the child robots 1 and
2 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 an
outdoor environment is shown in Figs.2 and 3. The positioning error after long distance movement of 323.9m including the difference of elevation of 10m was 0.97m (0.3%of the total distance). This positioning accuracy is quite
high comparing with other conventional techniques.

(64.6,87.3,10.0)90m(-14.5, 91.7,4.1)(-7.5,31.3,-0.2)(59.0,-3.6,10.0)
Initial position (0,0,0)

Parent robot

z

z

y

x

1

1

r1

0

0

2

2

x [m]

ψ1
φ1

2 r
2

Figure 3. Results of long distance measurement experiments(2) Robot 0 measures
the position of robot 1.
1

ψ2

0

0
2

r2
φ2

ψ2

r1
ψ1

80m
Final position (-0.4,1.1,0.5)

Child robots(1) Robot 1 and 2 move.

10m

y

1
φ1

φ2

(x0 − x2 )2 + (y0 − y2 )2(4) Robot 0 moves andmeasures the positionof robots 1 and 2.

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

z0

Figure 1. Cooperative Positioning System,
CPS
θ0

=
=

=

r22 cos2 ψ2= z1 − r1 sin ψ1= z2 − r2 sin ψ2y1 − y0x1 − x0
y2 − y0−φ2 + tan−1x2 − x0−φ1 + tan−1Child robot 1

AX0 = L + K1 X1,2 + K2 Θ
Figure 2. Long distance measurement experiments

Positioning accuracy of CPS

We discuss analytically about the positioning accuracy of
CPS 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 the
distances, azimuth, and elevation angles to the child robots
1 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 are
given by the following equations.
(x0 − x1 )2 + (y0 − y1 )2=

r12 cos2 ψ1

(3)
(4)
(5)
(6)

Here, since the variables to be determined are the positionof the robot 0, P0 (x0 , y0 , z0 , θ0 ), this system has redundancy. Therefore, by substituting xi = x̃i + dxi into theabove equations and using Taylor expansion, the following
equation is derived.

Parent robot
Child robot 2

2.2

(2)

(1)

(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.),r˜1 cos ψ˜1 − d1r˜2 cos ψ˜2 − d2 z˜ − r˜ sin ψ˜ − z˜


1
1
1
0
L=
(8)

˜
 z˜2 − r˜2 sin ψ2 − z˜0 

 ˜
−
y
˜
y
˜
−1
1
0
 φ1 + θ˜0 − tan x˜1 −x˜0 
y˜0φ˜2 + θ˜0 − tan−1 xy˜˜22 −−x˜0
and A ∈ R6×4 , K1 ∈ R6×8 , K2 ∈ R6×3 are all coefficientmatrices.
Next, by considering the case that all the elements of L
which indicates the observation error are 0, the square errorof the right part of Eq.(7), that is, the error variance matrix
is calculated as follows:ΣL = K1 ΣKT1 + K2 ΣΘ KT2(9)

where Σ is a matrix consisting of the error variance matrix of the robots 1 and 2, and the error covariance matrix
between the robots 1 and 2.
Σ11 Σ12(10)Σ=Σ21 Σ22ΣΘ isΣΘ = diag(σr2 σφ2 σψ2 )(11)where Xi = (dxi dyi dzi dθi )T andx̃0 + r̃i cos φ̃i cos ψ̃i − x̃i
Li =  ỹ0 + r̃i sin φ̃i cos ψ̃i − ỹi z̃0 + r̃i sin ψ̃i − z̃iTherefore, the residual position of the child robot i, the error
variance matrix, and the error covariance matrix are derived
as the following equations.

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

imuth, and elevation angles, respectively. Note that themeasurement error Θ is assumed to be obtained independently 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 solution
X0 . 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 Σ−1
L shown in
the following equation is minimized.
min VT Σ−1
L V

(13)

By substituting Eqs.(9) and (12) into Eq.(13) and differentiating it with respect to X0 , the residual position of the robot
0, X0 , which minimizes the error sum of squares is derived
as follows:−1 T −1
A ΣL L = BL
X0 = (AT Σ−1
L A)

(14)

In addition, the error variance matrix of the position of therobot 0 is derived from Eq.(14) as−1
Σ0 = BΣL BT = (AT Σ−1
L A)

(16)

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

=
=

x0 + ri cos φi cos ψiy0 + ri sin φi cos ψi

(17)
(18)

=

z0 + ri sin ψi(19)

By substituting xi = x̃i + dxi into above equations and applying Taylor expansion, the following equation is obtained.
Xi = Li + X0 + K2,i Θ

X i = Li(22)

Σii = Σ0 + K2,i ΣΘi KT2,i(23)

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

Consequently, the positioning procedure is summarized asfollow: i) assume the proper initial position of robot i as P̃iand calculate Xi by solving Eqs.(14) and (22), ii) substitute P̃i ← P̃i + Xi , ii) repeat (i) and (ii) until Xi becomes
small enough. The positioning accuracy after repeating theposition identification by CPS can be estimated by calculating the error variance and covariance matrices according toEqs.(15), (16), (23), and (24).
As an example, we substituted the actual measurement
error of the laser range finer explained in Section III (σr =3[mm], σφ = 5[sec.], and σψ = 5[sec.]), and calculated
the positioning error after 1km movement. CPS motion in
Fig.1 is repeated 100 times and the robots moved 10m in
one cycle, respectively. The obtained positioning accuracy
was σx2 + σy2 = 0.0203[m2 ] for the position elements of theerror variance matrix of the parent robot 0, Σ0 . In actual
scenario, due to the measurement error of the body inclination 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 which
utilizes the rotation angle of wheels [6].

(15)

and the covariance matrices between the robots 0 and 1, andthe robots 0 and 2 are obtained as(Σ01 , Σ02 ) = BK1 Σ

(21)

(20)

3

Construction of 3D environmental map bymultiple robots

This section proposes a new measurement system for
precise construction of a 3D environmental map by combining 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 identified
by CPS precisely using a parent and two child robots. Since
there is no need to apply laborious post-processing procedures, the whole picture of the large scale architecture canbe obtained with ease and high accuracy. Firstly, we introduce the fifth CPS machine model named CPS-V, which isequipped with a 2D laser range finder and a scanning mechanism, and show experimental results for the constructionof indoor and outdoor environmental maps by CPS-V.

3.1

The ﬁfth CPS machine model, CPS-V

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

Parent robot
P-cleChild robot 1

Child robot 2

Battery

Celeron
650 MHzDC servomotor(36 W)

Figure 5. Child robot

3.2

2-axisattitude sendor

Total station
AP-L1

Parent mobile
unit, P-cle

Figure 6. 3D laser measurement using rotation table around Yaw axis

AP-L1 (TOPCON Ltd.)Range
4 ∼ 400[m]0.2[mm]Resolution (distance)
5”
Resolution (angle)± 3+2ppm[mm]Precision (distance)±5”
Precision (angle)
Table 1. Specification of total station, AP-L1

Figure 4. Fifth CPS machine model, CPS-V

Corner cubes

Yaw
2D scanninglaser range finder
LMS200Construction experiment of indoor
environmental map

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

In this experiment, each robot moves 4m at a time andthe parent robot captures 3D range images at each static position by rotating the on-board 2D laser range finder aroundthe vertical axis (Fig.7). Obtained range images are transformed to the world coordinate system by simple coordinatetransformation calculation using the measured position by
CPS. No post-processing procedure such as ICP algorithm
is applied.
Path of the parent robot is shown in Fig.8. The parent robot moved up to 39m in x-direction and 10m in ydirection in the hall. Obtained 3D environmental maps areshown in Figs.9 and 10. Circles in the 3D range images indicate the measurement positions of the parent robot. The
parent robot captured the range images 23 times during the
movements in the hall. Total distance traveled by the parent
robot is 86.21m. From these results, it is confirmed that,without post-processing procedures such as ICP, 3D environmental maps in unknown environment can be easily obtained by applying simple coordinate transformation using
position information measured by CPS. The difference ofmeasured positions of the corner indicated in Fig.8 before
and 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 distances
between robots were about 4m, and this value is the measurement limit of the laser range finder as shown in Table

10 m

LMS 200 (SICK Corp.)
Range
80[m]
180◦
Field of view
Resolution (distance) 10[mm]0.5◦Resolution (angle)

Corridor(32.1, -0.6, -0.1)

Table 2. Specification of laser range finder,
LMS200Upstairs(14.5, -6.8, -0.3)
HallxPath of parent robot
Final position(0.5, 1.1, -0.4)

y: Scanning positions ofparent robot

Figure 7. 3D laser measurement using multiple robots

CornerInitial position(0, 0, 0)
Downstairs

Figure 8. Path of parent robot

2. From error measurement experiments of this sensor, it is
known that measurements for short distances tend to contain large distance errors, and the distance error is almost
uniform for the measurement from 10m to 70m. Thus, if
the robots keep larger relative distances between them, andmoves longer distance at one cycle, the accuracy is expected
to be higher than the results obtained by this experiments.
In addition, after the Step (3) in Fig.1, the parent robot can
move, stop, and measure the environment repeatedly while
the child robots keep stationary. This strategy can reduce
the number of CPS cycle and the measurement accuracy
may be greatly improved when the robots work in a large
area.

3.3

Construction experiment of large
scale outdoor environmental map

Next, we performed the construction experiment of 3D
environmental maps of large scale architecture. We measured outward walls of a building from 13 points in outdoor environment and constructed 3D models of the building. Path of the parent robot and obtained maps are shownin Figs.8 and 13. In this experiment, we used small number of measurements from sparse view points, and neither
dense range data nor sufficient overlapped regions, which
are indispensable for applying ICP algorithm, were available. From this experiment, we can conclude that it is possi-

ble to construct 3D maps of simple but large scale architecture by the proposed system with few scanning times. The
positioning error in this experiment is 0.63m against the total travel distance of the parent robot of 147.7m, which is
0.43% of the distance traveled.

4Conclusions

This paper proposed a new 3D measurement system forlarge scale architectures using the precise positioning system by multiple robots named CPS and an on-board laser
range finder. This system needs no post-processing procedures such as ICP algorithm, and can be applied to sparse
range measurements. Therefore, there is a possibility to realize a fully automatic 3D measurement system for large
scale architecture such as large cultural heritages with theproposed system. The measurement experiments in unknown and large indoor/outdoor environments are successfully carried out using the newly developed multiple robots
system named CPS-V.
Our future works include the development of the automatic 3D laser measurement system which selects optimum measurement points automatically instead of operator’s command. The positioning accuracy of CPS can also
be improved by the back-projection technique, that is, theobservation of some stable landmarks before and after the

(a1)

(a2)

(b1)

(b2)

(c1)

(c2)

40m

Figure 9. Obtained 3D map of indoor environment

movements and the correction of each measurement position by back-projecting errors. Combination of CPS withICP method depending on the situations is also worth considering in order to improve the accuracy of both positioning 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 for
3d slam in outoor environment. Proc. IEEE International
Conference 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 on
Virtual System and Multimedia, pages 334–343, November
2004.
[6] R. Kurazume and S. Hirose. An experimental study of a cooperative positioning system. Autonomous Robots, 8(1):43–
52, 2000.
[7] R. Kurazume, S. Nagata, and S. Hirose. Cooperative positioning with multiple robots. Proc. IEEE Int. Conf. on
Robotics 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.

Figure 10. Obtained 3D map of indoor environment[9] A. Nuchter and H. Surmann. 6d slam with an application
in autonomous mine mapping. Proc. IEEE International
Conference on Robotics and Automation, pages 1998–2003,2004.
[10] 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, pages 684–690, April 2004.
[11] J. Weingarten and R. Siegwart. Ekf-based 3d slam for structured environment reconstruction. Proc. IEEE/RSJ International Conference on Intelligent Robots and System,, pages
2089–2094, 2005.
[12] 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, 14:35–41, 2003.

(36.4, -2.5, -0.03)

Benches

Side of thebuilding
Initial position(0.0, 0.0, 0.0)

(4.3, -3.0, -0.03)

Final position(-1.4, 0.7, -0.03)

Rocks
x
y

: P-cle scanning position [m]Figure 11. Path of parent robot
(a1)

(a2)

(b1)

(b2)

(c1)

(c2)

Figure 13. 3D map of buildings in outdoor environment

Figure 12. 3D map of buildings in outdoor environment