!!mobile robot self localization and 3d

Mobile Robot Self Localization and 3DMap Building using a 3D PMD-Camera for

tele-robotic applications

Jens Bernshausen, Chanin Joochim, Hubert Roth

Institute of Automatic Control EngineeringDepartment of Electrical Engineering and Computer Science.

Hoelderlinstr. 3 57068.({jens.bernshausen, chanin.joochim, hubert.roth}@uni-siegen.de)

University of Siegen,Germany

Abstract: This work purposes using a 3D PMD camera as a sensor for mobile robotapplications. The use of image sensors for environment detection is substantial and commonusage in mobile robot exploration. Laser scanners, 3D stereo vision or a combination of themare be used generally. The PMD camera is a novel 3D measuring system, which is based on atime-of-flight principle. The main feature is an array sensor, which can measure the distance tothe target without scanning. This relatively new technology offers a cost-effective alternative tothe mentioned sensors. In the following we present methods for self-localization and 3D mapbuilding by remote sensor data acquisition. This allows the teleoperative control of a mobilerobot in an unknown environment. To enhance the mobile robot tasks, a CCD and the PMDcamera are registered in order to create the three dimensional mapping. The CCD texture datawill be overlaid with the PMD depth data to increase the accuracy of the lateral resolution.The visual input from the CCD camera not only delivers high resolution texture data, it canalso apply for object recognition in clutter environment.

Keywords: PMD camera, sensor and data fusion, remote sensor data acquisition, mobilerobots, telerobotics, self localization, 3D map building.

1. INTRODUCTION

In the following article methods with the objective of in-tegration of the PMD (photonic mixer device) technologyin teleoperative robotic applications for 3D mapping willbe presented. Because of this the PMD functional principleand the used robot system will be introduced to the readerat first. To generate a 3D environment map with a 3Dvision system mounted at a robot, it is importend to knowthe exact robot position and orientation. On this accountwe will go into methods for precise self localization. Theself localization methods combines informations gettingfrom the wheel encoders of the robot with position datacalculated from the PMD data. If the self localization isrealized, the recording of a coloured 3D map will be dis-cussed. Moreover, the enhancing 3D scenario by combina-tion 2D/PMD camera will be presented. The combinationdata can provide not only the depth data from PMD butalso provide the contour with depth data. This data caneasily adapt to use in 3D geometry application.

1.1 3D PMD-Camera System

The PMD (photonic mixer device) camera offers a costeffective alternative to other 2D or 3D measuring systemslike stereo vision systems or laserscanners in mobile robotapplications. The camera provides to measure distancevalues of a scene for each pixel. The functional principle ofthe camera bases on a ”time-of-flight” measuring system.

An illumination module sends out rectangular modulatedlight in the range of near infrared (wavelength l = 870nm).The Sensor receives the reflected light and mixes it with areference signal, like it is shown in fig.1. The referencesignal has the same modulation frequency fmod as thereceived light. Using a phase shift method an auto cor-relation function between both signals, the received lightand the reference signal can be built. The maximum ofthis function is an admeasurement for the phase shift φ,it depends on the distance of the light to the object andback. The resulting distance d can be calculated by:

d =c · φ

4π · fmod(1)

with the light velocity c.

Fig. 1. Functional principle of a PMD-Camera

Preprints of the 18th IFAC World CongressMilano (Italy) August 28 - September 2, 2011

Copyright by theInternational Federation of Automatic Control (IFAC)

6851

The camera facilitates to get a 3D image of the scene witha framerate of 20 images per second. For the followingapplications we used a [PMDVision]S3 camera of thecompany PMDTec with a lateral resolution of 64x48 pixel.The accuracy of the distance measurement of the choosencamera depends on the reflectivity of the object and can beadjusted to the scene by changing the integration time ofthe camera. The maximum achievable accuracy amountsonly a few millimeter. The integration time is equal tothe illumination time of a conventional camera and it isthe time the system needs for one phase measurement.The measuring range of the camera is dmax = 7.5m anddepends on the modulation frequency fmod of the camera.For more informations about PMD technology we refer toRingbeck (2007)

1.2 Robot System

Fig. 2. Setup of the mobile robot

Fig.2 shows the setup of the mobile robot. As the basisof the robot system serves a electro scooter. This scooteris featured with a path controlling unit, a steering motorand wheel encoder to get the functionality of a mobilerobot. For self localization, and 3D map building duringthe teleoperative control of the robot, it is equipped witha [PMDVision]S3 camera which observes the area in thefront of the robot.

