A method, system, and media of a lightweight indoor 3D laser ground extraction algorithm

Abstract

The application discloses a lightweight indoor 3D laser ground extraction algorithm method, system and medium. The method comprises the following steps: acquiring indoor 3D laser radar data; obtaining coordinate values based on a laser radar coordinate system according to the indoor 3D laser radar data; sending the coordinate values to a preset plane equation to obtain a ground plane normal vector; sending the ground plane normal vector to a first preset coordinate system to establish a constraint to obtain a comprehensive error value; and accumulating squares of values in the comprehensive error value to obtain a comprehensive error sum of squares; extracting a minimum value in the comprehensive error sum of squares, and setting a coordinate point corresponding to the minimum value as an optimal current pose. The application is suitable for various lasers, and by giving a preset threshold, whether it is 16 lines, 32 lines or 128 lines, the algorithm accuracy and robustness are ensured, and the accuracy of recognized ground points is improved.

Description

Technical Field

[0001] This application relates to the field of autonomous navigation mobile robots, and more specifically, to a method, system, and medium for a lightweight indoor 3D laser ground extraction algorithm. Background Technology

[0002] For indoor patrol and inspection robots, which typically operate in medium-sized environments such as warehouses, garages, and factories, prolonged operation leads to increasing sensor errors, resulting in z-axis drift. Typically, after extended system operation in an indoor warehouse, the z-axis drift can exceed 5 meters, severely impacting the accuracy of positioning and other functions, necessitating z-axis correction. Laser data, characterized by containing only the spatial location and reflection intensity of points, is processed by algorithms. In addition to extracted features, another feature is used to correct z-axis drift: extracting ground points. Therefore, most robots require ground-based auxiliary constraints to mitigate z-axis drift. However, current ground extraction algorithms suffer from very high time complexity or poor accuracy, and real-time performance is difficult to achieve.

[0003] Therefore, existing technologies have shortcomings and urgently need improvement. Summary of the Invention

[0004] In view of the above problems, the purpose of this invention is to provide a lightweight indoor 3D laser ground extraction algorithm, method, system and medium that can extract ground points more simply and accurately.

[0005] The first aspect of this invention provides a lightweight indoor 3D laser ground extraction algorithm method, comprising:

[0006] Acquire indoor 3D LiDAR data;

[0007] Based on indoor 3D LiDAR data, coordinate values ​​in the LiDAR coordinate system are obtained;

[0008] Send the coordinate values ​​to the preset plane equation to obtain the ground plane normal vector;

[0009] The ground plane normal vector is sent to the first preset coordinate system to establish constraints and obtain the comprehensive error value; the squares of the values ​​in the comprehensive error value are summed to obtain the comprehensive error sum of squares;

[0010] Extract the minimum value from the sum of squared errors and set the coordinate point corresponding to the minimum value as the optimal current pose.

[0011] This plan also includes:

[0012] Obtain the z-coordinate value of an indoor coordinate point;

[0013] Determine whether the z-coordinate value of the indoor coordinate point is greater than the first preset threshold. If so, delete the corresponding indoor coordinate value.

[0014] The retained indoor coordinate points are sent to a preset indoor point cloud for storage.

[0015] In this scheme, the preset plane equation also includes:

[0016] Let p be the set of all points in the preset indoor point cloud, where p = {p1, p2, ..., p...} n};

[0017] Based on all point sets in the indoor point cloud, obtain the point set p that is opposite to the normal vector of the preset plane equation. d ,in

[0018] Extract p d Given any four points in the matrix, labeled (x1, y1, z1), ..., (x4, y4, z4), we obtain the corresponding parameter matrix Q, where:

[0019]

[0020] The corresponding parameter matrix Q is sent to the preset homogeneous linear equation system to obtain the corresponding preset plane equation parameters X.

[0021] In this scheme, the preset plane equation also includes:

[0022] p d Other points and preset plane equation parameters X are sent to the preset plane equation to determine whether other points belong to ground points. If so, they are saved.

[0023] Obtain the number of points within the preset model corresponding to the preset plane equation parameter X;

[0024] Extract the maximum number of points within the preset model;

[0025] Set the parameter corresponding to the maximum number of points in the preset model as the actual parameter of the plane equation. The preset plane equation is: A*x+B*y+C*z+D=0.

