A linear feature fitting method combining regional information and corner detection operators

Abstract

The present invention proposes a linear feature fitting method combining regional information and corner detection operators, that is, a method of fusing the regional information of radar data and filtering out-of-region noise points, extracting corner features according to corner detection operators, and performing linear fitting using the weighted least squares method. First, the distance between two points is used to judge the regional breakpoints and out-of-region noise points. Then, the preliminary candidate corners are screened according to the distance from the point to the connecting line of the head and tail, the corner detection operator is calculated according to the cumulative distance from the point to the chord and the point to the tangent, and the candidate corners are determined. The pseudo-corners are removed according to the corner opening degree to determine the final corners. Finally, the noise points are filtered, and linear segmentation is performed at the regional breakpoints and corners, and the linear features of the laser scan points are fitted using the weighted least squares method. This method can effectively avoid the influence of noise points and fixed segmentation thresholds on corner extraction, and the fitted laser scan data features are closer to the actual environment.

Description

Technical Field

[0001] The present invention relates to the technical field of laser detection, and particularly relates to a linear feature fitting method combining regional information and corner detection operators. Background Art

[0002] Environmental perception is an embodiment of the intelligence of mobile robots and plays a decisive role in the application of mobile robots. LiDAR is a commonly used device for environmental perception on current mobile robots, with characteristics such as high efficiency, moderate price, and strong applicability. However, LiDAR is easily affected by light. In a single environment, due to feature degradation, the positioning accuracy may be reduced or positioning may be lost. By processing feature points and extracting environmental features with a certain geometric configuration, the positioning ability and robustness of the system can be effectively enhanced.

[0003] Linear features are recognized as important features for environmental perception of mobile robots. The extraction of linear features mainly includes two steps: region segmentation and feature extraction. In the region segmentation stage, the classification and identification of feature patterns are mainly completed. Feature patterns include straight lines, arcs, etc., and the regions belonging to the feature patterns and the laser data point sets within the regions are determined. Feature extraction mainly completes the determination of various feature pattern parameters and the extraction of feature points.

[0004] Common line feature extraction algorithms include Hough transform, LT (Line-tracting), PDBS (point distance-based methods), IEPF (iterative end point fit), etc. The Hough transform first converts the line feature from the Cartesian coordinate system to the polar coordinate system, and then counts the number of curves at the intersection points in the polar coordinate system. Only when a certain threshold is reached is it marked as a line feature. The disadvantage of this method is that the computational complexity is relatively large. The principle of the LT algorithm is to judge whether the points detected subsequently are on the same straight line as the points detected previously according to the straight line tracking criterion. The disadvantage is that the extracted line segments lack integrity and have large errors. PDBS mainly compares the distance between two adjacent points of the LiDAR in the Cartesian coordinate with a set threshold. If the distance is greater than the threshold, the two points are considered to belong to different regions. The threshold set in this method has an important impact on the segmentation of line segments. If the threshold is set too small, over-segmentation is likely to occur. If the threshold is set too large, under-segmentation will occur, and segmentation failure will occur at the corner points. The IEPF algorithm first fits the point set into a straight line, and then judges the relationship between the distance from the point to the straight line and the set threshold to segment the point set and fit the straight line. This method is superior in performance, but it is sensitive to the selection of the threshold, and over-segmentation or under-segmentation will also occur in line feature extraction.

[0005] In summary, there are many methods for extracting features from lidar scan point clouds at present, but there are problems of low efficiency, complexity and inaccuracy. This application makes the recognition of lidar more efficient and accurate through improvement. Summary of the Invention

[0006] Aiming at the deficiencies in the prior art, the present invention provides a linear feature fitting method combining regional information and corner detection operators to solve the problems of low efficiency and inaccuracy in current lidar recognition. First, the distance between two points is used to judge regional breakpoints and external noise points outside the region. Then, the initial corner points are screened according to the distance from the point to the connection line of the head and tail. The corner detection operator is calculated according to the cumulative distance from the point to the chord and the point to the tangent, and the candidate corner points are determined. The pseudo-corner points are removed according to the corner opening degree to determine the final corner points. Finally, the noise points are filtered, and linear segmentation is performed at the regional breakpoints and corner points, and the linear features of the lidar scan points are fitted by weighted least squares. This method can effectively avoid the influence of noise points and fixed segmentation thresholds on corner extraction, and the fitted lidar scan data features are closer to the actual environment.

[0007] To achieve the above object, the present invention adopts the following technical solutions:

[0008] A linear feature fitting method combining regional information and corner detection operators, comprising the following steps:

[0009] S1: Scan the surrounding environment data through a lidar, collect a plurality of point data, and convert the point data from the polar coordinate system to the plane rectangular coordinate system;

[0010] S2: Calculate the distance between adjacent points through the coordinates of each point in the plane rectangular coordinate system obtained in step S1, and use the distance between two points to judge regional breakpoints and external noise points outside the region;

[0011] S3: Determine a plurality of initial corner points Con i ;

[0012] S4: Calculate the cumulative value of the perpendicular distances from each initial corner point Con i to its corresponding chord length L, and then determine the corner response function of the corresponding initial corner point Con i to the corresponding chord; the process of determining the chord length L is as follows: for the corresponding initial corner point Con i , find a plurality of lidar data points near it, and connect the connection line of the head and tail of the plurality of lidar data points to determine the chord length;

[0013] For each initial corner point Con i , calculate the tangent distances from multiple lidar points within its support domain radius to the corresponding initial corner point Con i , obtain the cumulative value of the tangent distances, and then determine the corner response function of the corresponding initial corner point Con i to its tangent;

[0014] S5: Calculate the corner detection operator of the corresponding primary selected corner point Con according to the corner response function of the corresponding primary selected corner point Con to the corresponding chord and the corner response function of the corresponding primary selected corner point Con to its tangent line, and then determine the candidate corner points; i to the corner response function of the corresponding chord, and the corner response function of the corresponding primary selected corner point Con i to its tangent line, calculate the corner detection operator of the corresponding primary selected corner point Con i and then determine the candidate corner points;

[0015] S6: For all the determined candidate corner points, calculate their corner opening degrees θ respectively, and eliminate the false corner points according to the corner opening degrees to determine the final corner points;

[0016] S7: According to the regional breakpoints and out-of-region noise points determined in step S2, and the final corner points determined in step S6; filter the out-of-region noise points, linearly segment at the regional breakpoints and the final corner points, and use the weighted least squares method to fit the linear features of the laser scanning points.

[0017] To optimize the above technical solution, the specific measures taken also include:

[0018] Further, the specific content of step S1 is:

[0019] Scan the surrounding environment data through a SICK Tim571 lidar, with an angular resolution of 0.33°, a scanning angle of 160°, a measurement start angle of -80°, and an end angle of 80°. A total of 481 groups of point data are collected, and the point data is converted from the polar coordinate system to the plane rectangular coordinate system;

[0020] Among them, the polar coordinate data of the points collected by the lidar is expressed as:

[0021]

[0022] In the formula, r is the distance of the obstacle returned by the lidar, is the angle corresponding to each environmental distance;

[0023] According to the parameter configuration of the selected SICK Tim571 radar, the calculation method of

[0024]

[0025] The conversion method from the polar coordinate system to the plane rectangular coordinate system is as follows:

[0026]

[0027] In the formula, x l is the abscissa length of the laser scanning point, y l is the ordinate length value of the laser scanning point, r, are the length and angle of the laser scanning points in polar coordinates, respectively, and i is any one of the laser scanning points.

[0028] Further, the specific content of calculating the distance between adjacent points in step S2 and using the distance between two points to judge the regional breakpoints and the out-of-region noise points is as follows:

[0029] The calculation method of the distance d between two adjacent measurement points is as follows:

[0030]

[0031] In the formula, are the abscissa and ordinate values of the i-th point, respectively, are the abscissa and ordinate values of the (i + 1)-th point, respectively;

[0032] In the rectangular coordinate system, the distance d between two adjacent points of the lidar is compared with the set threshold ε, where the threshold ε = 0.015. If d > ε, it is considered that the current two points belong to different regions, and this point is the regional breakpoint of the lidar scanning data points; if d < ε, this point is a normal lidar point cloud point, and thus the distance judgment between all adjacent points is completed;

[0033] Among them, if the distances d from any point i to the previous point and to the next point are both greater than the threshold ε, it is considered that this point is an out-of-region noise point in the lidar scanning data points, and the value of this noise point is replaced by the average value of ten radar data points near the noise point.

[0034] Further, the specific content of step S3 to determine multiple primary candidate corner points Con i is as follows:

[0035] S3.1: Connect the line between the first and last data points, calculate the distances from the other data points except the first and last points to this line and arrange them in descending order, and select the first point in the sequence as the first primary candidate corner point Con 1 ;

