J. Cent. South Univ. Technol. (2009) 16: 0807-0814

DOI: 10.1007/s11771-009-0134-z

A new optimal approach to segmentation of 2D range scans to line sections

ZHANG Liang(张 亮), JIANG Rong-xin(蒋荣欣), CHEN Yao-wu(陈耀武)

(Institute of Advanced Digital Technologies and Instrumentation, Zhejiang University,

Hangzhou 310027, China)

Abstract: In order to obtain a compact and exact representation of 2D range scans, UKF (unscented Kalman filter) and CDKF (central difference Kalman filter) were proposed for extracting the breakpoint of the laser data. Line extraction was performed in every continuous breakpoint region by detecting the optimal angle and the optimal distance in polar coordinates, and every breakpoint area was constructed with two points. As a proof to the method, an experiment was performed by a mobile robot equipped with one SICK laser rangefinder, and the results of UKF/CDKF in breakpoint detection and line extraction were compared with those of the EKF (extended Kalman filter). The results show that the exact geometry of the raw laser data of the environments can be obtained by segmented raw measurements (combining the proposed breakpoint detection approach with the line extraction method), and method UKF is the best one compared with CDKF and EKF.

Key words: line extraction; breakpoint detection; unscented Kalman filter; central difference Kalman filter; extended Kalman filter

1 Introduction

Obtaining abstractions over raw measurements is an important capability of a mobile robot in navigation and localization. A precise estimation of the position always serves as the heart in any navigation system, such as localization, dynamic map building, and path planning. It is well known that solely using the data from odometry is not sufficient since the odometry provides unbounded position errors. 2D laser rangefinders are capable of sampling local environment contour with a reasonable resolution and accuracy. For example, laser scanners have been used in localization [1-2], dynamic map building [3-4] and collision avoidance, etc.

The primary issue is how to accurately match sensed data against information that has been collected. There are two common ways that have been used in mobile robotics: point-based matching and feature-based matching. Feature-based method first transforms the raw data into geometric features. Then these extracted features are used in the matching at the following step.

Among many geometric primitives, line segment is the simplest one. It is easy to describe most of the indoor environments using line segments. Many algorithms were proposed using line features from 2D range data: a line segmentation method inspired from an algorithm in computer vision, a dynamic map building algorithm based on geometrical features using a laser scanner [5], a technique for acquisition and tracking of the pose of a mobile robot with a laser scanner [1], and a line extraction algorithm using weighted line fitting for line-based map building [6]. For breakpoint detection, a method called adaptive breakpoint detector in which a KF-based method uses EKF (extended Kalman filter) as the breakpoint detection was introduced [7], the breakpoint was detected by the defined threshold BA [8], and data segmentation method was summarized [9]. Many other line extraction methods were proposed [10-11]. An attempt was made to compare all of these line extraction methods [12].

In this work, line extraction was broken into two steps: first, finding the breakpoint from the raw measurement by UKF (unscented Kalman filter), CDKF (central difference Kalman filter) and EKF, and second, in every continuous breakpoint region, constructing the optimal angle and optimal distance, and then using them to build the geometry of the environment. The proposed UKF method can detect information more precisely than the EKF, and the optimal angle and distance are more robust and accurate in detecting the line of the raw data from the breakpoint.

2 Breakpoint

Fig.1 describes the relationship between the

Fig.1 Relationship between successive data

successive range readings when a planner surface is scanned by laser rangefinder. The relationship among every three continuous points is given by Eq.(1) [13], and Eqs.(2) and (3) are used as the process model. In breakpoint detection process, χ² distribution is used. The measurement residual and its covariance can construct an expression that obeys χ² distribution.

 (i=1, 2, …, N)            (1)

where  d_i­, d_i+1 and d_i+2 are the continuous three points of the laser rangefinder, and γ is the resolution of the laser.

2.1 Process model

According to Eq.(1), the process model can be constructed as follows:

x₁(k+1)=x₂(k)                                (2)

x₂(k+1)=                   (3)

where  x₂(k) and x₁(k) are the state variables. The equation can be fully defined by the state space equation:

x(k+1)=f(x(k))+v(k)                           (4)

x(k+1)=[x₁(k+1)  x₂(k+1)]^T                     (5)

And

               (6)

And v(k) is the white noise with a given small variance Q(k) added to the process model.

The estimated value of the process mode is

_{x(k+1)-=Hkx(k)                               (7)}

The covariance of the process model can be defined as

                     (8)