[0026] In this scheme, the first preset coordinate system further includes:

[0027] Obtain the ground plane normal vector in the initial map coordinate system. in

[0028] The ground plane normal vector in the map coordinate system is obtained through a preset odometer. The conversion is performed using the following formula:

[0029] Will Set it as the plane normal vector corresponding to the first preset coordinate system.

[0030] In this solution, the establishment of constraints also includes:

[0031] Will Represented by a rotation matrix as follows

[0032] The plane normal vector of the i-th laser frame Switch to By rotating the space, we obtain the vector n, whose formula is:

[0033] Obtain the pitch angle α, azimuth angle β, and error d of vector n;

[0034] Let the comprehensive error value be δ, and its formula is:

[0035] A second aspect of the present invention provides a system for a lightweight indoor 3D laser ground extraction algorithm, characterized in that it includes a memory and a processor, wherein the memory stores a method program for a lightweight indoor 3D laser ground extraction algorithm, and when the method program for the lightweight indoor 3D laser ground extraction algorithm is executed by the processor, it performs the following steps:

[0036] Acquire indoor 3D LiDAR data;

[0037] Based on indoor 3D LiDAR data, coordinate values ​​in the LiDAR coordinate system are obtained;

[0038] Send the coordinate values ​​to the preset plane equation to obtain the ground plane normal vector;

[0039] The ground plane normal vector is sent to the first preset coordinate system to establish constraints and obtain the comprehensive error value; the squares of the values ​​in the comprehensive error value are summed to obtain the comprehensive error sum of squares;

[0040] Extract the minimum value from the sum of squared errors and set the coordinate point corresponding to the minimum value as the optimal current pose.

[0041] This plan also includes:

[0042] Obtain the z-coordinate value of an indoor coordinate point;

[0043] Determine whether the z-coordinate value of the indoor coordinate point is greater than the first preset threshold. If so, delete the corresponding indoor coordinate value.

[0044] The retained indoor coordinate points are sent to a preset indoor point cloud for storage.

[0045] In this scheme, the preset plane equation also includes:

[0046] Let p be the set of all points in the preset indoor point cloud, where p = {p1, p2, ..., p...} n};

[0047] Based on all point sets in the indoor point cloud, obtain the point set p that is opposite to the normal vector of the preset plane equation. d ,in

[0048] Extract p d Given any four points in the matrix, labeled (x1, y1, z1), ..., (x4, y4, z4), we obtain the corresponding parameter matrix Q, where:

[0049]

[0050] The corresponding parameter matrix Q is sent to the preset homogeneous linear equation system to obtain the corresponding preset plane equation parameters X.

[0051] In this scheme, the preset plane equation also includes:

[0052] p d Other points and preset plane equation parameters X are sent to the preset plane equation to determine whether other points belong to ground points. If so, they are saved.

[0053] Obtain the number of points within the preset model corresponding to the preset plane equation parameter X;

[0054] Extract the maximum number of points within the preset model;

[0055] Set the parameter corresponding to the maximum number of points in the preset model as the actual parameter of the plane equation. The preset plane equation is: A*x+B*y+C*z+D=0.

[0056] In this scheme, the first preset coordinate system further includes:

[0057] Obtain the ground plane normal vector in the initial map coordinate system. in

[0058] The ground plane normal vector in the map coordinate system is obtained through a preset odometer. The conversion is performed using the following formula:

[0059] Will Set it as the plane normal vector corresponding to the first preset coordinate system.

[0060] In this solution, the establishment of constraints also includes:

[0061] Will Represented by a rotation matrix as follows

[0062] The plane normal vector of the i-th laser frame Switch to By rotating the space, we obtain the vector n, whose formula is:

[0063] Obtain the pitch angle α, azimuth angle β, and error d of vector n;

[0064] Let the comprehensive error value be δ, and its formula is:

[0065] A third aspect of the present invention provides a computer-readable storage medium, characterized in that the computer-readable storage medium stores a method program for a lightweight indoor 3D laser ground extraction algorithm, wherein when the method program for the lightweight indoor 3D laser ground extraction algorithm is executed by a processor, it implements the steps of the method for the lightweight indoor 3D laser ground extraction algorithm as described above.

