Laser point cloud map updating method, system, product, medium and computer device

Abstract

The application belongs to the technical field of mobile robot positioning and mapping. A laser point cloud map updating method, system, product, medium and computer equipment are provided. The laser radar point cloud is subjected to distortion removal by using the predicted value of the inertial measurement unit. The reflection intensity in the point cloud after distortion removal is subjected to intensity correction. An intensity image is obtained according to the corrected reflection intensity. An intensity observation result is obtained according to the intensity image. The robot pose is updated according to the intensity observation result. The points in the point cloud after distortion removal are searched in the corresponding plane in the map to obtain all point-plane matching pairs. The obtained point-plane matching pairs are subjected to degeneration detection to obtain a geometric observation result. The laser point cloud map is updated on the basis of the robot pose updating result according to the geometric observation result to obtain an updated laser point cloud map. The application can effectively improve the stability and precision of robot pose control.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mobile robot positioning and mapping, and in particular to a laser point cloud map updating method, system, product, medium and computer equipment. BACKGROUND

[0002] The statements in this section merely provide background technology related to the present application and do not necessarily constitute prior art.

[0003] With the rapid development of unmanned delivery business, unmanned delivery vehicles, unmanned delivery drones and other products are emerging, which has given rise to the demand for autonomous positioning technology. Three-dimensional laser SLAM (Simultaneous Localization and Mapping) has become the core solution for environment perception and positioning due to its high precision characteristics.

[0004] However, in long corridors, tunnels and other sparse feature scenes, the positioning accuracy of laser radar is reduced or even fails due to the degradation of geometric information. Traditional methods improve robustness by fusing cameras, wheel speed meters and other sensors, but face problems such as noise interference, complex calibration or light sensitivity. The reflection intensity information returned by the laser radar can provide additional information about the surrounding environment, and proper use of laser radar reflection intensity can effectively improve the stability of the SLAM system. At the same time, since the geometric information measured by the laser radar has noise, if no effective measures are taken, the noise will cause false updates in the degradation direction, reducing the stability and precision of the robot pose control. SUMMARY

[0005] In order to solve the problems of the prior art, the present application provides a laser point cloud map updating method, system, product, medium and computer equipment, which can effectively improve the information acquisition efficiency in the degradation environment, effectively limit the negative impact of laser radar measurement noise on state update, ensure the update precision of the laser point cloud map, and improve the stability and precision of the robot pose control.

[0006] In order to achieve the above purpose, the present application adopts the following technical solutions:

[0007] In a first aspect, the present application provides a laser point cloud map updating method.

[0008] A laser point cloud map updating method, comprising the following processes:

[0009] Removing distortion from the laser radar point cloud using the predicted value of the inertial measurement unit;

[0010] Intensity correction is performed on the reflection intensity in the point cloud after distortion removal, an intensity image is obtained according to the corrected reflection intensity, an intensity observation result is obtained according to the intensity image, and the robot pose is updated according to the intensity observation result;

[0011] The corresponding plane of the points in the point cloud after distortion removal in the map is searched, all point-plane matching pairs are obtained, degeneration detection is performed on the obtained point-plane matching pairs to obtain a geometric observation result, and the laser point cloud map is updated on the basis of the robot pose update result according to the geometric observation result, to obtain an updated laser point cloud map.

[0012] In an implementation form of the first aspect of the present application, the intensity observation result is obtained according to the intensity image, comprising:

[0013] Feature points are extracted on the current frame intensity image, the pixel coordinates of the feature points in the next frame intensity image are tracked by optical flow, and the difference between the pixel coordinates of the feature points tracked by optical flow in the next frame image and the pixel coordinates predicted by the inertial measurement unit is taken as the intensity observation.

[0014] As a further limitation of the first aspect of the present application, feature points are extracted on the intensity image, and the three-dimensional coordinates of the laser points corresponding to the pixel coordinates closest to the extracted feature points are selected as the spatial three-dimensional coordinates of the feature points.

[0015] As a further limitation of the first aspect of the present application, the pixel coordinates of the feature points in the next frame intensity image are tracked by optical flow, comprising:

[0016] The three-dimensional coordinates corresponding to the feature points extracted from the current frame intensity image are converted to the next frame intensity image to obtain converted three-dimensional coordinates;

[0017] The converted three-dimensional coordinates are projected onto the next frame intensity image according to the projection model to obtain the pixel coordinate prediction value of the feature points on the current frame intensity image on the next frame intensity image, the pixel coordinate prediction value is taken as the initial guess of the optical flow, and finally the pixel coordinates of the feature points in the next frame image are obtained.

