2012 12th International Conference on Control, Automation and Systems Oct. 17-21, 2012 in ICC, Jeju Island, Korea

Natural Corners Extraction Algorithm in 2D Unknown Indoor Environment with Laser Sensor Ruijun Yan1, Jing Wu2, Weijun Wang2, Sungjin Lim2, Jiyeong Lee2, Changsoo Han2 1

Department of Mechatronics Engineering, Hanyang University, Ansan, 426-791, Korea (Tel : +82-10-9406-0837; E-mail: [email protected])

Abstract: The algorithm of line extraction and corner extraction in unknown indoor environment with laser sensor is presented in this paper. The corners, the intersection point of two line segments and the two endpoints of each line segment extracted from raw sensor data, can be chosen as landmarks to estimate the position of mobile robot or used to do mapping unknown environment without artificial landmarks intentionally. Sufficient landmarks are the precondition for realizing robot localization and Mapping Environment very well. In addition, the covariance matrix of corners, propagated from covariance matrices of line segments, is derived using the first-order Taylor expansion and it is proved by comparing the uncertainty ellipse drawing from the ellipse of line segments and that from covariance matrix of corners in real experiments. Experiment results of line extraction and corner extraction in the complex environment with the HOKUYO laser sensor located on the Pioneer mobile robot are presented to proving the validity of this algorithm. Keywords:

Line extraction, Corner extraction, Laser sensor, and Unknown environment. Split-and-Merge in [7], [10], Incremental Probabilistic Technique in [11], Hough Transform in [12], and so on. Some experiments is done to comparing with the different line extraction method to find one with best correctness and superior speed in [13], which gives a conclusion that Split-and-Merge and Incremental have best performance. The Split step, dividing all the sensor data into several lines, and Merge part, merging several line segments into one line segment if they belong to the same line, are the main steps in this method with just one input parameters. The principle of realization of line segments extraction is based on Split-and-Merge in this paper with greatly modification and the uncertainty of extracted corner is propagated from the uncertainty of lines, calculated in [14]. There are five sections in this paper. The line extraction algorithm with its covariance matrix and corner extraction algorithm with the propagation of uncertainties are presented in section II and section III respectively. Then the experiment results are explained in section IV to prove the availability of this algorithm. Finally, the last section of this paper shows the conclusion of the whole contents of the paper.

1. INTRODUCTION The landmarks in unknown environment take an important effect in the robot localization, navigation, perception, 2D mapping and 3D mapping. Most experiments, with laser sensor or vision sensor, of robot localization and mapping environment estimate the position of mobile robot using man-made landmarks, like Cup in [1], Green Circle in [2], and other artificial landmarks as in [3], [4]. The reason of using landmark in these applications to correct the position of mobile robot is its low accuracy with only odometry, integrating the incremental information over time to measuring the distance or angle transformed. Nevertheless, not all the natural landmarks can be chosen as landmarks because of its shape complexity or calculation complexity. The landmarks are easy or difficult to be extracted from the capture data is based on the choice of landmarks features, which should be prudently considered because the uncertainties of the landmarks position play an important role in both the simultaneous position of mobile robot and the position of other landmarks. The corners, there are too many in indoor environment, can be chosen as the landmarks without other external instruments or money in the robot localization experiments but now most of them are used in the experiments with vision sensors in [5], [6]. A little of them is realized with sonar or laser sensor but without the clearly calculation of distance uncertainty and angle uncertainty of corner landmarks in [7]. The basis of corner extraction is the line extraction, the essential part of which is achieving the line segmentation from the raw sensor data successfully. In the past several decades, line features, used in robot localization with laser sensor or vision sensor, run well in many experiments in [8], [9]. Also there are too many methods about line extraction, including

978-89-93215-04-5 95560/12/$15 ⓒICROS

2. LINE SEGMENTS EXTRACTION The data from laser sensor are just some points with angle and distance, which must be extracted as line segments because most of the features cannot be identified by just points. Line extraction is the basis for classifying the special geometrical characteristics from the whole environment features. In the following part, the improved line extraction algorithm, including two parts which are line segmentation and line merging, is explained in detail and its covariance matrix is derived step by step. The experiment of this algorithm is stated in the experiment results part.

983

solution for extracting line segments from the set with smaller numbers of sensor point.

