Method and system for detecting 2d laser positioning loss, and storage medium

Abstract

A method and a system for detecting 2D laser positioning loss, and a storage medium. The steps of the method comprise: performing Gaussian distribution on map points, to generate a likelihood Gaussian probability grid map, calculating matching degrees g between grids where laser points are located and nearest map points thereof, and calculating a matching degree mean G (S100); when determining that there is laser penetrating the map, recording map points and laser points where penetration points are located, so as to calculate a distance ratio score L between each map point and each laser point (S200); counting an overall penetration ratio N of the grid map, and calculating an average distance M between all of the laser points and all of the map points (S300); and when detecting that the matching degree mean G, the distance ratio score L, and the penetration ratio N meet a loss threshold determination condition, giving a positioning loss determination result, or otherwise, further determining whether the penetration ratio N or the average distance M exceeds an inverse ratio threshold determination condition, so as to obtain a positioning loss determination result (S400). Therefore, whether position loss occurs to the 2D laser positioning can be accurately determined in a complex environment.

Description

A method and system for detecting 2D laser positioning loss, and a storage medium Technical Field

[0001] The present invention relates to laser positioning technology, and in particular to a method and system for detecting loss of 2D laser positioning, and a storage medium. Background Art

[0002] Industrial robots play a vital role in automated production, and accurate positioning technology is crucial for precise robot operation and efficient production. Currently, 2D single-line lasers are commonly used for mapping, positioning, and navigation in indoor environments. Although single-line laser-based mapping technology is mature, 2D laser positioning technology is prone to the risk of position loss in complex environments, such as those with large-scale environmental fluctuations, high traffic volume, and an increase in similar obstacles.

[0003] Loss of positioning in an industrial environment can pose serious problems. First, product damage is a significant risk. Loss of positioning can lead to misalignment or incorrect positioning, preventing the robot from reaching the designated location. Second, loss of positioning can cause the robot's motion path to deviate from the intended path, requiring additional time and effort to readjust the positioning. This can lead to decreased production efficiency, extended production cycles, and even disruptions such as production line downtime.

[0004] In addition, there is a risk of safety accidents due to loss of positioning. The robot cannot accurately identify the boundaries and obstacles of the working area, which increases the risk of collision, accidental contact or pinching, which may cause personal injury, equipment damage or other safety accidents. It will reduce users' confidence in the robot system and affect the product's market competitiveness and corporate reputation.

[0005] Therefore, how to accurately and reliably detect whether the 2D laser positioning of an industrial robot is lost is an urgent problem to be solved by those skilled in the art.

[0006] Summary of the Invention

[0007] To this end, the main purpose of the present invention is to provide a method and system for detecting 2D laser positioning loss, and a storage medium, so as to accurately determine whether 2D laser positioning has lost positioning in a complex environment.

[0008] To achieve the above object, according to a first aspect of the present invention, a method for detecting loss of 2D laser positioning is provided, comprising the steps of:

[0009] Step S100: Gaussian distribution is performed on the map points to generate a likelihood Gaussian probability grid map, and the matching degree g between the grid where each laser point is located and its nearest map point is calculated, and the mean value G of the matching degree is calculated;

[0010] Step S200: When it is determined that the laser penetrates the map, the map point and the laser point at each penetration point are recorded to calculate the distance ratio score L between the two.

[0011] Step S300 counts the overall penetration ratio N of the grid map and calculates the average distance M between all laser points and map points;

[0012] Step S400 gives a positioning loss judgment result when it is detected that the matching degree mean G, distance ratio score L, and penetration ratio N meet the loss threshold judgment condition. Otherwise, it judges whether the penetration ratio N and average distance M exceed the inverse threshold judgment condition to obtain the positioning loss judgment result.

[0013] In a possible preferred embodiment, the step of calculating the matching degree mean G in step S100 includes:

[0014] Step S110 calculates

[0015] Where x-μ is the distance between the grid where the current laser point is located and its nearest map point;

[0016] In step S120 , all calculated matching degrees g are added together and divided by the total number of grids where the laser point is located to obtain a matching degree mean G.

