Method and System for Detecting Degradation of LiDAR SLAM Based on Pose Constraint

Abstract

The present invention provides a method and system for detecting the degradation of lidar SLAM based on pose constraints, including: collecting continuous frame point clouds through a lidar sensor; analyzing the continuous frame point clouds to obtain the point cloud constraint relationship of the anti-disturbance robustness of the robot pose; and identifying the pose constraint disturbance of the robot according to the point cloud constraint relationship to obtain the SLAM 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 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 more particularly, to a method and system for detecting the degradation of lidar SLAM based on pose constraints. Background Art

[0002] Lidar is a type of sensor that is widely used at present: 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 receiving module uses diode elements to detect the laser reflected after emission, and the precise ranging can be obtained through the flight time of the light. Lidar is widely used in applications such as target detection, scene modeling, and robot navigation, and has 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, due to its high-precision ranging information and robust positioning results, is widely used in high-difficulty positioning and navigation tasks such as unmanned logistics and unmanned minibuses.

[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 degradation 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 pose constraints.

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

[0007] Collecting continuous frame point clouds through a lidar sensor;

[0008] Analyzing the continuous frame point clouds to obtain the point cloud constraint relationship of the robot pose anti-disturbance robustness;

[0009] According to the point cloud constraint relationship, performing robot pose constraint disturbance identification to obtain the degradation direction.

[0010] Preferably, the analysis of the continuous frame point cloud to obtain the point cloud constraint relationship of the robot pose anti-disturbance robustness includes:

[0011] Approximate the local surface of the continuous frame point cloud as a plane, and each measurement point satisfies the following relational expression:

[0012]

[0013] Where, is the unit vector of the laser beam in the radar coordinate system, used to indicate the beam direction, corresponds to its beam length, i ∈ {1, 2, …, m} is the index of the laser beam, is the normal vector of the approximate plane, p i,0 is a point on the approximate plane, respectively describe the position and attitude of the robot;

[0014] Denote as d i , then the point cloud constraint relationship for obtaining the robot pose anti-disturbance robustness is:

[0015]

[0016] Preferably, the identification of the robot pose constraint disturbance according to the point cloud constraint relationship includes:

[0017] During the point cloud registration process, the changes in the position and attitude (R, t) of the robot are relatively small, and the changes in (R, t) can be regarded as disturbances;

[0018] By calculating the sensitivity of the laser measurement distance ρ i to the (R, t) disturbance, determine whether the lidar SLAM has scene degradation.

[0019] Preferably, if the robot pose is disturbed and the laser measurement distance changes greatly, then the current description of the robot pose constraint is strong; on the contrary, if the robot pose is disturbed and the laser measurement distance changes little, then the current description of the robot constraint is extremely weak, and the system is in a degraded environment.

[0020] Preferably, the obtaining of the degradation direction includes:

[0021] Based on the assumption of small-angle transformation in the robot pose, use R ≈ I + [θ] × to linearize the derivative problem:

[0022]

[0023] Based on the implicit function theorem, establish an equation:

[0024]

[0025]

[0026] It can be derived that

[0027] According to the formula obtained from the above implicit function theorem, construct the F matrix and the T matrix, which respectively represent the sensitivity of the laser constraint to the translation parameters and the sensitivity to the rotation parameters:

[0028]

[0029]

[0030] Perform eigenvalue decomposition on the F matrix and the T matrix to obtain a set of eigenvectors

[0031] The eigenvector corresponding to its minimum eigenvalue is the current degradation direction: the minimum eigenvalue of the F matrix is used to judge the degradation direction of the translation amount; the minimum eigenvalue of the T matrix is used to judge the degradation direction of the rotation.

[0032] Preferably, the projection of the matrix on any eigenvector is a fixed scaling of the eigenvector itself, and the scaling ratio is the eigenvalue. The eigenvalue reflects the ratio size 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 degradation is likely to occur in this direction.

[0033] Preferably, if λ min is less than the set threshold, it represents that the current scene degrades; at the same time, the eigenvector corresponding to the minimum eigenvalue λ min is the current degradation direction.

[0034] According to the second aspect of the present invention, there is provided a lidar SLAM degradation detection system based on pose constraints, including:

[0035] A data module, which collects continuous frame point clouds through a lidar sensor;

[0036] A constraint relationship module, which analyzes the continuous frame point clouds to obtain the point cloud constraint relationship of the robot pose anti-disturbance robustness;

[0037] A disturbance recognition module, which performs robot pose constraint disturbance recognition according to the point cloud constraint relationship.

[0038] 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-mentioned lidar SLAM degradation detection method based on pose constraints, or run the above-mentioned lidar SLAM degradation detection system based on pose constraints.

[0039] 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-mentioned lidar SLAM degradation detection method based on pose constraints, or run the above-mentioned lidar SLAM degradation detection system based on pose constraints.

[0040] Compared with the prior art, the present invention has the following beneficial effects:

[0041] 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. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] By reading the detailed description of the non-limiting embodiments with reference to the following drawings, other features, objects, and advantages of the present invention will become more apparent:

[0043] Figure 1 is a flowchart of the lidar SLAM degradation detection method based on pose constraints in an embodiment of the present invention;

[0044] Figure 2 is a flowchart of the lidar SLAM degradation detection based on pose constraints in a preferred embodiment of the present invention;

[0045] Figure 3 is a schematic diagram of the constraints on the robot pose in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0046] 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.

[0047] First, the terms 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 a local optimal solution, resulting in system failure. The degradation scenarios of LiDAR include tunnels, corridors, outdoor open parking lots, highways, etc.

[0048] The present invention provides an embodiment, a method for detecting LiDAR SLAM degradation based on pose constraints, see Figure 1 , including:

[0049] S100, collecting continuous frame point clouds through a LiDAR sensor;

[0050] S200, analyzing the continuous frame point clouds of S200 to obtain the point cloud constraint relationship of the robot pose anti-disturbance robustness;

[0051] S300, according to the point cloud constraint relationship of S200, performing robot pose constraint disturbance identification to obtain the degradation direction.

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

[0053] In a preferred embodiment of the present invention, when implementing S100, continuous frame point clouds are collected through a LiDAR sensor. Specifically, the 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, it is reflected, and the reflected laser after emission is received, thereby obtaining the original point cloud data. Each frame includes a cluster of point clouds, including the point clouds emitted by m laser points. Among them, each reflection of each laser on the object corresponds to a measurement point.

[0054] In a preferred embodiment of the present invention, when implementing S200, that is, for LiDAR positioning, obtaining the pose constraint of each measurement point of the LiDAR on the robot, see Figure 2 , the specific process is as follows:

[0055] Approximate the local surface of the point cloud as a plane, then for each measurement point, it satisfies:

[0056]

[0057] wherein is the unit vector of the laser beam in the radar coordinate system, used to indicate the beam direction, corresponds to its beam length, i ∈ {1, 2, …, m} is the index of the laser point beam, is the normal vector of the approximate plane, p i,0 is a point on the approximate plane, respectively describe the position and attitude of the robot, see Figure 3 .

[0058] Denote as d i , then there is:

[0059]

[0060] Take this formula as the point cloud constraint relationship of the pose anti-disturbance robustness of the robot. This constraint relationship describes the constraint relationship between the robot pose and the laser points, providing an accurate model basis for further detecting whether the measurement of the laser points has deteriorated.

[0061] In a preferred embodiment of the present invention, implement S300. In point cloud registration (which means that as the robot moves, the lidar continuously obtains two frames of point cloud data, and point cloud registration needs to find the matching relationship between points in these two frames of point cloud), the transformation matrix differences corresponding to the two registrations are not significant, so (R, t) is small. (R, t) can be regarded as a perturbation, describing the localizable ability of the system, and is converted into the laser measurement distance ρ i The sensitivity to (R, t). If the pose of the robot is slightly perturbed while the laser measurement distance changes greatly, then the current description of the robot pose has a strong constraint. On the contrary, if the robot pose is perturbed but the laser measurement distance changes little, then the robot constraint is extremely weak at this time, and the system is in a degraded environment.