2.1 Line Extraction Algorithm The precondition of corners extraction is separating the sensor data into different data sets to extracting the line segments. There are three big steps in Table 1, confining the distance dp between adjacent points si and si+1 from line 1 to line 6, restricting the angle of closed four points from line 7 to line 13 and limiting the distance of every points in the corresponding line segments with extracted line from line 14 to line 20. The maximum distance value dmax, not a constant, in first step is changed dynamically with the different measuring distance of raw sensor data of i-th point, whose value is the addition of 0.3 meter and 0.05 times of the sum of sensor distance si and si+1. If the distance of i-th point and i+1-th point is bigger than dmax, these two points will be separated into two different line segment sets.

Fig. 1 The special case of line segmentation 2.2 Line Merging Algorithm After dividing the raw sensor data into different set and extracting the corresponding line segment equation, the following task is combining the line segments belonging to the same line but segmented by the line extraction algorithm. The line merging algorithm, stated in Table 2, combines the line segment Ln and Ln+1 by finding the angle αmerge of these two lines and the distance dmerge from the begin point of Ln+1 to the end point of Ln. If the distance dmerge is smaller than limit distance dmergMae and αmerge is smaller than the limit angle αmergeMax, the two line segments will be merged and stored by replacing the previous twos. Here, Vline is the set of line segments constructed in the line segmentation part and updated in this merging part.

Table 1 The line extraction algorithm Algorithm 1: Line Segmentation Input: The raw sensor data {si} Output: The line segments vector Vline 1: for si do 2: Find the distance dp of point si and si+1 3: if dp > dmax then 4: Insert the line segment from the next point of end point of last line to point si 5: end if i = i + 1 6: end for 7: for Lk in Vline do 8: for sj in Lk do 9: Calculate angle δα of line (sj, sj+1) and (sj+2, sj+3) 10: if δα < αmax do 11: Insert the new line segment, end if 12: j = j + 1, end for 13: k = k + 1, end for 14: for Lm in Vline do 15: for sn in Lm do 16: Find the distance λ from point sn to Lm 17: if λ < λmax then 18: Insert line segment separated at point sn 19: end if, n = n + 1, end for 20: m = m + 1, end for

Table 2 The Line Merging Algorithm Algorithm 2: Line Merging Input: The line segments vector Vline Output: The line segments vector Vline 1:for Ln in Vline do Calculating the distance dmerge of the end point of Ln and the begin point of Ln+1 and angle αmerge between two line segments 2: If(dmerge < dmergMae and αmerge < αmergeMax) then 3: Merging line segments Ln and Ln+1 4: Insert new segment Ln, delete the old twos 5: else then 6: n = n + 1 7: end if 8:end for 2.3 The Uncertainty of Line Segments The line equation cannot very accurate because uncertainty exists in sensor detecting and sensor rotation. Here assuming that the Sensor-Reading data is Gaussian distributed like: (1) Pi ~ N ( ρi , σ ρ2 ), Qi ~ N (θ i , σ θ2 )

Moreover, the angle between two small line segments, line (sj, sj+1) and line (sj+2, sj+3), is compared with αmax for extracting the corners with special case, for example, as in Fig. 1 explained after 3-th step. In the final step of line extraction, the vertical distance between the points of line segment set and the extracted line of this set will be computed and compared with the maximum distance λmax to check whether this line set should be divided into two sets, which is introduced in [10] as The Split-and-Merge algorithm. But here the limit value λmax in line 17 is chosen as ten percent of the length of the line already extracted. In some case like Fig. 1, if the distance λ is smaller than λmax, the line segments may be extracted as the heavy line and our desired line is the dotted line. Based on this case, the 2-th step is a good

i

i

in which the uncertainty of sensing distance Pi and uncertainty of rotation angle Qi are independent as in [14]. The sum of the square of all errors together with weighted value wi =(1/σρ)2 for all measurement points can be expressed as: 2 (2) S = wd wi (ρi cos(θiκ −α)-r)2 i i =

∑ i

∑ i

The derivative of (2) with respect to variable α and r respectively can obtain the line equation (3) and (4):

984

1 ⎡ 2 ⎢ ∑ wi ρi sin 2θi − w 1 ∑ i α = a tan ⎢ 2 ⎢ w ρ 2 cos 2θ − 1 i ⎢∑ i i ∑ wi ⎣

r=

i

∑ w ρ cos(θ ∑w i

Table 3 The Corner Extraction Algorithm

⎤ sin(θi + θ j ) ⎥ (3) ⎥ ⎥ ρ ρ θ θ + cos( ) w w ∑∑ i j i j i j ⎥ ⎦

∑∑ w w ρ ρ

i

i

j

i

j

−α )