[0017] In a possible preferred embodiment, the step of determining whether the laser penetrates the map in step S200 includes:

[0018] Step S210 divides the map coordinate system into several equal areas according to the slope, takes the current position as the origin and each laser point as the end point, and constructs the laser ray using the Bresenham line drawing algorithm;

[0019] Step S220 traverses all vertical and horizontal coordinate positions on the laser rays in each area to determine whether there are coordinates that coincide with map points. If so, it is considered to have penetrated the map.

[0020] In a possible preferred embodiment, the step of traversing all vertical and horizontal coordinate positions of the laser beam in each area in step S220 includes:

[0021] Step S221 determines the slope between the current laser point and the origin coordinates to determine the area of ​​the map coordinate system where the laser ray is located;

[0022] Step S222 performs coordinate conversion based on the region of the map coordinate system where the laser ray is located to transform it into the main region;

[0023] Step S223 sets the increment of the comparison expression and the initial value of the comparison expression, loops through each horizontal coordinate on the laser beam, and calculates the corresponding vertical coordinate.

[0024] In a possible preferred embodiment, the step of calculating the distance ratio score L between the map point and the laser point at the penetration point position includes:

[0025] Step S230: Set the coordinates of the map point at the penetration point as (pp_x, pp_y), and the coordinates of the corresponding laser point as (x n 、y n ), calculate the distance between the two

[0026] Step S340 calculates the sum of the dist values ​​of all penetration point positions and divides it by the total number of penetration points pp_nums to obtain a distance ratio score LL=dist / pp_nums.

[0027] In a possible preferred embodiment, the loss threshold determination condition in step S400 includes:

[0028] When the matching degree mean G>0.7, the penetration ratio N>0.25 and the distance ratio score L>0.3;

[0029] When the matching degree mean G <= 0.7, the penetration ratio N is judged to be greater than 0.18 and the distance ratio score L is greater than 0.2;

[0030] If any of the conditions are met, the current positioning is determined to be lost.

[0031] In a possible preferred embodiment, the inverse ratio threshold judgment condition in step S400 includes:

[0032] When the penetration ratio N is less than 0.17 and the average distance M is greater than 0.3, it is determined that the current positioning is lost.

[0033] In a possible preferred embodiment, the method for detecting loss of 2D laser positioning further includes the following steps:

[0034] In step S500, when it is determined that the timestamps between the laser information and the positioning information are not synchronized, the positioning loss determination is stopped.

[0035] In order to achieve the above object, corresponding to the above method, according to a second aspect of the present invention, a system for detecting loss of 2D laser positioning is further provided, which includes:

[0036] A storage unit for storing a program including any of the above-described method steps for detecting loss of 2D laser positioning, for timely recall and execution by the processing unit and the navigation unit;

[0037] The navigation unit is used to Gaussian distribute the map points, generate a likelihood Gaussian probability grid map, obtain the lidar scanning frame, determine the position of the laser point on the grid map, and the current positioning position;

[0038] The processing unit is used to calculate the matching degree g between the grid where each laser point is located and its nearest map point, and calculate the matching degree mean G; and when it is determined that there is laser penetration in the map, record the map point and laser point at the location of each penetration point to calculate the distance ratio score L between the two; count the overall penetration ratio N of the grid map, and calculate the average distance M between all laser points and map points; then, when it is detected that the matching degree mean G, the distance ratio score L, and the penetration ratio N meet the loss threshold judgment condition, give a positioning loss judgment result; otherwise, judge whether the penetration ratio N and the average distance M exceed the inverse threshold judgment condition to obtain the positioning loss judgment result.

[0039] In order to achieve the above-mentioned purpose, corresponding to the above-mentioned method, according to the third aspect of the present invention, a computer-readable storage medium is also provided, on which a computer program is stored, wherein when the computer program is executed by a processor, the steps of the method for detecting loss of 2D laser positioning as described in any of the above items are implemented.

