LiDAR SLAM Degradation Detection Method and System Based on Point Cloud Residuals

Abstract

The present invention provides a method and system for detecting the degradation of lidar SLAM based on point cloud residuals, including: using a lidar sensor to collect continuous frame point clouds, and preprocessing the continuous frame point clouds to obtain non-ground continuous point cloud data; analyzing the preprocessed point cloud data by using the point-to-plane iterative closest point method to obtain a point cloud constraint relationship of residual anti-disturbance robustness; based on the point cloud constraint relationship, performing disturbance identification to determine whether degradation occurs and obtaining the degradation direction. The present invention can solve the problem that it is difficult to detect the failure of lidar SLAM in a degradation scenario, provide an early warning for the stable operation of lidar SLAM, can achieve high detection accuracy, and is beneficial to practical applications.

Description

Technical Field

[0001] The present invention relates to the field of computer vision, and specifically, to a method and system for detecting the degradation of lidar SLAM based on point cloud residuals. Background Art

[0002] Lidar is a type of sensor that is currently widely used: the emission module is based on the optical principle and emits laser with a specific wavelength by means of pulses, frequency modulation, amplitude modulation, etc.; the reception module uses diode elements to detect the laser reflected after emission, and precise ranging can be obtained through the time of flight of the light. Lidar is widely used in applications such as target detection, scene modeling, and robot navigation, and has relatively broad commercial and academic prospects.

[0003] The Simultaneous Localization and Mapping algorithm (SLAM) uses the information provided by sensors to build a map of the surrounding environment scene and simultaneously determine the corresponding position of the sensor in the map. Currently, SLAM technology is widely used in robot autonomous positioning and navigation tasks. Among them, the lidar-based SLAM algorithm is widely used in high-difficulty positioning and navigation tasks such as unmanned logistics and unmanned minibuses due to its high-precision ranging information and robust positioning results.

[0004] One of the application problems of the lidar SLAM algorithm is the problem of algorithm failure in the face of specific scenarios. These scenarios include: tunnels, corridors, outdoor empty parking lots, highways, etc., which are characterized by less structural texture in the scene, scene degradation, and insufficient data constraints provided by the lidar during positioning, resulting in algorithm failure. The degradation problem can be addressed through methods such as multi-sensor data fusion and state space modeling compensation. How to identify and detect the degraded scenarios of lidar SLAM has become a key step in algorithm optimization. Summary of the Invention

[0005] Aiming at the deficiencies in the prior art, the purpose of the present invention is to provide a method and system for detecting the degradation of lidar SLAM based on point cloud residuals.

[0006] According to one aspect of the present invention, a method for detecting the degradation of lidar SLAM based on point cloud residuals is provided, including:

[0007] Using a lidar sensor to collect continuous frame point clouds, and preprocessing the continuous frame point clouds to obtain non-ground continuous point cloud data;

[0008] Analyzing the preprocessed point cloud data using the point-to-plane iterative closest point method to obtain a point cloud constraint relationship with residual anti-disturbance robustness;

[0009] Based on the point cloud constraint relationship, perform disturbance identification to determine whether degradation has occurred and obtain the SLAM degradation direction.

[0010] Preferably, the preprocessing includes ground point cloud separation and clustering noise removal.

[0011] Preferably, the preprocessed point cloud data is analyzed based on the point-to-plane iterative closest point method to obtain the point cloud constraint relationship of residual anti-disturbance robustness, including:

[0012] Obtain the matching point of each point in the current frame in the previous frame based on the point-to-plane closest point iterative method;

[0013] Construct the residual from the point in the current frame to its corresponding matching point;

[0014] Based on the residual, obtain its sensitivity influence relationship on the robot pose as the point cloud constraint relationship of residual anti-disturbance robustness.

[0015] Preferably, obtaining the matching point of each point in the current frame in the previous frame based on the point-to-plane closest point iterative method includes:

[0016] Set t and t+1 as the point cloud data of two consecutive frames;

[0017] Through the point-to-plane closest point iterative method, match the point d in t+1 i with all points in t by iteration to obtain the point d i and its matching point s in t i .

[0018] Preferably, constructing the residual from the point in the current frame to its corresponding matching point includes:

[0019] Set the transformation matrix M for the point cloud registration of two consecutive frames to be composed of the translation transformation matrix T(t x ,t y ,t z ) and the rotation transformation R(α,β,γ) matrix, where t x ,t y ,t z are the distances that the robot moves in the directions of the rectangular coordinate system respectively, and α,β,γ are the angles of rotation in the directions of the rectangular coordinate system;

[0020] Approximate the transformation matrix M as

[0021]

[0022] For each point d in the target point set, i.e., the current frame t+1 i , obtain its corresponding normal vector Φ through normal vector estimation

[0023] Define the point d i and its corresponding matching point s iThe residual of the point-to-plane iterative closest point method is:

[0024] f i =(M·s i -d i )·n i .

[0025] Preferably, based on the residual f i =(M·s i -d i )·n i , the influence relationship of its sensitivity to the robot pose is obtained, that is: if the robot undergoes a pose transformation and the change of the residual term is small, it is defined that the constraint describing the current robot pose is weak and the system has degenerated.

[0026] Preferably, based on the point cloud constraint relationship, perturbation identification is performed to determine whether degeneration occurs and obtain the degeneration direction, including:

[0027] Based on the point cloud constraint relationship, the degeneration determination problem is transformed into a problem of solving the sensitivity of the residual term to (R, t). Let each residual f i take the partial derivative with respect to (R, t), that is:

[0028]

[0029] Concatenate each residual J i to form a matrix J;

[0030] Perform eigenvalue decomposition on the matrix J to obtain eigenvectors and eigenvalues;

[0031] If the minimum eigenvalue λ min is less than the set threshold, it represents that the current scene has degenerated, and the eigenvector corresponding to the minimum eigenvalue λ min is the current SLAM degeneration direction.

[0032] According to the second aspect of the present invention, a lidar SLAM degeneration detection system based on point cloud residuals is provided, including:

[0033] A data module that uses a lidar sensor to collect continuous frame point cloud data and preprocesses the continuous frame point cloud to obtain non-ground continuous point cloud data;

[0034] A constraint relationship module that analyzes the preprocessed point cloud data based on the point-to-plane iterative closest point method to obtain a point cloud constraint relationship with residual anti-perturbation robustness;

[0035] A degradation judgment module, which, based on the point cloud constraint relationship, performs perturbation recognition, determines whether degradation occurs, and obtains the SLAM degradation direction.

[0036] According to a third aspect of the present invention, there is provided a terminal, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it can be used to execute the above method, or, run the above system.

[0037] According to a fourth aspect of the present invention, there is provided a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, it can be used to execute the above method, or, run the above system.

[0038] Compared with the prior art, the present invention has at least one of the following beneficial effects:

[0039] A method and system for lidar SLAM degradation detection based on point cloud residuals in an embodiment of the present invention can solve the problem that it is difficult to detect the failure of lidar SLAM in a degradation scenario, provide a warning for the stable operation of lidar SLAM, can achieve high detection accuracy, and is beneficial to practical applications. Description of the Drawings

[0040] By reading the detailed description of the non-restrictive embodiments with reference to the following drawings, other features, objectives, and advantages of the present invention will become more obvious:

[0041] Figure 1 is a flowchart of a method for lidar SLAM degradation detection based on point cloud residuals in an embodiment of the present invention;

[0042] Figure 2 is a flowchart of the perturbation recognition process in a preferred embodiment of the present invention;

[0043] Figure 3 is an effect diagram in a specific example of the present invention. Detailed Embodiments

[0044] The present invention will be described in detail below with reference to specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that those of ordinary skill in the art can make several modifications and improvements without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0045] First, the nouns involved in the present invention (SLAM and degradation scenarios) are explained. SLAM: Simultaneous Localization and Mapping algorithm, which uses the information provided by sensors to build a map of the surrounding environment during movement and simultaneously determine the corresponding position of the sensors in the map. SLAM technology is widely used in the environmental modeling and autonomous positioning and navigation tasks of robots. LiDAR SLAM is to draw a map of the laser scanning scene based on the point cloud information obtained by the LiDAR sensor and determine the position of the LiDAR sensor in the map. Degradation scenario: There is less structural texture in the LiDAR usage scenario, so that the data provided by the sensor in the scenario cannot provide sufficient constraints when facing the optimization problem of the system, and the optimization object falls into the local optimal solution, resulting in system failure. The degradation scenarios of LiDAR include tunnels, corridors, outdoor open parking lots, highways, etc.