2. ROBOT-CAMERA CALIBRATION

To use the PMD camera mounted at the robot it is neces-sary to know the transformtion matrix RobTCam betweenrobot and camera coordinate system. Only with this infor-mation it is possible to fuse both data which is essential forPMD assisted self localization and map building. Becausethe position of the camera at the robot is fixed it canmeasured at the the mechanic installation. But the orien-tation can be variegated by the user. Because of differentreflectivities of the floor and the measurement range of thecamera it is advisable to adjust the inclination angle of thecamera. Here a automatic calibration algorithm is needful.To calculate the rotational partRobRCam =Cam R−1

Rob =(CamxRob,

Cam yRob,Cam zRob

)(2)

of the transformation matrix the direction vectors of therobot system must be specified with the PMD camera. Forthis the minimum of two vectors have to be determined.Camz

Rob equates to the normal vector of the plane recordedby the PMD camera which represent the floor.

Fig. 3. Calculation of the z-vector of the robot coordinationsystem

Fig.3 shows the plane and the normal vector calculated bythe measuring of the floor position using the PMD camera.The second step to calculate the transformation martixis the determination of the x vector. This is possible bymoving the robot with a steering angle of zero in forwarddirection. During the robot motion a fixed object in frontof the robot must be recognized and its position must becalculated.

Fig. 4. Calculation of the x-vector of the robot coordina-tion system

The principle of this calibration movement is shown in fig.4. Using the equation:

CamyRob =Cam zRob ×Cam xRob (3)

the rotational part can calculated completely.

3. SELF LOCALIZATION

The robot combines the possibilty of self localization usingthe wheel encoder and the localization using the PMDcamera. The localization with the wheel encoder is a wellknown method. Here we want to refer to the appropriateliterature. Using the PMD camera to localize the robotposition offers two possibilities. In this section we describetwo different options. The first option is the localizationwith the recognition of artificial landmarks with a knownposition in the robot environment. The second possibilityis a relative position localization method. Using the ICPalgotithm facilitates the calculation of moving vectorsbetween the recorded images.

3.1 Absolute Position Localization with Artificial Landmarks

The absolute position localization serves on the one handto calculate the start position of the robot. On the otherhand it offers to adjust mismeasurements of the relativeself localization methods.

Fig.5 shows the delineation to calculate the transformationmatrix to adjust the robot position. If the robot detectsthe landmark, the transformation matrix RobTLM can be


6852

Fig. 5. Position cerrection with artificial landmarks

calculated. The measured transformation matrix betweenlandmark and world coordinate system, which equates tothe position and orientation of the landmark is:

WCTLMmeasured=WC TRobrel

·Rob TLM (4)

The real transformation matrix of the landmark is givenin the map is:

WCTLMreal=WC TRobcorr

·Rob TLM (5)

Using equations 4 an 5 the transformation matrix to adjustthe actual robot position and orientation can be calculatedby:

WCTRobcorr =WC TLMreal·Rob T−1

LM (6)

To detect the artificial landmarks with the PMD camerathe grayscale values given by the PMD sensor can be used.The detection occures using the the Haar-Like-Featuremethod, developed by Viola and Jones (2001). To get astrong classifier for detecting the landmark in the PMDimages, more than thousand positive images (images whichinclude the landmark) and negative images (images whichdoesn’t include the landmark) nearly twice the number ofpositive images, are recorded.

Fig. 6. List of Haar-Like Features

Using the Haar-Like-Feature shown in fig.6 integral imagesof the recorded images can be calculated. With the help ofthe AdaBoost algorithm it is possible to get several weakclassifiers which can be cascaded to a strong classifier. Thiscan be used to recognize the landmark in the PMD imageswith 20 frames per second.

Fig.7 shows the result of the recognition algorithm. Thelandmark is indicated by a square. The position of thelandmark is calculated but the orientation is still unknown.Using the Canny algorithm for edge detection, the pixelsat the transition between the black and white regionsof the landmark can be marked. Like it is highlightedin Fig.7. Under consideration of the 3D values of the

Fig. 7. Recognized landmark inside of a PMD image

marked pixel straight lines can be approximated in thepoint clouds using a linear approximation in combinationto the RANSAC algorithm [M. A. Fischler (1981)].