where  H_k is the Jacobian matrix of function f and the covariance can be viewed as the uncertainty of the process model estimation as compared with the final optimal estimation.

2.2 Observation model

Since there is no other control input for the laser rangefinder, only the measurement data are considered. The observation model can be represented as

_{z(k+1)=h(x(k+1))+w(k+1)                      (9)}

where  h(x(k+1))=x₂(k+1), and w(k+1) is a zero means Gaussian noise with a known variance

2.3 EKF method

EKF was used for breakpoint detection in Ref.[14]. The breakpoint was detected by the χ² distribution which was constructed by measurement residual and its covariance. Measurement residual in EKF is defined as

                    (10)

This residual value represents how much a predicted measurement differs from the real value that has been captured by operational method. It can also be regarded as the error of the measurement process estimation. In the final optimal estimation, this value can be used to revise the process predicted value in order to get the optimal value of the state.

The covariance of measurement model is

          (11)

The measurement covariance can be viewed as the degree of the spread around the real value, and the residual value complies with the Gaussian distribution whose mean and covariance are expressed by Eqs.(10) and (11), respectively. From the view of the probability distribution, the larger the covariance is, the less the residual value will reflect on the optimal measurement value. In the estimation of optimal measurement value, if more uncertainty of the measurement model was observed as compared with the process uncertainty, the final estimation will greatly depend on the process  value. Otherwise, it will depend mainly on the residual value.

In order to detect breakpoint from the raw data, all points are assumed to belong to the same line in the application. Then in the process and measurement model estimation, if there was a breakpoint, there would be a remarkable variety in Eq.(10). In order to detect the breakpoint, Eqs.(10) and (11) are used to construct the χ² distribution which is defined as

c(k+1)=v(k+1)^TS^-1(k+1)v(k+1)                  (12)

According to the χ² distribution, a validation gate δ can be used to decide whether the measure point is the breakpoint of the line segment or not. The validation gate of δ=9 [15] was chosen in this application.

2.4 UKF method

UKF is chosen as the method for breakpoint detection because it approximates better than EKF in nonlinear systems. EKF only uses the first order term of the Taylor series expansion of the nonlinear system, so it often introduces large errors in the estimated statistics of the posterior distributions of the states when the nonlinearity is very severe. Unlike EKF, UKF does not explicitly approximate the nonlinear process through Taylor series expansion. In UKF, the state distribution is specified using a minimal set of deterministically chosen sample points that are called sigma-points. The sigma-points completely capture the true mean and variance of the system. When propagated through the true nonlinear system, it can capture the true mean and variance to the 2nd order for any nonlinear systems, which has been proved in Refs.[16-17].

When UKF is used to get breakpoint of the measurement (the measurement residual and measurement model covariance are used to construct the χ² distribution), the covariance in UKF measurement model is defined as follows:

  (13)

where  z_i,k+1|k is the result of the sigma point that passes through the process model and the observation model.  It is defined as

                          (14)

The mean coefficient and covariance coefficient are

                         (15)

 i=1, …, 2L              (16)

where  κ is scaling parameter, and L is the number of sigma points which not only contain the model’s dimension but also the noise’s dimension. In the application, L_process and L_observation are as follows:

L_process=L_{process_dim_in}+V_{dim_in}                     (17)

L_observation=L_{observation_dim_in}+W_{dim_in}                (18)

In UKF, the measurement residual value and covariance represent the same meaning as those in EKF, so Eq.(12) can be used as the approach to segment the laser data.

For some special case where the process and observation noise are purely additive, which means that there is no relationship between system noise and system state, then the dimension of the sigma points can be reduced in these conditions as follows:

L_process=L_{process_dim_in}                           (19)

L_observation=L_{observation_dim_in}                      (20)

In measurement predicted value and covariance computing, the noise parameters should be added as follows:

                    (21)

(22)

where  L_S is the dimension of the input variable to measurement process, P_mean is the mean of the measurement process noise, and  is its covariance.

2.5 CDKF method

CDKF is another method for nonlinear estimation based on Sterling’s polynomial interpolation formula. This formula is the basis of Norgaard’s derivation [18] of the divided difference filter as well as Ito’s central difference filter [19]. The essential Sterling’s polynomial interpolation formulas are Eqs.(23) and (24):

                         (23)

                        (24)

Like UKF, CDKF can also capture the true mean and covariance to the 2nd order for any nonlinear system. In CDKF computing model, the measurement covariance is depicted as

           (25)

where   and  are

  i=1, …, L_x

   i=1, …, L_n.

And the predicted mean  is

     (26)