[0018] In an implementation form of the first aspect of the present application, the degeneration detection is performed on the obtained point-plane matching pairs to obtain a geometric observation result, comprising:

[0019] The updated six directions are divided into three levels of strong, medium and weak, and degeneration processing is performed, the state update in the weak constraint direction is completely limited, the state update in the medium constraint direction is partially limited, and the state update in the strong constraint direction is not limited, and the six directions are three update directions of the rotating part of the robot and three update directions of the displacement part of the robot.

[0020] As a further limitation of the first aspect of the application, the six updated directions are divided into three levels of strong, medium and weak, including:

[0021] The sum of the constraint values of all point surfaces in each updated direction is determined.

[0022] According to the sum of the constraint values of all point surfaces in each updated direction, the six updated directions are divided into strong constraint directions, medium constraint directions and weak constraint directions.

[0023] In a second aspect, the application provides a laser point cloud map updating system.

[0024] A laser point cloud map updating system, comprising:

[0025] A point cloud preprocessing unit configured to remove distortion from the lidar point cloud using inertial measurement unit prediction values;

[0026] A primary state updating unit configured to correct the reflection intensity in the distortion-removed point cloud, obtain an intensity image according to the corrected reflection intensity, obtain an intensity observation result according to the intensity image, and update the robot pose according to the intensity observation result;

[0027] A secondary state updating unit configured to search for the corresponding plane of the points in the distortion-removed point cloud in the map to obtain all point surface matching pairs, perform degeneration detection on the obtained point surface matching pairs to obtain a geometric observation result, and update the laser point cloud map based on the robot pose update result according to the geometric observation result to obtain an updated laser point cloud map.

[0028] In a third aspect, the application provides a computer device, comprising a processor and a computer readable storage medium.

[0029] The processor is adapted to execute a computer program.

[0030] The computer readable storage medium has a computer program stored therein, and the computer program, when executed by the processor, implements the laser point cloud map updating method of the first aspect of the application.

[0031] In a fourth aspect, the application provides a computer readable storage medium storing a computer program, and the computer program is adapted to be loaded and executed by a processor to implement the laser point cloud map updating method of the first aspect of the application.

[0032] In a fifth aspect, the application provides a computer program product comprising a computer program, and the computer program, when executed by a processor, implements the laser point cloud map updating method of the first aspect of the application.

[0033] Compared with the prior art, the present application has the following advantages:

[0034] 1、The laser point cloud map updating method innovatively proposed by the present application generates an intensity image by using the intensity information of the laser radar, and extracts feature points on the intensity image, without relying on additional sensors such as cameras, thereby avoiding the complexity of camera calibration and the problem of light sensitivity, and constructing a texture image by using the intensity, which significantly improves the environmental perception ability in a sparse environment feature scene.

[0035] 2、The laser point cloud map updating method innovatively proposed by the present application is based on the geometric constraint intensity classification and hierarchical updating strategy of the Hessian matrix, quantizes the directional constraint intensity by SVD decomposition, dynamically suppresses the noise interference of the weak constraint direction, and effectively avoids the robot positioning drift and system failure in a degenerative scene such as a long corridor and an open area.

[0036] 3、The laser point cloud map updating method innovatively proposed by the present application only relies on the laser radar and the IMU (Inertial Measurement Unit), reduces the hardware cost and noise accumulation risk of multi-sensor fusion, and the reflection intensity correction and geometric constraint correction method is universal and suitable for various complex scenes such as tunnels and underground mines, which improves the accuracy of the three-dimensional laser SLAM system while considering the robustness and practicality, and provides a reliable solution for long-term stable operation in the fields of autonomous driving and industrial inspection.

[0037] The advantages of the additional aspects of the present application will be partially given in the following description, partially will become obvious from the following description, or will be understood by the practice of the present application. BRIEF DESCRIPTION OF DRAWINGS

[0038] The drawings accompanying the specification of the present application serve to provide a further understanding of the present application, and the illustrative embodiments of the present application and their descriptions serve to explain the present application, and do not constitute an improper limitation of the present application.

[0039] Figure 1 A schematic diagram of the laser point cloud map updating method provided for an exemplary embodiment of the present application is shown in the following figure:

[0040] Figure 2 A schematic diagram of the laser point cloud map updating system provided for an exemplary embodiment of the present application is shown in the following figure:

[0041] Figure 3 A schematic diagram of the computer device provided for an exemplary embodiment of the present application is shown in the following figure. DETAILED DESCRIPTION

[0042] The application will be further described below with reference to the accompanying drawings and examples.