3.2 Relative Position Localization Using the InteractiveClosest Point (ICP) Algorithm

The relative positon localization uses the possibility tocalculate the translation vectors between two in seriallyrecorded PMD images. To get the translation betweenthem, we use the ICP algorithm [Paul J. Besl (1992)]. TheICP algorithm is a iterative algorithm, which allows thecomputation between two 3D point clouds. The algorithmconsists of three steps. At first the next neighbour of eachpoint pi in the first P point cloud to the each point mi

in the second M point cloud will be calculated. Withthe minimization of the quadratic failure between thecorresponding points the transformation matrix betweenthe point clouds can be determined. The second step is tominimize the quadratic cost function:

d(R, t) =1Pp0

Np∑i=1

‖mi −R · pi + t‖2 (7)

The rotational part of the transformation matrix can becalculated using the SVD algorithm[Wall (2003)]. TheSVD algorithm generates the correlation matrix H of thecorresponding points.

H =Np∑i=1

p′

i ·m′Ti =

(Sxx Sxy Sxz

Syx Syy Syz

Szx Szy Szz

)(8)

The matrix elements can calculated by:

Sxx =Np∑i=1

m′

ix · p′

ix (9)

Sxy =Np∑i=1

m′

ix · p′

iy (10)

m′

i and p′

i are the 3D values shifted by the mean valueof the corresponding point cloud. The rotation matrix Rcorresponds to

R = V · UT (11)

The components U and V results from the singular valuedecomposition of the matrix H:

H = UΛV T (12)

The translational part of the transformation matrix is:t = cm −R · cp (13)


6853

cm and cp are the mean values of the corresponding pointclouds. The third step is the registration of the calculatedtransformation matrix to the point clouds. This threesteps will be iteratively used, till the number of maximumiteration is reached or the sum of the quadratic failuresbetween the corresponding points is lower than a definedlimit.

(a) original 3D images (b) shifted 3D images

Fig. 8. Using the ICP algorithm to shift PMD images

Fig.8 shows the result of the implemented ICP algorithm.The translation and orientation can be calculated. Butthe disadvantage of the used method is the calculationperiod. The calculation period amounts approximate 7seconds using a camera with 64x48 pixel and 20 iterationsduring the calculation of the ICP algorithm. To speed upthe calculation we implemented a kd-tree search methodfor the next neighbour search. In combination with aprecalculation, which determines the distinctive pointsonly, it is possible to raise up the calculation period.

(a) original 3D pointclouds

(b) shifted 3D point clouds

Fig. 9. Using the ICP algorithm to shift the precalculatedpoint clouds

With the precalculation and the use of a kd-tree searchalgorithm the calculation period can be decreased to t =60ms, in the example shown in fig.9. Of course the timedepends on the number of iterations and the number ofthe considered points during the calculation of the ICPalgorithm.

3.3 Sensor Fusion

After different methods for self localization of the mobilerobot are implemented a procedure to fuse the differentmeasurements is needed. The aim is to get a more accuratemeasurement as using only one method.

At first both implemented relative position measuringmethods must be fused. The fusion is displayed in fig.10.With the Kalman filter, developed from Kalman (1960),the failure between the wheel encoder data and the imagevectors, calculated by the ICP algorithm, can be estimate.Considering this estimation the relative self localizationcan be corrected. If a landmark is detected it allowsan inference to the real robot position and the assumedposition can be corrected.

Fig. 10. Sensor fusion of absolute and relative self local-ization algorithms

4. 2D/3D CAMERAS COMBINATION

4.1 Camera Combination

The depth data of the PMD camera provides good precisedepth informations. But as a result of the low lateralresolution and the missing colour information, comparedto normal CCD cameras, the recognition of objects inclutter environments is turned out to be difficulty. Indeeda precise object detection is fundamental for 3D mapping.The approach of the described problem is, that the ex-cellent PMD depth data can be enhanced, if the depthinformation is fulfilled by color data from a CCD camera.The 2D/3D combination approach can provides enoughinformation to handle the object detection problem inclutter scenario. The PMD data estimates the distanceand contour information use for observing objects. Thecontour information can be used to identify the objects inthe PMD camera coordinate system. The transformationof the three-dimensional coordinates enhances the systemis applicable in real time operation. Prasad et al. (2006)presented a fundamental approach of enhancement the 3Dvision output. They placed the 2D and 3D camera in a spe-cial structure and assume the field of view of both camerasalmost the same. The real 3D range information with highintensity was generated. Mischler (2007) reconstructed the3D model of stereo and PMD data by using an OpenCVlibrary. This paper uses the hardware setup principles andprojective texture on geometry.