Algorithm 3: Corner Extraction Input: The line segments vector Vline Output: The corners vector Vcor 1:for li do 2: j = i +1 3: Calculate the distance d, angle φ of li and lj 4: If(d< dmin) 5: Insert the two end points of line li, lj in Vcor 6: continue; 7: End If 8: While (d< dmin and φmin<φ< φmax) do 9: Calculate the intersection point of li and lj 10: Calculate the distance dp-1 and dp-2 between the endpoint of two line segments and the extracted corner point 11: If(dp-1 < dpMax and dp-2 < dpMax) 12: Insert the point into Vcor 13: end if 14: j = j + 1 15: Update d and φ 16: end while 16: i = i + 1 17: end for

(4)

i

The uncertainty distribution of distance and angle of HOKUYO laser sensor is revealed as: ⎧ ( ± 30) 2 mm 2 − − − − − −30 → 1000mm ⎫ (5) σ ρ2 = ⎨ ⎬ 2 2 ⎩( ± 0.03* ρ ) mm − −1000 → 8000mm ⎭ 1 (6) σ θ2 = ( *360D /1024)D2 2 in which there are two sections about different calculating principle of distance error with sensing distance from 30 mm to 8000 mm on the basis of instruction book and the variance of angle error in [15]. The covariance matrix of extracted line should be derived for estimating its reliability while the calculation of line equation is completed. All the sensor data in extraction would comply with the range, if not, ignored. The first-order Taylor expansion is used to calculating the covariance matrix of line: (7) Cl = M ρ Cρ M ρT + M θ Cθ M θT including the uncertainty of sensing distance and rotation angle. The first part about laser capture distance is the error matrix Cρ =diag ((σρ)2) signified in line space using the 2 × n Jacobian matrix Mρ: ⎡ ∂α ⎤ ⎡ ∂α ⎤ ⎢ ∂Q ⎥ ⎢ ∂P ⎥ (8) M ρ = ⎢ ⎥ , Mθ = ⎢ ⎥ ⎢ ∂r ⎥ ⎢ ∂r ⎥ ⎢ ∂Q ⎥ ⎣⎢ ∂P ⎦⎥ ⎣ ⎦ Also it is easy to extract the covariance of angle, the second entry in (7) where Cθ =diag ((σθ)2), using the similar 2 × n matrix Mθ, which is ignored in [14]. Finally, the covariance matrix of line can be expressed by the sum of two matrices transformed in line space.

3.2 The Uncertainty of Extracted Corner The corners in indoor environment can be extracted as landmarks for robot localization after the extraction of lines from raw sensor data. The line equations is the combination of (ri, αi) and can be simply written as traditional equation y-ai*x+bi=0 with ai=tan (αi+90), bi=ri*cos(αi)*ai - ri*sin(αi).There are three parameters in the expression of every corner vector, the position xcor, ycor and angle фcor. As we know, the uncertainty exists in these extracted corners. So after extracting corners, the covariance matrix of corners should be propagated from the covariance matrix of two lines. The propagation is a multi-input and multi-output system based on first-order Taylor Expansion in [16]. The covariance matrix Ccor is a 3 × 3 matrix expressed as:

3. CORNER EXTRACTION

⎡ σ l2i σ lil j ⎤ T (9) ⎥ = Fcor Clil j Fcor Ccor = ⎢ 2 ⎢⎣σ li l j σ l j ⎥⎦ Which is coming from the similarity transformation with respect to the 4 × 4 matrix Cl1l2 of li and lj : ⎡ Cli 02×2 ⎤ (10) Clil j = ⎢ ⎥ ⎣⎢02×2 Cl j ⎦⎥ Where the two zero matrices are the covariance matrix of two line segments because of its independence. The Jacobean matrix Fcor is a 3 by 4 matrix:

3.1 Corner Extraction Algorithm There are two parts in the corner extraction algorithm in Table 3, including the end point extraction of each line segment and intersection point extraction of two line segments. First of all, the distance between lj and li is calculated and compared with the limit value dmin for storing the end point of line segments into corners vector Vcor if the condition is satisfied. Then for extract all the intersection corners from raw sensing data, the intersection point between line segments lj and li will be calculated if the distance d is smaller than the corresponding limit value dmin and angle φ in the range from φmin to φmax. The index j increases until the two values, updated in every calculation, out of the range. Also the distance dp-1 and dp-2, should be smaller than the limit value dpMax, is the distance from extracted corner to the two line segments, containing this corner, for checking whether this two line segments are closed to parallel or not.