[0043] It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the application. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0044] SLAM (Simultaneous Localization and Mapping) is a key technology in the fields of robots, autonomous driving, augmented reality, etc., and its core is to simultaneously complete the self-positioning and the construction of the surrounding environment map when the device moves autonomously in an unknown environment. As described in the background art, due to the noise of the geometric information measured by the laser radar, if no effective measures are taken, the noise will cause an error update in the degenerative direction, which will reduce the stability and accuracy of the robot pose control. In view of this, the present application proposes a laser point cloud map updating method, which adopts a two-stage state updating mechanism. The first stage is to obtain an intensity odometry from the reflection intensity information of the laser radar, to eliminate the influence of distance and incident angle by correcting the reflection intensity, to construct a texture intensity image and extract ORB (Oriented FAST and Rotated BRIEF) features, to combine the IMU (Inertial Measurement Unit) prediction to accelerate the LK (Lucas-Kanade) optical flow tracking, to form a re-projection constraint, and then to obtain an intensity observation, to update the state based on the intensity observation result, and to obtain the updated pose; the second stage is to obtain a geometric observation from the geometric information measured by the laser radar, and to perform degenerative detection and degenerative processing on the geometric observation, and then to obtain a geometric observation. The present application quantifies the geometric constraint strength based on the singular value decomposition of the Hessian matrix, classifies the update direction into three levels of strong, medium and weak, suppresses the noise interference of the weak constraint direction by dynamically correcting the Jacobian matrix, realizes the adaptive state update in the degenerative scene, and significantly improves the positioning accuracy and anti-noise robustness of the SLAM system in the feature sparse scene by reducing the dependence on additional sensors, enhancing the environmental representation ability through the reflection intensity information, and combining the hierarchical optimization strategy of geometric constraints.

[0045] As shown in Figure 1 , first, a forward propagation is performed according to an IMU prediction model to obtain a prediction value, and the point cloud input by the laser radar is removed from distortion using the IMU prediction value. The point cloud after distortion removal contains the geometric scale information of the surrounding environment obtained by the laser radar, which is referred to as geometric information, and the point cloud contains the reflection intensity information of the surrounding environment. The two kinds of information are processed respectively.

[0046] For the reflection intensity information, first, the point cloud reflection intensity is corrected in intensity, the corrected reflection intensity is taken as the pixel value, and the reflection intensity is arranged in order to obtain an intensity image. Extract the ORB feature points on the obtained intensity image, and then track the pixel coordinates of the feature points on the next frame intensity image through LK optical flow. The difference between the pixel coordinates of the ORB feature points tracked through the LK optical flow in the next frame image and the pixel coordinates obtained through the IMU prediction is taken as the intensity observation, and then the system state is updated using the iterative error state Kalman filtering algorithm. After updating the system state in the above steps to obtain the latest pose, the point cloud geometric information is processed. Search for the corresponding plane of the points in the current frame point cloud in the map to obtain all point-plane matching pairs and calculate the point-plane distance residual. Degeneration detection is performed on the obtained point-plane matching pairs, and the six directions of the system update are divided into three levels of strong, medium and weak. Degeneration processing is performed to limit the state update of the system in the weak constraint direction, partially limit the state update of the medium constraint direction, and not limit the state update of the strong constraint direction. The geometric observation is obtained through the degeneration processing, and then the system state is updated using the iterative error state Kalman filtering algorithm and output to the odometer.

[0047] Through the above steps, the information acquisition of the system in the degeneration environment can be effectively improved, and the negative influence of the laser radar measurement noise on the system state update can be effectively limited, so that the stability and precision of the SLAM system can be effectively improved.

[0048] The point cloud measured by the laser radar not only contains the geometric scale information of the surrounding environment, but also returns the laser reflection intensity information. Because the reflectivity of different materials is different, the laser reflection intensity is different. By analyzing the laser reflection intensity information, the texture information of the surrounding environment can be obtained. Through certain processing, the reflection intensity information can achieve the effect similar to a camera. These texture information can help update the system state. In order to improve the precision of the system state update, the IMU integral is used to predict the system state, and the precision of the subsequent intensity image frame matching is improved.

[0049] The first level state update process of the application comprises:

[0050] S101: Correct the laser reflection intensity.

[0051] The laser reflection intensity obtained by the laser radar is affected by the object material, distance and incident angle. In order to distinguish different materials of objects through reflection intensity, it is necessary to eliminate or reduce the influence of distance and incident angle on laser reflection intensity.

[0052] (1);

[0053] In formula (1), is the reflectivity of the object surface, is the acquired laser radar reflection intensity, is the distance of the laser point to the laser radar, is the laser beam incidence angle, is proportional to the reflection intensity.

[0054] The corrected reflection intensity can be obtained by formula (1) :

[0055] (2);

[0056] wherein, is a proportional parameter.

[0057] S102: generate a laser radar reflection intensity image.