[0062] In another preferred embodiment, see Figure 2 , the preferred process of S300 is as follows:

[0063] S301: Calculate the partial derivative of the laser measurement distance ρ i with respect to (R, t). Based on the assumption of small-angle transformation, use R ≈ I + [θ] × to linearize the derivative problem:

[0064]

[0065] S302: Based on the implicit function theorem, establish the following equation:

[0066]

[0067]

[0068] It can be derived by combining with S301 that:

[0069]

[0070]

[0071] Step S303: According to the formula obtained in step S302, construct the F matrix and the T matrix, which respectively represent the sensitivity of the laser constraint to the translation parameter and the sensitivity to the rotation parameter:

[0072]

[0073]

[0074] Step S304: Perform eigenvalue decomposition according to the formula obtained in step S303 to obtain the corresponding eigenvectors.

[0075]

[0076]

[0077] Step S305: The eigenvector corresponding to its minimum eigenvalue is the current degradation direction: the minimum eigenvalue of the F matrix is used to judge the degradation direction of the translation amount; the minimum eigenvalue of the T matrix is used to judge the degradation direction of the rotation.

[0078] For an eigenvector, the projection of any one of the vectors on this coordinate system (matrix) is only a fixed scaling of itself, and the scaling ratio is the size of the eigenvalue. Therefore, the eigenvalue can reflect the proportional size of the vector direction in the matrix. The smaller the eigenvalue, the smaller the constraint of the matrix in the direction of this eigenvector. A small constraint means that degradation is likely to occur in this direction. Therefore, the minimum eigenvalue λ min can describe the localizability of the current position.

[0079] Furthermore, if λ min is less than the set threshold, it represents that the current scene has degraded. At the same time, the eigenvector corresponding to the minimum eigenvalue λ min is the current degradation direction.

[0080] Based on the same inventive concept, in other embodiments of the present invention, a lidar SLAM degradation detection system based on pose constraints is provided, including a data module, a constraint relationship module, and a perturbation identification module. The data module collects consecutive frame point clouds through a lidar sensor; the constraint relationship module analyzes the consecutive frame point clouds to obtain the point cloud constraint relationship of the robot pose anti-perturbation robustness; the perturbation identification module performs robot pose constraint perturbation identification according to the point cloud constraint relationship.

[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 method above, or run the system above.

[0082] Optionally, the memory is used to store programs; the memory may include volatile memory (English: volatile memory), such as random access memory (English: random-access memory, abbreviation: RAM), such as static random access memory (English: static random-access memory, abbreviation: SRAM), double data rate synchronous dynamic random access memory (English: Double Data Rate Synchronous Dynamic Random Access Memory, abbreviation: DDR SDRAM), etc.; the memory may also include non-volatile memory (English: non-volatile memory), such as flash memory (English: 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 stored in partitions 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 stored in partitions in one or more memories. And the above computer programs, computer instructions, data, etc. can be called by the processor.

[0084] The processor is used to execute the computer program stored in the memory to implement each step in the method involved in the above embodiments. For specific details, please refer to the relevant descriptions in the previous method embodiments.

[0085] The processor and the memory can be in an independent structure or an integrated structure integrated together. When the processor and the memory are in an 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, and when the program is executed by a processor, it can be used to execute the above-mentioned method or run the above-mentioned system.

[0087] Among them, the computer-readable medium includes a computer storage medium and a communication medium, where the communication medium includes any medium that facilitates the transmission of a computer program from one place to another. The storage medium can be any available medium that can be accessed by a general-purpose or special-purpose computer. An exemplary storage medium is coupled to the processor, so that the processor can read information from the storage medium and write information to the storage medium. Of course, the storage medium can also be a component of the processor. The processor and the storage medium can be located in an ASIC. In addition, the ASIC can be located in a user device. Of course, the processor and the storage medium 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.) containing computer-usable program code.

[0089] The present application is described with reference to the flowcharts and / or block diagrams of the method, device (system), and computer program product according to the present application. It should be understood that each process and / or block in the flowchart and / or block diagram, and the combination of processes 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, so that the instructions executed by the processor of the computer or other programmable data processing devices generate a device for implementing the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple 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, so that the instructions stored in the computer-readable memory generate a manufactured product including an instruction device, and the instruction device implements the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0091] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus, so that a series of operation steps are executed on the computer or other programmable apparatus to produce a computer-implemented process, thereby the instructions executed on the computer or other programmable apparatus provide steps for realizing the functions specified in one process Figure 1 one process or more processes and / or blocks Figure 1 or steps for realizing the functions specified in a block or more blocks.

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