[0036] S3.2: Connect the line between the first data point and the first primary candidate corner point Con 1 , calculate the distances from the other data points except the first data point and the first primary candidate corner point Con 1 to this line and arrange them in descending order, and select the first point in the sequence as the second primary candidate corner point Con 2 ;

[0037] S3.3: Connect the line between the first primary candidate corner point Con 1 and the second primary candidate corner point Con 2 , calculate the distances from the points except point Con 1 and point Con 2The distances from other data points except this one to this line are arranged in descending order, and the first point in the sequence is selected as the third primary corner point Con 3 ;

[0038] S3.4: Similarly, when conducting the next screening of primary corner points, connect the lines of the primary corner points determined in the previous two times, calculate the distances from other data points to this line and arrange them in descending order, and select the first point in the sequence as the primary corner point to be determined currently; in this way, multiple primary corner points Con i .

[0039] Furthermore, in step S4, "calculate the cumulative value of the perpendicular distances from each primary corner point Con i to its respective chord length L, and then determine the corner response function of the corresponding primary corner point Con i to the corresponding chord; the process of determining the chord length L is as follows: for the corresponding primary corner point Con i , find multiple laser data points near it, and connect the line connecting the first and last points among the multiple laser data points to determine the chord length" is specifically as follows:

[0040] For the corresponding primary corner point Con i , select a chord with a length of L = 10 laser scanning points, where the corresponding primary corner point Con i needs to be above and within the two endpoints of the determined chord L, so there are multiple situations for the determined chord; under the condition that there are multiple situations for the chord L, calculate the cumulative value of the perpendicular distances from the primary corner point Con i to the chord L:

[0041]

[0042] In the formula, D j，k is the perpendicular distance from the point Con i to the chord of L = 10 laser scanning points; j represents the laser scanning point, and k represents the chord;

[0043] Normalize h L (j) to obtain the corner response function H i (j) of the corresponding primary corner point Con L to the corresponding chord.

[0044] Furthermore, in step S4, "for each primary corner point Con i , calculate the tangent distances from multiple laser points within its support domain radius to the corresponding primary corner point Con i , obtain the cumulative value of the tangent distances, and then determine the corner response function of the corresponding primary corner point Con i to its tangent" is specifically as follows:

[0045] For a corresponding primary selected corner point Con i , its tangent equation is:

[0046]

[0047] In the formula, α 1 , β 1 are the direction vectors of the tangent at point Con i , x i , y i are the horizontal and vertical coordinate values of point Con i , t represents the calculation coefficient of the direction vector, and R represents a real number;

[0048] For a corresponding primary selected corner point Con i , multiple laser points within its support domain radius are combined into a point sequence Point sequence Any point (x i+j , y i+j ) on it to the distance from the tangent line l is:

[0049]

[0050] In the formula, α 1 , β 1 are the direction vectors of the tangent at point Con i , x i , y i are the horizontal and vertical coordinate values of point Con i , x i+j , y x+j are the horizontal and vertical coordinate values of the nearby points within the support domain radius from point Con i ;

[0051] Then the sum of the distances from the point sequence to the tangent line l, that is, the corner response function of the corresponding primary selected corner point Con i to the corresponding tangent line is expressed as:

[0052]

[0053] In the formula, d i+j is the distance from any point (x , y i+j , y i+j ) on the point sequence

[0054] Furthermore, the specific content of calculating the corner detection operator of the corresponding primary selected corner point Con i in step S5 to further determine the candidate corner points is:

[0055] Calculate the corresponding primary selected corner point Con iCorner detection operator:

[0056] H(j) = H L (j)·H q (j)

[0057] Wherein, H L (j) is the corner response function of the chord, and H q (j) is the corner response function of the tangent;

[0058] When the corner detection operator H(j) corresponding to the primary selected corner Con i is greater than the threshold T = 0.66, then the primary selected corner Con i is determined as a candidate corner.

[0059] Furthermore, the specific content of step S6 is:

[0060] Extract the candidate corner set at a certain scale to ensure that all candidate corners can be detected, and then screen out the false corners in the candidate corner set according to the corner opening degree, so as to obtain the valid points in the laser scan points;

[0061] Among them, the corner opening degree defines the local dynamic support area of the currently to-be-judged candidate corner as all the laser scan points on the edge between the previous candidate corner and the next candidate corner; when the corner opening degree θ satisfies 160° ≤ θ ≤ 200°, then the candidate corner is a false corner and is deleted from the candidate corners to determine the final corner.