[0058] The SLAM system of the present application adopts a mechanical laser radar, and the encoder value of the mechanical laser radar represents the discrete positions of the horizontal rotation of the laser radar, wherein , is the horizontal resolution; the laser head number represents the number of the vertically arranged laser emitters , is the number of vertical beams; taking the encoder value as the column and the laser head number as the row, each point in the point cloud obtained by scanning has different two-dimensional coordinates, and an image matrix can be obtained by arranging the coordinates, the pixel value of each pixel in the image is set as the corrected reflection intensity, and finally a laser radar reflection intensity image is obtained, and the intensity image obtained in this way can reflect the information of the surrounding environment and provide additional constraints for the system.

[0059] Since the pixel value of the reflection intensity image obtained through the above steps is discontinuous, it is necessary to smooth the intensity image by Gaussian filtering and retain the edge features. In order to highlight the edge features of the intensity image, histogram equalization is used on the intensity image after Gaussian filtering to highlight the edge features.

[0060] S103: extract ORB feature points.

[0061] The reflection intensity image can reflect the texture information of the surrounding environment, such as graffiti on the wall, and the use of these texture information can help determine the system state.

[0062] First, extract ORB feature points on the current frame intensity image, most of these feature points are in the area where the pixel changes greatly on the image. Since each pixel on the intensity image corresponds to a laser point, this makes each pixel on the intensity image have depth information. However, since the extracted ORB feature points are sub-pixels, that is, the pixel coordinates of the ORB feature points on the intensity image are floating-point numbers. In order to make full use of the depth information of the intensity image, the nearest pixel to the ORB feature point is selected to represent the ORB feature point. The pixel coordinates of the ORB feature points on the image are , and the three-dimensional coordinates of the laser point corresponding to the pixel coordinates closest to the extracted ORB feature point are selected as the spatial three-dimensional coordinates of the ORB feature point.

[0063] S104: Track the ORB feature points using the LK optical flow method.

[0064] After extracting the ORB feature points on the current frame reflection intensity image, it is necessary to track the positions of the ORB feature points on the next frame reflection intensity image in order to construct the re-projection error subsequently. The data of the IMU can be used to obtain the motion transformation between the adjacent two frames of intensity images . is a special orthogonal group, which means the rotation of a three-dimensional space, represents the rotation of the system between the current frame intensity image and the next frame intensity image, represents a three-dimensional vector, represents the displacement of the system between the current frame intensity image and the next frame intensity image. The three-dimensional coordinates of the ORB feature points extracted from the current frame intensity image are converted to the three-dimensional coordinates in the next frame intensity image:

[0065] (3);

[0066] According to the projection model of the laser radar intensity image, project on the next frame intensity image to obtain the pixel coordinates of the ORB feature points on the next frame intensity image on the current frame intensity image. Since there is an error in the IMU prediction value, the LK optical flow is used to further track the pixel coordinates of the ORB feature points on the next frame intensity image on the current frame intensity image.

[0067] In order to make the process of tracking the ORB feature points by the LK optical flow converge faster, the pose transformation of the adjacent two frames of reflection intensity images is predicted by the IMU, and the predicted value is used as the initial guess of the LK optical flow. Finally, the pixel coordinates of the ORB feature points in the next frame image are obtained .

[0068] ​​S105: Constructing intensity image observation.

[0069] The difference between the pixel coordinates of the ORB feature points tracked by the LK optical flow in the next frame image and the pixel coordinates predicted by the IMU is taken as the intensity observation:

[0070] (4);

[0071] In the above formula, represents the intensity observation residual, and the first-order state update is performed by the iterative error state Kalman filtering algorithm (i.e., the first-order intensity odometry is obtained).

[0072] The second-order state update against the noise influence in the degenerate scene, specifically, includes:

[0073] The point cloud generated by the laser radar scanning can provide geometric information of the surrounding environment, but in the degenerate scene such as long corridor and open space, the geometric information of the laser radar point cloud is not uniformly distributed in all directions, and the constraint on the SLAM system state in some directions is weak. In this case, if there is no effective degenerate processing method, the state update of the system in the weak constraint direction is easily affected by the laser radar measurement noise, which leads to the error update of the SLAM system in the weak constraint direction, which will cause the SLAM system to reduce the accuracy or even fail.

[0074] In order to reduce or avoid the error update of the system in the weak constraint direction, an effective detection means of the constraint strength of the laser radar point cloud in all directions and a method of limiting the update of the system in the weak constraint direction are needed.

[0075] The constraint strength detection process, specifically, includes:

[0076] The state of the system is defined as:

[0077] (5);

