THE INSTITUTE OF ELECTRONICS, INFORMATION AND COMMUNICATION ENGINEERS

IEICE ICDV 2011

A 3D image capture system using a laser range finder for autonomous mobile robots.
HOANG Tran Thuan, VIET Dang Anh and VINH Tran Quang
University of Engineering and Technology (UET), Vietnam National University, Hanoi (VNU) 144 Xuan Thuy Road, Hanoi-VietNam E-mail: thuanhoang@donga.edu.vn
Abstract Laser range finder (LRF) can provide a high resolution and accurate data receipt. This paper presents a 3D image capture system, which is built on a 2D range finder SICK-LMS 221 and controlled by a PID servo. The system is interfaced with a computer running Windows. The electronics and software used for controlling and synchronizing of signal are developed and reported. The system has generated images of good 3D maps and environment surrounding robots. The speed of data acquisition and the quality of the captured image are good enough so that it can be used for mapping in robot navigation. Due to a big data bottleneck in the real-time operation mode, a FPGA solution has been applied for processing such as filtering, segmentation, feature extraction, PID controlling, etc. Keyword Laser range finder, 3D-laser image, 2D-laser scanner, PID controlling, FPGA.

Introduction
An important task of autonomous mobile robot is t o build the map of the environment based on sensory data. Using the co-ordinate data of the surrounding objects in the map, the robotic navigation or other operations can be solved. There are some of distance measuring sensors available which can be used to detect obstacles and path planning of robot. Each sensor has advantages and limitations. The ultrasonic sensor is a low-cost device and has an advantage of producing results faster than other devices. However, ultrasonic range measurements suffer from some fundamental drawbacks which limit the usefulness of these devices in mapping at the indoor environment. In these cases, the accurate result is affected by such phenomena like the beam spread or cross -talk reflection of the sonar beam [1]. Nowadays, video cameras are widely used in mobile robots but the image data is much dependent on the lighting conditions and the surface texture of objects. Moreover, ordinary vision systems can not directly measure such a geometric parameter like distance of the objects. Stereo camera can overcome this problem partly but requires a big computing process with not so high accurate results. An alternative is a laser scanner, or a laser range finder (LRF). One advantage of the laser scanner is that it has the ability to collect distance measurements at a high rate and with high accuracy. Another advantage is that the result is not so much dependent on environmental condition [2][3]. A laser scanner is a sensor that bases on a

time-of-flight measurement principle (Laser Radar). A single laser pulse is emitted out and reflected b y an object surface within the range of the sensor. The lapsed time between emission and reception of the laser pulse is used to calculate the distance between the object and the sensor. By an integrated rotating mirror the laser pulses sweep a radial range in front of the laser scanner so that a 2-dimentional measurement field/area is defined as shown in figure 1 .
3D obst acl e Pl ane of l ase r be am Mobi le robot

Figu re 1 .

2D - La ser bea m sca nning pla ne.

However, because the pitching angle of the scanning plane is fixed, the information of this 2D image may cause problems with overhanging objects as shown in the figure. Here, there are onl y legs of table that could be detected but not its flat surface and base. In this case, a 3D image is necessary [2]. As the 2D scanner is popular and a low-cost, some groups try to build a 3D laser range finder based on the 2D laser scanner. The most popular solution is to use a standard 2D scanner and a scanning mechanical actuator to reach the third dimension. Some of scanning methods have been used, namel y pitching scan , rolling scan , yawing scan , etc [4][6]. In this work, we report a 3D image capture system

Copyright 2011 by IEICE

using the model of pitching scan with an improvement on the mechanical and electronic designs. The detail of building the 3D-laser range finder based on the 2D-laser scanner is presented in section 2. The hardware and software for interfacing between the laser scanner and the computer in order to receive an accuracy data are reported in sections 3 and 4. The experimental results are presented in section 5. Section 6 deals with the idea to use the pipelining technique with a FPGA chip in order to increase the data processing performance of the system.
Figu re 3 . Dra wing of the tu rning mecha nism of the LRFs ba se.

2. Building the 3D-laser range finder

A 2D-laser range finder LMS-221 was used in our system [8]. The LMS has a view angle of 100 , scanned with angular resolutions of 0.25 , 0.5 and 1 . Though our system is also a kind of the pitching scan one but its mechanism is different from that Oliver Wulf et al. [4] and Alastair Harrison et al. [5] used. In their works, the base of LMS is controlled to turn continuously in order to reach a constant pitching angle speed. Consequently, electric cables must be replaced by a slip ringer which is used for the continuous contact of power and signal transfer. This may arise the unstableness for the system due to the problem of electric contact. On the other hand, in our s ystem the base of LMS is designed to turn up and down with a limited angular range as shown in figure 2 so that electric wires can be fixed without using slip rings.

