A method for extracting coordinate angles of environmental rectangular points based on two-dimensional laser radar

Abstract

The present invention discloses a method for extracting the coordinate angle of an environmental right-angle point based on a two-dimensional laser radar, comprising S100: traversing laser points in a natural environment point cloud frame scanned by a two-dimensional laser; S200: selecting a point set dots_near in the neighborhood of a random point Pk, calculating the angles between the x-axis of the laser coordinate system and all other points in dots_near starting from Pk, and calculating the mean of all angles as an angle dividing line of dots_near; S300: separating the point sets dots_left on the left and right sides of dots_near according to the angle dividing line and dots_right, and extract inner points inner_left and inner_right whose variance is less than a threshold value from dots_left and dots_right; S400: fit line_left and line_right according to the inner points inner_left and inner_right, and find the angle between the two straight lines; S500: take the right-angle point as the vertex and make two rays coincident with line_left and line_right, and determine the angle with one of the rays as the reference. The present invention can extract the coordinates and angles of the right-angle points of the building.

Description

Technical Field

[0001] The present invention relates to the technical field of data processing, and in particular to a method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar. Background Art

[0002] With the development of autonomous driving technology and low-speed mobile robot technology, the application of real-time positioning and mapping (SLAM) technology based on two-dimensional laser observation of natural features is becoming more and more common. There are two types of SLAM mapping: one is sparse feature points; the other is dense point cloud. In the former, it is necessary to extract stable feature points in the environment for real-time positioning and map updating, and in the latter, it is necessary to extract feature points in the environment for loop matching to give accurate initial values. Summary of the invention

[0003] The present invention focuses on the accuracy of loop closure matching in the SLAM problem, and provides a method for extracting the coordinate angles of rectangular points in the environment based on a two-dimensional laser radar. The acquired rectangular point information includes not only the xy coordinates of the plane, but also the angle theta, which enhances the accurate scheduling of loop closure matching.

[0004] The present invention adopts the following technical solutions:

[0005] A method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar, characterized in that it comprises the following steps: S100: traversing the laser points in the natural environment point cloud frame scanned by the two-dimensional laser; S200: selecting a point set dots_near in the neighborhood of a random point Pk, calculating the angle between the x-axis of the laser coordinate system and all other points in dots_near starting from Pk, and calculating the mean of all angles as an angle dividing line of dots_near; S300: separating the point sets dots_left and dots_right on the left and right sides of dots_near according to the angle dividing line, and performing an angle division on dots_near. left and dots_right use the straight line random sampling consistency algorithm to extract the inner points inner_left and inner_right whose variance is less than the threshold; S400: fit line_left and line_right according to the inner points inner_left and inner_right in a straight line fitting manner, and calculate the angle between the two straight lines. When the angle is close to 90 degrees, the intersection of the two straight lines is determined to be a right angle point; S500: use the right angle point as the vertex to make two rays that coincide with line_left and line_right, and determine its angle based on one of the rays.

[0006] Preferably, in S500, according to the right-hand rule, the two rays are set as the X1Y1 axis, wherein the angle between the X1 axis and the X axis is taken as the angle of the right angle point.

[0007] Preferably, in S200, a Pk point is selected once for every five laser points.

[0008] Preferably, the number of the point set dots_near is greater than 20 and the numbers of the point set dots_left and the point set dots_right are greater than 10, otherwise a random point Pk is reselected.

[0009] Preferably, the diameter of the neighborhood is 0.5 m.

[0010] Preferably, in S400, when the included angle is between 80 degrees and 100 degrees, the intersection of the two straight lines is determined to be a right angle point.

[0011] Preferably, in S500, the step of taking the right-angle point as the center, respectively calculating the full quadrant angles of the rays on both sides, and taking the angle between a ray and the X-axis as the angle of the right-angle point.

[0012] Preferably, the method further includes S600: saving the right-angle point in vector vector_corner, and if the distance and angle difference between a subsequent right-angle point Pm and any point in vector_corner is within a threshold, they are determined to be the same point, and Pm is not saved in vector_corner.

[0013] Compared with the prior art, the present invention has the following advantages:

[0014] The present invention provides a method for extracting the coordinate angle of an environmental right-angle point based on a two-dimensional laser radar. The acquired right-angle point is used as a feature point for loop matching, and two straight lines are fitted from the point sets on the left and right sides of the angle dividing line to determine whether it is a right-angle point. Two rays are made with the right-angle point as the vertex, and the angle is determined based on one of the rays. The right-angle point is determined by plane coordinates and angles, thereby enhancing the accurate scheduling of loop matching. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is a flow chart of the present invention.

[0016] Figure 2 Schematic diagram of the right-angle point determination results. DETAILED DESCRIPTION

[0017] In order to facilitate the understanding of the technical solution of the present invention, a detailed description is given below in conjunction with the accompanying drawings and specific embodiments.

[0018] Example 1

[0019] like Figure 1-2 As shown, a method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar comprises the following steps:

[0020] S100: traversing laser points in a natural environment point cloud frame scanned by a two-dimensional laser;

[0021] Specifically, the laser is installed on the AGV car, and the car is driven to travel in the natural environment. The laser is used to perform two-dimensional scanning and the scanned natural environment point cloud is input into the laser composition software. The two-dimensional laser slam composition is performed in real time, and all laser points are traversed. To improve efficiency, the step is set to 5 in this embodiment, that is, all laser points are calculated every 5;

[0022] S200: Select a point set dots_near in the neighborhood of a random point Pk, calculate the angles between Pk and all other points in dots_near and the x-axis of the laser coordinate system, and calculate the average of all angles as the angle dividing line of dots_near;