[0078] wherein, is a special orthogonal group, which refers to the rotation of a three-dimensional space, is a rotation matrix, which represents the spatial attitude of the system, represents the spatial position of the system.

[0079] The point cloud map is constructed by converting each frame of point cloud of the laser radar to the map coordinates, searching the plane matched with the current frame of point cloud in the map, and calculating the point-to-plane distance to construct the point-plane residual as the system observation equation.

[0080] The Hessian matrix of the system observation equation is:

[0081] ​​ (6);

[0082] wherein, represents a point-plane pair that matches successfully, is the Jacobian matrix of the observation equation composed of the point-plane pairs, is the Jacobian matrix of the observation equation of each point-plane pair, represents the Hession matrix is a 6-by-6 matrix. A 3-by-3 sub-block in the upper left of the Hession matrix is extracted, which is only related to the system rotation, and a 3-by-3 sub-block in the lower right of the Hession matrix is extracted, which is only related to the system spatial position:

[0083] (7);

[0084] (8);

[0085] wherein, represents the Jacobian matrix of the observation equation composed of the point-plane pairs with respect to the system state, represents the Jacobian matrix of the observation equation composed of the point-plane pairs with respect to the system state Pos, is the Jacobian matrix of the observation equation of each point-plane pair with respect to the system state, is the Jacobian matrix of the observation equation of each point-plane pair with respect to the system state Pos. SVD decomposition is performed on and

[0086]

[0087] (9);

[0088] (10);

[0089] (11);

[0090] (12);

[0091] (13);

[0092] (14);

[0093] wherein, and ​​​All are matrixes composed of three eigenvectors, which respectively represent three update directions of the rotation part and three update directions of the displacement part of the system, representing six update directions of the system; and is a diagonal matrix composed of eigenvalues corresponding to the eigenvectors, , and are eigenvectors of , , and are eigenvectors of , is the eigenvalue corresponding to , is the eigenvalue corresponding to , is the eigenvalue corresponding to , is the eigenvalue corresponding to , is the eigenvalue corresponding to , is the eigenvalue corresponding to .

[0094] According to the above equation, the following equations can be derived:

[0095] (15);

[0096] (16);

[0097] (17);

[0098] (18);

[0099] (19);

[0100] (20);

[0101] wherein, and The values of the diagonal elements reflect the strength of the point cloud geometric information constraint in the direction of the corresponding eigenvector. By analyzing each element in and , the constraint size of each point to the six update directions and can be obtained:

[0102] (21);

[0103] (22);

[0104] Since the Jacobian matrix in the rotation direction is affected by the size of the point coordinate, it usually leads to a larger value. In order to unify the constraint scale in the translation direction and the rotation direction, the Jacobian matrix in the rotation direction is normalized:

[0105] (23);

[0106] where, represents the norm of .

[0107] Since there is noise in the laser radar measurement value, when and are too small, the constraint value is likely to be affected by noise, and if not limited, the point pair corresponding to these smaller constraint values may cause the SLAM system to produce an error update in this direction. Therefore, a minimum constraint threshold is set, when or is less than , it is considered that the constraint value is affected by the laser radar measurement noise, and in the calculation of the total constraint value of all point pairs in the update direction, it is regarded as zero, and the adjusted constraint value and is obtained:

[0108] (24);

[0109] (25);

[0110] Get the constraint value of each point pair in each update direction, and then get the sum of the constraint values of all point pairs in each update direction and :

[0111] (26);

[0112] (27);

[0113] The size of and obtained by calculation can represent the constraint strength of the observation equation composed of all point pairs in each update direction. When the constraint is too weak, the system is prone to produce an error update in this direction due to the influence of laser radar noise, so measures need to be taken to limit the update of the system in this direction.

[0114] According to the total constraint value of the observation equation formed by all point surface pairs in each update direction, the six update directions are divided into strong constraint direction, medium constraint direction and weak constraint direction, and then the update directions are processed according to the classification.

[0115] The update in the strong constraint direction is little affected by the laser radar measurement noise, and the update of the system in these directions does not need additional processing.

[0116] The update in the medium constraint direction is affected by the laser radar measurement noise, and the update of the system in these directions needs to be partially limited. In order to weaken the influence of the laser radar measurement noise, the point surface pairs with constraint values greater than a certain value in the medium constraint direction are selected from all point surface pairs, and the observation equation is formed by using these point surface pairs to update the system state in the medium constraint direction.

[0117] The update in the weak constraint direction is extremely affected by the laser radar measurement noise, and the update of the system in this direction guided by the observation equation formed by the point surface pairs needs to be prohibited.

[0118] The specific processing method is as follows:

[0119] Firstly, the SVD decomposition of the Jacobian matrix is needed,