[0040] The method, system, and storage medium for detecting 2D laser positioning loss provided by the present invention can cleverly calculate the degree of matching between the laser and the map, and determine whether the laser penetrates the map. At the same time, the numerical values ​​corresponding to these features are combined to comprehensively evaluate whether the current positioning is lost, thereby accurately and reliably detecting in real time whether the industrial robot's 2D laser positioning is lost, thereby reducing the risk of safety accidents of mobile robots and enhancing users' confidence in robot automated production. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] The accompanying drawings, which constitute part of this application, are intended to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are intended to explain the present invention and do not constitute an undue limitation of the present invention. In the accompanying drawings:

[0042] FIG1 is a schematic diagram of the steps of a method for detecting loss of 2D laser positioning according to the present invention;

[0043] FIG2 is a schematic diagram showing the correspondence between the laser scanning frame and the map under normal positioning conditions;

[0044] FIG3 is a schematic diagram illustrating a method for detecting 2D laser positioning loss according to the present invention, wherein the coordinate system is divided into eight equal regions to reduce computing resources when determining whether the laser penetrates the map;

[0045] FIG4 is a schematic diagram of all vertical and horizontal coordinate positions of a laser ray calculated in a main area when determining whether the laser penetrates a map in the method for detecting loss of 2D laser positioning according to the present invention;

[0046] FIG5 is a schematic diagram showing a method for detecting loss of 2D laser positioning according to the present invention, in which whether the laser penetrates the map is determined based on whether there is a position on the laser beam that overlaps with the map;

[0047] FIG6 is a logic diagram of determining whether positioning is lost in the method for detecting 2D laser positioning loss of the present invention;

[0048] FIG7 is a schematic diagram illustrating an example of satisfying an inverse threshold judgment condition in the method for detecting 2D laser positioning loss according to the present invention;

[0049] FIG8 is a schematic structural diagram of a system for detecting 2D laser positioning loss according to the present invention. DETAILED DESCRIPTION

[0050] In order to enable those skilled in the art to better understand the technical solutions of the present invention, the specific technical solutions of the present invention will be clearly and completely described below in conjunction with the embodiments to help those skilled in the art further understand the present invention. Obviously, the embodiments described in this case are only embodiments of a part of the present invention, rather than all embodiments. It should be pointed out that, for those of ordinary skill in the art, the embodiments in this application and the features in the embodiments can be combined with each other without departing from the concept of the present invention and without conflicting with each other. Based on the embodiments in the present invention, all other embodiments obtained by those of ordinary skill in the art without making creative work should fall within the disclosure and protection scope of the present invention.

[0051] In addition, the terms "first", "second", "S100", "S200", etc. in the specification, claims and drawings of the present invention are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those described herein. At the same time, the terms "including" and "having" and any variations thereof in the present invention are intended to cover non-exclusive inclusions. Unless otherwise expressly specified and limited, the terms "setting", "laying out", "installing", "connecting" and "connecting" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be an indirect connection through an intermediate medium, or it can be a communication between the two elements. For those of ordinary skill in the art, the specific meanings of the above terms in this case can be understood based on the specific circumstances and in combination with the prior art.

[0052] To accurately determine whether the robot has lost its positioning, the present invention considers several indicators. First, the degree of match between the current laser and the map can be used to determine positioning accuracy. A higher degree of match indicates good positioning, while a lower degree of match may indicate a loss of positioning.

[0053] Secondly, the positioning is lost by checking whether the laser penetrates the original constructed map. If the laser mostly penetrates the map or the distance between each laser point and its nearest map is large, it may indicate a high possibility of positioning loss.

[0054] Furthermore, since the navigation map is a Gaussian likelihood map, there may be cases where positioning is lost but the laser remains near the map but does not penetrate it. Therefore, in this case, the inventors considered calculating the average distance from the entire laser point to the map. A smaller average distance may indicate good positioning, while a larger average distance may indicate positioning loss.

[0055] Therefore, based on the above-mentioned inventive concept, the present invention proposes a method for detecting 2D laser positioning loss, so as to accurately determine whether 2D laser positioning has lost positioning in a complex environment. As shown in Figures 1 to 6, the method comprises the following steps:

[0056] Step S100 performs Gaussian distribution on the map points to generate a likelihood Gaussian probability grid map, calculates the matching degree g between the grid where each laser point is located and its nearest map point, and calculates the mean value G of the matching degree.

[0057] Specifically, to improve computational efficiency, we can first calculate the matching degree g score for each grid cell that the laser point might scan on the grid map. For example, we can construct a Gaussian probability grid map, dividing the received map points into discrete grid cells. Within each grid cell, we store information about the location. Later, when the laser point lands in that grid cell, we can determine its matching degree g and the distance from the current grid cell to the nearest map point.

[0058] The example construction steps are as follows: first determine the size of the grid map and the size of each grid, create a blank grid map, and initialize the values ​​in the grid. Then, based on the original map point received, set the initial matching degree score to 1 at the corresponding grid position. Finally, with each map point grid as the center, check the distance between the 4 grids around it and the map point grid. If the distance is less than the threshold, calculate the matching degree g of the location of the 4 expanded grids. Then, with each new grid as the center, expand 4 grids again and calculate the distance between these grids and the map grid. If the distance is greater than the threshold, the grid exits the loop; if the distance is less than the threshold, the loop is iterated to calculate the matching degree g of each grid.

[0059] The calculation steps of the matching degree g include:

[0060] Step S110 calculates

[0061] Where x-μ is the distance between the grid where the current laser point is located and its nearest map point. Through the above steps, we can know that each grid near each map point has a different score value, which can be used to more comprehensively analyze the matching score between the current laser and the map.

[0062] After each laser scan, the matching degree mean G can be calculated. The steps include:

[0063] In step S120 , the matching degrees g of all calculated grids where the laser points fall in the current laser scanning frame and the map points are added together, and then divided by the total number of grids where the laser points fall, to obtain the matching degree mean G.

[0064] In step S200 , when it is determined that the laser penetrates the map, the map point and the laser point at each penetration point are recorded to calculate a distance ratio score L between the two.

[0065] In this example, the example steps of determining whether the laser penetrates the map in step S200 include:

[0066] In step S210 , the map coordinate system is divided into several equal areas according to the slope, the current position is used as the origin, and each laser point is used as the end point, and the Bresenham line drawing algorithm is used to construct the laser ray.

[0067] Step S220 traverses all vertical and horizontal coordinate positions on the laser rays in each area to determine whether there are coordinates that coincide with map points. If so, it is considered to have penetrated the map.

[0068] Specifically, referring to Figures 3 to 5, to determine whether the current laser point has penetrated the map, this example preferably uses the Bresenham line drawing algorithm to simulate the laser ray. Since a large number of laser points need to be calculated, computational efficiency must be guaranteed. To this end, this example preferably uses only addition, subtraction, and comparison operations to achieve straight line drawing, greatly reducing the required computing resources. As shown in Figure 3, the overall idea is to divide the coordinate system into eight equal parts according to the slope, and calculate the vertical and horizontal coordinate positions of the laser ray in each area separately.

[0069] For example, let the robot origin coordinates be (x0, y0), and the position coordinates of the end point laser point be (x n ,y n ), with the robot origin as the starting point and the laser point as the end point, draw a straight line. The idea is to traverse all the horizontal coordinates and find the x coordinate of each horizontal coordinate according to the straight line equation. iThe corresponding vertical coordinate y i (As shown in FIG4 , each grid represents a pixel).

[0070] As shown in Figure 4, the ordinate corresponding to x0+i is i*slope+y0+0.5, and the ordinate value is rounded off. Taking x0+1 as an example, if 1*slope<0.5, then y1 is equivalent to y0, and if 1*slope>0.5, then y1 is equivalent to y0+1. Multiplying both sides of 1*slope<0.5 by 2, it becomes 2*slope<1, where slope is equal to δy / δx, and the equation becomes 2*δy-δx<0, (δy=y i -y0,δx=x i -x0), so 1*slope can be replaced by 2δy-δx as the initial value of the expression. It is only necessary to judge the comparison expression of 2δy-δx and 0 to find the corresponding y value. At this time, y i ,y0,x i , x0 are all integers, completely avoiding the complex calculation of floating-point types.