In some conditions, CDKF process and observation noise are purely additive, the dimension of sigma points and the computing complexity can be reduced to  Eqs.(27) and (28), respectively:

      (27)

 (28)

Based on the same meaning about the measurement residual value and its covariance, Eq.(12) can be used as the principle of the segmentation for the raw data.

3 Line extraction

3.1 Optimal angle

The laser rangefinder data captured by the robot are as follows:

{D_i}=<ρ_ij, α+λj>, j=1, …, N                   (29)

In application α is the start angle of the laser scan, λ is the resolution of the laser scan, and N is the number points of the laser scan.

After getting the breakpoint, the line parameters of every breakpoint area should be constructed. By defining the line parameters from its optimal angle and the distance from zero point to the line, Fig.2 shows the relationship between the optimal angel and the raw data point.

Supposing the following information about every

Fig.2 Relationship between optimal angle and data points

breakpoint region:

(1) K points are included.

(2) The optimal angle of this line is α_i.

(3) The data set of the ith breakpoint is

{D_i}=<ρ_ij, θ_i,j>,  j=1, …, K

The optimal angle α_i to every data point’s angle difference is defined as follows:

{d_{angle, i, j}}=<α_i-θ_i,j>,  j=1, …, K                (30)

Every data point on the optimal angle α_i’s mapping distance can be viewed as

{D_{mapping, i, j}}=<ρ_ij, cos(α_i-θ_{i, j})>,  j=1, …, K       (31)

If data point set Eq.(30) lies on the same line, the mapping distance in Eq.(31) should be the same. Then, the difference between every point’s mapping distances would be used as the principle to determine the optimal angle. Thus, the total value can be defined as

   (32)

In order to achieve the minimum value of Eq.(32), the partial derivative of function J_{sum, i, mapping} about the variable α_i can be used to obtain

                                      (33)

3.2 Optimal distance

After the optimal angle is obtained, the optimal α_i is used to get the optimal distance that is the distance from zero point to the optimal line of this breakpoint area.

Fig.3 illustrates the method to get the optimal distance when the optimal angle α_i is computed.

Fig.3 Relationship between optimal distance and data points

In Fig.3, the following items are defined.

(1) {L_i}(i=1, …, K) is the line of every data point based on the optimal angle α_i.

(2) {d_i}(i=1, …, K) is the distance of every L_i to zero point.

Based on the optimal angle, the optimal distance is defined as the average of distance d_i, which is defined  as

               (34)

where  α_i is the optimal angle computed by Eq.(33), and K is the number count of this breakpoint region.

3.3 Line construction

After the optimal angle and distance of the breakpoint area are derived, the line information is depicted as

{d_{i, optimal}, α_i}, i=1, …, B                       (35)

where  B is the total number of breakpoint regions.

Combing the raw data of breakpoint area with the optimal distance and angle, the information about every breakpoint area can also be described as follows:

{Pixel_i,start, Pixel_{i, end}}                         (36)

where  Pixel_i,start and Pixel_i,end are the start and end points in Cartesian coordinate of the optimal line.

As a result, all breakpoint areas can be described as

{Pixel_i,start, Pixel_i,end}, i=1, …, B               (37)

Based on the result depicted by Eq.(37), the geometry of the raw map can be constructed.

From the above, it is clearly seen that the more the number of the breakpoint area is, the more time and resources will be used, and the more precise the geometry will be.

4 Experiments

In experiment, first, SICK LMS200 laser rangefinder was used to capture the raw data of laboratory environment. Then, EKF, UKF and CDKF were used to get the breakpoint of the raw measurement and the results were analyzed. Finally, the method mentioned in section 3 was used to obtain the line parameters of every breakpoint region, and construct the geometry of the raw data. The geometry was also compared with the raw measurement data.

The settings of SICK LMS200 used were: α=40?, λ=0.25?, N=401.

4.1 Breakpoint result

After capturing the raw data, the process model and measurement model for the data point like Eqs.(4) and  (9) can be built. In order to reduce the computational complexity in UKF and CDKF, the process and measurement model noise are assumed to be additive. Thus, in computing the predicted value, Eqs.(21) and  (27) are used and, and Eqs.(22) and (28) are applied to computing covariance.

The number of the breakpoints in every method is shown in Table 1.

Table 1 Comparison of breakpoint count

From Table 1, different methods detect different breakpoint numbers. UKF detects the most numbers as compared to CDKF and EKF.

Fig.4(a) shows the χ² values acquired through EKF method, in which the measurement residual and covariance for every point are computed. The points whose χ² value is equal to or larger than validation gate 9 will be regarded as the breakpoint.