[0120] (28);

[0121] (29);

[0122] Here, the obtained , and are the same matrices as obtained by the SVD decomposition of and , and are matrices composed of three eigenvectors, respectively representing the three update directions of the rotation part of the system and the three update directions of the displacement part of the system, representing the six update directions of the system; and are diagonal matrices composed of the eigenvalues corresponding to the eigenvectors, and are the left singular matrices obtained after SVD decomposition. By replacing the eigenvalues corresponding to the prohibited update directions with a very small value, such as 0.001, the modified Jacobian matrix and is obtained, and are used to replace the original Jacobian matrix and during the update, which will make the system hardly update in the weak constraint direction.

[0123] Firstly, the strongly constrained directions are processed. Since the laser radar measurement noise has little influence on the system, the system state in other directions is prohibited from updating. The strongly constrained directions are updated using the observation equation composed of all point-plane pairs. The eigenvalues corresponding to the other directions are set to small values, such as 0.001.

[0124] (30);

[0125] (31);

[0126] wherein, and is a diagonal matrix obtained by setting the eigenvalues corresponding to the other directions to small values, and is the Jacobian matrix of the strongly constrained directions after processing, the original Jacobian matrix is replaced by and and is brought into the system state update quantity obtained by the iterative error state Kalman filter. The system state update quantity is only in the strongly constrained directions, and there is almost no update quantity in the other directions.

[0127] The update in the moderately constrained directions is influenced by the laser radar measurement noise. In order to reduce the influence, the observation equation is composed of the point-plane pairs whose constraint values in the moderately constrained directions are greater than According to the selected point-plane pairs, the selected Jacobian matrix and is obtained. Since the strongly constrained directions have been updated, the weakly constrained directions are not updated at present. The Jacobian matrix and is processed so that the newly composed Jacobian matrix updates the system state only in the moderately constrained directions. After processing, the Hessian matrix of the newly obtained observation equation is and :

[0128] (32);

[0129] (33);

[0130] In the above formula, represents the number of selected point-plane pairs, and represents the observation equation Jacobian matrix of each selected point-plane pair. The Hessian matrix containing only the moderately constrained direction information can be obtained by the following equation: and :

[0131] (34);

[0132] (35);

[0133] In the above formula, , and The same matrix is obtained by SVD decomposition, and The diagonal matrix obtained by setting the corresponding value in the medium constraint direction to a small value, and by substituting and into the iterative error state Kalman filter, the system state update value with only the update amount in the medium constraint direction can be obtained. Through the above steps, the SLAM system has generated updates in the strong constraint direction and the medium constraint direction. For the weak constraint direction, no update is performed to avoid errors in the SLAM system due to false updates.

[0134] Through the above steps, the SLAM system has generated updates in the strong constraint direction and the medium constraint direction. For the weak constraint direction, no update is performed to avoid errors in the SLAM system due to false updates.

[0135] Figure 2 A schematic diagram of a laser point cloud map updating system provided by an exemplary embodiment of the present application is shown, comprising:

[0136] The point cloud preprocessing unit is configured to remove distortion from the lidar point cloud using the inertial measurement unit prediction value;

[0137] The primary state update unit is configured to correct the reflection intensity in the distortion-removed point cloud, obtain an intensity image according to the corrected reflection intensity, obtain an intensity observation result according to the intensity image, and update the robot pose according to the intensity observation result;

[0138] The secondary state update unit is configured to search for the corresponding plane of the points in the distortion-removed point cloud in the map to obtain all point-plane matching pairs, perform degeneration detection on the obtained point-plane matching pairs to obtain a geometric observation result, and update the laser point cloud map based on the robot pose update result according to the geometric observation result to obtain an updated laser point cloud map.

[0139] ​It can be understood that the above-mentioned units can be combined into one or several other units respectively or entirely, or some of the units can be further split into a plurality of units with smaller functions to constitute, which can achieve the same operation without affecting the implementation of the technical effects of the embodiments of the present application. The above-mentioned units are divided based on logical functions, and in actual application, the functions of one unit can also be implemented by multiple units, or the functions of multiple units can be implemented by one unit. In other embodiments of the present application, the system can also include other units, and in actual application, these functions can also be assisted by other units to be implemented, and can be implemented by multiple units in cooperation.

[0140] According to another embodiment of the present application, the system described in the embodiment can be constructed and the laser point cloud map updating of the present application can be implemented by running a computer program (including program code) capable of performing each step involved in the corresponding method described in Embodiment 1 on a general computing device such as a computer including processing elements and storage elements such as a Central Processing Unit (CPU), a Random Access Memory (RAM), a Read Only Memory (ROM), etc., the computer program can be recorded on a computer readable recording medium, loaded into the above-mentioned computing device through the computer readable recording medium, and run therein.