[0046] See Figure 1 , the present invention provides an embodiment, a LiDAR SLAM degradation detection method based on point cloud residuals, including:

[0047] S100, use a LiDAR sensor to collect continuous frame point clouds, and preprocess the continuous frame point clouds to obtain non-ground continuous point cloud data;

[0048] S200, analyze the preprocessed point cloud data in S100 based on the point-to-plane iterative closest point method to obtain the point cloud constraint relationship of residual anti-disturbance robustness;

[0049] S300, based on the point cloud constraint relationship obtained in S200, perform disturbance identification to determine whether degradation occurs and obtain the SLAM degradation direction.

[0050] This embodiment can solve the problem that it is difficult to detect the failure of LiDAR SLAM in degradation scenarios, provide early warning for the stable operation of LiDAR SLAM, can achieve high detection accuracy, and is beneficial to practical applications.

[0051] In a preferred embodiment of the present invention, when implementing S100, continuous frame point clouds are collected through a LiDAR sensor. Specifically, a robot is equipped with a LiDAR, and the LiDAR emits laser with a specific wavelength by means of pulse, frequency modulation, amplitude modulation, etc. After the laser irradiates an object and is reflected, the reflected laser after emission is received, thereby obtaining the original point cloud data. The preprocessing includes ground point cloud separation and clustering noise removal.

[0052] In a preferred embodiment of the present invention, when implementing S200, the specific implementation process includes:

[0053] S201, obtain the matching point of each point in the current frame in the previous frame based on the point-to-plane closest point iterative method;

[0054] S202. Construct the residual of the points in the current frame to their corresponding matched points obtained in S201.

[0055] S203. Based on the residual constructed in S202, obtain the influence relationship of the residual on the robot pose, and use it as the point cloud constraint relationship of the residual anti-disturbance robustness.

[0056] S201. Construct the residual of the point-to-plane iterative closest point algorithm pairing.

[0057] S202. Based on the residual, obtain the influence relationship of the residual on the robot pose, and use it as the point cloud constraint relationship.

[0058] In a preferred embodiment, S201 is implemented, and the specific process is as follows:

[0059] S2011. Set t and t + 1 as the point cloud data of two consecutive frames.

[0060] S2012. Through the point-to-plane closest point iteration method, iteratively match the point di in t + 1 with all the points in t, and obtain the most matched point si in t. Generally, the most matched here means the closest distance between points.

[0061] Among them, the point-to-plane closest point iteration method is a relatively mature technology. Through this method, lidar SLAM relies on point cloud matching to complete the positioning task.

[0062] In a preferred embodiment, S202 is implemented, and the specific process is as follows:

[0063] S2021. Since the transformation matrix M of point cloud registration is composed of the translation transformation matrix T(t x ,t y ,t z ) and the rotation transformation R(α,β,γ) matrix; based on the fact that in the SLAM process, the transformation matrix solved based on point cloud registration often changes little from the initial value, that is, it is approximately considered that the rotation angle is small, the transformation matrix is approximately reduced to:

[0064]

[0065] S2022. For each point d of the target point set i , obtain its corresponding normal vector n i through normal vector estimation;

[0066] S2023. The point-to-plane iterative closest point algorithm residual between point d i and the corresponding matched point s i is defined as:

[0067] f i =(M·s i -di )·n i 。

[0068] In the above embodiments, the robot uses lidar SLAM for positioning, and lidar SLAM relies on point cloud matching to complete the positioning task. During the point cloud matching process, the residual of the point cloud matching can be calculated. Therefore, the residual can be used to evaluate the sensitivity of the robot's positioning.

[0069] Further, in a preferred embodiment, S203 is implemented. Based on the residual f in S201 i =(M·s i -d i )·n i , the influence relationship of its sensitivity to the robot's pose is obtained, that is: if the robot's pose changes and the change of the residual term is small, it is defined that the constraint describing the robot's current pose is weak and the system has degenerated.

[0070] In a preferred embodiment of the present invention, S300 is implemented, and its specific process is as follows:

[0071] S301, based on the point cloud constraint relationship, transform the degradation determination problem into a problem of solving the sensitivity of the residual term to (R, t). Let each residual f i take the partial derivative with respect to (R, t), that is:

[0072]

[0073] S302, splice each residual J i to form a matrix J;

[0074] S303, perform eigenvalue decomposition on the matrix J:

[0075]

[0076] where the smallest eigenvalue λ min describes the localizability of the current position;