The base of LRF is attached on a steel plane with size of 337 mm 50 mm which is welded to an end of a link. The other end of the link is a joint (ball-bearing) which is attached at the position in a diameter of a 120 turning disk. This position defines the range of scanning angle. As the disk continuously rotates, the base will turn up and down.

Figu re 4 . T he turning mecha nism a nd servo motor.

During the measurement ti me, two value set of the deflect angle of laser beam and distance R are received from LRF. Each data set of one horizontal scan ( , R) is combined with a pitching angle . The pitching speed is defined by experiment. Based on these data, we can define the Cartesian
Fire 2 . Pitching sca n method with tu rning u p a nd down.

coordinates of an image point as follows (Fig.5):

x R cos cos y R cos sin z R sin

These half rotations are called a pitching-up scan or pitching-down one. During the time of pitching scan, the horizontal scanning plane is pitched. The measured distance data create a cloud of measured points, they are distributed on a virtual sphere surrounding the 3D-laser scanner. In our design, the data is acquisited during the time of pitching-up scan, other processes are carried out during the time of pitching-down scan. The difficulty to get the pitching speed stabilization is overcome by using a PID electronic controller which will be described in the next section. Technical drawing of the mechanical system is introduced in figure 3 and the picture of this system is shown in figure 4.
x 0

z
L ase r beam object

Figu re 5 . Definition of the co-ordina te of a 3D point.

The picture of mobile robot is shown in figure 6.

down of the base of LRF), its speed with PID controlling

O mn i ca mer a

is more stable with a variable is onl y approximately 5%.

W ith P I D

LMS -2 21
W ith o u t P I D

PC

Figu re 7 . T he resu lt of the PID control .

The data acquisition from LRF is started at a point of

Elec tr o ni c c o n tr oll ers w hee ls

the beginning time of a pitching rotation. This action is realized b y a trigger electronic circuit. Although the measured time for one scan inside LMS is rather short of 13.32 ms, but the data transfer rate at the output of the instrument is limited by the RS-232 standard of onl y 38,400 baud. The electronic block scheme of system used for pitching and data acquisition is shown in figure 8.

Figu re 6 . Pictu re of the mobile robot with a LRF.