Fig.4(b) gives the result of method UKF on the χ² distribution value of every data point. The validation gate value used to detect breakpoint is the same as that of EKF. It is clearly seen that in the final data points 300-400, there are more breakpoints as compared with EKF method. For many non-breakpoints, the χ² values are smaller than those in the EKF results.

Fig.4(c) displays the result of method CDKF, which is used to get the χ² value of every data point. The breakpoint positions are different from the previous two methods in many points. This implies that perhaps, there is a more logical choice among CDKF, UKF and EKF.

Fig.5 gives the comparison between these three methods’ breakpoint position. It is easily seen that the UKF and EKF breakpoint positions are similar, but CDKF’s breakpoint positions are different from those of UKF and EKF.

4.2 Line extraction result

For every breakpoint area, the line parameter angle and distance are computed by the methods described in section 3. Then, by using the parameters, the geometry is constructed and compared with the raw data. The results are shown in Fig.6, where X axis value and Y axis value denote the positions in Cartesian coordinate.

Fig.6(a) describes the geometry computed by EKF breakpoints compared with the raw measurement data. The differences between these two are very small.

Fig.6(b) displays the result of geometry acquired by UKF breakpoints as compared to the raw data. Besides a few notable differences in the position around zero, the similarities are very good.

Fig.6(c) presents the result of geometry constructed

Fig.4 χ² distribution values acquired through different methods: (a) EKF; (b) UKF; (c) CDKF

Fig.5 Breakpoint position comparison through different methods

Fig.6 Raw data and breakpoint line comparison through different methods: (a) EKF; (b) UKF; (c) CDKF

by the breakpoint detected through CDKF. It can be seen that there are more notable differences as compared with the raw data.

To differentiate the comparison in further clarity, the raw measurement data are split into three sections according to their X axis values in Figs.6(a)-(c):

(1) Section 1: X axis values between 0 and 10 m;

(2) Section 2: X axis values between -1 and 0 m;

(3) Section 3: X axis values between -30 and -1 m.

There are 401 data point numbers in this work.

Fig.7 represents the difference values compared with the raw data when the raw measurement point’s X coordinate position is in the region between 0 and 10 m. The differences for UKF are quite smaller than those for the other two.

Fig.7 Difference value comparison in section 1

Fig.8 displays the difference values compared with the raw data when the raw measurement point’s X coordinate position is between -1 and 0 m. It can be seen that EKF in this area has the best results.

Fig.9 displays the differences between the three

Fig.8 Difference value comparison in section 2

Fig.9 Difference value comparison in section 3

methods output with the raw data when X coordinate position of the raw measurement data is in the region between -30 and -1 m. In this area, all the three methods have smaller differences (Y difference value) and the UKF is a bit more favorable.

Table 2 gives the summary of the comparison in the three sections.

Table 2 Comparison between each section

As seen from Table 2, it can be seen that the UKF’s in section 1, the EKF’s in section 2, and the UKF’s in section 3 show better match when they are compared them with the raw data.

4.3 Result analysis

From the breakpoint results, the numbers of breakpoints in UKF are more than those of the other two methods, and the distribution of the breakpoint differs between EKF and CDKF, also between UKF and CDKF.

Form the constructed geometry by line extraction, the EKF and the UKF methods are more accurate than the CDKF.

For the nonlinear system approximation in EKF, the Jacobian coefficients must be computed for every variable in every step, and it only approximates to the 1st order of the nonlinear system in computing the predicted value and its covariance. There is no need to compute the Jacobian coefficient in UKF or CDKF that can also approximate the predicted values more accurately up to the 2nd or high orders.

Form all these analyses above, it can be concluded that the UKF method is the best to obtain the breakpoint and construct the best geometry as compared with the raw measurement.

5 Conclusions

(1) Two new methods, UKF and CDKF are proposed for breakpoint detection from the raw laser  data. The breakpoint position depends on the value of χ² distribution which is constructed by the residual value and residual covariance of the measurement model.

(2) A new method for line construction to form the geometry of the environments is proposed. In this method, the optimal distance and optimal angle are computed in polar coordinate. In the optimal angle finding step, the difference between mapping distances of every point at the optimal angle is used; and in the optimal distance finding step, only the average distance is used. The final geometry is composed of two points of every breakpoint area.

