CN103777192B - A kind of extraction of straight line method based on laser sensor - Google Patents

Abstract

The present invention relates to a kind of laser image extraction of straight line method.Segmentation with merge method on the basis of, raising method efficiency is detected adjacent to point set by self adaptation, utilize fuzzy partition with merge method carry out line segment segmentation the ameliorative way sensitivity to parameter, finally utilizing the parameter that least square method simulates in environment under each straightway polar coordinate system, method includes: neighbouring point set detection, line segment segmentation, least squares line fitting, the big step of calculating fitting a straight line error four.Method result of implementation shows when the point that straightway is comprised is more, and result is very accurate, and most straight line errors are the least, and footpath, pole error is both less than 1mm, and angular error is less than 0.01rad, and fitting effect is preferable.The advantage that the present invention compares compared with traditional line extracting method is: the iterations of (1) method reduces, and improves method efficiency；(2) robustness of method improves a lot；(3) precision of line segments extraction also significantly improves.

Description

Linear feature extraction method based on laser sensor

Technical Field

The invention belongs to the technical field of artificial intelligence, and relates to a laser image linear feature extraction method.

Background

With the rapid development of computer technology and artificial intelligence technology, the mobile robot technology has achieved huge achievements, and is widely applied to the fields of home service, space exploration, deep sea salvage, mineral exploration, security medical treatment, military reconnaissance and the like. In the related art research of robots, the problem of Simultaneous Localization and mapping (SLAM) of robots is one of the key problems of the robot technology, and is even known as a "holy cup" in the research field of mobile robots. The straight line is one of the most common feature descriptions in the robot SLAM problem because selecting it as an environmental feature for robot localization has many advantages, such as: straight lines exist in indoor environments comparatively much; the straight line is easy to extract from the original point detected by laser test system LMS (laser Measurement systems) laser radar, and the straight line feature is easy to store in a map; the robot can be positioned by only using two non-parallel straight lines in a plane coordinate system.

At present, methods related to straight line extraction are more, and a Split-and-Merge (Split-and-Merge) method is the best method for comprehensive performance. Thus, has been widely used. However, the traditional segmentation and combination method has the problems of poor robustness to the linear parameters and low efficiency.

Disclosure of Invention

The technical problem is as follows: in order to solve the above problems, an object of the present invention is to provide an adaptive line extraction method based on a laser sensor, so as to improve the efficiency and robustness of line extraction.

The technical solution of the present invention is now described as follows:

the invention relates to a linear characteristic extraction method based on a laser sensor, which improves the efficiency of the method through self-adaptive adjacent point set detection on the basis of a segmentation and combination method, improves the sensitivity of the method to parameters by using a fuzzy segmentation and combination SMF (split-and-merge fuzzy) method to segment segmentation, and finally fits the parameters under each linear polar coordinate system in the environment by using a least square method, and comprises the following steps (see figure 2):

step 1: detecting a neighboring point set;

step 2: segment division;

and step 3: fitting a least square straight line;

and 4, step 4: and calculating the fitting error of the straight line.

The invention further provides a linear feature extraction method based on the laser sensor, which is characterized by comprising the following steps: setting the discrete points of the angle phi and the distance rho at intervals of the angle delta phi as the scanning result of the laser radar; the specific process of detecting the neighboring point set in the step 1 is as follows:

step 1.1: let the continuous laser sensor detect two continuous points as s_k-1And s_kWherein s is_k-1Laser beamDistance of sensor is rho_k-1Δ l is the distance between two points;

step 1.2: the constant m ' is 5, and the constant Δ l ' is m ' ρ_k-1And delta phi, if delta l is less than or equal to delta l', the adjacent point set is obtained, otherwise, the adjacent point set is not obtained.

The invention further provides a linear feature extraction method based on the laser sensor, which is characterized by comprising the following steps: the specific process of segment segmentation described in step 2 is as follows (see fig. 3):

step 2.1: let each set of neighboring points contain only one straight line: including all points in the sensor observation data z, the initial straight line numberSetting the generated iteration algebra m to be 1;

step 2.2: updating the number of the mth generation straight line