⎡ xcor ⎤ Fcor = ⎢⎢ ycor ⎥⎥ ∇Tcor ⎢⎣φcor ⎥⎦

985

⎡ ∂x ⎢ cor ⎢ ∂ri ⎢ ∂y = ⎢ cor ⎢ ∂ri ⎢ ∂φ ⎢ cor ⎢⎣ ∂ri

∂xcor ∂α i

∂xcor ∂rj

∂ycor ∂α i

∂ycor ∂rj

∂φcor ∂α i

∂φcor ∂rj

∂xcor ⎤ ⎥ ∂α j ⎥ ∂ycor ⎥ ⎥ ∂α j ⎥ ⎥ ∂φcor ⎥ ∂α j ⎥⎦

(11)

defined by the transpose of the gradient of extracted corner vector. Whereas the gradient is the outer product of the corresponding vector and gradient operator: ⎡∂ ∂ ∂ ∂ ⎤ (12) ∇ cor = ⎢ ⎥ ⎢⎣ ∂ri ∂α i ∂rj ∂α j ⎥⎦

4. EXPERIMENT RESULTS The experiment is done in the first underground floor of Engineering building #5 in Hanyang University, shown in Fig. 2 including the flow chart and pictures of real experiment environment, 60 meters by 12 meters, and the sketch map of motion trajectory, the heavy solid line, of mobile robot in the experiment. This experiment is done to detect whether at least one expected corner can be extracted and to check if the same corners can be found in the continuous transformation of robot. Also the corners are extracted using two different algorithms to testify the superiority of the algorithm explained in Table 3.

Fig.3 The line segments (straight line), extracted corners (pentagon and circle) Left: algorithm in Table 3; Right: corners of li and li+1. The right figure in Fig.3 just has one corner because of the condition stated in Table 3. To the contrary, the left has fourteen corners which are better for the robot localization problem. Furthermore, the number of corners in first 45 captures using the two methods, N-1 and N-2 respectively, is compared in Table 4, proving that many more corners are found by the algorithm presented in our paper. Under this circumstance, more than one discovered corners in the previous sensing can be matched in the following sensing and extraction. The contrasting number of corners using these two methods turn out that the algorithm is improved and more suitable for being chosen as landmarks. Table 4 The number of corners using two methods Index 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15

Fig. 2 The flow chart and real picture of experiment environment and trajectory in experiment (60m*12m) 4.1 Experiments of Line and Corner Extraction The robot localization or SLAM problems can be solved based on the expected corners extracted using our algorithm successfully in all the captures when the robot locates in different positions. There are 108 captures in this experiment and the trajectory of mobile robot is sketched in Fig. 2. Besides, the corners are extracted by two methods, just consider the two line segments li, li+1 and consider all the line segments satisfying the condition in Table 3. The line extraction and corner extraction of raw sensing data in first group using different principle is shown in Fig.3, including the position of mobile robot, extracted corners expressed by pentagon and circle and extracted line segments drawn by lines. The nine pentagonal corners are the endpoints of li ne segment and the five circular corners are the intersection point of two line segments.

N-1 4 5 1 1 2 3 5 3 0 1 0 0 1 2 1

N-2 14 13 11 8 7 10 16 10 5 7 10 8 5 7 13

Index 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30

N-1 2 0 3 0 3 1 2 1 3 6 3 4 6 4 7

N-2 16 11 15 11 15 11 12 13 9 8 9 8 16 10 16

Index 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45

N-1 4 6 4 7 7 6 0 2 3 3 0 2 2 0 5

N-2 16 10 13 15 19 14 6 16 9 9 16 11 11 6 16

4.2 The Uncertainty ellipse of Extracted Corner The uncertainty ellipse of the pentagonal corner, in Fig. 4, is proved by comparing the ellipse drawn by the covariance matrix of this corner and the ellipse derived from the ellipse of the point and line segment. The dashed line in the figure is drawn by hand and the solid line is dawn by the experiment results. There are three solid ellipses and one dashed ellipse, the ellipse of on extracted corner, the ellipse of the line segment, the ellipse of on raw sensing data, from which the perpendicular line to the line segment for extracting the corner and the ellipse similar to the top corner ellipse.

986

[4]

[5]

[6] [7] Fig. 4 The proof of the uncertainty ellipse of second kind of corner

5. CONCLUSION

[8]