(3) The raw data is captured by the laser scan SICK LMS200. The breakpoint number detected by method UKF is more than that by CDKF and EKF, so the constructed line’s precision and similarity are better compared with those of CDKF and EKF. From the comparisons of the constructed geometries with the raw data, it can be seen that this breakpoint detection method and line construction method are feasible and effectual.

References

[1] JENSFELF P, CHRISTERNSEN H I. Laser based position acquisition and tracking in and indoor environment [C]// Proceedings of the IEEE International Symposium on Robotics and Automation. Saltillo, 1998: 331-338.

[2] FANG Zheng, TONG Guo-feng, XU Xin-he. A robust and efficient algorithm for mobile robot localization [J]. Acta Automatica Sinica, 2007, 33(1): 48-53. (in Chinese)

[3] STRACK A, FERREIN A, LAKEMEYER G. Laser-based localization with sparse landmarks [C]// Proc RoboCup Symposium. Osaka: Springer Press, 2005: 569-576.

[4] ZHANG Yong-shun, GUO Rui, LIU Yu, JIA Zhen-yuan, GUO Dong-min. On-line locating method on micro swimming robot inside pipe [J]. Journal of Harbin Institute of Technology, 2004, 36(10): 1382-1384. (in Chinese)

[5] VANDORPE J, van BRUSSEL H, XU H. Exact dynamic map building for a mobile robot using geometrical primitives produced by a 2D range finder [C]// Proceedings of the IEEE International Conference on Robotics and Automation. Minneapolis, 1996: 901-908.

[6] PFISTER S T, ROUMELIOTIS S I, BURDICK J W. Weighted line fitting algorithms for mobile robot map building and efficient data representation [C]// Proceedings of the IEEE International Conference on Robotics and Automation. Taipei, 2003: 1304-1311.

[7] BORGES G A, ALDON M J. Line extraction in 2D range images for mobile robotics [J]. Journal of Intelligent and Robotic Systems, 2004, 40(3): 267-297.

[8] HARATI A, SIEGWART R. A new approach to segmentation of 2D range scans into linear regions [C]// Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. San Diego, 2007: 2083-2088.

[9] PREMEBIDA C, NUNES U. Segmentation and geometric primitives extraction from 2D laser range data for mobile robot application [C]// Proceedings of 5th National Festival of Robotics. Coimbra, 2005.

[10] FORSYTH D A, PONCE J. Computer vision: A modern approach [M]. London: Prentice Hall, 2003.

[11] ARRAS K O, SIEGWART Y. Feature extraction and scene interpretation for map-based navigation and map building [C]// Proceedings of the SPIE, Mobile Robotics XII. Pittsburgh, 1997: 42-53.

[12] NGUYEN V, MARTINELLI A, TOMATIS N, SIEGWART R. A comparison of line extraction algorithms using 2D laser rangefinder for indoor mobile robotics [C]// Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Edmonton, 2005: 929-1934.

[13] ADAMAS M D, KERSTENS A. Tracking naturally occurring indoor features in 2D and 3D with lidar range/amplitude data [J]. International Journal of Robotics Research, 1998, 17(9): 907-923.

[14] BORGES G A, ALDON M J. Line extraction in 2D range images for mobile robotics [J]. Journal of Intelligent and Robotic Systems, 2004, 40(3): 267-297.

[15] ZHANG S, XIE L, ADAMS M, TANG F. Geometrical feature extraction using 2D range scanner [C]// Proceedings of IEEE International Conference on Control and Automation. Montreal, 2003: 901-905.

[16] JULIER S J. The scaled unscented transformation [C]// Proceedings of the American Control Conference. Anchorage, 2002: 4555-4559.

[17] JULIER S J, UHLMANN J, DURRANT-WHYTE H F. A new method for the nonlinear transformation of means and covariances in filters and estimators [J]. IEEE transactions on Automatic Control, 2000, 45(3): 477-482.

[18] NORGAARD M, POULSEN N, RAVN O. New developments in state estimation for nonlinear systems [J]. Automatica, 2000, 36(11): 1627-1638.

[19] ITO K, XIONG K. Gaussian filters for nonlinear filtering problems [J]. IEEE Transactions on Automatic Control, 2000, 45(5): 910-927.

(Edited by YANG You-ping)

                      

Foundation item: Project(2003AA1Z2130) supported by the National High-Tech Research and Development Program of China; Project(2005C11001-02) supported by the Sci­ence and Technology Project of Zhejiang Province, China

Received date: 2008-11-27; Accepted date: 2009-03-12

Corresponding author: CHEN Yao-wu, Professor, PhD; Tel: +86-13605813934; E-mail: cyw@mail.bme.zju.edu.cn