[0077] S304, if the smallest eigenvalue λ min is less than the set threshold, it represents that the current scene has degenerated, and the eigenvector corresponding to the smallest eigenvalue λ min is the current degradation direction.

[0078] It should be noted that the meanings of matrix eigenvalues and eigenvectors are as follows: The projection of a matrix onto any eigenvector is a fixed scaling of the eigenvector itself, and the scaling ratio is the magnitude of the eigenvalue. Therefore, the eigenvalue reflects the ratio of the projection of the matrix in the direction of the eigenvector. The smaller the eigenvalue, the smaller the constraint of the matrix in the direction of the eigenvector. A small constraint means that degeneration is likely to occur in this direction. Therefore, the smallest eigenvalue reflects the positioning reliability in this direction.

[0079] See Figure 3 , which is a case of tunnel degeneration detection in a specific example of the present invention. In the figure, the abscissa is time and the ordinate is the reciprocal of the smallest eigenvalue. The larger the value, the smaller the eigenvalue. When the eigenvalue is less than the threshold, a degeneration phenomenon is detected.

[0080] Based on the same inventive concept, in other embodiments of the present invention, a lidar SLAM degeneration detection system based on point cloud residuals is provided, including a data module, a constraint relationship module, and a degeneration judgment module; the data module uses a lidar sensor to collect continuous frame point cloud data and preprocesses the continuous frame point cloud; the constraint relationship module analyzes the preprocessed point cloud data based on the point-to-plane iterative closest point method to obtain a point cloud constraint relationship with residual anti-disturbance robustness; the degeneration judgment module performs disturbance identification based on the point cloud constraint relationship to obtain a result of whether scene degeneration has occurred.

[0081] Based on the same inventive concept, in other embodiments of the present invention, a terminal is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it can be used to execute the lidar SLAM degeneration detection method based on point cloud residuals, or run the lidar SLAM degeneration detection system based on point cloud residuals.

[0082] Optionally, a memory for storing programs; the memory may include volatile memory (e.g., random-access memory, such as static random-access memory (SRAM), Double Data Rate Synchronous Dynamic Random Access Memory (DDR SDRAM), etc.); the memory may also include non-volatile memory, such as flash memory. The memory is used to store computer programs (such as application programs and functional modules for implementing the above method), computer instructions, etc. The above computer programs, computer instructions, etc. can be partitioned and stored in one or more memories. And the above computer programs, computer instructions, data, etc. can be called by the processor.

[0083] The above computer programs, computer instructions, etc. can be partitioned and stored in one or more memories. And the above computer programs, computer instructions, data, etc. can be called by the processor.

[0084] A processor for executing the computer program stored in the memory to implement each step in the method described in the above embodiments. For specific details, reference can be made to the relevant descriptions in the previous method embodiments.

[0085] The processor and the memory can be of independent structure or integrated into an integrated structure. When the processor and the memory are of independent structure, the memory and the processor can be coupled and connected through a bus.

[0086] Based on the same inventive concept, in other embodiments of the present invention, a computer-readable storage medium is provided, on which a computer program is stored. When the program is executed by a processor, it can be used to execute the method for detecting the degradation of lidar SLAM based on point cloud residuals, or to run the system for detecting the degradation of lidar SLAM based on point cloud residuals.

[0087] Among them, the computer-readable medium includes computer storage media and communication media, where the communication media includes any medium that facilitates the transmission of a computer program from one place to another. The storage media can be any available medium accessible by a general-purpose or special-purpose computer. An exemplary storage media is coupled to the processor, enabling the processor to read information from and write information to the storage media. Of course, the storage media can also be a component of the processor. The processor and the storage media can be located in an ASIC. Additionally, the ASIC can be located in a user device. Of course, the processor and the storage media can also exist as discrete components in a communication device.

[0088] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0089] The present application is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to the present application. It should be understood that each flow and / or block in the flowchart and / or block diagram, as well as the combination of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in Figure 1 one or more of the flows Figure 1 or blocks or combinations of blocks.

[0090] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including instruction means that implement the functions specified in Figure 1 one or more of the flows Figure 1 or blocks or combinations of blocks.

[0091] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are performed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide means for implementing the functions specified in Figure 1Steps specified for the functions in one process or multiple processes and / or one block or multiple blocks Figure 1 Steps for the functions specified in one block or multiple blocks.