3. The electronics for controlling the pitching scan and data acquisition
In the model of pitching scan, in order to get a linear image in the z-dimension, the pitching speed must be constant. Due to our mechanical system is designed with the pitching up and down that creates an unsymmetrical moving s ystem. During moving, factors like weight and friction of mechanical details may affect to the stable of the pitching speed. In order to overcome this drawback we used an electronic servo drive which guarantees a stable shaft speed of motor. This is a microprocessor-based electronic circuit with an embedded firmware, which permits to control the dc motor speed by a PID algorithm (Proportional -Integral-Derivative

Figu re 8 . T he block scheme of the ele ctronic circu it.

Control). The controlling routine is assigned as the low-level operation in the total program of robot so that the speed of the motor can be controlled independently. Those mean that this action does not appropriate the time of robot operating system. The adjustment of PID controlling coefficients and speed measurement are carried out with K P = 6000, K I = 35 and K D = 20. The speed is defined by counting the number of pulses of an optical decoder which is attached to the motor shaft. In order to get the reality, one measured resulting value is the average of 64 velocity measurements each which takes 5 ms. The stability of system is checked by a measuring program, which is written b y LABVIEW language and the result is shown in figure 7. While the speed of a motor without PID controlling is unstable (especially at the point of time for changing the moving direction up and The flow chart of the typical program used for

4. Development of the program for data acquisition and process from the laser scanner
The program for receiving and processing data from LRF is developed in the programming environment of Microsoft Visual C++. After receiving the command of continuous data output, LRF sends back stream of bytes corresponding to distance measurements at a given angle. The format of transferring frames contains 7 header bytes, 2N data bytes with N is the number of measured points, 2 bytes for CRC error code.
7 He ade r byte s 2 N data byte s 2 CRC bytes

capturing data is as follows:

Star t

Send com m and to s et the m easu rm ent m ode Two le gs Star t co ntin uo us L R F data ou tpu t doo r Parc e inc om ing data str eam fo r st rin g h ead e r bo x

Hea de r Hea fou ndde ?r y

(b)
Paral le d Task Stop con tinu ous LR F d ata ou tp ut

Buffe r LR F dat a p oints in data st ruc tu re i n P C

Proc ess/fil te r/ displ ay/ e valua te L RF dat a

End

Figu re 9 . Flow cha rt for da ta a cqu isition.

The program needs to determine the start of an LRF out data string b y identifying a specific header in the incoming data stream. The 7 bytes string header is different for each measurement mode. (c)
Figu re 10 . T he vision ima ge (a ), 2D -la ser ima ge (b) a nd the 3 -D la ser ima ge (c).

5. Results for 3D-laser image capturing

Figure 10a shows a vision image from a camera for comparison. Figure 10b is a 2D-laser image with a horizontal scan plane at the pitching angle of 0 . Due to the laser scanner is laid at 40 cm of high level to the ground, only two legs of a man can be detected. On the other hand, figure 10c proves the presence of all of the body of the tall man. Some of the 3D-laser image capturing experiments are carried out at the in-door environment with the size of room is approximately 8 m. The number of horizontal scanning line in one image frame (in the pitching angle range from -5 to 20 ) depends on the pitching speed.

The experiment was carried out with the view field of 100 . The result received with two pitching speeds corresponding to the pitching time of 10 s and 42 s is shown in the table below.
Pit chi ng Angular Number of scans time re solu ti o n per 1 i ma g e fra me 81 1 .0 10 s 40 0 .5 24 0 .2 5 320 1 .0 42 s 160 0 .5 100 0 .2 5 Sca ns per secon d 8 .1 4 .0 2 .4 7 .6 3 .8 2 .4

Depending on the application, a specific pitching time (corresponding to pitching speed) is selected. For example, the mode of long pitching time with high angular resolution (42 s / 0.25 ) gives 100 scans/frame. This mode is used for the image capturing in the static environment. In this case, the robot stops at a position and collects data in 42 s in order to build a map of the room. The figure 11a is the vision image picked up from a (a) camera and figure 11b is the 3D-laser image received from data of our system. The result shows that the

accuracy, resolution and linearity are reasonable. Because the laser image point cloud is distributed on a sphere space so that the point density (related to the image resolution) is reduced proportionally to the distance from objects to LRF.

Figu re 13 . 3D -la ser ima ge with a forwa rd moving robot.

5. Pipelining process by FPGA technology

The LRF is used in the mobile robot with several tasks such as: obstacle avoidance, navigation, localization. (a) Common operations present in these tasks are the processes related to filtering, segmentation or feature extraction. With a large amount of data for a 3D-laser image (e.g. 80,000 bytes for a cloud of 40,000 points with a resolution of 100 0.25 ), it would take a lot of time for the completing those processes. Moreover, there are some additional processes such as the PID controlling for the pitching motor and wheel motors. All this operations require to be realized in the real time. In a normal PC-based s ystem, these processes are programmed into two phases in sequenced as shown in figure 14b. In order (b)
Figu re 11. T he vision ima ge (a ) a nd 3 D-la ser ima ge (b).

to reduce the effective processing time of the s ystem, a pipelining technique has been used in our design. In this idea, we used the PC as a buffer for data acquisition from LRF. After that the data were transferred to a second processing unit, which was connected serially with the PC.
( a)
Wit h ou t Pi pe li ne

Applying for the dynamic environment, it is necessar y to use the mode of short pitching time and low angular resolution, e.g. the mode of 10 s/1 , which gives 81 scans/frame. Figure 12 is the result of capturing the 3D-laser image of a moving man while the robot is stopping. Figure 13 is the image picked up in the case of the robot is moving forward with a speed of 0.3 m/s while objects are in static.

P it ch in g tu rn -up

P it ch in g tu rn -down

P it ch in g tu rn -up t ime

Rec e ive #(n ) f ram e (b )

Pro ces s #(n ) f ram e

Rec e ive # (n + 1) f r am e t ime

Tot al tim e fo r tr ans fe r # (n ) f ram e On P C (c ) R ec e ive # (n ) f ram e

#( n ) f r am e tr ans fe r # (n + 1) f r am e tr ans fe r # (n + 2) f r am e

Wit h P i pel in e

R ec e ive # (n + 1) f r am e

R ec e ive # (n + 2) f ram e Rec e ive # (n + 2) f r am e

Rec e ive # (n ) f ram e On s econd nu i t (d )

Rec e ive # (n + 1) f r am e

Pro ces s #(n ) f ram e Tot a l t im e fo r # (n ) f r am e

Pro ces s #(n+ 1) fr am e Tot a l t im e fo r # (n + 1) f ram e

Figu re 14 . T he time slice s: two ha lf-turns of LRF (a ), a ction withou t pipelining (b) with pipelining in PC (c), a nd with Figu re 12 . T he 3 D -la ser ima ge of a moving ma n. pipelining in the secon d processing u nit (d).

While the PC receives data of the imaging frame # (n+1), this second unit processes the data of the image frame # (n). Such pipelining process will increase the effective performance of system as shown in figure 14c and 14d. The block scheme of the pipelining system is sh own in figure 15. The second unit is developed on a soft -core MicroBlaze microprocessor, which is embedded in the Xilinx XC3S500E FPGA chip.

6. Conclusion
In this report, we have presented: a) How to develop a 3D-laser image capturing s ystem with use of a 2D laser scanner. Electronic and mechanical blocks were designed and installed for a mobile robot in the laboratory. The system was als o strengthened by an advanced PID controlling block, which brings to the stabilization of pitching speed s o that the linearity of the z-dimension of image can be received.