Step 2.3: inspecting each straight line l_k，Whether the variance σ of the straight line parameter is satisfied in the set of neighboring points_k≤σ_maxAnd if not, then:

step 2.3.1: dividing l by fuzzy straight line_kPoint in (b) becomes two straight lines l'_kAnd l ″)_kSimultaneously, making C equal to 2;

step 2.3.2: from l'_kAnd l ″)_kReplacement of l_kAdding to the mixture;

step 2.3.3:$N_L^m=N_L^m+1;$

step 2.3.4: go to step 2.2;

step 2.4: selecting straight lines

Step 2.4.1: for every two distinct straight lines l_aAnd l_bAre all considered as candidates for merging;

step 2.4.2: judging whether the coordinate barycenters of the two straight lines are close to each other: step 2.4.3 is performed when approaching, and step is performed when not approaching

Step 2.4.3: the straight line combined by l 'and l' is represented by l ═ l '^ l', and the combined straight line l is_fIs a_k^l_aAnd l_k^l_bMinimum dispersion, while sigma must be satisfied_f≤σ_maxUntil no more mergers are possible; wherein the only limiting parameter σ_maxGiven by the statistical error of the laser sensor.

The invention further provides a linear feature extraction method based on the laser sensor, which is characterized by comprising the following steps: the specific procedure of least squares straight line fitting described in step 3 is as follows (see fig. 4):

