Robot-assisted positioning method for triangular identifier detection

Abstract

The invention belongs to the technical field of robot positioning, and particularly relates to a robot-assisted positioning method for triangular identifier detection, which comprises the following steps: setting a plurality of groups of triangular identifiers with different angles on site, identifying reflective stripes and lengths, calculating the angles between the reflective stripes, determining the identifiers through the angles, and setting the id of the identifiers. Sending to a Cartograder algorithm for marking, carrying out auxiliary robot positioning and closed loop, and reducing accumulative errors; according to the method, the robot can be helped to construct a more compact and concise map identifier, compared with dependence on a large amount of irregular point cloud data, the map is easier to store and process, and the problems of inaccurate laser matching and difficult positioning caused by environmental changes are solved.

Description

Technical Field

[0001] The invention belongs to the technical field of robot positioning, and in particular relates to a robot-assisted positioning method for triangle mark detection. Background Art

[0002] With the rapid development of inspection robots, automatic guided vehicles and other fields, mobile robots have put forward higher requirements for autonomous navigation. SLAM (Simultaneous Localization and Mapping) is a key technology for map construction and positioning, and is a core issue in the fields of robotics, computer vision and autonomous driving. Cartographer is the most mainstream 2D ​​laser SLAM algorithm, with advantages such as high accuracy and maps that can be used for path planning.

[0003] However, due to the complex and changeable environment of robot positioning in actual use, such as the presence of dynamic obstacles that make it difficult to match laser scanning data, poor performance of visual sensors due to different lighting conditions, or sensor limitations such as low resolution of LiDAR devices and sensor noise that causes erroneous data points, the laser is inaccurate. Once the LiDAR is wrong, the robot's self-positioning accuracy may drop significantly, and it will not be able to correctly identify obstacles or understand environmental changes. The robot's path planning and obstacle avoidance functions will be seriously affected, which may cause the robot to hit obstacles or be unable to effectively bypass obstacles, resulting in mission failure or hardware damage, or even loss of loop detection function, resulting in damage to the consistency and accuracy of the map. In certain application scenarios where safety risks increase, such as autonomous vehicles or industrial automation equipment, inaccurate LiDAR positioning may cause serious safety hazards. The robot may collide with the environment because it cannot respond to environmental changes in a timely manner, endangering human life and property. In short, inaccurate laser matching is an issue that cannot be ignored. It is directly related to whether the robot can perform tasks stably and reliably. Summary of the invention

[0004] The technical problem solved by the present invention is to provide a robot-assisted positioning method for triangle mark detection, which solves the problem that the robot is difficult to position or the robot positioning is lost due to inaccurate laser matching of the robot due to complex environment, sensor limitations and other factors.

[0005] The basic solution provided by the present invention is a robot-assisted positioning method for triangle mark detection, which specifically includes the following steps:

[0006] S1: Place multiple groups of triangular markers with different angles into the robot positioning scene, and the same group of triangular markers all include two reflective strips of the same length;

[0007] S2: Obtain radar point cloud data, filter out laser points that meet the reflection intensity threshold and store them in set A, and calculate the two-dimensional coordinate system and angle of the laser points in set A in the radar coordinate system;

[0008] S3: The laser points in set A with continuous indexes and a number of continuous laser points greater than a preset number are recorded as a subset as Bn, and all subsets are stored in a set to form a set B = {B1, B2, ..., Bn};

[0009] S4: traverse the set B, and determine whether the laser points of each subset in the set B meet the characteristics of the reflective strip by calculation. If at least two subsets in the set B meet the characteristics of the reflective strip, proceed to the next step, otherwise return to S2;

[0010] S5: Determine whether the reflective strips are connected, if so, proceed to the next step, otherwise return to S2;

[0011] S6: Calculate the included angle of the reflective strip. If the included angle meets the included angle of the arranged triangular mark, it is considered that a triangular mark is recognized, otherwise, return to S2;

[0012] S7: Determine whether the triangle mark is recognized for the first time. If so, set the id of the triangle mark to 0 and send the information of the triangle mark to the Cartograph algorithm for marking. Otherwise, proceed to the next step.

