A two-dimensional laser global positioning algorithm based on reflective markers

Abstract

The present invention discloses a two-dimensional laser global positioning algorithm based on reflective markers, comprising: Step 1: performing local scanning using a two-dimensional laser radar, and forming a point set from the center points of all reflective objects; Step 2: randomly extracting and combining different point pairs in the point set, and calculating the Cartesian distance of any point pair; Step 3: matching the Cartesian distance of the point pair with the Cartesian distances of all point pairs in a global map and calculating two initial poses of the point pair; Step 4: calculating the relative global coordinates of other points in the point set according to the initial poses, determining the corresponding matching points in the global map, and then inferring the corresponding positions of the two-dimensional laser radar through the matching points, and performing averaging processing on the corresponding positions of the two-dimensional laser radar inferred from all matching points; Step 5: repeating Step 2-Step 4, and scoring and ranking all the averaging results to determine the global positioning of the two-dimensional laser radar.

Description

Technical Field

[0001] The present invention relates to the technical field of low-speed mobile robots, and in particular to a two-dimensional laser global positioning algorithm based on reflective markers applicable to low-speed mobile robots. Background Art

[0002] In the field of low-speed mobile robots, there is a type of scenario where automatic navigation accuracy is very high, such as automatic forklifts picking up goods, and the automatic navigation accuracy is required to be plus or minus 5mm or even higher.

[0003] In such situations, a laser reverse positioning mode is often used. This involves using a two-dimensional laser as an observation device and placing high-reflection markers in the environment (e.g., pillars covered with high-reflectivity film, or reflective sheets directly attached to walls). The center of the reflective pillar or sheet is extracted from the two-dimensional laser, optimized and matched with the previous reflective object map to calculate the LiDAR's position within the map. Positioning calculations are divided into global and local positioning. Global positioning is based on the LiDAR's startup position, and the approximate initial values ​​of the LiDAR are not known in advance, requiring full map comparison calculations. Local positioning involves optimizing and matching within a local area of ​​the map after the global positioning calculation is successful.

[0004] In existing technologies, distance measurement is typically achieved using triangulation or time-of-flight methods. Triangulation determines distance by measuring the angle and length of a laser beam, while time-of-flight calculates distance by measuring the time it takes for a laser beam to travel to a target and back. These methods are computationally intensive and have low accuracy. Summary of the Invention

[0005] In order to solve the above technical problems, the present invention provides a two-dimensional laser global positioning algorithm based on reflective markers.

[0006] The present invention adopts the following technical solution: a two-dimensional laser global positioning algorithm based on reflective markers, comprising:

[0007] Step 1: Use a two-dimensional laser radar to perform local scanning and group the center points of all reflectors into a point set;

[0008] Step 2: Randomly extract and combine different point pairs in the point set, and calculate the Cartesian distance between any point pairs;

[0009] Step 3: Match the Cartesian distance of the point pair with the Cartesian distance of all point pairs in the global map and calculate the two initial poses of the point pair;

[0010] Step 4: Calculate the relative global coordinates of other points in the point cloud based on the initial pose, determine the corresponding matching points in the global map, and then reverse-calculate the corresponding positions of the 2D lidar through these matching points. Average the corresponding positions of the 2D lidar obtained from all the matching points.

[0011] Step 5: Repeat Step 2 - Step 4, and score and sort all the averaged results to determine the global positioning of the 2D lidar.

[0012] Preferably, in Step 1, use the 2D lidar for local scanning, extract the central coordinate point set {Pi} of the reflection markers from the currently scanned laser point cloud. The number of coordinate points in the point set {Pi} ≥ 4. If the number of reflection markers within the scanning range is less than 4, move the 2D lidar until the number of coordinate points in the point set {Pi} ≥ 4.

[0013] Preferably, in Step 2, randomly select at most 10 non-repeating point pairs, denote each point pair as (Pm, Pn), and calculate the Cartesian distance dist_mn between Pm and Pn.

[0014] Preferably, in Step 3, the coordinates of all reflection markers are recorded in the global map and form a point set {Li}. Traverse each point pair (Lj, Lk) in {Li}, calculate the Cartesian distance dist_jk between Lj and Lk. When |dist_jk - dist_mn| < e1, perform a calculation for global matching.

[0015] Preferably, in Step 4, move the pose of the lidar so that Pm coincides with Lk, and Pn lies on or extends on the line connecting Lj and Lk, obtain the global coordinates of the new positions of Pm and Pn, and thus reverse-calculate the global coordinate LaserInit_1 of the lidar at this time. Move the pose of the lidar so that Pn coincides with Lj, and Pm lies on or extends on the line connecting Lj and Lk, and thus obtain the global coordinates of the new positions of Pm and Pn, and reverse-calculate the global coordinate LaserInit_2 of the lidar at this time.