LM S 221

PC Pro ces s in g
U ART1 U AR T 2

b) The system provided 3D-laser images with acceptable accuracy and reliability. They can be used for mobile
to wh ee l mo tors

P it ch Moto r

H br id ge

I/O

PI D cont ro l
I/O

robots operations, especially in mapping. c) In order to improve the performance of the system, a pipelining technique has been developed with usin g FPGA chip. The results showed that this is a good solution and should be developed more in the comin g time.

Com m and FP G A

De code r

Figu re 15 . T he system u sing FPGA for pipelining.

The FPGA chip is programmed into 5 blocks: - UART1: receives data from PC with a high baud rate of 115,200 and transfers raw data to the processing block of FPGA. - The processing block: manipulates data and transfers the result to the command block. - The commands, such as a setting pitching speed and wheel speeds, are sent to the PID controlling block and the drive for wheel motors, alternatively. - The PID block controls the setting speed of the pitching motor through the power H-bridge circuit. - UART2 : receives command and transfers it to a controller for wheel motors of the mobile robot. The VHDL source code is s ynthesized on Xilinx ISE 10.1 by Xilinx XST tool, the result as follows:
Used Nu m. o f sli d es Nu m. o f slid e Fli p -Fl o p N u m. o f 4 i npu t L UTs Num. of bonded IOB s Num. of BR AM Num. of GCL Ks 2092 2363 3218 42 4 4 Av a i l a b l e 4650 9312 9312 232 20 24 % used 44% 25% 34% 18% 20% 16%

References
[1] Zi-xing Cai, Jin-xia Yu, Zhuo-hua Duan, Xiao-bin g Zou1Oliver, Bernardo Wagner, Design of the 3D perceptive system based on laser scanner for a mobile robot, IJCSNS International Journal of Computer Science and Network Security, VOL.6 No.3B, March 2006. [2] Hartmut Surmann, Kai Lingemann, Andreas Nuchter and Joachim Hertzberg, A 3D laser range finder for autonomous mobile robots, Proceedings of the 32nd ISR(International Symposium on Robotics), pp. 153 - 158, 19-21 April 2001. [3] Andrzej TypiakHartmut, Use of lasser rangerfinder to detecting in surroundings of mobile robot the obstacles, The 25 th International S ymposium on Automation and Robotics in Construction, June 26-29, 2008. [4] Oliver Wulf, Bernardo Wagner, Fast 3d scannin g methods for laser measurement systemsume, Institute for S ystems Engineering, University of Hannover, German y, 2004. [5] Alastair Harrion and Paul Newman, High Quality 3D Laser Ranging Under General Vehicle Motion, Robotics Research Group, University of Oxford, 2007. [6] Cang Ye and Johann Borenstein , A new terrain mapping method for mobile robots obstacle negotiation, Advanced Technologies Lab The University of Michigan, 2002 [7] Aiwu Zhang, Shaoxing Hu, Yulin Chen, Haiyun Liu, Fan Yang, Jia Liu, Fast continuous 360 degree color 3d laser scanner, The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences. Vol. XXXVII. Part B1. Beijing 2008. [8] Sick AG. : 2006-08-01 Telegrams for Operating/ Configuring the LMS 2xx (Firmware Version V2.30/X1.27), www.sick.com , Germany.

The pipelining processing system is installed for a robot in our laboratory. The operating status has been surveyed and measured. The result shows that the systems performance is 1.5 times higher than usual.