[0062] Furthermore, the calculation formula of the corner opening degree θ is:

[0063]

[0064]

[0065]

[0066] Wherein, Δx 1 , Δx 2 , Δy 1 , Δy 2 all represent intermediate quantities, u, u 1 , u 2 are the current candidate corner, the previous and the next corners of the current candidate corner respectively, L 1 represents the number of laser scan points between u 1 and u, L 2 represents the number of laser scan points between u 2 and u, and n is the subscript of the current candidate corner.

[0067] The beneficial effects of the present invention are:

[0068] 1. The present invention can effectively avoid the influence of noise points and fixed segmentation thresholds on corner point extraction. In an environment with transparent obstacles and multi-level obstacles, it can significantly reduce the influence of reflection and occlusion on the extraction of environmental information by lidar. First, different levels of obstacles are divided into different regions, and at the same time, reflected noise points are filtered. Then, combined with corner point features, different levels of environmental information are fitted, thus avoiding the situation where fitting fails due to different levels of obstacles and reflected noise points being divided into the same level.

[0069] 2. The present invention has high angular positioning and line positioning accuracy, can accurately obtain the positions of noise points and corner points, and significantly improves the accuracy of fitting feature line segments. It is applicable to autonomous robot SLAM using embedded development. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 It is a schematic diagram of the overall process steps of the present invention.

[0071] Figure 2 It is a schematic diagram of the angle definition of the laser scanner in the embodiment of the present invention.

[0072] Figure 3 It is a schematic diagram of noise point modeling and preprocessing of the present invention.

[0073] Figure 4 It is a schematic diagram of determining primary corner points of the present invention.

[0074] Figure 5 It is a schematic diagram of calculating the cumulative distance from a point to a chord of the present invention.

[0075] Figure 6 It is a schematic diagram of calculating the cumulative distance from a point to a tangent of the present invention.

[0076] Figure 7 It is a schematic diagram of the average repetition rate broken line in the embodiment of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0077] Now, the present invention will be further described in detail with reference to the accompanying drawings.

[0078] As Figure 1 shown, the radar linear feature fitting combining regional information and corner point detection operators includes:

[0079] Step S1: The SICK Tim571 lidar scans the surrounding environmental data with an angular resolution of 0.33°, a scanning angle of 160°, a measurement start angle of -80°, and an end angle of 80°. A total of 481 sets of point data are collected, and the point data is transformed from the polar coordinate system to the plane rectangular coordinate system.

[0080] Step S2: Calculate the distance between adjacent points, and use the distance between two points to judge the regional breakpoints and the out-of-region noise points;

[0081] Step S3: Calculate the distance from the data points except the first and last points to the line connecting the first and last points. Screen the preliminary candidate corner points according to the distance from the points to the line connecting the first and last points, and define them as the preliminary candidate corner points Con i ;

[0082] Step S4: Use the preliminary candidate corner points found in Step S3 as the end point and the starting point of the next iteration (redefine the first and last points, and use the corner points found in the previous two times as the new first and last points), and find the second point with the farthest distance Con 2 , and so on, perform cyclic operations until the maximum value of the distance from the points to the line connecting the first and last points is less than the threshold ε, end the loop, and find all the preliminary candidate corner points Con i ;

[0083] Step S5: Calculate the cumulative value of the vertical distance from the preliminary candidate corner points Con i to the chord with a chord length of 10 laser data points, and the sum of the cumulative distances from the p i points with a support domain radius of 6 laser data points to the tangent line. Calculate the corner point detection operator according to the cumulative distances from the points to the chord and the points to the tangent line, and determine the candidate corner points, denoted as u i ;

[0084] Step S6: Calculate the corner point opening degree θ for all the found candidate corner points u i respectively, and eliminate the false corner points according to the corner point opening degree to determine the final corner points;

[0085] Step S7: Filter the noise points, linearly segment at the regional breakpoints and corner points, and use the weighted least squares method to fit the linear features of the laser scanning points.

[0086] In the embodiment, the conversion of the point data from the polar coordinate system to the rectangular coordinate system described in Step S1 is specifically as follows:

[0087] The polar coordinate information representation of each group of collected data is as follows:

[0088]

[0089] In formula (1), r is the distance of the obstacle returned by the lidar; is the angle corresponding to each environmental distance.

[0090] As Figure 2 shown, starting from the first angle, a value is output every 0.33°, and each beam angle is represented as follows:

[0091]

[0092] In formula (2), i is the serial number of the lidar return data.