[0023] Specifically, for the point Pk selected in S100, select the point set dots_near in the neighborhood of Pk. If the number of points in dots_near is less than 20, the calculation fails and the loop continues until the number of points in the point set dots_near in the neighborhood of a certain point Pk is greater than 20. Then, take Pk as the starting point, calculate the angles of all other points in dots_near, and find the average of all angles to make the angle dividing line of dots_near.

[0024] Among them, when calculating the angles of all other points in dots_near, a laser coordinate system is first made in the natural environment point cloud frame, and then the Pk point is connected with other points to form several straight lines, and the angles of each straight line and the X-axis are calculated; the angle dividing line may not be one of all the lines. For example, if the mean of all angles is 40 degrees, then a ray with an angle of 40 degrees is made based on the X-axis, and the ray is the angle dividing line; in addition, the diameter of the neighborhood can be selected according to the size of the actual environment, and in this embodiment, it is selected as 0.5m;

[0025] S300: Separate the point sets dots_left and dots_right on the left and right sides of dots_near according to the angle segmentation line, and use the straight line random sampling consistency algorithm to extract the inner points inner_left and inner_right whose variances are less than the threshold value from dots_left and dots_right;

[0026] Among them, if the number of points in any of the point sets dots_left and dots_right is less than 10, the current calculation fails and the loop continues; if the number of points in any of inner_left and inner_right is less than 10, the current calculation fails and the loop continues; the threshold can be set according to the ranging accuracy of the laser;

[0027] S400: fitting line_left and line_right in a straight line fitting manner according to the inner points inner_left and inner_right, and finding the angle between the two straight lines. When the angle is close to 90 degrees, the intersection of the two straight lines is determined to be a right angle point, and the coordinates of the intersection of the two straight lines are the coordinates of the right angle point.

[0028] Specifically, in this embodiment, when the angle is between 80 degrees and 100 degrees, the intersection of the two straight lines is determined to be a right angle point;

[0029] S500: Draw two rays coinciding with line_left and line_right with the right angle point as the vertex, and determine the angle with one of the rays as the reference;

[0030] Specifically, according to the right-hand rule, the two rays are set as the X1Y1 axis, and the full quadrant angles of the two side rays in the laser coordinate system are calculated respectively, and the angle between the X1 axis and the X axis is taken as the angle of the right angle point;

[0031] S600: Save the right-angle point in vector vector_corner. If the distance and angle difference between a subsequent right-angle point Pm and any point in vector_corner is within a threshold, they are determined to be the same point, and Pm is not saved in vector_corner. In other words, if the obtained right-angle point is close to the previously obtained one to a certain extent, it is determined to be a duplicate point and is not retained.

[0032] The above are only preferred embodiments of the present invention. The protection scope of the present invention shall be based on the scope defined by the claims. Several improvements and modifications made by those skilled in the art without departing from the spirit and scope of the present invention should also be regarded as the protection scope of the present invention.

Claims

1. A method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar, characterized in that: The following steps are involved: S100: traversing laser points in a natural environment point cloud frame scanned by a two-dimensional laser; S200: Select a point set dots_near in the neighborhood of a random point Pk, calculate the angles between Pk and all other points in dots_near and the x-axis of the laser coordinate system, and calculate the average of all angles as the angle dividing line of dots_near; S300: Separate the point sets dots_left and dots_right on the left and right sides of dots_near according to the angle segmentation line, and use the straight line random sampling consistency algorithm to extract the inner points inner_left and inner_right whose variances are less than the threshold value from dots_left and dots_right; S400: fitting line_left and line_right in a straight line fitting manner according to the inner points inner_left and inner_right, and finding the angle between the two straight lines. When the angle is close to 90 degrees, determining that the intersection of the two straight lines is a right angle point; S500: With the right-angle point as the vertex, draw two rays that coincide with line_left and line_right, and determine the angle of one of the rays based on the right-angle point. In S500, according to the right-hand rule, the two rays are set as the X1Y1 axis, wherein the angle between the X1 axis and the X axis is taken as the angle of the right-angle point. For the point Pk selected in S200, select the point set dots_near in the neighborhood of Pk. If the number of points in dots_near is less than 20, the calculation fails and the loop continues until the number of points in the point set dots_near in the neighborhood of a certain point Pk is greater than 20. After that, take Pk as the starting point, calculate the angles to all other points in dots_near, and find the average of all angles to make the angle dividing line of dots_near.

2. The method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar according to claim 1, characterized in that: In the above S200, a Pk point is selected once for every 5 laser points.

3. The method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar according to claim 1, characterized in that: The number of the point set dots_near is greater than 20 and the number of the point set dots_left and the point set dots_right is greater than 10, otherwise a random point Pk is reselected.

4. The method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar according to claim 1, characterized in that: The diameter of the neighborhood is 0.5 m.

5. The method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar according to claim 1, characterized in that: In the above S400, when the included angle is between 80 degrees and 100 degrees, the intersection of the two straight lines is determined to be a right angle point.

6. The method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar according to claim 1, characterized in that: The above S500 further includes taking the right angle point as the center, respectively calculating the full quadrant angles of the rays on both sides, and taking the angle between a ray and the X-axis as the angle of the right angle point.

7. The method for extracting the coordinate angle of an environmental rectangular point based on a two-dimensional laser radar according to claim 1, characterized in that: Also includes: S600: The right-angle point is saved in vector vector_corner. If the distance and angle difference between a subsequent right-angle point Pm and any point in vector_corner is within a threshold, they are determined to be the same point, and Pm is not saved in vector_corner.

Images