[0013] S8: Determine whether the currently recognized triangle marker is a marked triangle marker. If so, directly send the currently recognized triangle marker information to the Cartograher algorithm for marking. Otherwise, add 1 to the currently recognized triangle marker id as the id of the triangle marker, and then send the triangle marker information to the Cartograher algorithm for marking, and return to S2 until all triangle markers are recognized.

[0014] The principle and advantages of the present invention are: by setting special marks on site, using radar to extract and identify the mark information, thereby providing landmark observation information to assist robot positioning; using special marks can help build more compact and concise map marks, and the map is easier to store and process than relying on a large amount of irregular point cloud data;

[0015] The marker is a triangular marker with the same length of reflective strips but different angles in each group. By identifying the reflective strips and their length and calculating the angles between the reflective strips, the marker is determined by the angle, and the marker ID is set and sent to the Cartograher algorithm for marking to assist the robot in positioning and closing the loop and reduce cumulative errors. The triangular marker is a clear feature point that is easily recognized by the radar system, which helps to simplify map construction and position estimation, reduces the risk of feature matching errors, effectively improves the reliability and safety of the positioning system, and solves the problem of robot positioning difficulties caused by inaccurate laser matching or sensor failure.

[0016] Preferably, the length of the reflective strip is 10 to 30 cm, and the length of the reflective strip is 15 cm. By selecting a reflective strip of appropriate length, it is ensured that the robot can effectively detect the mark in the entire range of motion, thereby improving the accuracy of navigation and positioning. Reflective strips of this length are easier to identify, which can simplify the image processing algorithm, improve processing speed and reliability, and reduce the possibility of misidentification.

[0017] Preferably, the included angle between the two reflective strips in the triangular logo is 90° to 150°. The included angle between the two reflective strips in the triangular logo is set within a suitable range to increase the characteristics of the logo and make it easier to identify and calculate.

[0018] Preferably, in step S4, the method for determining whether a subset in set B satisfies the characteristics of the reflective strip is as follows:

[0019] S4-1) calculating the average reflection intensity of the laser points in the subset and calculating the obstacle length;

[0020] S4-2) If the average reflection intensity is greater than the average reflection intensity threshold, and the obstacle length error is within the error threshold, it is considered that the subset in set B meets the characteristics of the reflective strip.

[0021] The method of determining whether it is a reflective strip by the reflection intensity and length of the reflective strip not only helps to reduce misidentification, but also can further verify whether the detected object meets the expected reflective strip shape when combined with the length information, thereby improving the accuracy of recognition and enhancing the robustness and flexibility of the system.

[0022] Preferably, in step S5, if the Euclidean distance between the two reflective strips is less than the distance threshold, the two reflective strips are considered to be connected. By setting a reasonable distance threshold, it is possible to effectively avoid mistakenly identifying reflective strips that are far apart as connected, and accurately determine that they are reflective strips of the same group of triangle marks.

[0023] Preferably, in step S6, the steps of calculating the angle of the reflective strip are as follows:

[0024] S6-1) calculating the inclination angle of the reflective strip by using the RANSAC straight line fitting method;

[0025] S6-2) Calculating the included angle of the reflective strips by the inclination angles of the two reflective strips.

[0026] By fitting a straight line through RANSAC to calculate the tilt angle, there is no need to make specific assumptions about the distribution of data points. It can process data point sets of arbitrary shapes and densities. In complex environments or when the amount of data is large, such as when the reflective strip is partially blocked or there are multiple reflective strips interlaced, it can still be accurately identified. The iterative process is relatively independent and parallel computing is easy to implement, which further improves the speed and response time of the algorithm.

[0027] Preferably, the information of the triangle identification includes the triangle identification ID, the coordinates of the triangle identification in the map coordinate system, and the angle information.

[0028] Further preferably, the coordinates and angles of the triangle marker in the map coordinate system are calculated as follows:

[0029] a. Calculate the two-dimensional coordinates of the midpoint of the base of the triangle mark in the radar coordinate system, and the coordinates of the connecting point of the two reflective strips in the triangle mark in the radar coordinate system;

[0030] b. Calculate the angle between the connecting point of the two reflective strips and the midpoint of the base of the triangle mark as the angle of the triangle mark in the radar coordinate system;