[0016] Preferably, in Step 4, take LaserInit_1 as the current global pose of the lidar, calculate the global coordinates {Pglobal_i} of all points in {Pi}. For each Pglobal_i, find the unique point Li in {Li} that is closest to it and has a distance less than e2 as the matching point of Pglobal_i in the map. Find the matching points of all points in {Pi} through the above method, and record the number of matching points count_match.

[0017] Preferably, in the Step 4, the g2o model is used to establish the observation edge and information matrix, and LaserInit1 is used as the initial value to perform g2o optimization, calculate the precise position Popti_i of the laser radar, and obtain the information matrix error chi of each matching point, and calculate the average value avg_chi of the information matrix error chi, and save Popti_i, count_match and avg_chi as a state s1 into std::vector, denoted as {Si}; then LaserInit_2 is used as the initial pose, and the above operation is repeated to calculate the state s2 and save it into {Si}.

[0018] Preferably, in the Step 5, all states in {Si} are scored according to the evaluation formula, and the evaluation formula is: value = cnt_match + 1.0 / avg_chi; the scores are then sorted, and the one with the highest score is used as the first candidate global pose; if the score of the second or lower score differs from the highest score by less than e4, it is considered that there is an ambiguous position, and the global positioning fails, and the user is informed that the area needs to be switched and the global positioning is repeated; otherwise, the global positioning is declared successful, and the global positioning pose is output.

[0019] Preferably, in Step 3, two circles are made with Li as the center and dist_mn±e1 as the radius, all points in {Pi} are traversed, the point between the two circles is taken as the undetermined point of Li, and the initial pose of the undetermined point is calculated.

[0020] Preferably, in Step 3, a radar is provided on each reflective marker, which is first scanned with a radius of dist_mn-e1 to exclude the scanned points in {Pi}, and then scanned with a radius of dist_mn+e1, and the scanned points are used as pending points and the initial pose of the pending points is calculated.

[0021] Compared with the prior art, the present invention has the following advantages: the present application utilizes a two-dimensional laser radar for local scanning, and groups the center points of all reflectors into a point set, randomly extracts and combines different point pairs in the point set, calculates the Cartesian distance of any point pair, matches the Cartesian distance of the point pair with the Cartesian distance of all point pairs in the global map and calculates the two initial poses of the point pair, determines the corresponding matching point in the global map, and then infers the corresponding position of the two-dimensional laser radar, scores and sorts the results to determine the global positioning of the two-dimensional laser radar, reduces the existence of ambiguity, has a small amount of calculation and high accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 This is the flow chart of the two-dimensional laser global positioning algorithm.

[0023] Figure 2 is the pre-acquired global map.

[0024] Figure 3 Schematic diagram of pose matching. DETAILED DESCRIPTION

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

[0026] like Figure 1 As shown, a two-dimensional laser global positioning algorithm based on reflective markers includes:

[0027] Step 1: Use a two-dimensional laser radar to perform local scanning and group the center points of all reflectors into a point set;

[0028] Step 2: Randomly extract and combine different point pairs in the point set, and calculate the Cartesian distance between the two center points in any point pair;

[0029] Step 3: Match the Cartesian distance of the point pair with the Cartesian distance of all point pairs in the global map and calculate the initial pose of the two center points in the point pair;

[0030] Step 4: Calculate the relative global coordinates of other points in the point set based on the initial pose, and determine their corresponding matching points in the global map. Then, use the matching points to infer the corresponding positions of the 2D LiDAR. The corresponding positions of the 2D LiDAR inferred from all matching points are averaged.

[0031] Step 5: Repeat Step 2-Step 4, and score and sort all the averaged processing results to determine the global positioning of the 2D lidar. The one with the highest score is used as the first candidate global pose.

[0032] Example 1

[0033] like Figure 1-Figure 3 As shown, a two-dimensional laser global positioning algorithm based on reflective markers uses a two-dimensional laser radar for local scanning, and extracts the center coordinate point set {Pi} of the reflective marker from the currently scanned laser point cloud. If the global position of the laser radar is to be uniquely determined, the number of {Pi} must be ≥3, so that at least three point pairs can be formed. In this embodiment, considering the map error and the accuracy required for positioning, the number of coordinate points in the point set {Pi} is ≥4. If the number of reflective markers within the scanning range is less than 4, a failure state is returned, and the two-dimensional laser radar is moved until the number of coordinate points in the point set {Pi} is ≥4.

[0034] After that, randomly select at most 10 non-repeating pairs of points from the point set {Pi}, denote each pair of points as (Pm, Pn), and calculate the Cartesian distance dist_mn between Pm and Pn.