[0141] Figure 3 A computer device is shown, which includes a processor, a communication interface and a computer readable storage medium. Wherein, the processor, the communication interface and the computer readable storage medium can be connected through a bus or other means.

[0142] Wherein, the communication interface is used for receiving and sending data, the computer readable storage medium can be stored in the memory of the electronic device, the computer readable storage medium is used for storing computer programs, the computer programs include program instructions, and the processor is used for executing the program instructions stored in the computer readable storage medium.

[0143] The processor is the computing core and control core of the electronic device, which is suitable for implementing one or more instructions, and is particularly suitable for loading and executing one or more instructions to implement a corresponding method flow or a corresponding function.

[0144] The processor is configured to perform the following process:

[0145] The laser radar point cloud is removed from distortion by using the predicted value of the inertial measurement unit;

[0146] Intensity correction is performed on the reflection intensity in the point cloud after distortion removal, an intensity image is obtained according to the corrected reflection intensity, an intensity observation result is obtained according to the intensity image, and the robot pose is updated according to the intensity observation result;

[0147] The corresponding plane of the points in the point cloud after distortion removal in the map is searched, all point-plane matching pairs are obtained, degeneration detection is performed on the obtained point-plane matching pairs to obtain a geometric observation result, and the laser point cloud map is updated on the basis of the robot pose update result according to the geometric observation result, to obtain an updated laser point cloud map.

[0148] The application further provides a computer readable storage medium, which is a memory device in an electronic device and is used for storing programs and data.

[0149] In the storage space, one or more instructions suitable for being loaded and executed by the processor are further stored, and the instructions can be one or more computer programs (including program codes).

[0150] In one embodiment, one or more instructions are stored in the computer readable storage medium; the processor loads and executes the one or more instructions stored in the computer readable storage medium to realize the following process:

[0151] The laser radar point cloud is removed from distortion by using the predicted value of the inertial measurement unit;

[0152] Intensity correction is performed on the reflection intensity in the point cloud after distortion removal, an intensity image is obtained according to the corrected reflection intensity, an intensity observation result is obtained according to the intensity image, and the robot pose is updated according to the intensity observation result;

[0153] The corresponding plane of the points in the point cloud after distortion removal in the map is searched, all point-plane matching pairs are obtained, degeneration detection is performed on the obtained point-plane matching pairs to obtain a geometric observation result, and the laser point cloud map is updated on the basis of the robot pose update result according to the geometric observation result, to obtain an updated laser point cloud map.

[0154] The application further provides a computer program product or computer program, which comprises computer instructions stored in a computer readable storage medium. A processor of an electronic device reads the computer instructions from the computer readable storage medium, and the processor executes the computer instructions to enable the electronic device to perform the following processes:

[0155] Distortion removal is performed on the lidar point cloud by using the predicted value of the inertial measurement unit;

[0156] Intensity correction is performed on the reflection intensity in the point cloud after the distortion removal, an intensity image is obtained according to the corrected reflection intensity, an intensity observation result is obtained according to the intensity image, and the robot pose is updated according to the intensity observation result;

[0157] The corresponding plane of the points in the point cloud after the distortion removal in the map is searched to obtain all point-plane matching pairs, degeneration detection is performed on the obtained point-plane matching pairs to obtain a geometric observation result, and the lidar point cloud map is updated on the basis of the robot pose update result according to the geometric observation result to obtain an updated lidar point cloud map.

[0158] Those skilled in the art can understand that the units and algorithm steps of each example described in combination with the embodiments disclosed in the present application can be realized by electronic hardware or a combination of computer software and electronic hardware. Whether the functions are realized in hardware or software mode depends on the specific application and design constraints of the technical solution. The skilled person can use different methods to realize the described functions for each specific application, but such implementation should not be considered beyond the scope of the present application.

[0159] In the above embodiments, all or part of the embodiments can be implemented by software, hardware, firmware or any combination thereof. When implemented by software, all or part of the embodiments can be implemented in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions according to the embodiments of the present application are generated. The computer can be a general purpose computer, a special purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in or transmitted by a computer readable storage medium. The computer instructions can be transmitted from one website, computer, server or data center to another website, computer, server or data center through a wired (for example, coaxial cable, optical fiber, digital line) or wireless (for example, infrared, wireless, microwave, etc.) manner. The computer readable storage medium can be any available medium that can be accessed by a computer or a data processing device such as a server, data center, etc. integrated with one or more available media. The available media can be magnetic media (for example, floppy disk, hard disk, magnetic tape), optical media (for example, DVD), or semiconductor media (for example, solid state disk) and the like.