[0071] At the same time, according to Figure 3, in area 1 (main area), the slope is greater than 0 and less than 1, so the vertical coordinates corresponding to adjacent horizontal coordinates either remain unchanged or increase by 1 at most. From this, it can be inferred that if 2δy-δx<0 is satisfied, then the current y i It is derived from the East of the previous moment, and its y i The value remains unchanged. If the condition is not met, it means that the current y i The value is derived from the northeast direction of the previous moment, and its y i The value is directly increased by 1.

[0072] Next, consider that the vertical coordinate corresponding to x0+2 is 2*slope+y0+0.5. If the previous pixel is derived from the northeast, it is necessary to compare 2*slope<1.5 (because at x0+2, its y value range is y0+1 to y0+1, and its watershed is 1.5), which is equivalent to 4*δy-3*δx<0, and is also equivalent to 2*δy-δx+2*δy-2δx<0, and its increment is 2(δy-δx).

[0073] If the previous pixel is derived from the northeast, it is necessary to compare 2*slope<0.5, which is equivalent to 4*δy-δx<0 and also equivalent to 2*δy-δx+(2*δy)<0, and the increment is 2*δy.

[0074] Similarly, we can continue to deduce to the end of the line segment and find the pattern. If the previous position is derived from the east, the increment is incrE = 2*δy. If the previous position is derived from the northeast, the increment is: incrNE = 2*(δy-δx).

[0075] Based on this concept, it can actually be found that the laser rays in different areas can be converted into the main area (area 1) and then calculated using the above scheme, thereby avoiding complex floating-point type calculations.

[0076] To this end, in step S220 of this example, the preferred steps of traversing all vertical and horizontal coordinate positions on the laser beam in each area include:

[0077] Step S221 determines the slope between the current laser point and the origin coordinates to determine the area of ​​the map coordinate system where the laser ray is located.

[0078] Step S222 performs coordinate conversion based on the area of ​​the map coordinate system where the laser ray is located to transform it into the main area.

[0079] Step S223 sets the increment of the comparison expression and the initial value of the comparison expression, loops through each horizontal coordinate on the laser beam, and calculates the corresponding vertical coordinate.

[0080] For example, first determine the slope between the current laser point and the robot's origin coordinates.

[0081] in

[0082] If the slope is less than 1, it means that the current laser point is in area 1 and no transformation is required.

[0083] If the slope is greater than 1, it means that the current laser point is in area 2. You can swap the horizontal and vertical coordinates to transform it to area 1.

[0084] If the slope is less than 0, it means that the current laser point is in areas 7 and 8. A mirror transformation can be performed on the x-axis, that is, y = -x, and the laser point is transformed into area 1.

[0085] According to the above concept, other situations indicate that the current laser point is in areas 3, 4, 5, and 6. Then, by exchanging the starting and ending points of the linear transformation, it can be transformed to the area on the right side of the y-axis. That is, first determine which area the current point is in based on the slope, and then transform it to area 1 according to the transformation criteria of different areas.

[0086] After that, set the increment of the comparison expression. If the previous position is derived from east, the increment is: incrE=2*δy;

[0087] If the previous position is derived from the northeast, the increment is: incrNE = 2*(δy-δx);

[0088] Set the initial value of the comparison expression: d = 2*δy-δx;

[0089] Loop through each horizontal coordinate and find the corresponding y coordinate:

[0090] If d < 0, it means it is derived from the east, the y value remains unchanged, and the corresponding increment d + = incrE is added;

[0091] Otherwise, it indicates that it is derived from the northeast, and the y value is increased by one, plus the corresponding increment d+=incrNE.

[0092] In this way, all the vertical and horizontal coordinate positions on the laser beam can be calculated. At this time, it is only necessary to traverse all the vertical and horizontal coordinate positions on the laser beam in each area to determine whether there is a coordinate that coincides with the map point. If so, it is considered to have penetrated the map, thereby completing the judgment of whether the laser point has penetrated the map point.