[0031] c. The coordinates and angles of the triangle mark in the map coordinate system are calculated based on the coordinates and angles of the triangle mark in the radar coordinate system and the coordinates and angles of the radar in the map coordinate system. The triangle mark information is determined by combining the radar coordinate system and the map coordinate system to assist the robot in positioning and form a closed loop, reduce cumulative errors, and improve positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 is a flow chart of the present invention;

[0033] Figure 2 It is a schematic diagram of the triangle identification structure of the present invention;

[0034] Figure 3 It is a top view of the triangle mark of the present invention. DETAILED DESCRIPTION

[0035] The following is further described in detail through specific implementation methods:

[0036] The specific implementation process is as follows: Figures 1 to 3 , a robot-assisted positioning method for triangle mark detection, specifically comprising the following steps:

[0037] S1: Place multiple groups of triangular markers 1 with different included angles into the robot positioning scene, wherein the same group of triangular markers all include two reflective strips of the same length, the reflective strips are placed vertically, the long sides of the reflective strips are close to the plane of the placement platform 2, and the short sides are connected, so that the two reflective strips are in a triangular shape; the length of the reflective strip, i.e., the length of the long side, is 10 to 30 cm, and the included angle of the two reflective strips in the triangular marker is 90° to 150°;

[0038] In this embodiment, the triangular marks are preferably three groups, and the angles of the reflective strips of each group of triangular marks are 110°, 125°, and 140° respectively. The length of the reflective strips of each group of triangular marks is the same, and the length of the reflective strips is preferably 15 cm. When placing the triangular marks, each triangular mark is placed in a different orientation to prevent the robot radar from scanning two triangular marks at the same time, thereby reducing the calculation difficulty of positioning.

[0039] S2: Obtain radar point cloud data, filter out laser points that meet the reflection intensity threshold and store them in set A, and calculate the two-dimensional coordinate system and angle of the laser points in set A in the radar coordinate system; in this embodiment, the reflection intensity threshold is set to 900.

[0040] After the robot receives the mapping instruction, the robot radar emits laser at equal angles with itself as the center. The robot receives and obtains radar point cloud data, which includes the distance between the reflection point corresponding to the laser point on the obstacle and the radar center (hereinafter referred to as the laser point distance), the index of the laser point, and the angular resolution and reflection intensity value.

[0041] When receiving radar point cloud data, if the number of laser points in set A is less than 10, it is considered that there is no triangle mark in this data and the point cloud data continues to be received; if the number of laser points in set A is greater than or equal to 10, the two-dimensional coordinate system and angle of the laser point in the radar coordinate system are calculated according to the laser point distance, the index of the laser point and the angular resolution, which is recorded as [x, y, theta].

[0042] Among them, the angle of the laser point in the two-dimensional coordinate system in the radar coordinate system is:

[0043] theta=P×R

[0044] Where theta is the angle of the laser point in the two-dimensional coordinate system in the radar coordinate system, P is the index of the laser point, and R is the angular resolution of the laser point;

[0045] The two-dimensional coordinate system of the laser point in the radar coordinate system is calculated as follows:

[0046] x=range×cos(P×R)

[0047] y = range × sin (P × R)

[0048] Where x is the coordinate of the two-dimensional coordinate system in the radar coordinate system in the x direction, y is the coordinate of the two-dimensional coordinate system in the radar coordinate system in the y direction, range is the distance of the laser point, P is the index of the laser point, and R is the angular resolution of the laser point;

[0049] S3: The laser points in set A with continuous indexes and a number of continuous laser points greater than a preset number are recorded as a subset as Bn, and all subsets are stored in a set to form a set B = {B1, B2, ..., Bn}; the preset number is related to the distance between the obstacle and the center of the radar. Specifically, the relationship between the preset number and the distance between the obstacle and the center of the radar is shown in the following table:

[0050] Distance between obstacle and radar center X(m) Preset quantity (pcs) 1 X≤0.5 150 2 0.5＜X≤1 60 3 1＜X≤3 21 4 3＜X≤5 12 5 5＜X≤8 9 6 8＜X≤10 5

[0051] In this embodiment, the preset number is five.