[0160] The above only describes the preferred embodiments of the present application and is not intended to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A laser point cloud map updating method, characterized in that: The following processes are included: Dedistortion of LiDAR point clouds using inertial measurement unit predictions; Performing intensity correction on the reflection intensity in the point cloud after distortion removal, obtaining an intensity image based on the corrected reflection intensity, obtaining an intensity observation result based on the intensity image, and updating the robot posture based on the intensity observation result; Search for the corresponding planes in the map for the points in the point cloud after distortion removal to obtain all point-plane matching pairs. Perform degradation detection on the obtained point-plane matching pairs to obtain geometric observation results. Based on the geometric observation results and the robot's updated pose results, update the laser point cloud map to obtain an updated laser point cloud map. Degeneration detection is performed on the obtained point-surface matching pairs to obtain geometric observation results, including: The six update directions are divided into three levels: strong, medium and weak, and degenerate processing is performed to completely restrict the state update in the weakly constrained direction, partially restrict the state update in the medium constrained direction, and not restrict the state update in the strongly constrained direction. The six directions are the three update directions of the robot's rotation part and the three update directions of the robot's displacement part; The six updated directions are divided into three levels: strong, medium and weak, including: Determine the sum of the constraint values ​​of all points facing each update direction; According to the sum of the constraint values ​​of all points in each update direction, the six update directions are divided into strong constraint directions, medium constraint directions and weak constraint directions.

2. The laser point cloud map updating method according to claim 1, wherein: Obtaining an intensity observation result according to the intensity image, including: Feature points are extracted from the intensity image of the current frame, and the pixel coordinates of the feature points in the intensity image of the next frame are tracked by optical flow. The difference between the pixel coordinates of the feature points tracked by optical flow in the next frame image and the pixel coordinates predicted by the inertial measurement unit is used as the intensity observation.

3. The laser point cloud map updating method according to claim 2, wherein: Feature points are extracted from the intensity image, and the three-dimensional coordinates of the laser point corresponding to the pixel coordinates closest to the extracted feature point are selected as the spatial three-dimensional coordinates of the feature point.

4. The laser point cloud map updating method according to claim 2, wherein: Track the pixel coordinates of the feature points on the next frame intensity image through optical flow, including: Convert the three-dimensional coordinates corresponding to the feature points extracted from the current frame intensity image to the next frame intensity image to obtain the converted three-dimensional coordinates; According to the projection model, the converted three-dimensional coordinates are projected onto the next frame intensity image to obtain the pixel coordinate prediction value of the feature point on the current frame intensity image on the next frame intensity image. The pixel coordinate prediction value is used as the initial guess of the optical flow, and finally the pixel coordinate of the feature point in the next frame image is obtained.

5. A laser point cloud map updating system, characterized in that: include: The point cloud pre-processing unit is configured to: remove distortion from the lidar point cloud using the inertial measurement unit prediction value; a first-level state update unit configured to: perform intensity correction on the reflection intensity in the point cloud after distortion removal, obtain an intensity image based on the corrected reflection intensity, obtain an intensity observation result based on the intensity image, and update the robot posture based on the intensity observation result; The secondary state update unit is configured to: search for corresponding planes in the map for points in the point cloud after distortion removal to obtain all point-plane matching pairs, perform degradation detection on the obtained point-plane matching pairs to obtain geometric observation results, and update the laser point cloud map based on the robot pose update results according to the geometric observation results to obtain an updated laser point cloud map; Degeneration detection is performed on the obtained point-surface matching pairs to obtain geometric observation results, including: The six update directions are divided into three levels: strong, medium and weak, and degenerate processing is performed to completely restrict the state update in the weakly constrained direction, partially restrict the state update in the medium constrained direction, and not restrict the state update in the strongly constrained direction. The six directions are the three update directions of the robot's rotation part and the three update directions of the robot's displacement part; The six updated directions are divided into three levels: strong, medium and weak, including: Determine the sum of the constraint values ​​of all points facing each update direction; According to the sum of the constraint values ​​of all points in each update direction, the six update directions are divided into strong constraint directions, medium constraint directions and weak constraint directions.

6. A computer device, characterized in that: include: a processor and a computer-readable storage medium; a processor adapted to execute a computer program; A computer-readable storage medium having a computer program stored therein, wherein the computer program, when executed by the processor, implements the laser point cloud map updating method according to any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, which is suitable for being loaded by a processor and executing the laser point cloud map updating method according to any one of claims 1 to 4.

8. A computer program product, characterized in that The computer program product includes a computer program, and when the computer program is executed by a processor, it implements the laser point cloud map updating method according to any one of claims 1 to 4.

Images