[0035] The global map records the coordinates of all reflective markers, and forms a point set {Li} with the coordinate points of all reflective markers. Traverse each pair of points (Lj, Lk) in {Li}, calculate the Cartesian distance dist_jk between Lj and Lk. When |dist_jk - dist_mn| < e1, perform a calculation for global matching. Here, e1 needs to be set according to the maximum error during the construction of the global map. For example, in this embodiment, it is set to 0.05m.

[0036] During global matching, first move the pose of the lidar so that Pm coincides with Lk, and Pn lies on or extends on the line connecting Lj and Lk, and obtain the global coordinates of the new positions of Pm and Pn (that is, the coordinates of the new positions of Pm and Pn in the global map), so as to inversely deduce the global coordinate LaserInit_1 of the lidar at this time; move the pose of the lidar so that Pn coincides with Lj, and Pm lies on or extends on the line connecting Lj and Lk, and thus obtain the global coordinates of the new positions of Pm and Pn, so as to inversely deduce the global coordinate LaserInit_2 of the lidar at this time.

[0037] After that, take LaserInit_1 as the current global pose of the lidar, calculate the global coordinates {Pglobal_i} of all points in {Pi}. For each Pglobal_i, find the unique point Li in {Li} that is closest to it and the distance is less than e2 (the selection of e2 needs to be set according to the map error and the lidar resolution error. In this embodiment, it is set to 0.17m), and use it as the matching point of Pglobal_i in the map. Find the matching points of all points in {Pi} through the above method, and record the number of matching points count_match; then use the g2o model to establish the observation edge and the information matrix, and take LaserInit1 as the initial value to perform g2o optimization, calculate the accurate position Popti_i where the lidar is located, and obtain the information matrix error chi of each matching point, and calculate the average value avg_chi of the information matrix error chi. Save Popti_i, count_match and avg_chi as a state s1 into the std::vector, denoted as {Si}; then take LaserInit_2 as the initial pose, repeat the above operations, calculate the state s2, and save it into {Si}.

[0038] If there are other combinations of global map point pairs with similar Cartesian distances (dist_mn) to (Pm, Pn), the above process needs to be repeated to calculate the state Si until there are no similar distance point pairs in the map. All Si that meet the conditions are saved in {Si}. Because there may be duplicate points, they are compared with the points in {Si} before being stored. When both the distance and angle are less than a certain threshold, they are considered duplicate points and are not stored.

[0039] All states in {Si} are scored according to the evaluation formula: value = cnt_match + 1.0 / avg_chi. The scores are then sorted, and the one with the highest score is selected as the first candidate global pose. If the difference between the second or lower score and the highest score is less than e4, it is considered that there is an ambiguous position and global positioning fails. The user is informed that they need to switch regions and perform global positioning again. Otherwise, global positioning is declared successful and the global positioning pose is output.

[0040] Example 2

[0041] Example 2 is basically the same as Example 1, with the difference being that, when matching points, two circles are made with Li as the center and dist_mn±e1 as the radius, and all points in {Pi} are traversed. The points between the two circles are taken as the pending points of Li, and the initial pose of the pending points is calculated. Then, global matching is performed, and the pose of the laser radar is moved so that Pm coincides with Lk, and Pn is on the line connecting Lj and Lk or on the extension line, and the global coordinates of the new positions of Pm and Pn are obtained (that is, the coordinates of the new positions of Pm and Pn in the global map), thereby inferring the global coordinates LaserInit_1 of the laser radar at this time; the pose of the laser radar is moved so that Pn coincides with Lj, and Pm is on the line connecting Lj and Lk or on the extension line, thereby inferring the global coordinates LaserInit_2 of the new positions of the laser radar at this time.

[0042] Example 3

[0043] Example 3 is basically the same as Example 1, with the difference that each reflective marker is provided with a radar. When matching points, the scan is first performed with a radius of dist_mn-e1, and the points in the scanned {Pi} are excluded. Then, the scan is performed with a radius of dist_mn+e1, and the scanned points are used as the pending points and the initial pose of the pending points is calculated. Then, global matching is performed, and the pose of the laser radar is moved so that Pm coincides with Lk, and Pn is on the line connecting Lj and Lk or on the extension line, and the global coordinates of the new positions of Pm and Pn are obtained (that is, the coordinates of the new positions of Pm and Pn in the global map), thereby inferring the global coordinates LaserInit_1 of the laser radar at this time; the pose of the laser radar is moved so that Pn coincides with Lj, and Pm is on the line connecting Lj and Lk or on the extension line, thereby inferring the global coordinates of the new positions of Pm and Pn, thereby inferring the global coordinates LaserInit_2 of the laser radar at this time.

[0044] The above are only preferred embodiments of the present invention. The scope of protection 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 scope of protection of the present invention.

Claims