[0093] Furthermore, after the map penetration is determined, the distance ratio score L between the map point and the laser point at the penetration point can be calculated. The exemplary steps include:

[0094] Step S230: Set the coordinates of the map point at the penetration point as (pp_x, pp_y), and the coordinates of the corresponding laser point as (x n 、y n ), calculate the distance between the two

[0095] Step S340 calculates the sum of the dist values ​​of all penetration point positions and divides it by the total number of penetration points pp_nums to obtain a distance ratio score LL=dist / pp_nums.

[0096] Step S300 counts the overall penetration ratio N of the grid map and calculates the average distance M between all laser points and map points.

[0097] Specifically, after completing the above map penetration judgment, we can know the number of penetration points in a laser radar scan frame. By counting the ratio of the number of all laser points to the number of penetration points, we can calculate the overall map penetration ratio N. The calculation example of the average distance M between all laser points and map points is:

[0098] where x i Represents the position of each laser point, μ iRepresents the position of the nearest map point corresponding to each laser point, and n represents the number of laser points, that is, the sum of the distances from each laser point to its nearest map point is divided by the total number of laser points in a laser radar scan frame.

[0099] Step S400 gives a positioning loss judgment result when it is detected that the matching degree mean G, distance ratio score L, and penetration ratio N meet the loss threshold judgment condition. Otherwise, it judges whether the penetration ratio N and average distance M exceed the inverse threshold judgment condition to obtain the positioning loss judgment result.

[0100] Specifically, the overall judgment process is as follows:

[0101] First, determine whether the current robot is at an illegal point (outside the map or on an obstacle). If so, the default positioning is lost.

[0102] Secondly, if the robot is not at an illegal point, as shown in FIG6 , the loss threshold determination in step S400 includes:

[0103] Judgment condition 1: When the matching degree mean G>0.7, determine whether the penetration ratio N>0.25 and the distance ratio score L>0.3;

[0104] Judgment condition 2: When the matching degree mean G <= 0.7, determine whether the penetration ratio N > 0.18 and the distance ratio score L > 0.2;

[0105] If any of the above conditions are met, the current position is considered lost. Otherwise, the position is considered not lost. By setting two different thresholds for judgment, the strictness can be adjusted according to different situations, thereby more accurately detecting position loss.

[0106] On the other hand, considering that there are situations in practice where the penetration ratio is small but the average distance between the laser and the map is large, experience shows that positioning loss may occur at this time.

[0107] For example, as shown in Figure 7, only about 8% of the laser beams penetrate the map, resulting in positioning loss. Therefore, based on experience, it's also necessary to calculate the average distance between all laser points and the map to determine whether positioning is lost. For example, if the average distance is greater than 2 meters and less than 5 meters, positioning is considered lost; however, if the average distance is greater than 5 meters, positioning is considered normal. This is because when a vehicle is surrounded by multiple people, most of the laser beams are completely blocked, resulting in a significant distance from the laser beam to the map.

[0108] Therefore, the inverse ratio threshold judgment condition in step S400 includes:

[0109] When the penetration ratio N is less than 0.17 and the average distance M is greater than 0.3, it is determined that the current positioning is lost.

[0110] In addition, the above-mentioned various threshold judgment condition parameters in this example are only examples, and those skilled in the art can also adjust them according to actual conditions. Therefore, the examples of the present invention are not limited thereto.

[0111] Furthermore, to avoid false alarms, a new judgment condition is introduced in the pre-selected example: only when positioning loss is detected for several consecutive frames (such as five frames) is the current positioning lost and an error code is reported. Compared with the traditional single-frame judgment method, continuous frame judgment can reduce the impact of environmental noise and instantaneous deviations, improve the stability and accuracy of judgment, and thus enhance the accuracy of positioning loss judgment.