[0052] S4: traverse the set B, and determine whether the laser points of each subset in the set B meet the characteristics of the reflective strip by calculation. If at least two subsets in the set B meet the characteristics of the reflective strip, proceed to the next step, otherwise return to S2;

[0053] In step S4, the method for determining whether a subset in set B satisfies the characteristics of a reflective strip is as follows:

[0054] S4-1) calculating the average reflection intensity of the laser points in the subset, and calculating the obstacle length by the Larmer-Douglas-Peucker algorithm;

[0055] S4-2) If the average reflection intensity is greater than the average reflection intensity threshold, and the obstacle length error is within the error threshold, then the subset in set B is considered to meet the characteristics of the reflective strip. In this embodiment, the average reflection intensity threshold is preferably 1000, and the error threshold is 1 cm. Specifically, if the average reflection intensity of the subset is greater than 1000, and the obstacle length is 15±1 cm, then the subset is considered to meet the characteristics of the reflective strip, and the scanned obstacle is a reflective strip.

[0056] S5: Determine whether the reflective strips are connected, if so, proceed to the next step, otherwise return to S2;

[0057] In step S5, if the Euclidean distance between the two reflective strips is less than a distance threshold, it is considered that the two reflective strips are connected. In this embodiment, the distance threshold is 1 cm.

[0058] Specifically, all points in the subset involved in determining the reflective strips are considered as line segments, so there will be two 15 cm long line segments, and these two line segments are used as reference line segments to calculate whether the two reflective strips are connected, such as Figure 2As shown, line segments AB1 and B2C are line segments with a length of 15 cm, and line segments AB1 and B2C are used as reference line segments; when calculating, according to the laser point index information, the coordinates of the two endpoints with similar indexes of the two line segments are taken for distance calculation, that is, the coordinates of the last point of line segment AB1 are taken as B1 (x1, y1), and the coordinates of the first point of line segment B2C are taken as B2 (x2, y2), then the distance of B1B2, that is, the Euclidean distance calculation formula of the two reflective strips is:

[0059]

[0060] Wherein, L is the Euclidean distance between the two reflective strips, x1 is the x-coordinate of point B1, x2 is the x-coordinate of point B2, y1 is the y-coordinate of point B1, and y2 is the y-coordinate of point B2.

[0061] If the distance between the two reflective strips, i.e. the distance L between B1 and B2, is less than 1 cm, the two reflective strips are considered to be connected.

[0062] S6: Calculate the angle between the two reflective strips. If the error of the calculated angle is within the error range of the angle of the arranged triangle mark, it is considered that a triangle mark is recognized, otherwise return to S2;

[0063] In step S6, the steps of calculating the angle between the two reflective strips are as follows:

[0064] S6-1) calculating the tilt angles of the two reflective strips by using the RANSAC straight line fitting method;

[0065] S6-2) calculating the slope of the two reflective strips according to the inclination angle of the reflective strips, and calculating the angle between the two reflective strips according to the slope of the reflective strips.

[0066] The slope calculation formula of the reflective strip is as follows:

[0067] k = tanθ

[0068] Where k is the slope of the reflective strip, and θ is the inclination angle of the reflective strip;

[0069] The angle calculation formula of two reflective strips is as follows:

[0070] β=tan -1 (mn)÷(1+m×n)

[0071] Wherein, β is the angle between the two reflective strips, m is the slope of one reflective strip, and n is the slope of the other reflective strip.

[0072] The error of the angle is 5°. If the error of the calculated reflective strip angle is within the error range, the triangle mark is considered to be recognized. In this embodiment, the reflective strip angle is 127°. If it is within 125°±5°, the triangle mark is considered to be recognized.

[0073] S7: Determine whether the triangle mark is recognized for the first time. If so, set the id of the triangle mark to 0 and send the information of the triangle mark to the Cartograph algorithm for marking. Otherwise, proceed to the next step.

[0074] The information of the triangle identification includes the triangle identification ID, the coordinates of the triangle identification in the map coordinate system, and the angle information.

[0075] Further preferably, the coordinates and angles of the triangle marker in the map coordinate system are calculated as follows:

[0076] a. Calculate the two-dimensional coordinates of the midpoint of the base of the triangle in the radar coordinate system, and the coordinates of the points where the two reflective strips in the triangle are connected, i.e., the vertices of the triangle in the radar coordinate system;

[0077] Since the length and connection of the reflective strips have been confirmed, the laser points involved in the identification calculation are in the shape of a triangle, and the coordinates of the triangle mark can be confirmed by calculating the coordinates of the triangle. Figure 2 , the laser points involved in identifying the triangle mark form a triangle AB1C, then the midpoint of the base of the triangle mark is D, and the coordinates of the midpoint of the base are calculated as follows:

[0078]

[0079] In the formula, x D is the x-coordinate of the midpoint of the bottom edge in the radar coordinate system, y D is the y coordinate of the midpoint of the base in the radar coordinate system, x3 is the x coordinate of point A in the radar coordinate system, x4 is the x coordinate of point C in the radar coordinate system, y3 is the y coordinate of A in the radar coordinate system, and y4 is the y coordinate of point C in the radar coordinate system.

[0080] The coordinates of the point where the two reflective strips in the triangle mark are connected, that is, the vertex B of the triangle mark, are calculated as follows:

[0081]

[0082] In the formula, x B is the x coordinate of vertex B in the radar coordinate system, y B is the y coordinate of vertex B in the radar coordinate system, x1 is the x coordinate of point B1 in the radar coordinate system, x2 is the x coordinate of point B2 in the radar coordinate system, y1 is the y coordinate of B1 in the radar coordinate system, and y2 is the y coordinate of point B2 in the radar coordinate system.

[0083] b. Calculate the angle between the connecting point of the two reflective strips and the midpoint of the base of the triangle mark as the angle of the triangle mark in the radar coordinate system;

[0084] The angle between the connecting point of the two reflective strips and the midpoint of the base of the triangle mark is calculated as follows:

[0085]

[0086] In the formula, θ BD is the angle between the connecting point of the two reflective strips and the midpoint of the base of the triangle mark, x B is the x-coordinate of the connected point in the radar coordinate system, y B is the y coordinate of the connected point in the radar coordinate system, x D The x-coordinate and y-coordinate of the midpoint of the base of the triangle in the radar coordinate system D The y coordinate of the midpoint of the base of the triangle in the radar coordinate system.

[0087] c. The coordinates and angles of the triangle marker in the map coordinate system are calculated based on the coordinates and angles of the triangle marker in the radar coordinate system and the coordinates and angles of the radar in the map coordinate system.

[0088] The coordinate and angle calculation formula of the triangle marker in the map coordinate system is as follows:

[0089] P BtoMap =P BtoLidar *P LidartoMap

[0090] Where P BtoMap The coordinates and angles of the triangle in the map coordinate system, P BtoLidar is the coordinate and angle of the triangle marker in the radar coordinate system, P LidartoMap It is the coordinate and angle of the radar in the map coordinate system.

[0091] S8: Determine whether the currently recognized triangle identifier is a marked triangle identifier. If so, directly send the currently recognized triangle identifier information to the Cartograher algorithm for marking. Otherwise, add 1 to the currently recognized triangle identifier ID as the triangle identifier ID, and then send the triangle identifier information to the Cartograher algorithm for marking, and return to S2 until all triangle identifiers are recognized. That is, in this embodiment, triangle identifiers with angles of 110°, 125°, and 140° are all recognized and marked.

[0092] The above are only embodiments of the present invention. Common knowledge such as the known specific structures and characteristics in the scheme is not described in detail here. Ordinary technicians in the relevant field know all the common technical knowledge in the technical field to which the invention belongs before the application date or priority date, can obtain all the existing technologies in the field, and have the ability to apply conventional experimental means before that date. Ordinary technicians in the relevant field can improve and implement this scheme in combination with their own abilities under the enlightenment given by this application. Some typical known structures or known methods should not become obstacles for ordinary technicians in the relevant field to implement this application. It should be pointed out that for those skilled in the art, without departing from the structure of the present invention, several deformations and improvements can be made, which should also be regarded as the scope of protection of the present invention, and these will not affect the effect of the implementation of the present invention and the practicality of the patent. The scope of protection required by this application shall be based on the content of its claims, and the specific implementation methods and other records in the specification can be used to interpret the content of the claims.