1. A two-dimensional laser global positioning algorithm based on reflective markers, characterized in that: Including: Step 1: Conduct local scanning using a 2D lidar and form a point set consisting of the center points of all reflectors. Step 2: Randomly extract and combine different point pairs in the point set and calculate the Cartesian distance of any point pair. Step 3: Match the Cartesian distance of this point pair with the Cartesian distances of all point pairs in the global map and obtain the two initial poses of this point pair. Step 4: Calculate the relative global coordinates of other points in the point set based on the initial poses, determine their corresponding matching points in the global map, then inversely deduce the corresponding position of the 2D lidar through this matching point, and perform an averaging process on the corresponding positions of the 2D lidar inversely deduced from all matching points. Step 5: Repeat Step 2 - Step 4 and score and sort all the averaging results to determine the global positioning of the 2D lidar.

2. The two-dimensional laser global positioning algorithm based on reflective markers according to claim 1 is characterized in that: In the said Step 1, local scanning is conducted using a 2D lidar, and the central coordinate point set {Pi} of the reflection markers is extracted from the currently scanned laser point cloud. The number of coordinate points in the point set {Pi} ≥ 4. If the number of reflection markers within the scanning range is less than 4, move the 2D lidar until the number of coordinate points in the point set {Pi} ≥ 4.

3. A two-dimensional laser global positioning algorithm based on reflective markers according to claim 2, characterized in that: In the said Step 2, randomly extract at most 10 non - repeating groups of point pairs, denote each point pair as (Pm, Pn), and calculate the Cartesian distance dist_mn between Pm and Pn.

4. A two-dimensional laser global positioning algorithm based on reflective markers according to claim 3, characterized in that: In the said Step 3, the coordinates of all reflection markers are recorded in the global map and form a point set {Li}. Traverse each point pair (Lj, Lk) in {Li}, calculate the Cartesian distance dist_jk between Lj and Lk. When |dist_jk - dist_mn| < e1, perform a calculation for global matching.

5. The two-dimensional laser global positioning algorithm based on reflective markers according to claim 4, characterized in that: In the said Step 4, move the pose of the lidar so that Pm coincides with Lk, and Pn lies on or extends on the line connecting Lj and Lk, obtain the global coordinates of the new positions of Pm and Pn, and thus inversely deduce the global coordinate LaserInit_1 of the lidar at this time; move the pose of the lidar so that Pn coincides with Lj, and Pm lies on or extends on the line connecting Lj and Lk, thereby obtain the global coordinates of the new positions of Pm and Pn, and thus inversely deduce the global coordinate LaserInit_2 of the lidar at this time.

6. The two-dimensional laser global positioning algorithm based on reflective markers according to claim 5, characterized in that: In the said Step 4, take LaserInit_1 as the current global pose of the lidar, calculate the global coordinates {Pglobal_i} of all points in {Pi}. For each Pglobal_i, find the unique point Li in {Li} that is the closest to it and the distance is less than e2 as the matching point of Pglobal_i in the map, and find the matching points of all points in {Pi} through the above method, and record the number of matching points count_match.

7. The two-dimensional laser global positioning algorithm based on reflective markers according to claim 6, characterized in that: In the Step 4, the g2o model is used to establish the observation edge and information matrix, and LaserInit1 is used as the initial value to perform g2o optimization, calculate the precise position Popti_i of the laser radar, and obtain the information matrix error chi of each matching point, and calculate the average value avg_chi of the information matrix error chi, and save Popti_i, count_match and avg_chi as a state s1 into std::vector, recorded as {Si}; then LaserInit_2 is used as the initial pose, and the above operation is repeated to calculate the state s2 and save it into {Si}.

8. The two-dimensional laser global positioning algorithm based on reflective markers according to claim 7, characterized in that: In Step 5, all states in {Si} are scored according to the evaluation formula. The evaluation formula is: value=cnt_match+1.0 / avg_chi; The scores are then sorted, and the one with the highest score is selected as the first candidate global pose. If the difference between the second or lower score and the highest score is less than e4, it is considered that there is an ambiguous position and global positioning fails. The user is informed that they need to switch areas and re-perform global positioning. Otherwise, global positioning is declared successful and the global positioning pose is output.

9. The two-dimensional laser global positioning algorithm based on reflective markers according to claim 3, characterized in that: In Step 3, two circles are made with Li as the center and dist_mn±e1 as the radius, and all points in {Pi} are traversed. The points between the two circles are taken as the undetermined points of Li, and the initial pose of the undetermined points is calculated.

10. The two-dimensional laser global positioning algorithm based on reflective markers according to claim 3, characterized in that: In Step 3, a radar is installed on each reflective marker, which is first scanned with a radius of dist_mn-e1 to exclude the scanned points in {Pi}, and then scanned with a radius of dist_mn+e1. The scanned points are used as pending points and the initial pose of the pending points is calculated.

Images