step 3.1: for each set of line segment points { ρ_i，φ_iN, where (r, α) represents the polar parameters of a straight line, the formula being:

$\mathrm{\α}=\frac{1}{2}\arctan \frac{p}{q}---\left(11\right)$

$r=\frac{1}{N}\mathrm{\Σ}\left({\mathrm{\ρ}}_i\cos ({\mathrm{\φ}}_i-\mathrm{\α})\right)---\left(12\right)$

in the formula,

$p=\frac{2}{N}\mathrm{\Σ\Σ}{\mathrm{\ρ}}_i{\mathrm{\ρ}}_j\cos {\mathrm{\φ}}_i\sin {\mathrm{\φ}}_j-\mathrm{\Σ}{\mathrm{\ρ}}_i^2\sin 2{\mathrm{\φ}}_i---\left(13\right)$

$q=\frac{1}{N}\mathrm{\Σ\Σ}{\mathrm{\ρ}}_i{\mathrm{\ρ}}_j\cos ({\mathrm{\φ}}_i+{\mathrm{\φ}}_j)-\mathrm{\Σ}{\mathrm{\ρ}}_i^2\cos 2{\mathrm{\φ}}_i---\left(14\right)$

the invention further provides a linear feature extraction method based on the laser sensor, which is characterized by comprising the following steps: the specific process of calculating the straight line fitting error in the step 4 is as follows:

step 4.1: setting a linear parameter error covariance matrix as follows:

$C_{\mathrm{\η}}=\left[\begin{array}{cc}{\mathrm{\σ}}_{\mathrm{rr}}& {\mathrm{\σ}}_{\mathrm{r\α}}\\ {\mathrm{\σ}}_{\mathrm{r\α}}& {\mathrm{\σ}}_{\mathrm{r\α}}\end{array}\right]---\left(15\right)$

wherein,

${\mathrm{\σ}}_{\mathrm{rr}}=\frac{1}{N^2}\mathrm{\Σ}{[\cos ({\mathrm{\φ}}_i-\mathrm{\α})+\frac{\∂\mathrm{\α}}{\∂P_i}{\mathrm{\ρ}}_i\sin ({\mathrm{\φ}}_i-\mathrm{\α})]}^2{\mathrm{\σ}}_{{\mathrm{\ρ}}_i}^2---\left(16\right)$

${\mathrm{\σ}}_{\mathrm{\α\α}}=\frac{1}{4{(p^2+q^2)}^2}\mathrm{\Σ}{[p_i^{\′}q-p{q}_i^{\′}]}^2{\mathrm{\σ}}_{{\mathrm{\ρ}}_i}^2---\left(17\right)$

in the formula:

$p_i^{\′}=\frac{2}{N}\mathrm{\Σ}{\mathrm{\ρ}}_j\cos {\mathrm{\φ}}_i\sin {\mathrm{\φ}}_j-2{\mathrm{\ρ}}_i\sin 2{\mathrm{\φ}}_i---\left(18\right)$

$q_i^{\′}=\frac{1}{N}\mathrm{\Σ}{\mathrm{\ρ}}_j\cos ({\mathrm{\φ}}_i+{\mathrm{\φ}}_j)-2{\mathrm{\ρ}}_i\cos 2{\mathrm{\φ}}_i---\left(19\right)$

${\mathrm{\σ}}_{\mathrm{r\α}}=\mathrm{\Σ}\frac{\∂r}{\∂P_i}\frac{\∂\mathrm{\α}}{\∂P_i}{\mathrm{\σ}}_{{\mathrm{\ρ}}_i}^2---\left(20\right)$

$\frac{\∂r}{\∂P_i}=\frac{1}{N}[\cos ({\mathrm{\φ}}_i-\mathrm{\α})+\frac{\∂\mathrm{\α}}{{\∂P}_i}{\mathrm{\ρ}}_i\sin ({\mathrm{\φ}}_i-\mathrm{\α})]---\left(21\right)$

$\frac{\∂\mathrm{\α}}{\∂P_i}=\frac{1}{2(p^2+q^2)}(p_i^{\′}q-p{q}_i^{\′})---\left(22\right)$

the linear feature extraction method based on the laser sensor is completed.

Compared with the traditional straight line extraction method, the method has the advantages that: (1) the iteration times of the method are reduced, and the efficiency of the method is improved; (2) the robustness of the method is greatly improved; (3) the accuracy of line segment extraction is also obviously improved.

Drawings

FIG. 1: distance variation graph of adjacent points of the invention

FIG. 2: straight line feature extraction flow chart of the invention

FIG. 3: the invention is a flow chart of the detection of neighboring point set in step 1 in FIG. 2

FIG. 4: step 2 line segment segmentation flowchart in fig. 2 of the present invention

Detailed Description

The method of the invention will now be further described with reference to the accompanying drawings:

examples

This example uses SICK200 industrial 180 DEG range laser sensors from SICK corporation, Germany, in a laboratory environment of 6m x 6 m. The LMS laser radar measures the distance between the LMS laser radar and surrounding obstacles by calculating the round trip time of laser, namely, a laser radar transmitter transmits a beam of laser which is reflected when encountering the obstacles; the receiver of the lidar records the reflected light and calculates the distance between the obstacle and the lidar from the time the laser is transmitted to be reflected back to the lidar.

The laboratory raw laser data is shown in fig. 5, which takes one data every 0.5 ° from 0 ° to 180 °, the mean square error of the distance measurement is 5mm, the measurement error of the angle is ignored, and 361 data are obtained in each frame. Fig. 4-2 is a frame of laser data acquired in the laboratory herein, where blue dots represent laser data points, black dots represent the origin of the lidar coordinate system, black plus 0 ° on the right represents laser data points, and black plus on the left represents 180 ° laser data points.

Setting sigma_maxThe extraction results of fig. 5 using the conventional Split-and-Merge algorithm and the third part straight line extraction algorithm are shown in fig. 6 at 0.005 mm. The extraction result of the invention is shown in figure 7.

In fig. 6 and 7, the red straight line segment is the straight line segment detected by the algorithm, and + represents the starting point of the line segment, and as can be seen from fig. 6 and 7, 12 straight lines, L1 and L2.. L12, are detected by the improved algorithm, while only 8 straight lines, L4, L6, L9 and L10, are detected by the algorithm before improvement. From the viewpoint of the extraction accuracy of the straight line, the improved one is also higher than the one before the improvement, and particularly L8 is particularly remarkable. Meanwhile, the traditional Split-and-Merge algorithm directly segments and extracts all data, and a method of firstly detecting adjacent point clusters and then segmenting and extracting straight lines is adopted, so that the iteration times of the algorithm are reduced, and the algorithm efficiency is improved. In addition, as can be seen from fig. 6 and 7, the robustness of the improved algorithm is also greatly improved compared with the improved algorithm.

The improved extracted straight line L1, L2.. L12 fitting error is as in table 1 below:

table 1: fitting error table for straight line L1, L2.. L12

Table 1 shows that the results are very accurate when the straight line segment contains more points, such as L1, L3, L7 and L11, but the errors are relatively large when the line segment contains fewer points, such as L6 and L10. From table 1, it can be seen that the errors of the polar diameters of the straight lines are small and generally in the millimeter level, and only L4, L6 and L10 are in the centimeter level because of the small number of points. In contrast, the angle error is large, 10 max^-1rad; but most of straight line errors are very small, and the diameter errors are less than 1mmAnd the angle error is less than 0.01rad, and the fitting effect is better.

Claims (4)

1. A linear feature extraction method based on a laser sensor is characterized by comprising the following steps: on the basis of a segmentation and combination method, the efficiency of the method is improved through self-adaptive adjacent point set detection, the sensitivity of the method for improving the line segment segmentation to parameters is improved by using a fuzzy segmentation and combination method, and finally the parameters under each linear segment polar coordinate system in the environment are fitted by using a least square method, and the method comprises the following steps:

step 1: detecting a neighboring point set;

step 2: fuzzy line segment segmentation, comprising:

step (ii) of2.1: let each set of neighboring points contain only one straight line: including all points in the sensor observation data z, the initial straight line numberSetting the generated iteration algebra m to be 1;

step 2.2: updating the number of the mth generation straight line

Step 2.3: inspecting each straight line l_k，Whether the variance σ of the straight line parameter is satisfied in the set of neighboring points_k≤σ_max；

Step 2.4: selecting a straight line;

and step 3: a least squares straight line fit comprising:

step 3.1: for each set of line segment points { ρ_i，φ_iN, where ρ is 1, 2, 3_iRepresents the polar distance measurement value phi of the ith measurement point in the line segment under the laser coordinate system_iAnd (r, α) represents a polar coordinate parameter of the straight line, and the formula is as follows:

in the formula,

and 4, step 4: calculating a straight line fitting error, comprising:

setting a linear parameter error covariance matrix as follows:

wherein,

in the formula:

2. the method for extracting the linear features based on the laser sensor according to claim 1, wherein: the specific process of detecting the neighboring point set in the step 1 is as follows:

step 1.1: let the continuous laser sensor detect two continuous points as s_k-1And s_kWherein s is_k-1Distance from laser sensor is rho_k-1Δ l is the distance between two points;

step 1.2: setting the discrete points of the angle phi and the distance rho at intervals of the angle delta phi as the scanning result of the laser radar; the constant m ' is 5, and the constant Δ l ' is m ' ρ_k-1And delta phi, if delta l is less than or equal to delta l', the adjacent point set is obtained, otherwise, the adjacent point set is not obtained.

3. The method for extracting the linear features based on the laser sensor according to claim 1, wherein:

"checking each straight line l" as described in step 2.3_k，Whether the variance σ of the straight line parameter is satisfied in the set of neighboring points_k≤σ_max", if not, the following steps are performed:

step 2.3.1: dividing l by fuzzy straight line_kPoint in (b) becomes two straight lines l'_kAnd l ″)_kSimultaneously, making C equal to 2;

step 2.3.2: from l'_kAnd l ″)_kReplacement of l_kAdding to the mixture;

step 2.3.3:

step 2.3.4: go to step 2.2.

4. The method for extracting the linear features based on the laser sensor according to claim 1, wherein: the specific process of "selecting a straight line" described in step 2.4 is as follows:

step 2.4.1: for every two distinct straight lines l_aAnd l_bAre all considered as candidates for merging;

step 2.4.2: judging whether the coordinate barycenters of the two straight lines are close to each other: step 2.4.3 is performed when approaching, and step is performed when not approaching

Step 2.4.3: the line l ═ l 'Λ l "represents a line formed by merging l' and l", and the merged line l_fIs a_k∧l_aAnd l_k∧l_bMinimum dispersion, while sigma must be satisfied_f≤σ_maxUntil no more mergers are possible; wherein the only limiting parameter σ_maxGiven by the statistical error of the laser sensor.