[0093] For each measured distance of the laser sensor, the coordinates in the rectangular coordinates X L OY L are as follows:

[0094]

[0095] In formula (3), x l is the horizontal coordinate length of the laser scanning point, y l is the vertical coordinate length value of the laser scanning point, r, are respectively the length and angle of the laser scanning point in polar coordinates, and i is the number of laser scanning points.

[0096] For all measured points, the x l and y l coordinate sets are represented by X z and Y z as follows:

[0097]

[0098] In the embodiment, in step S2, the method for judging the regional break point is as follows:

[0099] Compare the distance d i between two adjacent points of the lidar with the set threshold ε. If the distance d i between two adjacent measured points is greater than the threshold ε, it is considered that the two points belong to different regions, and this point is the regional break point of the lidar scan data points.

[0100] In step S2, the method for judging the out-of-region noise points is as follows:

[0101] If the distance d i from any point to the previous point and the distance d i+1 from this point to the next point are both greater than the threshold ε, then this point is considered to be an out-of-region noise point in the lidar scan data points.

[0102] The calculation formula for the distance d i between two adjacent measured points is as follows:

[0103]

[0104] In formula (5), x l , y l are the horizontal and vertical coordinate values of the l-th point, and x l+1 , y l+1 are the horizontal and vertical coordinate values of the (l + 1)-th point.

[0105] As Figure 3 shown, point pi-1 and point p i+1 are respectively the end point of line L 1 and the start point of line L 2 There is a noise point p in the middle, and in the distance distribution diagram, the distances between this point and the two adjacent points are both greater than the threshold ε = 0.15. Therefore, this point is a noise point. i

[0106] After detecting the noise point, take five radar data points before and after the noise point, and use the method of mean filtering to replace the noise point value with the average value of the ten radar data points before and after the noise point. After the radar data points are filtered, the distances between adjacent two data points are all less than the threshold ε = 0.15.

[0107] In the embodiment, in step S3, the judgment method for the preliminary selected corner points is as follows:

[0108] As Figure 4 shown, connect a point p 1 in the data with another point p n to form a line L 1 , and then calculate the distance D 2 from point P n-1 to all points between point P 1 and line L i . Find the point Con 1 with the farthest distance, and define this point as the preliminary selected corner point. Then use point Con 1 as the end point and start point for the next iteration, find the second point Con 2 with the farthest distance, and so on, until all the preliminary selected corner points Con in the laser scan data points are found.

[0109] Among them, the calculation formula of D i is as follows:

[0110]

[0111]

[0112]

[0113] In formulas (6), (7), and (8), are respectively the coordinate values of the start point p 1 , are respectively the coordinate values of the end point p n , are respectively the coordinate values of any intermediate point p i , T x , T y ​They are the coordinate values of the projection points of the points on the straight line (the projection points refer to the projection points of all points inside the head-to-tail connection line to the head-to-tail connection line).

[0114] In the embodiment, in the step S5, the method for calculating the corner detection operator according to the cumulative distances from the point to the chord and from the point to the tangent and determining the candidate corners is as follows:

[0115] As Figure 5 shown, for calculating the cumulative distance from the point to the chord, when calculating the corner response value of the point P 1 , P 2 , …, P n constituting the chord, select a chord with a length of L = 10 laser scanning points, and calculate the perpendicular distance from the point P i to L. It is required to calculate all cases where the point P i is within the two end points of the chord (including the case where P i coincides with the two end points), and sum up the perpendicular distances from the point to the chord calculated in all cases. (Refer to i to illustrate a specific embodiment: for example, if there is 1 point on the left side of the initially selected corner point and 9 points on the right side, connect the first point on the left and the ninth point on the right and calculate the distance, which is a case of the chord L; if there are 2 points on the left side of the initially selected corner point and 8 points on the right side, connect the second point on the left and the eighth point on the right and calculate the distance, which is also a case of the chord L. And so on for accumulation. Specifically Figure 5 in: P Figure 5 is the initially selected corner point, and among its two sides, P i -P j-l -P j are 10 points, and there is a connection line between the two points, which is a case of the chord L).

[0116] The cumulative distance calculation formula is as follows:

[0117]

[0118] In formula (9), D j，k is the perpendicular distance from the point P i to the chord with L = 10 laser scanning points.

[0119] Perform normalization processing on the sum of the perpendicular distances from the points on this section of the contour line to the chord, and the calculation formula of the corner response function from the point to the chord is as follows:

[0120]

[0121] In formula (10), h L is the sum of the perpendicular cumulative distances from the point to the chord with L = 10 laser scanning points.

[0122] As Figure 6As shown, the cumulative distance from a point to the tangent line. In the figure, a solid line represents the curve of the function y = 5x 2 , where x ∈ [-1, 1], and another solid line represents the curve of the function y = x 2 , where x ∈ [-1, 1]. The function y = 5x 2 intersects with the function y = x 2 at the origin, and their tangent lines at the origin (0, 0) are both y = 0.

[0123] Let x 0 > 0, and (x 0 , 0) be a point near the origin on the tangent line. Then the straight line x = x 0 that passes through the point (x 0 , 0) and is perpendicular to the tangent line y = 0 intersects with the function y = 5x 2 and the function y = x 2 at the points (x 0 , 5x 0 2 ) and the point (x 0 , x 0 2 ), respectively. It is easy to know that the distances from the points (x 0 , 5x 0 2 ) and the point (x 0 , x 0 2 ) to the tangent line y = 0 are D = 5x 0 2 and d = x 0 2 , respectively, and D > d. Define the number of radar scan data points on both sides of the tangent point as the support domain radius. (Simply put, for each corner point, calculate its tangent line; at the same time, find the distances from 6 nearby laser points to the tangent line and accumulate them. Specific embodiments: Figure 6 The origin in

[0124] is a corner point, the x-axis is the tangent line, and calculate the distances from other points to it). i (x i , y i ) is as follows:

[0125]

[0126] In formula (11), α 1 and β 1 are the direction vectors of the tangent line at p i , and x i and y i are the abscissa and ordinate values of p i .

[0127] Point sequence Any point (x i+j , y i+j ) to the distance calculation formula of the tangent line l is as follows:

[0128]

[0129] In formula (12), α 1 , β 1 are the direction vectors of the tangent line at p i , x i , y i are the abscissa and ordinate values of p i , x i+j , y x+j are the abscissa and ordinate values of the point whose distance from point p i is the support domain radius.

[0130] Then there is a point sequence to the cumulative sum of the distances from the tangent line l, that is, the calculation formula of the corner response function of this point to the tangent line is as follows:

[0131]

[0132] In formula (13), d i+j is the distance from any point (x , y i+j ) on the point sequence i+j to the tangent line l.

[0133] The calculation formula of the corner monitoring operator for the finally initially selected corners is as follows:

[0134] H(j) = H L (j)·H q (j) (13)

[0135] In formula (14), H L (j) is the corner response function of the chord when L = 10 laser scanning points, and H q (j) is the corner response function of the tangent line when the support domain radius is 6 radar scanning points.

[0136] As Figure 7 (a) shows, the average repetition rate (the average value of the ratio of the number of repeatedly recognized corner points to the actual corner points after changing the direction of the radar data) increases continuously with the increase of the support domain radius. When the support domain radius is 6, the average repetition rate reaches the maximum value, and then begins to decrease. It can be seen that too large a support domain radius will cause the corner fusion phenomenon of adjacent corner points with similar distances.

[0137] As Figure 7As shown in (b), the average repetition rate reaches its maximum value at the corner detection operator threshold T = 0.66, and the oscillation amplitude at the maximum point is not large, indicating that the corner detection operator obtained by multiplying the accumulated distances from a point to a chord and from a point to a tangent has high robustness. Through the above discussion, the support domain radius is finally determined to be 6 and the threshold is taken as T = 0.66.

[0138] In the embodiment, in the step S6, the method for determining the final corners by eliminating false corners according to the corner opening degree is as follows:

[0139] Extract the candidate corner set at a small scale to ensure that all determined corners can be detected, and then screen out the false corners in the candidate corner set according to the corner opening degree, so as to obtain the valid points in the laser scan points.

[0140] The corner opening degree defines the local dynamic support area of the currently to-be-determined candidate corner as all the laser scan points on the edge between the previous candidate corner and the next candidate corner.

[0141] The calculation formula of the corner opening degree is as follows:

[0142]

[0143]

[0144]

[0145] In formulas (15), (16), and (17), u is the position of the currently to-be-determined candidate corner, L 1 represents the number of laser scan points between u 1 and u, and L 1 represents the number of laser scan points between u 2 and u.

[0146] In the embodiment, in the step S7, filter the noise points, linearly segment at the regional breakpoints and corners, and use the weighted least squares method to fit the linear features of the laser scan points, specifically:

[0147] Given the two-dimensional data point sequence (x 1 , y 1 ), (x 2 , y 2 ),..., (x n , y n ), then assume the mathematical model of the line fitting is y = kx + b, and then find the parameters in the model through the residual e i = y i - kx i - b (i = 1, 2, 3,..., n), and consider the equation of the sum of the squares of the residuals that is the smallest with respect to the parameters k and b.

[0148] The calculation formulas for the parameters k and b in the weighted least squares method are as follows:

[0149]

[0150]

[0151]

[0152]

[0153] It should be noted that the terms such as "upper", "lower", "left", "right", "front", "rear", etc. cited in the invention are only for the convenience of clear description, rather than used to limit the scope of implementation of the present invention. The change or adjustment of their relative relationship, without substantial change in the technical content, should also be regarded as the scope of implementation of the present invention.

[0154] The above is only the preferred implementation mode of the present invention. The protection scope of the present invention is not limited to the above embodiments. All technical solutions falling within the idea of the present invention belong to the protection scope of the present invention. It should be pointed out that for those of ordinary skill in the art, several improvements and refinements made without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A linear feature fitting method combining regional information and corner detection operators, characterized in that, it includes the following steps: S1: Scan the surrounding environment data by lidar, collect multiple point data, and convert the point data from polar coordinates to the plane rectangular coordinate system; S2: Calculate the distance between adjacent points through the coordinates of each point in the plane rectangular coordinate system obtained in step S1, and use the distance between two points to judge regional breakpoints and out-of-region noise points; S3: Determine multiple primary candidate corner points Con i ; Step S3 determines multiple primary candidate corner points Con i The specific content is as follows: S3.1: Connect the line between the first and the last data points, calculate the distances from the other data points except the first and the last points to this line and sort them in descending order, and select the first point in the sequence as the first preliminary corner point Con 1 ; S3.2: Connect the first data point and the first initially selected corner point Con 1 with a line, calculate the distances from the other data points except the first data point and the first initially selected corner point Con 1 to this line and sort them in descending order, and select the first point in the sequence as the second initially selected corner point Con 2 ; S3.3: Connect the first initially selected corner point Con 1 and the second initially selected corner point Con 2 to draw a line between them, calculate the distances from other data points except point Con 1 and point Con 2 to this line and sort them in descending order, and select the first point in the sequence as the third initially selected corner point Con 3 ; S3.4: Similarly, when conducting the next primary corner point screening, connect the straight lines of the primary corner points determined in the previous two times, calculate the distances from other data points to this line and arrange them in descending order, and select the first point in the sequence as the primary corner point to be determined currently; in this way, multiple primary corner points Con are obtained. i ; S4: Calculate the cumulative value of the perpendicular distances from each initially selected corner point Con i to its respective chord length L, and then determine the corner response function of the corresponding initially selected corner point Con i to the corresponding chord; The process of determining the chord length L is as follows: For the corresponding initially selected corner point Con i , find multiple laser data points in its vicinity, and the chord length is determined by connecting the first and last points among the multiple laser data points; For each initially selected corner point Con i , calculate the tangent distances from multiple laser points within its support region radius to the corresponding initially selected corner point Con i , obtain the cumulative value of the tangent distances, and further determine the corner response function of the corresponding initially selected corner point Con i to its tangent line; S5: According to the corner response function from the corresponding initially selected corner point Con determined in step S4 to the corresponding chord, and the corner response function from the corresponding initially selected corner point Con to its tangent, calculate the corner detection operator of the corresponding initially selected corner point Con, and then determine the candidate corner points; i to the corner response function of the corresponding chord, and the corresponding initially selected corner point Con i to the corner response function of its tangent, calculate the corresponding initially selected corner point Con i of the corner detection operator, and then determine the candidate corner points; S6: For all the determined candidate corner points, calculate their corner opening degrees θ respectively, and eliminate the false corner points according to the corner opening degrees to determine the final corner points; S7: According to the regional breakpoints and out-of-region noise points determined in step S2, and the final corner points determined in step S6; filter out the out-of-region noise points, linearly segment at the regional breakpoints and the final corner points, and use the weighted least squares method to fit the linear features of the lidar scan points.