[0112] Furthermore, in order to reduce the false alarm rate and improve the detection accuracy and control the timestamp synchronization of each data, the method for detecting the loss of 2D laser positioning further includes the following steps:

[0113] In step S500, when it is determined that the timestamps between the laser information and the positioning information are not synchronized, the positioning loss determination is stopped. That is, the determination step in the above example is only performed when the timestamps between the laser information and the positioning information are synchronized.

[0114] On the other hand, referring to FIG8 , corresponding to the above method example, the present invention further provides a system for detecting loss of 2D laser positioning, which includes:

[0115] A storage unit for storing a program including any of the above-described method steps for detecting loss of 2D laser positioning, for timely recall and execution by the processing unit and the navigation unit;

[0116] The navigation unit is used to Gaussian distribute the map points, generate a likelihood Gaussian probability grid map, obtain the lidar scanning frame, determine the position of the laser point on the grid map, and the current positioning position;

[0117] The processing unit is used to calculate the matching degree g between the grid where each laser point is located and its nearest map point, and calculate the matching degree mean G; and when it is determined that there is laser penetration in the map, record the map point and laser point at the location of each penetration point to calculate the distance ratio score L between the two; count the overall penetration ratio N of the grid map, and calculate the average distance M between all laser points and map points; then, when it is detected that the matching degree mean G, the distance ratio score L, and the penetration ratio N meet the loss threshold judgment condition, give a positioning loss judgment result; otherwise, judge whether the penetration ratio N and the average distance M exceed the inverse threshold judgment condition to obtain the positioning loss judgment result.

[0118] On the other hand, corresponding to the above method examples, the present invention also provides a computer-readable storage medium on which a computer program is stored, wherein when the computer program is executed by a processor, the steps of the method for detecting loss of 2D laser positioning as described in any of the above items are implemented.

[0119] In summary, the method, system, and storage medium for detecting 2D laser positioning loss provided by the present invention can cleverly calculate the degree of matching between the laser and the map, and determine whether there is a phenomenon of laser penetration of the map. At the same time, the numerical values ​​corresponding to these features are combined to comprehensively evaluate whether the current positioning is lost, thereby accurately and reliably detecting in real time whether there is a loss problem in the 2D laser positioning of the industrial robot, thereby reducing the risk of safety accidents of mobile robots and enhancing users' confidence in robot automated production.

[0120] The preferred embodiments of the present invention disclosed above are only used to help illustrate the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to the specific implementation methods described. Obviously, many modifications and changes can be made based on the contents of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can better understand and utilize the present invention. The present invention is only limited by the claims and their full scope and equivalents. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

[0121] Those skilled in the art will understand that, in addition to implementing the system, device, and various modules provided by the present invention in pure computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, embedded microcontrollers, etc. by logically programming the method steps. Therefore, the system, device, and various modules provided by the present invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing the method and structures within the hardware component.

[0122] In addition, all or part of the steps in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a program, which is stored in a storage medium and includes a number of instructions for causing a single-chip microcomputer, chip or processor to execute all or part of the steps of the methods described in each embodiment of the present application. The aforementioned storage medium includes: a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and other media that can store program code.

[0123] In addition, various implementations of the embodiments of the present invention may be arbitrarily combined, and as long as they do not violate the concept of the embodiments of the present invention, they should also be regarded as the contents disclosed in the embodiments of the present invention.

Claims

1. A method for detecting loss of 2D laser positioning, comprising: Step S100: Gaussian distribution is performed on the map points to generate a likelihood Gaussian probability grid map, the matching degree g between the grid where each laser point is located and its nearest map point is calculated, and the matching degree mean G is calculated; Step S200: when it is determined that there is laser penetration in the map, the map point and the laser point at each penetration point are recorded to calculate the distance ratio score L between the two; Step S300 counts the overall penetration ratio N of the grid map and calculates the average distance M between all laser points and map points; Step S400 gives a positioning loss judgment result when it is detected that the matching degree mean G, distance ratio score L, and penetration ratio N meet the loss threshold judgment condition. Otherwise, it judges whether the penetration ratio N and the average distance M exceed the inverse threshold judgment condition to obtain the positioning loss judgment result.