Claims

1. A robot-assisted positioning method for triangle mark detection, characterized in that: The specific steps include: S1: Place multiple groups of triangular markers with different angles into the robot positioning scene, and the same group of triangular markers all include two reflective strips of the same length; S2: Obtain radar point cloud data, filter out laser points that meet the reflection intensity threshold and store them in set A, and calculate the two-dimensional coordinate system and angle of the laser points in set A in the radar coordinate system; S3: The laser points in set A with continuous indexes and a number of continuous laser points greater than a preset number are recorded as a subset as Bn, and all subsets are stored in a set to form a set B = {B1, B2, ..., Bn}; S4: traverse the set B, and determine whether the laser points of each subset in the set B meet the characteristics of the reflective strip by calculation. If at least two subsets in the set B meet the characteristics of the reflective strip, proceed to the next step, otherwise return to S2; S5: Determine whether the reflective strips are connected, if so, proceed to the next step, otherwise return to S2; S6: Calculate the included angle of the reflective strip. If the included angle meets the included angle of the arranged triangular mark, it is considered that a triangular mark is recognized, otherwise, return to S2; S7: Determine whether the triangle mark is recognized for the first time. If so, set the id of the triangle mark to 0 and send the information of the triangle mark to the Cartograph algorithm for marking. Otherwise, proceed to the next step. S8: Determine whether the currently recognized triangle marker is a marked triangle marker. If so, directly send the currently recognized triangle marker information to the Cartograher algorithm for marking. Otherwise, add 1 to the currently recognized triangle marker id as the id of the triangle marker, and then send the triangle marker information to the Cartograher algorithm for marking, and return to S2 until all triangle markers are recognized.

2. The robot-assisted positioning method for triangle mark detection according to claim 1, characterized in that: The length of the reflective strip is 10 to 30 cm.

3. The robot-assisted positioning method for triangle mark detection according to claim 2, characterized in that: The length of the reflective strip is 15 cm.

4. The robot-assisted positioning method for triangle mark detection according to claim 1, characterized in that: The included angle between the two reflective strips in the triangular logo is 90° to 150°.

5. The robot-assisted positioning method for triangle mark detection according to claim 1, characterized in that: In step S4, the method for determining whether a subset in set B satisfies the characteristics of a reflective strip is as follows: S4-1) calculating the average reflection intensity of the laser points in the subset and calculating the obstacle length; S4-2) If the average reflection intensity is greater than the average reflection intensity threshold, and the obstacle length error is within the error threshold, it is considered that the subset in set B meets the characteristics of the reflective strip.

6. The robot-assisted positioning method for triangle mark detection according to claim 1, characterized in that: In step S5, if the Euclidean distance between the two reflective strips is less than the distance threshold, it is considered that the two reflective strips are connected.

7. The robot-assisted positioning method for triangle mark detection according to claim 1, characterized in that: In step S6, the steps of calculating the angle of the reflective strip are as follows: S6-1) calculating the inclination angle of the reflective strip by using the RANSAC straight line fitting method; S6-2) Calculating the included angle of the reflective strips by the inclination angles of the two reflective strips.

8. The robot-assisted positioning method for triangle mark detection according to claim 1, characterized in that: The information of the triangle identification includes the triangle identification ID, the coordinates of the triangle identification in the map coordinate system, and the angle information.

9. The robot-assisted positioning method for triangle mark detection according to claim 8, characterized in that: The calculation method of the coordinates and angles of the triangle marker in the map coordinate system is as follows: a. Calculate the two-dimensional coordinates of the midpoint of the base of the triangle mark in the radar coordinate system, and the coordinates of the connecting point of the two reflective strips in the triangle mark in the radar coordinate system; b. Calculate the angle between the connecting point of the two reflective strips and the midpoint of the base of the triangle mark as the angle of the triangle mark in the radar coordinate system; c. The coordinates and angles of the triangle marker in the map coordinate system are calculated based on the coordinates and angles of the triangle marker in the radar coordinate system and the coordinates and angles of the radar in the map coordinate system.

Images