[0066] This invention discloses a lightweight indoor 3D laser ground extraction algorithm, method, system, and medium. This application is applicable to various lasers; by giving a preset threshold, whether it's 16 lines, 32 lines, or 128 lines, it ensures both algorithm accuracy and robustness, improving the accuracy of ground point identification. Attached Figure Description

[0067] Figure 1 A flowchart of a lightweight indoor 3D laser ground extraction algorithm according to the present invention is shown;

[0068] Figure 2 The flowchart of the entire algorithm calculation is shown;

[0069] Figure 3 A block diagram of a lightweight indoor 3D laser ground extraction algorithm according to the present invention is shown. Detailed Implementation

[0070] To better understand the above-mentioned objectives, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments and features described in these embodiments can be combined with each other.

[0071] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and therefore the scope of protection of the invention is not limited to the specific embodiments disclosed below.

[0072] Figure 1 A flowchart of a lightweight indoor 3D laser ground extraction algorithm according to the present invention is shown.

[0073] like Figure 1 As shown, this invention discloses a lightweight indoor 3D laser ground extraction algorithm method, comprising:

[0074] S102, acquire indoor 3D LiDAR data;

[0075] S104, Based on indoor 3D LiDAR data, obtain the coordinate values ​​in the LiDAR coordinate system;

[0076] S106, send the coordinate values ​​to the preset plane equation to obtain the ground plane normal vector;

[0077] S108: Send the ground plane normal vector to the first preset coordinate system to establish constraints and obtain the comprehensive error value; and sum the squares of the values ​​in the comprehensive error value to obtain the comprehensive error sum of squares;

[0078] S110, extract the minimum value from the sum of squared errors, and set the coordinate point corresponding to the minimum value as the optimal current pose.

[0079] It should be noted that 3D LiDAR data is received through a preset callback function, including the x, y, and z coordinates and timestamp of each point. The obtained coordinate values ​​are based on the LiDAR coordinate system, and the obtained coordinate points are sent to a preset indoor point cloud for storage. A preset plane equation is solved in the LiDAR coordinate system. The preset plane equation has the form A*x + B*y + C*z + D = 0, and the parameters to be solved are A, B, C, and D. If the LiDAR's xy plane is set to be parallel to the ground, then A and B are both 0. If C is 1, then D represents the height of the LiDAR above the ground, which is 0.7. The final form of the preset plane equation is 0*x + 0*y + 1*z + 0.7 = 0. Based on the preset plane equation, the ground plane normal vector corresponding to the coordinate values ​​is obtained. This ground plane normal vector is sent to a first preset coordinate system to establish constraints, such as the LiDAR coordinate system, to obtain the comprehensive error value δ. Let the sum of squared errors be W, and its formula is: W = a 2 +β 2 +d 2 Extract the minimum value W from the sum of squared errors. min And set the coordinate point corresponding to the minimum value as the optimal current pose.

[0080] According to an embodiment of the present invention, it further includes:

[0081] Obtain the z-coordinate value of an indoor coordinate point;

[0082] Determine whether the z-coordinate value of the indoor coordinate point is greater than the first preset threshold. If so, delete the corresponding indoor coordinate value.

[0083] The retained indoor coordinate points are sent to a preset indoor point cloud for storage.

[0084] It should be noted that the point cloud acquired in each frame is downsampled to reduce the amount of computation. The z-coordinate value of the indoor coordinate points is extracted and compared with a first preset threshold. If the first preset threshold is 0.2, it means that the preset filtering parameter is 0.2. The corresponding indoor coordinate points with z-coordinate values ​​less than 0.2 are retained and sent to the preset indoor point cloud for storage.

[0085] According to an embodiment of the present invention, the preset plane equation further includes:

[0086] Let p be the set of all points in the preset indoor point cloud, where p = {p1, p2, ..., p...} n};

[0087] Based on all point sets in the indoor point cloud, obtain the point set p that is opposite to the normal vector of the preset plane equation. d ,in

[0088] Extract p d Given any four points in the matrix, labeled (x1, y1, z1), ..., (x4, y4, z4), we obtain the corresponding parameter matrix Q, where:

[0089]

[0090] The corresponding parameter matrix Q is sent to the preset homogeneous linear equation system to obtain the corresponding preset plane equation parameters X.