2. The method for detecting 2D laser positioning loss according to claim 1, wherein the step of calculating the matching degree mean G in step S100 comprises: Step S110: Calculation Where x-μ is the distance between the grid where the current laser point is located and its nearest map point; In step S120, all calculated matching degrees g are added together and divided by the total number of grids where the laser point is located to obtain a matching degree mean G.

3. The method for detecting loss of 2D laser positioning according to claim 1, wherein the step of determining whether the laser penetrates the map in step S200 comprises: Step S210 divides the map coordinate system into several regions according to the slope, takes the current position as the origin and each laser point as the end point, and constructs the laser ray using the Bresenham line drawing algorithm; Step S220 traverses all the vertical and horizontal coordinate positions on the laser rays in each area to determine whether there are coordinates that coincide with map points. If so, it is considered to have penetrated the map.

4. The method for detecting loss of 2D laser positioning according to claim 3, wherein the step of traversing all the vertical and horizontal coordinate positions on the laser rays in each area in step S220 comprises: Step S221 determines the slope between the current laser point and the origin coordinates to determine the area of ​​the map coordinate system where the laser ray is located; Step S222: performing coordinate conversion according to the area of ​​the map coordinate system where the laser ray is located to transform it into the main area; Step S223 sets the increment of the comparison expression and the initial value of the comparison expression, loops through each horizontal coordinate on the laser beam, and finds the corresponding vertical coordinate.

5. The method for detecting 2D laser positioning loss according to claim 1, wherein the step of calculating the distance ratio score L between the map point and the laser point at the penetration point position comprises: Step S230: Set the coordinates of the map point at the penetration point as (pp_x, pp_y), and the coordinates of the corresponding laser point as (x n ,y n ), calculate the distance between the two Step S340 calculates the sum of dist of all penetration point positions and divides it by the total number of penetration points pp_nums to obtain the distance ratio score L L=dist / pp_nums.

6. The method for detecting 2D laser positioning loss according to claim 1, wherein the loss threshold judgment condition in step S400 includes: When the matching degree mean G>0.7, the penetration ratio N>0.25 and the distance ratio score L>0.3; When the matching degree mean G <= 0.7, the penetration ratio N> 0.18 and the distance ratio score L> 0.2; When any of the conditions are met, the current positioning is determined to be lost.

7. The method for detecting 2D laser positioning loss according to claim 1, wherein the inverse threshold judgment condition in step S400 includes: When the penetration ratio N is less than 0.17 and the average distance M is greater than 0.3, it is determined that the current positioning is lost.

8. The method for detecting loss of 2D laser positioning according to claim 1, wherein the steps further include: Step S500: When it is determined that the timestamps between the laser information and the positioning information are not synchronized, the positioning loss determination is stopped.

9. A system for detecting loss of 2D laser positioning, comprising: A storage unit, used to store a program including the method steps for detecting the loss of 2D laser positioning as claimed in any one of claims 1 to 8, so that the processing unit and the navigation unit can call and execute it in a timely manner; The navigation unit is used to Gaussian distribute the map points, generate a likelihood Gaussian probability grid map, and obtain the laser radar scanning frame to determine the position of the laser point on the grid map and the current positioning position; A processing unit is used to calculate the matching degree g between the grid where each laser point is located and its nearest map point, and calculate the matching degree mean G; When it is determined that there is laser penetration in the map, the map points and laser points at each penetration point are recorded to calculate the distance ratio score L between the two; the overall penetration ratio N of the grid map is counted, and the average distance M between all laser points and map points is calculated; Afterwards, when it is detected that the matching degree mean G, distance ratio score L, and penetration ratio N meet the loss threshold judgment condition, the positioning loss judgment result is given; otherwise, it is judged whether the penetration ratio N and the average distance M exceed the inverse threshold judgment condition to obtain the positioning loss judgment result.

10. A computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the steps of the method for detecting loss of 2D laser positioning as claimed in any one of claims 1 to 8 are implemented.

Images