2. A linear feature fitting method combining regional information and corner detection operators according to claim 1, characterized in that, the specific content of calculating the distance between adjacent points in step S2 and using the distance between two points to judge regional breakpoints and out-of-region noise points is: The calculation method of the distance d between two adjacent measurement points is as follows: wherein, are respectively the horizontal and vertical coordinate values of the i-th point, are respectively the horizontal and vertical coordinate values of the (i + 1)-th point; In the rectangular coordinate system, compare the distance d between two adjacent points of the lidar with the set threshold ε. If d > ε, it is considered that these two points belong to different regions, and this point is the regional breakpoint of the lidar scan data points; if d < ε, this point is a normal lidar point cloud point, and thus complete the distance judgment between all adjacent points; Among them, if the distance d from any point i to the previous point and the distance d to the next point are both greater than the threshold ε, it is considered that this point is an out-of-region noise point in the lidar scan data points, and the value of this noise point is replaced by the average value of ten radar data points near the noise point.

3. A linear feature fitting method combining regional information and corner detection operators according to claim 1, characterized in that, In step S4, "calculate the cumulative value of the perpendicular distances from each initially selected corner point Con i to its respective chord length L, and then determine the corner response function of the corresponding initially selected corner point Con i to the corresponding chord; Among them, the process of determining the chord length L is as follows: For the corresponding initially selected corner point Con i , find multiple laser data points near it, and the specific content of "determining the chord length by connecting the first and last points among the multiple laser data points" is as follows: For the corresponding primary selected corner point Con i , a chord with a length of L = N laser scanning points is selected, where the corresponding primary selected corner point Con i needs to be above and within the two endpoints that determine the chord L, so there are multiple cases for the determined chord; under the condition that there are multiple cases for the chord L, calculate the cumulative value of the perpendicular distance from the primary selected corner point Con i to the chord L: where D j，k is the perpendicular distance from the Con i point to the chord of the L = N laser scanning points; j represents the laser scanning point and k represents the chord; For h L (j) is normalized to obtain the corresponding primary candidate corner point Con i and the corner response function H L (j) corresponding to the chord.

4. A linear feature fitting method combining regional information and corner detection operators according to claim 3, characterized in that, In step S4, "for each initially selected corner point Con i , calculate the tangent distances from multiple laser points within its support domain radius to the corresponding initially selected corner point Con i , obtain the cumulative tangent distance value, and further determine the corner response function of the corresponding initially selected corner point Con i to its tangent" specifically refers to: For a corresponding primary selected corner point Con i , its tangent equation is: where α 1 and β 1 are the direction vectors of the tangent line at point Con i , x i and y i are the abscissa and ordinate values of point Con i , t represents the calculation coefficient of the direction vector, and R represents the real number; For a corresponding primary candidate corner point Con i , a plurality of laser points within the support domain radius thereof are combined into a point sequence Point sequence For any point (x i+j , y i+j ) on it, the distance to the tangent line l is: where α 1 , β 1 are the direction vectors of the tangent line at point Con i , x i , y i are the horizontal and vertical coordinate values of point Con i , and x i+j , y x+j are the horizontal and vertical coordinate values of the nearby points within the radius of the support domain from point Con i ; There is a point sequence The cumulative sum of the distances to the tangent line l, that is, the corresponding initial selected corner point Con i The corner response function of the corner point to the corresponding tangent line is expressed as: where d i+j is the distance from any point (x , y i+j , i+j ) on the point sequence to the tangent line l, and n represents a part of the support domain radius.

5. A linear feature fitting method combining regional information and corner detection operators according to claim 4, characterized in that, In step S5, calculate the corner detection operator corresponding to the initially selected corner Con i , and the specific content for determining the candidate corner is as follows: Calculate the corner detection operator corresponding to the initially selected corner Con i : H(j) = H L (j)·H q (j) Where, H L (j) is the corner response function of the chord, and H q (j) is the corner response function of the tangent; When the corner detection operator H(j) corresponding to the primary selected corner point Con i is greater than the threshold T = 0.66, then the primary selected corner point Con i is determined as a candidate corner point.

6. A linear feature fitting method combining regional information and corner detection operators according to claim 5, characterized in that, the specific content of step S6 is: Extract the candidate corner point set to ensure that all candidate corner points can be detected, and then screen out the false corner points in the candidate corner point set according to the corner opening degree, so as to obtain the valid points in the lidar scan points; Among them, the corner opening degree defines the local dynamic support area of the current candidate corner point to be judged as all the lidar scan points on the edge between the previous candidate corner point and the next candidate corner point; when the corner opening degree θ satisfies 160° ≤ θ ≤ 200°, then this candidate corner point is a false corner point and is deleted from the candidate corner points, so as to determine the final corner points.

Images