[0091] It should be noted that p d It contains all ground points, and is obtained through random sampling from the point set p. d Four points are randomly selected from the data, namely (x1, y1, z1), ..., (x4, y4, z4), and the parameter matrix formed by them is denoted as Q, where:

[0092]

[0093] Set the parameters of the preset plane equation to X = [ABCD] TBased on the parameter matrix Q, solve the preset homogeneous linear equation system, the formula of which is: Q*X=0, and obtain the parameter X of the preset plane equation.

[0094] According to an embodiment of the present invention, the preset plane equation further includes:

[0095] p d Other points and preset plane equation parameters X are sent to the preset plane equation to determine whether other points belong to ground points. If so, they are saved.

[0096] Obtain the number of points within the preset model corresponding to the preset plane equation parameter X;

[0097] Extract the maximum number of points within the preset model;

[0098] Set the parameter corresponding to the maximum number of points in the preset model as the actual parameter of the plane equation. The preset plane equation is: A*x+B*y+C*z+D=0.

[0099] It should be noted that p d Substitute other points into the solved homogeneous linear equation system to calculate whether they belong to the ground points (i.e., the inner group). If not, delete them; if yes, save them and extract the number of inner group points. Based on the point set p... d Four different points are randomly selected to obtain different preset plane equation parameters X. Based on the different preset plane equation parameters X and the preset homogeneous linear equation system, different values ​​of the number of inner group points are obtained. The parameter of the preset homogeneous linear equation system with the largest number of inner group points is extracted, and the corresponding parameter is set as the actual parameter of the preset plane equation, with the formula A*x+B*y+C*z+D=0. [ABCD] T Let it be the ground plane normal vector. The number of times the internal group point count is calculated shall not be less than 1000.

[0100] According to an embodiment of the present invention, the first preset coordinate system further includes:

[0101] Obtain the ground plane normal vector in the initial map coordinate system. in

[0102] The ground plane normal vector in the map coordinate system is obtained through a preset odometer. The conversion is performed using the following formula:

[0103] Will Set it as the plane normal vector corresponding to the first preset coordinate system.

[0104] It should be noted that the initial ground plane normal vector in the initial map coordinate system is: The ground plane normal vector in the current map coordinate system is obtained through a preset odometer. Transforming to the first preset coordinate system, the formula is: The first preset coordinate system is the lidar coordinate system.

[0105] According to an embodiment of the present invention, the establishment of constraints further includes:

[0106] Will Represented by a rotation matrix as follows

[0107] The plane normal vector of the i-th laser frame Switch to By rotating the space, we obtain the vector n, whose formula is:

[0108] Obtain the pitch angle α, azimuth angle β, and error d of vector n;

[0109] Let the comprehensive error value be δ, and its formula is:

[0110] It should be noted that the plane normal vector and Establish constraints, among which Let the plane normal vector of the i-th laser frame be... Switch to The rotation space is used to obtain vector n. The pitch angle α, azimuth angle β, and error d are calculated for n. The specific conversion method is as follows: The specific error calculation is as follows: The comprehensive error value δ is then obtained, and its formula is: Where α is the pitch angle, β is the azimuth angle, d is the error value, and [3] represents the third index value of the plane normal vector.

[0111] According to an embodiment of the present invention, it further includes:

[0112] Obtain indoor floor slope information;

[0113] Determine if the indoor floor slope is greater than a first preset threshold; if so, trigger an alert message.

[0114] Warning messages are sent to the indoor robot's control terminal to provide alerts.

[0115] It should be noted that the indoor ground slope information is obtained through a preset detection device, such as a level or other instrument capable of measuring slope. The indoor ground slope reflects the slope at which the indoor patrol robot operates. If the first preset threshold is set to 10, it means that when the indoor ground slope is greater than 10 degrees, the slope at which the indoor patrol robot operates is too high, triggering an alarm.

[0116] According to an embodiment of the present invention, it further includes:

[0117] Obtain the angle between the plane formed by the x and y axes of the 3D laser and the ground;

[0118] Determine whether the angle between the plane formed by the x and y axes of the 3D laser and the ground is greater than a second preset threshold. If so, obtain the 3D laser x or y axis adjustment information.

[0119] The 3D laser x or y axis adjustment information is sent to the indoor robot control terminal for automatic adjustment.