Fig. 11. Camera calibration model

As Fig. 11 denotes that (Qx, Qy, Qz) are coordinatesof point Q in the world space. Assume the object waspointed out by the CCD camera, q(qa, qb) is the projectedcoordinate onto an artificial camera frame, if the original of


6854

the image coordinate does not align on Z image plan axis.Denote (a0, b0) is the center of the artificial frame and fis the focal length of the camera. By using a perspectivetransformation we can define:

f

Z=

a

X=

b

Y

a = a0 +fX

Z(14)

b = b0 +fY

Z

Using homogeneous coordinates, can be performed Q in amatrix.

[ABC

]=

1px

0 a0

01py

b0

0 0 1

[f 0 0 00 f 0 00 0 0 1

] XQ

YQ

ZQ

1

(15)

where a = AC and b = B

C

If we define that fx = fpx

and fy = fpy

are the focallengths in x and y axis respectively, finally a matrix canbe arranged as

U =

[fx 0 0 00 fy 0 00 0 0 1

](16)

Where U is an intrinsic matrix of camera. The projectionof point Q onto the camera screen is

q = U ·Q (17)

Both cameras have to find the intrinsic matrix in the sameway. By this, Bouguet and Jean-Yves (2008) created aMATLAB toolbox, which can be used to find out all ofthe parameters. This method requires, that the cameraobserves a chessboard pattern at different orientations. Af-ter the intrinsic parameters are estimated, the next step isto calculate the transformation between the two cameras.Assume, the object are projected onto the CCD frame andPMD frame with different orientations, the CCD frameis transformed into the PMD frame through a rotationand translation matrix. Denote (xPMD, yPMD, zPMD) arethe coordinate of PMD camera on artificail frame and(xCCD, yCCD, zCCD) are the coordinates of the CCD cam-era frame, the CCD frame can project onto PMD frameusing equation (18). xPMD

yPMD

zPMD

1

= [R, t]

xCCD

yCCD

zCCD

1

(18)

qPMD = [R, t] qCCD (19)

We have to find the intrinsic matrix for both cameras inthe same way. After the intrinsic parameters are estimated,the next step is to calculate the transformation betweenthe two cameras.

The relative vector can be generated from the calibrationmodel. Table 1 shows the R and T which the two cameras

Fig. 12. The output before/after combination (a) 2D image(b) PMD data (c) combination 2D/PMD

Relative vector

Rotation vector(R) [0.05058 0.07940 − 0.01120]

Translation vector(T) [14.31872 63.96501 70.83500]

Table 1. Relative coordinate CCD/PMD

relate to each other. We can see the CCD camera is faraway from the PMD center, approximate 14.31, 63.96 and70.83 mm in the x, y and z axis respectively. The Rotationfrom the PMD center is 0.05058, 0.07940 and -0.01120degrees of roll, pitch and yaw angle respectively. Fig. 12shows the output of CCD and PMD camera before andafter the combination approach. The figure expresses thedifference between the field of view of both cameras. Theprecise camera calibration model of the previous sectionis essential to minimize the error, which may occur fromthe combined hardware setup. Fig. 12 (a) shows the imageoutput from the field of view from 2D camera. Fig. 12(b) shows the depth data from PMD camera in gray scalemode. Fig. 12 (c) shows the output after combination2D and PMD camera. The experiment proves that thecalibration model can register the contour to the depthdata and generates an output, which looks more reliable.The output of the combination is the base image framefor using a PMD camera to detect the object in 3Denvironments and to build a 3D map.

Fig. 13. 3D artificial landmark detection

4.2 Object Detection in A 3D Environment

In an experiment the advantages using a combinationbetween the 3D PMD camera and a normal CCD camerashould be tested. As the object which is obtained to bedetected the artificial landmark is used. After 2D and 3Ddata are registered, the coordinates of both pixels areregistered. It can be assumed that each pixel of CCDand PMD camera are the same. The object detectiontechnique which can be acquired from the CCD camera istransferred to the 3D coordinate system. A proper distancecalibration is absolutely necessary due to the algorithm.It is dependent on an accurate geometric reconstruction.


6855