[0092] Obviously, those skilled in the art can make various modifications and variations to this application without departing from the spirit and scope of this application. Thus, if these modifications and variations of this application fall within the scope of the claims of this application and their equivalent technologies, this application is also intended to include these modifications and variations.

[0093] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various deformations or modifications within the scope of the claims, which does not affect the essence of the present invention. The above preferred features can be used in any combination without conflict.

Claims

1. A method for detecting the degradation of lidar SLAM based on point cloud residuals, characterized in that, Including: Using a lidar sensor to collect continuous frame point clouds and preprocessing the continuous frame point clouds to obtain non-ground continuous point cloud data; Analyzing the preprocessed point cloud data using the point-to-plane iterative closest point method to obtain a point cloud constraint relationship with residual anti-disturbance robustness; Based on the point cloud constraint relationship, performing disturbance identification, determining whether degradation occurs, and obtaining the SLAM degradation direction; The preprocessing includes separating ground point clouds and removing clustering noise; The analysis of the preprocessed point cloud data based on the point-to-plane iterative closest point method to obtain a point cloud constraint relationship with residual anti-disturbance robustness includes: Obtaining the matching point of each point in the current frame in the previous frame based on the point-to-plane closest point iterative method; Constructing the residual of the point in the current frame to its corresponding matching point; Based on the residual, obtaining its sensitivity influence relationship on the robot pose as the point cloud constraint relationship with residual anti-disturbance robustness; The obtaining of the matching point of each point in the current frame in the previous frame based on the point-to-plane closest point iterative method includes: Setting t and t + 1 as the point cloud data of two consecutive frames; Through the point-to-plane closest point iteration method, the point d in t+1 i is iteratively matched with all points in t to obtain the point d i and the matching point s in t i ; Based on the point cloud constraint relationship, performing disturbance identification, determining whether degradation occurs, and obtaining the degradation direction, including: Based on the above point cloud constraint relationship, the problem of degradation determination is transformed into the problem of solving the sensitivity of the residual pair (R, T), and let each residual f i Take the partial derivative of (R, T), that is: Concatenate each residual J i to form matrix J; Perform eigenvalue decomposition on the matrix J to obtain eigenvectors and eigenvalues; If the minimum eigenvalue λ min is less than the set threshold, it represents that the current scenario has degraded, and the eigenvector corresponding to the minimum eigenvalue λ min is the current SLAM degradation direction.

2. The method for detecting the degradation of lidar SLAM based on point cloud residuals according to claim 1, wherein, The constructing of the residual of the point in the current frame to its corresponding matching point includes: Set the transformation matrix M for point cloud registration of two consecutive frames to be composed of a translation transformation matrix T(t x ,t y ,t z ) and a rotation transformation R(α,β,γ) matrix. t x ,t y ,t z are the distances that the robot moves respectively in the directions of the rectangular coordinate system, and α,β,γ are the angles of rotation in the directions of the rectangular coordinate system; Approximating the transformation matrix M as For each point d in the target point set, i.e., in the current frame t+1 i , its corresponding normal vector n is obtained through normal vector estimation i Define the point d i and the residual of the point-to-plane iterative closest point method with the corresponding matching point s i as follows: f i = (M·s i - d i )·n i .

3. A method for detecting the degradation of lidar SLAM based on point cloud residuals according to claim 2, characterized in that Based on the residual f i =(M·s i -d i )·n i , the sensitivity influence relationship of the robot pose is obtained, that is: if the robot undergoes a pose transformation and the change of the residual is small, it is defined that the constraint of the current description of the robot pose is weak and the system has degenerated.

4. A lidar SLAM degradation detection system based on point cloud residuals, which adopts the lidar SLAM degradation detection method described in any one of claims 1-3, is characterized in that, Including: A data module that uses a lidar sensor to collect continuous frame point cloud data and preprocesses the continuous frame point clouds to obtain non-ground continuous point cloud data; A constraint relationship module that analyzes the preprocessed point cloud data based on the point-to-plane iterative closest point method to obtain a point cloud constraint relationship with residual anti-disturbance robustness; A degradation judgment module that, based on the point cloud constraint relationship, performs disturbance identification, determines whether degradation occurs, and obtains the SLAM degradation direction.

5. A terminal, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the processor executes the program, it can be used to execute the method according to any one of claims 1-3.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it can be used to execute the method according to any one of claims 1-3.

Images