[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 pose constraints, characterized in that, Including: Collecting consecutive frame point clouds through a lidar sensor; Analyzing the consecutive frame point clouds to obtain the point cloud constraint relationship of the anti-disturbance robustness of the robot pose; According to the point cloud constraint relationship, performing robot pose constraint disturbance identification to obtain the SLAM degradation direction; The analyzing the consecutive frame point clouds to obtain the point cloud constraint relationship of the anti-disturbance robustness of the robot pose includes: Approximating the local surface of the consecutive frame point clouds as a plane, and each measurement point satisfies the following relational expression: Among them, is the unit vector of the laser beam in the lidar coordinate system, used to indicate the laser beam direction, corresponds to the laser measurement distance, i ∈ {1, 2, …, m} is the index of the laser beam, is the normal vector of the approximate plane, p i,0 is a point on the approximate plane, describes the pose of the robot, R is the rotation matrix, and t is the translation distance; Denote - as d i , then the point cloud constraint relationship for obtaining the pose anti-disturbance robustness of the robot is: The performing robot pose constraint disturbance identification according to the point cloud constraint relationship includes: During the point cloud registration process, regarding the change of the robot's pose (R, t) as a disturbance; By calculating the laser measurement distance ρ i The sensitivity to the perturbation of (R, t) is judged to determine whether scene degradation occurs in lidar SLAM; If the robot pose is disturbed and the laser measurement distance changes greatly, then the current description of the robot pose constraint is strong; on the contrary, if the robot pose is disturbed and the laser measurement distance changes little, then the current description of the robot constraint is extremely weak, and the system is in a degraded environment.

2. The method for detecting the degradation of lidar SLAM based on pose constraint according to claim 1, wherein The obtaining the SLAM degradation direction includes: Based on the assumption of small-angle θ transformation in the robot pose, use R≈I+[θ] × Linearize the derivative problem: Based on the implicit function theorem, establishing an equation: The derived formula According to the equation and the formula obtained by the implicit function theorem, constructing an F matrix and a T matrix, which respectively represent the sensitivity of the laser constraint to the translation parameter and the sensitivity to the rotation parameter: Perform eigenvalue decomposition on the F matrix and the T matrix to obtain a set of eigenvectors, The eigenvector corresponding to its minimum eigenvalue is the current degradation direction: the minimum eigenvalue of the F matrix is used to judge the degradation direction of the translation amount; the minimum eigenvalue of the T matrix is used to judge the degradation direction of the rotation.

3. The method for detecting the degradation of lidar SLAM based on pose constraint according to claim 2, wherein The projection of the matrix on any eigenvector is a fixed scaling of the eigenvector itself, and the scaling ratio is the eigenvalue. The eigenvalue reflects the proportion 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, and the smaller the constraint means that degradation is likely to occur in this direction.

4. A method for detecting the degradation of lidar SLAM based on pose constraints according to claim 3, characterized in that, If the minimum eigenvalue λ min is less than the set threshold, it represents that the current scene has degraded; meanwhile, the eigenvector corresponding to the minimum eigenvalue λ min is the current degradation direction.

5. A lidar SLAM degradation detection system based on pose constraints, which uses the lidar SLAM degradation detection method according to any one of claims 1-4, characterized in that, Including: A data module that collects consecutive frame point clouds through a lidar sensor; A constraint relationship module that analyzes the consecutive frame point clouds to obtain the point cloud constraint relationship of the anti-disturbance robustness of the robot pose; A disturbance identification module that performs robot pose constraint disturbance identification according to the point cloud constraint relationship.

6. 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 described in any one of claims 1-3.

7. 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 described in any one of claims 1-3.

Images