Fig. 13 shows the acquisition of an artificial landmarkdetection. The artificial landmark was detected in a 3Denvironment. It can be applied for the mobile robotapplications e.g. mobile robot can search the interestedobjects in the unknown environment.

5. 3D MAP GENERATION

This section shows the real time operation of a 3D map-ping. The experiment was tested for generating the threedimensional mapping from data of a combination between2D and 3D cameras.During the recording of the 3D map, the robot moves au-tonomously or teleoperative along a corridor on a specifiedpath. Because of the limited aperture angle of the PMDcamera with less than 40 degrees, the camera is mounted atthe side of the robot. In this way the camera observes thearea on the left hand of the robot and it can be guaranteedthat the wall is inside of the PMD image at any time, whilethe robot is driving through the corridor.The matching algorithms for generating a 3D map out ofthe sequently recorded PMD images based on the ICP al-gorithm[Paul J. Besl (1992)]. To accelerate the proccesingtime an initial transformation based on the described selflocalization algorithm will be executed before running theICP algortihm. This yields to a more precise calculationof the image matchig procedure, arcordingly.

Fig. 14. 3D mapping zoom in a short distance

Fig. 15. 3D mapping at the indoor corridor

The combination of the cameras results in an output thatcan describe the countour and show more understandingdetail of 3D environment to the users, instead use onlyPMD camera. Fig. 14 zooms in the short distance inthe map along the corner around 10 meters. We can seethe countour of the corner and details of that scenario.The coloured map avoids a good view to the user forteleoperative robot control. Fig. 15 shows a long distancetest. The mobile robot moved in the closed loop at aindoor corridor. The shape of corridor is rectangular, thedimension from measurement is around 25×20 meters. Theoutput could also show the map of mobile robot trajectory.

6. CONCLUSION

In the previous sections the integration of the PMD camerafor mobile robot self localization and the 3D map buildingwith a sensor combination of 3D PMD and normal CCDsensor were described. It can be verrified, that the combi-nation of normal wheel encoder and image vectors, whichwere calculated from the PMD images, provides a goodpossibility to determine the relative position of a mobilerobot. The relative self localization fused with the absoluteself localization, using the detection of artificial landmarks,offers an excellent basis for 3D map building. The combi-nation of 2D/3D camera can enhance the performance ofPMD camera. That can apply to use for object detection inthe complex environment and create the three dimensionenvironment. With the colour informations inside of themap, it is easy for a user to interpret the map. Oneprominent problem to use a PMD camera to build a 3Dmapping, is the small field of view. All results could expressonly one side (right or left hand side) because of thisthe camera must turn on one side, otherwise it couldn’tgenerate any surrounding area because of the low field ofview. Methods of resolution of the described problem isto build the rotating mechanical part which can rotatefrom 0-180 degree can probably improve the quality of3D mapping. Another possibility is to use more than onecamera, one in front and one camera at each side of therobot.

REFERENCES

Bouguet and Jean-Yves (2008). Cam-era calibration toolbox for matlab.www.vision.caltech.edu/bouguetj/calibdoc/index.html.

Kalman, R. (1960). A new approach to linear filtering andprediction problems. Transaction of the ASME, Journalof Basic Engineering, 35–45.

M. A. Fischler, R.C.B. (1981). Random sample consensus:A paradigm for model fitting with applications to imageanalysis and automated cartography. Comm. of theACM, 24, 381–395.

Mischler, A. (2007). 3d reconstruction from depth andstereo images for augmented reality applications. In-stitute for Computer Engineering Computer GraphicsGroup.

Paul J. Besl, N.D.M. (1992). A method for registration of3-d shapes. IEEE Transaction on Pattern Analysis andMachine Intelligence, 14, 239–256.

Prasad, T.A., Hartmann, K., Weihs, W., Ghobadi, S.E.,and Sluiter, A. (2006). First steps in enhancing 3d visiontechnique using 2d/3d sensors. Computer Vision WinterWorkshop.

Ringbeck, T. (2007). A 3d time of flight camera for objectdetection. Optical 3D Measurement Techniques, ETHZuerich.

Viola, P. and Jones, M. (2001). Rapid object detectionusing a boosted cascade of simple features. Conferenceon Computer Vision and Pattern Recognition, 511–518.

Wall, Michael E., A.R.L.M.R. (2003). Singular value de-composition and principal component analysis. PracticalApproach to Microarray Data Analysis, 91–109.


6856

!!mobile robot self localization and 3d

Documents