[0120] It should be noted that, using the horizontal plane of the indoor floor as a reference, an xy-plane is set up, formed by the x and y axes of the 3D laser. The angle between this plane and the floor is extracted, and it is determined whether this angle exceeds a second preset threshold. For example, if the second preset threshold is set to 5, and the angle between the plane and the floor is 6 degrees, it indicates that the angle between the xy-plane and the indoor floor is too large, and the xy-plane is adjusted. The smaller the angle between the xy-plane and the floor, the smaller the error in extracting the floor point.

[0121] Figure 2 The flowchart of the entire algorithm calculation is shown.

[0122] As shown in the figure, 3D LiDAR data is received through a callback function, and coordinate values ​​are extracted. These coordinate values ​​are then processed, such as downsampling and segmenting the point cloud using preset plane equations. Random sampling is then used to extract the ground plane normal vector. This normal vector is then transferred to a first preset coordinate system using a preset odometer to obtain the plane normal vector. Finally, through plane constraint optimization and optimization equations, the comprehensive error value is determined, and the minimum value of the sum of squared comprehensive errors is set as the optimal current pose.

[0123] Figure 3 A block diagram of a lightweight indoor 3D laser ground extraction algorithm according to the present invention is shown.

[0124] like Figure 2As shown, a second aspect of the present invention provides a system 3 for a lightweight indoor 3D laser ground extraction algorithm, comprising a memory 31 and a processor 32. The memory stores a method program for a lightweight indoor 3D laser ground extraction algorithm. When the method program for the lightweight indoor 3D laser ground extraction algorithm is executed by the processor, it performs the following steps:

[0125] Acquire indoor 3D LiDAR data;

[0126] Based on indoor 3D LiDAR data, coordinate values ​​in the LiDAR coordinate system are obtained;

[0127] Send the coordinate values ​​to the preset plane equation to obtain the ground plane normal vector;

[0128] The ground plane normal vector is sent to the first preset coordinate system to establish constraints and obtain the comprehensive error value; the squares of the values ​​in the comprehensive error value are summed to obtain the comprehensive error sum of squares;

[0129] Extract the minimum value from the sum of squared errors and set the coordinate point corresponding to the minimum value as the optimal current pose.

[0130] It should be noted that 3D LiDAR data is received through a preset callback function, including the x, y, and z coordinates and timestamp of each point. The obtained coordinate values ​​are based on the LiDAR coordinate system, and the obtained coordinate points are sent to a preset indoor point cloud for storage. A preset plane equation is solved in the LiDAR coordinate system. The preset plane equation has the form A*x + B*y + C*z + D = 0, and the parameters to be solved are A, B, C, and D. If the LiDAR's xy plane is set to be parallel to the ground, then A and B are both 0. If C is 1, then D represents the height of the LiDAR above the ground, which is 0.7. The final form of the preset plane equation is 0*x + 0*y + 1*z + 0.7 = 0. Based on the preset plane equation, the ground plane normal vector corresponding to the coordinate values ​​is obtained. This ground plane normal vector is sent to a first preset coordinate system to establish constraints, such as the LiDAR coordinate system, to obtain the comprehensive error value δ. Let the sum of squared errors be W, and its formula is: W = a 2 +β 2 +d 2 Extract the minimum value W from the sum of squared errors. min And set the coordinate point corresponding to the minimum value as the optimal current pose.

[0131] According to an embodiment of the present invention, it further includes:

[0132] Obtain the z-coordinate value of an indoor coordinate point;

[0133] Determine whether the z-coordinate value of the indoor coordinate point is greater than the first preset threshold. If so, delete the corresponding indoor coordinate value.

[0134] The retained indoor coordinate points are sent to a preset indoor point cloud for storage.

[0135] It should be noted that the point cloud acquired in each frame is downsampled to reduce the amount of computation. The z-coordinate value of the indoor coordinate points is extracted and compared with a first preset threshold. If the first preset threshold is 0.2, it means that the preset filtering parameter is 0.2. The corresponding indoor coordinate points with z-coordinate values ​​less than 0.2 are retained and sent to the preset indoor point cloud for storage.

[0136] According to an embodiment of the present invention, the preset plane equation further includes:

[0137] Let p be the set of all points in the preset indoor point cloud, where p = {p1, p2, ..., p...} n};

[0138] Based on all point sets in the indoor point cloud, obtain the point set p that is opposite to the normal vector of the preset plane equation. d ,in

[0139] Extract p d Given any four points in the matrix, labeled (x1, y1, z1), ..., (x4, y4, z4), we obtain the corresponding parameter matrix Q, where:

[0140]

[0141] The corresponding parameter matrix Q is sent to the preset homogeneous linear equation system to obtain the corresponding preset plane equation parameters X.

[0142] It should be noted that p d It contains all ground points, and is obtained through random sampling from the point set p. d Four points are randomly selected from the data, namely (x1, y1, z1), ..., (x4, y4, z4), and the parameter matrix formed by them is denoted as Q, where:

[0143]

[0144] Set the parameters of the preset plane equation to X = [ABCD] T Based on the parameter matrix Q, solve the preset homogeneous linear equation system, the formula of which is: Q*X=0, and obtain the parameter X of the preset plane equation.

[0145] According to an embodiment of the present invention, the preset plane equation further includes:

[0146] p d Other points and preset plane equation parameters X are sent to the preset plane equation to determine whether other points belong to ground points. If so, they are saved.

[0147] Obtain the number of points within the preset model corresponding to the preset plane equation parameter X;

[0148] Extract the maximum number of points within the preset model;

[0149] Set the parameter corresponding to the maximum number of points in the preset model as the actual parameter of the plane equation. The preset plane equation is: A*x+B*y+C*z+D=0.

[0150] It should be noted that p d Substitute other points into the solved homogeneous linear equation system to calculate whether they belong to the ground points (i.e., the inner group). If not, delete them; if yes, save them and extract the number of inner group points. Based on the point set p... d Four different points are randomly selected to obtain different preset plane equation parameters X. Based on the different preset plane equation parameters X and the preset homogeneous linear equation system, different values ​​of the number of inner group points are obtained. The parameter of the preset homogeneous linear equation system with the largest number of inner group points is extracted, and the corresponding parameter is set as the actual parameter of the preset plane equation, with the formula A*x+B*y+C*z+D=0. [ABCD] T Let it be the ground plane normal vector. The number of times the internal group point count is calculated shall not be less than 1000.

[0151] According to an embodiment of the present invention, the first preset coordinate system further includes:

[0152] Obtain the ground plane normal vector in the initial map coordinate system. in

[0153] The ground plane normal vector in the map coordinate system is obtained through a preset odometer. The conversion is performed using the following formula:

[0154] Will Set it as the plane normal vector corresponding to the first preset coordinate system.

[0155] It should be noted that the initial ground plane normal vector in the initial map coordinate system is: The ground plane normal vector in the current map coordinate system is obtained through a preset odometer. Transforming to the first preset coordinate system, the formula is: The first preset coordinate system is the lidar coordinate system.

[0156] According to an embodiment of the present invention, the establishment of constraints further includes:

[0157] Will Represented by a rotation matrix as follows

[0158] The plane normal vector of the i-th laser frame Switch to By rotating the space, we obtain the vector n, whose formula is:

[0159] Obtain the pitch angle α, azimuth angle β, and error d of vector n;

[0160] Let the comprehensive error value be δ, and its formula is:

[0161] It should be noted that the plane normal vector and Establish constraints, among which Let the plane normal vector of the i-th laser frame be... Switch to The rotation space is used to obtain vector n. The pitch angle α, azimuth angle β, and error d are calculated for n. The specific conversion method is as follows: The specific error calculation is as follows: The comprehensive error value δ is then obtained, and its formula is: Where α is the pitch angle, β is the azimuth angle, d is the error value, and [3] represents the third index value of the plane normal vector.

[0162] According to an embodiment of the present invention, it further includes:

[0163] Obtain indoor floor slope information;

[0164] Determine if the indoor floor slope is greater than a first preset threshold; if so, trigger an alert message.

[0165] Warning messages are sent to the indoor robot's control terminal to provide alerts.

[0166] It should be noted that the indoor ground slope information is obtained through a preset detection device, such as a level or other instrument capable of measuring slope. The indoor ground slope reflects the slope at which the indoor patrol robot operates. If the first preset threshold is set to 10, it means that when the indoor ground slope is greater than 10 degrees, the slope at which the indoor patrol robot operates is too high, triggering an alarm.