In this paper, the algorithms of line extraction and corner extraction is explained and turned out to be successful by the experiment results in the above figures and tables. Nowadays, more and more artificial landmarks are used in the applications grounding on the mobile vehicle but actually these landmarks cannot be put in the expected position in most cases, like environment survey in nuclear area, mobile robot localization in outer space and other conditions people cannot go into the space. So the features in natural environment should be considered much more to finish the corresponding task without other things outside of the workspace. As a result of these reasons, the algorithms of extracting features is improved and designed well in different ways during the recent several decades. The lines and corners are extracted very well in unknown indoor environment using the algorithm presented in this paper, which can be used in all of the experiments done based on any mobile cars.

[9]

[10]

[11]

[12]

ACKNOWLEDGEMENT [13]

This research is part of the Dual-Arm Project which is supported by Korea Ministry of Knowledge Economy (Project NO: 10038660).

REFERENCES [1]

[2]

[3]

[14]

K. S. Choi and S. J. Lee, “Enhanced SLAM fo r a Mobile Robot using Extended Kalman Filt er,” International Journal of Precision Engineer ing and Manufacturing, Vol. 11, No. 2, pp. 25 5-264, April 2010. M. Mata, J. M. Armingol, A. de la Escalera, and M. A. Salics. “A Visual Landmark Recogn ition System for Topological Navigation of Mo bile Robot,” in Proceeding of the 2001 ICRA/I EEE International Conference on Robotics and Automation, Vol. 2, pp. 1124-112, May 2001. G. H. Li, and Z. J. Jiang, “An Artificial Land mark Design Based on Mobile Robot Localizat ion and Navigation,” International conference on Intelligent Computation Technology and Aut

[15]

[16]

987

omation, pp. 588-591, March 2011. S. Y. Lee, and J. B. Song, “Mobile Robot Loc alization using Infrared Light Reflecting Land marks,” International Conference on Control, Automation and Systems, pp. 674-677, 2007. E. Rosten and T. Drummond. “Machine learning for high-speed corner detection,” In Proc. 9th European Conference on Computer Vision (ECCV’06), Graz, May 2006. F. Shen and H. Wang. “Corner detection based on modified Hough transform,” Pattern Recognition Letters, Vol. 23, pp. 1039–1049, June 2002. M.Namoshe1, O.Matsebe1, and N. Tlale1, “Corner Feature Extraction: Techniques for Landmark Based Navigation Systems,” Ciza Thomas (Ed.), ISBN: 978-953-307-101-5, In Tech, August 2010. S.Y. An, J.G. Kang, L.K. Lee, and S.Y. Oh, “SLAM with Salient Line Feature Extraction in Indoor Environments,” International Conference on Control, Automation, Robotics Vision, 2010. N. Bahari, M. Becker, and H. Firouzi, “Feature Based Localization in an Indoor Environment for a Mobile Robot Based on Odometry, Laser, and Panoramic Vision Data,” ABCM Symposium Series in Mechatronics, Vol. 3, pp. 266-275, 2008. G.A. Borges, M. J. Aldon, “A split-and-merge segmentation algorithm for line extractions in 2D range images,” in Proc. 15th International Conference on Pattern Recognition, 2000. E. Brunskill, and N. Roy, “SLAM using incremental probabilistic PCA and dimensionality reduction,” In Proceedings of the IEEE international conference on robotics and automation, 2005. S. T. Pfister, S. I. Roumeliotis, and J. W. Burdick, “Weighted line fitting algorithms for mobile robot map building and efficient data representation,” In Proceedings of the IEEE international conference on robotics and automation, pp. 1304–1311, 2003. V. Nguyen, S. Gächter, A. Martinelli, et al., “A comparison of line extraction algorithms using 2D range data for indoor mobile robotics,” Autonomous Robots, vol. 23, no. 2, pp. 97–111, August 2007. K. O. Arras and R. Y. Siegwart, “Feature extraction and scene interpretation for map-based navigation and map building,” in Proc. SPIE, Mobile Robotics XII, vol. 3210, 1997. J. E. Muelaner, Z. Wang, J. Jamshidi, P. G. Maropoulos, A. M. Mileham, E. B. Hughes, A. B. Forbes, "Study of the uncertainty of angle measurement for a rotary-laser automatic theodolite (R-LAT)", Proceedings of the Institution of Mechanical Engineers, Part B: Journal of Engineering Manufacture, 223 (B3), pp. 217-229, 2009. R. Hartley, “Multiple View Geometry in Computer Vision Second Edition”, Cambridge University Press, pp. 157-163, 2003.