[0167] According to an embodiment of the present invention, it further includes:

[0168] Obtain the angle between the plane formed by the x and y axes of the 3D laser and the ground;

[0169] Determine whether the angle between the plane formed by the x and y axes of the 3D laser and the ground is greater than a second preset threshold. If so, obtain the 3D laser x or y axis adjustment information.

[0170] The 3D laser x or y axis adjustment information is sent to the indoor robot control terminal for automatic adjustment.

[0171] It should be noted that, using the horizontal plane of the indoor floor as a reference, an xy-plane is set up, formed by the x and y axes of the 3D laser. The angle between this plane and the floor is extracted, and it is determined whether this angle exceeds a second preset threshold. For example, if the second preset threshold is set to 5, and the angle between the plane and the floor is 6 degrees, it indicates that the angle between the xy-plane and the indoor floor is too large, and the xy-plane is adjusted. The smaller the angle between the xy-plane and the floor, the smaller the error in extracting the floor point.

[0172] A third aspect of the present invention provides a computer-readable storage medium, characterized in that the computer-readable storage medium stores a method program for a lightweight indoor 3D laser ground extraction algorithm, wherein when the method program for the lightweight indoor 3D laser ground extraction algorithm is executed by a processor, it implements the steps of the method for the lightweight indoor 3D laser ground extraction algorithm as described above.

[0173] This invention discloses a lightweight indoor 3D laser ground extraction algorithm, including a method, system, and medium. The method includes: acquiring indoor 3D LiDAR data; obtaining coordinate values ​​based on the LiDAR coordinate system using the indoor 3D LiDAR data; sending the coordinate values ​​to a preset plane equation to obtain a ground plane normal vector; sending the ground plane normal vector to a first preset coordinate system to establish constraints and obtain a comprehensive error value; accumulating the squares of the comprehensive error values ​​to obtain a comprehensive error sum of squares; extracting the minimum value from the comprehensive error sum of squares, and setting the coordinate point corresponding to the minimum value as the optimal current pose. This application is applicable to various lasers. By giving a preset threshold, whether it is 16 lines, 32 lines, or 128 lines, it ensures both the accuracy and robustness of the algorithm, improving the accuracy of ground point identification.

[0174] In the several embodiments provided in this application, it should be understood that the disclosed devices and methods can be implemented in other ways. The device embodiments described above are merely illustrative. For example, the division of units is only a logical functional division, and in actual implementation, there may be other division methods, such as: multiple units or components can be combined, or integrated into another system, or some features can be ignored or not executed. In addition, the coupling, direct coupling, or communication connection between the various components shown or discussed can be through some interfaces, and the indirect coupling or communication connection between devices or units can be electrical, mechanical, or other forms.

[0175] The units described above as separate components may or may not be physically separate. The components shown as units may or may not be physical units. They may be located in one place or distributed across multiple network units. Some or all of the units may be selected to achieve the purpose of this embodiment according to actual needs.

[0176] In addition, in the various embodiments of the present invention, each functional unit can be integrated into one processing unit, or each unit can be a separate unit, or two or more units can be integrated into one unit; the integrated unit can be implemented in hardware or in the form of hardware plus software functional units.

[0177] Those skilled in the art will understand that all or part of the steps of the above method embodiments can be implemented by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When the program is executed, it performs the steps of the above method embodiments. The aforementioned storage medium includes various media capable of storing program code, such as mobile storage devices, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0178] Alternatively, if the integrated units of this invention are implemented as software functional modules and sold or used as independent products, they can also be stored in a computer-readable storage medium. Based on this understanding, the technical solutions of the embodiments of this invention, or the parts that contribute to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as mobile storage devices, ROM, RAM, magnetic disks, or optical disks.

Claims

1. A lightweight indoor 3D laser ground extraction algorithm method, characterized in that, include: Acquire indoor 3D LiDAR data; Based on indoor 3D LiDAR data, coordinate values ​​in the LiDAR coordinate system are obtained; Send the coordinate values ​​to the preset plane equation to obtain the ground plane normal vector; Send the ground plane normal vector to the first preset coordinate system to establish constraints and obtain the comprehensive error value; Then, the squares of the values ​​in the comprehensive error are summed to obtain the sum of squares of the comprehensive error; Extract the minimum value from the sum of squared errors, and set the coordinate point corresponding to the minimum value as the optimal current pose; The preset plane equation further includes: Let p be the set of all points in the preset indoor point cloud. ; Based on all point sets in the indoor point cloud, obtain all point sets in the opposite direction of the normal vector of the preset plane equation. ,in ; extract Let any 4 points be denoted as . The corresponding parameter matrix Q is obtained, where: ； The corresponding parameter matrix Q is sent to the preset homogeneous linear equation system to obtain the preset plane equation parameters X.

2. The method for a lightweight indoor 3D laser ground extraction algorithm according to claim 1, characterized in that, Also includes: Obtain the z-coordinate value of an indoor coordinate point; Determine whether the z-coordinate value of the indoor coordinate point is greater than the first preset threshold. If so, delete the corresponding indoor coordinate value. The retained indoor coordinate points are sent to a preset indoor point cloud for storage.

3. The method for a lightweight indoor 3D laser ground extraction algorithm according to claim 1, characterized in that, The preset plane equation also includes: Will Other points and preset plane equation parameters X are sent to the preset plane equation to determine whether other points belong to ground points. If so, they are saved. Obtain the number of points within the preset model corresponding to the preset plane equation parameter X; Extract the maximum number of points within the preset model; Set the parameter corresponding to the maximum number of points within the preset model as the actual parameter of the plane equation. The preset plane equation is as follows: .

4. The method for a lightweight indoor 3D laser ground extraction algorithm according to claim 1, characterized in that, The first preset coordinate system further includes: Obtain the ground plane normal vector in the initial map coordinate system. ,in ; The ground plane normal vector in the map coordinate system is obtained through a preset odometer. The conversion is performed using the following formula: ; Will Set it as the plane normal vector corresponding to the first preset coordinate system.

5. The method for a lightweight indoor 3D laser ground extraction algorithm according to claim 1, characterized in that, The establishment of constraints also includes: Will Represented by a rotation matrix as follows ; The plane normal vector of the i-th laser frame Switch to By rotating the space, we obtain the vector n, whose formula is: ; Obtain the pitch angle of vector n Azimuth And error d; Set the comprehensive error value to Its formula is: .

6. A lightweight indoor 3D laser ground extraction algorithm system, characterized in that, The system includes a memory and a processor. The memory stores a method program for a lightweight indoor 3D laser ground extraction algorithm. When the processor executes the method program for the lightweight indoor 3D laser ground extraction algorithm, it performs the following steps: Acquire indoor 3D LiDAR data; Based on indoor 3D LiDAR data, coordinate values ​​in the LiDAR coordinate system are obtained; Send the coordinate values ​​to the preset plane equation to obtain the ground plane normal vector; The ground plane normal vector is sent to the first preset coordinate system for constraint optimization to obtain the comprehensive error value; Then, the squares of the values ​​in the comprehensive error are summed to obtain the sum of squares of the comprehensive error; Extract the minimum value from the sum of squared errors, and set the coordinate point corresponding to the minimum value as the optimal current pose; The preset plane equation further includes: Let p be the set of all points in the preset indoor point cloud. ; Based on all point sets in the indoor point cloud, obtain all point sets in the opposite direction of the normal vector of the preset plane equation. ,in ; extract Let any 4 points be denoted as . The corresponding parameter matrix Q is obtained, where: ； The corresponding parameter matrix Q is sent to the preset homogeneous linear equation system to obtain the preset plane equation parameters X.

7. The system for a lightweight indoor 3D laser ground extraction algorithm according to claim 6, characterized in that, Also includes: Obtain the z-coordinate value of an indoor coordinate point; Determine whether the z-coordinate value of the indoor coordinate point is greater than the first preset threshold. If so, delete the corresponding indoor coordinate value. The retained indoor coordinate points are sent to a preset indoor point cloud for storage.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a method program for a lightweight indoor 3D laser ground extraction algorithm. When the method program for the lightweight indoor 3D laser ground extraction algorithm is executed by a processor, it implements the steps of the method for the lightweight indoor 3D laser ground extraction algorithm as described in any one of claims 1 to 5.

Images