3D laser SLAM method, system, medium and equipment based on full constraints

Abstract

The present invention belongs to the field of robot simultaneous positioning and mapping, and provides a 3D laser SLAM method, system, medium and equipment based on full constraints. The technical solution is to calculate the contribution value of the point cloud geometric constraint and the intensity image intensity constraint to the optimization direction constraint strength, select the one with the larger contribution value to constrain the optimization direction, obtain the full constraint observation of the system, and realize the strong constraint system with complementary geometric constraints and intensity constraints. Add additional constraints in the degenerate direction to realize the fully constrained LIO system; in the system update step, by deriving the residual error and corresponding Jacobian matrix of the intensity image inter-frame matching, combined with the residual error and Jacobian matrix of the point cloud point-surface matching, jointly realize the full constraint state update of the system. Introduce additional constraints in the degenerate direction, use the Lagrange multiplier method to correct the full constraint state update of the system. Through the above steps, a SLAM system that is robust in all directions is realized, and the robustness of the SLAM system is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot simultaneous positioning and mapping, and in particular to a 3D laser SLAM method, system, medium and equipment based on full constraints, which can achieve high-precision and high-robustness positioning and map construction in a degraded environment with sparse features. Background Art

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

[0003] In recent years, the development of a series of automated products such as self-driving cars and intelligent inspection robots has promoted the development of robot autonomous mobile technology. Simultaneous localization and mapping (SLAM), as a key technology, has received widespread attention. 3D laser SLAM has become one of the important directions of SLAM development due to its high precision. Existing 3D laser SLAM usually integrates inertial measurement units (IMUs) to provide high-frequency motion data for the system. However, in scenarios such as long corridors, tunnels, and open outdoor areas, the laser radar will degrade in certain directions due to the sparse features of the surrounding environment, which will reduce the system's positioning and mapping accuracy and even cause errors in the SLAM system.

[0004] At present, the main method to solve the degradation of 3D laser SLAM is to introduce additional sensors, such as cameras, wheel speed meters, radars, etc. Many 3D laser SLAM systems will integrate IMU to provide prediction values ​​for the system. The integrated wheel speed meter can provide additional speed observations, provide the system with more accurate prediction values, and improve the performance of the SLAM system. The integrated camera can improve the SLAM system's acquisition of information about the external environment, but the introduction of more sensors means the introduction of more noise, and the camera needs to be calibrated, which increases the workload. At the same time, it is difficult for the camera to work normally in dimly lit places, which is not conducive to the SLAM system's acquisition of information about the surrounding environment. Summary of the invention

[0005] In order to solve at least one technical problem existing in the above-mentioned background technology, the present invention provides a 3D laser SLAM method, system, medium and equipment based on full constraints. In the case of sparse features, a SLAM system is designed that integrates a wheel speed meter and utilizes the reflection intensity information of a laser radar, which can significantly improve the SLAM system's acquisition of surrounding environment information and improve the robustness of the SLAM system.

[0006] In order to achieve the above object, the present invention adopts the following technical solution:

[0007] A first aspect of the present invention provides a 3D laser SLAM method based on full constraints, comprising the following steps:

[0008] Get LiDAR point cloud data;

[0009] Based on the lidar point cloud data, the information matrix of point-to-surface matching is obtained. The sub-blocks related only to the rotation variable and the displacement variable in the information matrix of point-to-surface matching are decomposed to obtain multiple eigenvectors. Each eigenvector is used as an optimization direction to calculate the constraint contribution value of point-to-surface matching in each optimization direction.

[0010] Generate an intensity image based on the lidar point cloud data, and calculate the constraint contribution value of the intensity image in each optimization direction through the intensity image frame matching;

[0011] Compare the constraint contribution values ​​of point-to-surface matching and intensity image frame-to-frame matching in each optimization direction, select the one with the larger contribution value, and construct a fully constrained observation;

[0012] Degradation detection is performed on the fully constrained observation. If degradation occurs, additional constraints are added in the degradation direction and the fully constrained state is updated. Otherwise, the fully constrained state is updated directly to obtain the fully constrained observation system increment and update the system state.

[0013] Furthermore, before obtaining the information matrix of point-surface matching based on the lidar point cloud data, each frame of point cloud data is dedistorted using the predicted value of the system state, wherein the predicted value of the system state is calculated as follows: when only IMU data is available, the predicted value is obtained by forward propagation according to the IMU prediction model; when the tachometer data is received, the IMU prediction data and the tachometer data are fused through the extended Kalman filter to obtain the predicted value.

[0014] Furthermore, the constraint contribution value of the intensity image in each optimization direction is obtained by calculating the intensity image frame matching, including:

[0015] After selecting a pixel block on the intensity image, according to the disturbance given to the laser radar posture, the Jacobian matrix of the laser point corresponding to the central pixel of the pixel block projected onto the intensity image is calculated, which is the projected gradient. The projected gradient is projected in each optimization direction to obtain the projected gradient in each optimization direction.

[0016] Calculate the second-order moment of the pixel block at its central pixel point, and use the eigenvector corresponding to the maximum eigenvalue of the second-order moment to represent the pixel gradient of the pixel block;

[0017] Calculate the projection of the projected gradient in each optimization direction on the pixel gradient direction of the pixel block, which is the constraint contribution value of the pixel block to the optimization direction;

[0018] The sum of the constraint contribution values ​​of all pixel blocks is calculated to obtain the constraint contribution value of the intensity image in each optimization direction.

[0019] Furthermore, when an intensity image is generated based on the laser radar point cloud data, the intensity image is generated by projecting the laser radar point cloud data according to a spherical projection model.

[0020] Furthermore, the step of adding additional constraints in the degenerate direction and then updating the full constraint state includes:

[0021] Obtain the part of the calculated system state variable increment related to the system attitude and position;

[0022] Construct the update constraint in the degradation direction, combine the part of the system state variable increment related to the system posture and position, and obtain the constraint equation;

[0023] The Lagrange multiplier method is used to calculate the system state increment obtained under the condition of introducing constraint equations.

[0024] Furthermore, when constructing the fully constrained observation, it includes calculating the photometric error of the intensity image and the geometric residual of the point cloud data;

[0025] Among them, a direct method is used to obtain the photometric error between two frames of intensity images, and then the constraint contribution value of the intensity image in each optimization direction is obtained by matching the intensity images between frames;

[0026] Among them, for the points in the laser radar point cloud of this frame, the matching surface is searched in the local map, the distance residual between the point and the surface is constructed, and the geometric residual of the point cloud data is obtained.

[0027] Furthermore, the full constraint state is updated, including the following steps:

[0028] Calculate the residual of point-to-surface matching and the matching residual of intensity image;

[0029] Update the Jacobian matrix based on the calculated matching residual of the intensity image;

[0030] The residuals of point-surface matching and the updated Jacobian matrix are fused to obtain the residuals and Jacobian matrix of the fully constrained observation;

[0031] Based on the residual and Jacobian matrix of the fully constrained observation, the system increment of the geometric constraint and the system increment of the strength constraint are obtained, and the system increment of the fully constrained observation is obtained by summing them up.

[0032] A second aspect of the present invention provides a fully constrained 3D laser SLAM system, comprising:

[0033] A data acquisition module, which is used to acquire laser radar point cloud data;

[0034] The geometric constraint module is used to obtain the information matrix of point-to-surface matching based on the lidar point cloud data, perform eigendecomposition on the sub-blocks related only to the rotation variable and the displacement variable in the information matrix of the point-to-surface matching, obtain multiple eigenvectors, take each eigenvector as an optimization direction, and calculate the constraint contribution value of the point-to-surface matching in each optimization direction;

[0035] An intensity constraint module is used to generate an intensity image based on the lidar point cloud data, and obtain the constraint contribution value of the intensity image in each optimization direction by matching the intensity image frames.

[0036] The fully constrained construction module is used to compare the constraint contribution values ​​of point-to-surface matching and intensity image frame-to-frame matching in each optimization direction, select the one with the larger contribution value, and construct a fully constrained observation;

[0037] The state update module is used to perform degradation detection on the fully constrained observation. If degradation occurs, an additional constraint is added in the degradation direction and then a fully constrained state update is performed. Otherwise, a fully constrained state update is performed directly to obtain the fully constrained observation system increment and update the system state.

[0038] A third aspect of the present invention provides a computer-readable storage medium.

[0039] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps in the above-mentioned fully-constrained 3D laser SLAM method.

[0040] A fourth aspect of the present invention provides a computer device.

[0041] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps in the above-mentioned fully-constrained 3D laser SLAM method are implemented.

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

[0043] 1. The present invention constructs geometric constraints and intensity constraints, selects the point-to-surface matching and intensity image frame-to-frame matching that contributes more in the optimization direction, constructs fully constrained observations, and performs degradation detection and degradation processing on the fully constrained observations, thereby realizing a SALM system with effective constraints in all directions.

[0044] 2. When degradation is detected in certain directions of the system, additional constraints are introduced in the degraded directions, and the Lagrange multiplier method is used to correct the full constraint state update of the system, thereby improving the robustness of the SLAM system.

[0045] 3. When the system is updated, the residual error and the corresponding Jacobian matrix of the intensity image frame matching are derived, combined with the residual error and Jacobian matrix of the point cloud point-surface matching to jointly implement the full constrained state update of the system, further improving the robustness of the SLAM system.

[0046] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0048] Figure 1 is a flow chart of a 3D laser SLAM method based on full constraints provided by an embodiment of the present invention;

[0049] Figure 2 This is a mechanical laser radar measurement model provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0050] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0051] It should be noted that the following detailed descriptions are all illustrative and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0052] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0053] As mentioned in the background technology of this application, the fusion camera can improve the SLAM system's acquisition of information about the external environment, but the introduction of more sensors means the introduction of more noise, and the camera needs to be calibrated, which increases the workload. At the same time, the camera is difficult to work normally in dim light. The point cloud data of the laser radar includes not only geometric information, but also reflection intensity information, but there is less work using reflection intensity information. Reasonable use of reflection intensity information can approximate the effect of the camera, which is of great help to the robustness of the SLAM system.

[0054] Therefore, in the case of sparse features, the present invention designs a SLAM system that integrates a wheel speed meter and utilizes the reflection intensity information of a laser radar, which can significantly improve the SLAM system's acquisition of surrounding environment information and improve the robustness of the SLAM system, which is very beneficial to the 3D laser SLAM system.

[0055] Embodiment 1

[0056] like Figure 1 As shown, this embodiment provides a 3D laser SLAM method based on full constraints, including the following steps:

[0057] Step 1: Get the predicted value of the system state based on the fusion of IMU data and wheel speed meter data;

[0058] In this embodiment, in order to improve the accuracy of the predicted value of the system state, a wheel speed meter is added to the system, and the data of the IMU and the wheel speed meter are fused through an extended Kalman filter to provide a more accurate predicted value of the system state for the update step.

[0059] When there is only IMU data, the prediction value is obtained by forward propagation according to the IMU prediction model. When the wheel speed meter data is received, the IMU prediction data and the wheel speed meter data are fused through the extended Kalman filter to obtain a more accurate prediction value. This part is called the IW module. The process is as follows: Figure 2 .

[0060] Step 2: Obtain the laser radar point cloud data, use the predicted value of the system state to perform dedistortion processing on each frame of point cloud data, obtain the information matrix of point-surface matching based on the dedistorted point cloud data, and process the obtained laser radar point cloud data to obtain an intensity image;

[0061] For the geometric information in the lidar point cloud data, we first use the predicted value of the system state to dedistort a frame of point cloud, and remove points that are too close or too far from the lidar. By constructing point-to-surface matching, we obtain the information matrix of point-to-surface matching. Then, we perform feature decomposition on the information matrix of point-to-surface matching to obtain six eigenvectors, which are the optimization directions. Then, we calculate the contribution value of point-to-surface matching in the optimization direction in the geometric constraint extractor.

[0062] For the intensity information of lidar point cloud data, the received current frame point cloud is first projected according to the spherical projection model to generate an intensity image. Then, pixel blocks with pixel gradients greater than the set threshold are extracted from the intensity image. After the next frame of intensity image is generated, the direct method is used to obtain the photometric error between the two frames of intensity images. Then, the contribution value of the intensity image frame matching in the optimization direction is calculated in the intensity constraint extractor.

[0063] The process of generating an intensity image is to project the LiDAR point cloud to generate an intensity image, and the pixel value of the image pixel is set to the reflection intensity of the processed point cloud.

[0064] The processing of the acquired laser radar point cloud data to obtain an intensity image specifically includes:

[0065] For solid-state LiDAR, set the scan time The point cloud obtained is a frame of point cloud. For a frame of point cloud, the scanning time of the solid-state laser radar is Horizontal resolution and vertical resolution , the intensity image size is ;

[0066] Get the coordinates of a point in the point cloud , according to the spherical projection model, the coordinates of the point on the intensity image are:

[0067] ,

[0068] in, is the column coordinate of the point on the intensity image, The row coordinates of the point on the intensity image are rounded to integers.

[0069] However, since the scanning method of solid-state laser radar is non-repetitive scanning, there may be multiple points with the same coordinates on the intensity image. In order to avoid the point being blocked during subsequent movement, the intensity value of the last point that falls on the coordinate is taken as the pixel value.

[0070] For mechanical laser radar, multiple laser beams are scanned simultaneously in the vertical direction. One circle of scanning is a frame of point cloud. The size of a frame of point cloud is The intensity image of the pixels, where is the number of laser radar beams, It is the number of points obtained by scanning a circle with a line beam according to the horizontal resolution of the lidar.

[0071] Get the coordinates of a point in the point cloud as . However, since the laser radar receiver is not at the center of the laser radar, such as Figure 2 As shown, the offset needs to be taken into account when performing spherical projection on the point. After the offset, the coordinates of the point relative to the receiver are ,and , ,in is the offset of the receiver relative to the center of the lidar.

[0072] According to the spherical projection model, the pixel coordinates of the point in the intensity image can be obtained for:

[0073] ,

[0074] In the formula, is the horizontal resolution of the lidar, is the vertical resolution of the lidar, is the vertical field of view of the LiDAR, is the column coordinate of the point on the intensity image, is the row coordinate of the point on the intensity image.

[0075] In this embodiment, in order to avoid overexposure or underexposure of the intensity image, which will affect the subsequent matching, the intensity image needs to be whitened; image whitening is to change the average pixel value of the image to 0 and the variance to 1, so that the pixel value of the image is more uniform. First, calculate the average pixel value of the intensity image and variance As follows:

[0076] ,

[0077] ,

[0078] In the formula, is the pixel value of the pixel in the intensity image (the reflection intensity of the point), and the pixel value of the intensity image after whitening for:

[0079] ,

[0080] The pixel values ​​after whitening may be positive or negative. The pixel values ​​need to be mapped to the range [0,255]. The mapped pixel values ​​are calculated using the following formula:

[0081] ,

[0082] In the formula and are the maximum pixel value and the minimum pixel value in the intensity image after whitening. Through the above processing, the pixel values ​​of the pixels of the final intensity image are obtained. The pixel values ​​of the processed intensity image change more evenly, which can effectively reduce the overexposure or underexposure of the image and facilitate the subsequent extraction of feature pixel blocks.

[0083] Step 3: Combine the point-to-surface matching information matrix and the geometric constraint extractor to obtain the contribution value of the point-to-surface matching in the optimization direction constraint, and obtain the contribution value of the intensity image in the optimization direction constraint based on the intensity image and the intensity constraint extractor;

[0084] Since the 3D laser SLAM system has six degrees of freedom, the system is mainly updated in six directions, which are called optimized directions. In order to make the system have strong constraints in all six directions and to compensate for the degradation of the laser radar ranging, after obtaining the geometric residual and photometric error, a method of using the laser radar reflection intensity information to achieve additional observation of the surrounding environment is designed to construct a strong constraint system with complementary geometric constraints and intensity constraints.

[0085] The 3D laser SLAM system has six degrees of freedom. When the system is updated, it is mainly updated in six directions, which are called optimization directions. In order to make the system have strong constraints in all six directions, after obtaining the geometric residual and photometric error, the geometric constraint extractor and the intensity constraint extractor are designed respectively.

[0086] The geometric constraint extractor and the intensity constraint extractor obtain the constraint strength of the point cloud point-surface matching and the intensity image frame-to-frame matching on the six optimization directions respectively, and express them with contribution values. The larger the contribution value, the stronger the constraint. Finally, the geometric constraint extractor and the intensity constraint extractor are selected from the one with the larger contribution value in the optimization direction to obtain the fully constrained observation of the system. Finally, the fully constrained observation is subjected to degradation detection, and the degradation direction is processed to make the system robust in all directions.

[0087] In the geometric constraint extractor, the sub-blocks related to the rotation variables and displacement variables in the information matrix of the point-to-surface matching are firstly eigen-decomposed to obtain the eigenvector matrix consisting of six three-dimensional eigenvectors. , where the first three vectors are only related to the selection variables, and the last three variables are only related to the displacement variables. In order to facilitate the subsequent comparison with the contribution values ​​obtained by the strength constraint extractor, these six eigenvectors are regarded as the six main optimized directions in the update process.

[0088] Step 301: Combining the point-to-surface matching information matrix and the geometric constraint extractor to obtain the contribution value of the point-to-surface matching in the optimization direction constraint, including:

[0089] Step 3011: Obtaining the points of successful matching and the normal vector of the face , and obtain the constraint information matrix and :

[0090] ,

[0091] ,

[0092] In the formula, represents the antisymmetric matrix of a vector, and |·| represents the magnitude of the vector.

[0093] Step 3012: According to the constraint information matrix and Calculate the contribution matrix of the successfully matched point surface to the optimization direction and :

[0094] ,

[0095] ,

[0096] Since the contribution value is only positive, and The absolute value of each item in represents the contribution value of each pair of points and surfaces. In order to reduce the impact of errors on the final results, the contribution value threshold is set. ,for and Each item in Compare, will be less than The items of are set to 0 to obtain the final contribution value matrix. The sum of each column of the contribution value matrix is ​​the contribution value of a frame of point cloud matching in the corresponding optimization direction.

[0097] Step 302: Obtaining the contribution value of the intensity image in the optimization direction constraint based on the intensity image and the intensity constraint extractor, specifically including the following steps:

[0098] Step 3021: After selecting the pixel block, a small disturbance is given to the laser radar posture (position and attitude), and the Jacobian matrix of the laser point corresponding to the central pixel of the pixel block projected onto the intensity image is calculated, which is the projected gradient. The projected gradient is projected in each optimization direction to obtain the projected gradient in each optimization direction;

[0099] In this embodiment, based on the idea of ​​disturbance, a small disturbance is given to the system posture (including position and attitude, six dimensions), calculate the change in coordinates of the point corresponding to the center pixel of the extracted pixel block projected onto the intensity image, which is , is the projection model function of the point cloud onto the intensity image. exist The projection on , the projection coordinates are The change on is the projected gradient :

[0100] .

[0101] Step 3022: Calculate the second-order moment of the pixel block at its central pixel point, and use the eigenvector corresponding to the maximum eigenvalue of the second-order moment Represents the pixel gradient of a pixel block, which is more stable than using pixel gradient directly;

[0102] Step 3023: Calculate the projected gradient in the optimization direction In the pixel block information direction The projection on is used to obtain the contribution value of the pixel block.

[0103] But the projected gradient It will change depending on the distance of the point from the laser radar. In order to standardize the contribution value of the pixel block, the projected gradient Normalize and get the contribution value for:

[0104] · ,

[0105] In order to filter out too small contribution values, set the threshold , for less than 's contribution value and reset it to 0.

[0106] Step 3023: Calculate the sum of the contribution values ​​of all pixel blocks to obtain the contribution value of the intensity image to the six optimization directions. for:

[0107] .

[0108] Step 4: Compare the contribution values ​​of point-to-surface matching and intensity image constraints in the optimization direction, select the one with the larger contribution in each optimization direction to constrain the optimization direction, and obtain the fully constrained observation;

[0109] In order to compare the constraint effect of the intensity image contribution value and the point cloud point surface matching contribution value, it is necessary to multiply the intensity image contribution value by the scale factor , so that when the contribution value of the point cloud point-to-surface matching is equal to the contribution value of the intensity image, there is an approximate constraint effect. In order to obtain a fully constrained observation of the system, for each optimization direction, the one with a larger contribution value is selected from the geometric constraint extractor and the intensity constraint extractor to constrain the optimization direction.

[0110] When constructing a fully constrained observation, it includes calculating the photometric error of the intensity image and the geometric residual of the point cloud data;

[0111] Among them, a direct method is used to obtain the photometric error between two frames of intensity images, and then the constraint contribution value of the intensity image in each optimization direction is obtained by matching the intensity images between frames;

[0112] Among them, for the points in the laser radar point cloud of this frame, search for the matching surface in the local map, construct the distance residual between the point and the surface, and obtain the geometric residual of the point cloud data. Step 5: Perform degradation detection on the fully constrained observation. If degradation occurs, add additional constraints in the degradation direction and then update the fully constrained state. Otherwise, directly update the fully constrained state to obtain the fully constrained observation system increment and update the system state.

[0113] In some special cases, both geometric constraints and strength constraints may not be sufficient to constrain one or some optimization directions. In order to avoid affecting the accuracy during subsequent state updates, it is necessary to perform degradation detection and degradation processing on the fully constrained observations. In the above steps, the contribution value of the fully constrained observations in the optimization direction has been obtained. In the degradation detection part, by setting the degradation threshold , if the contribution of the fully constrained observation in a certain optimization direction is less than the threshold , indicating that the system has degraded in this direction and needs to be degraded.

[0114] In the degradation processing part, it is necessary to add additional constraints to the system in the degradation direction in the error state iterative Kalman filter update step. Get the part of the system state variable increment calculated in the update step that is related to the system attitude and position for:

[0115] ,

[0116] In the formula, represents the system attitude increment, Represents the system position increment.

[0117] In order to constrain the update in the degenerate direction, the constraints are constructed as follows:

[0118] ,

[0119] ,

[0120] Each row in the formula constitutes a constraint plane, and is a point on the surface. In the degenerate direction, let and is 0, and no constraints are added in the non-degenerate direction. Since the dimension of the above constraints is three-dimensional, it needs to be expanded into six dimensions, and the constraint equation is as follows:

[0121] ,

[0122] ,

[0123] In the formula and Represent the number of degenerate directions of rotation and translation respectively.

[0124] Finally, the Lagrange multiplier method is used to calculate the system state increment obtained under the condition of introducing constraint equations to avoid divergence when the system updates the state variables in the degenerate direction.

[0125] In this embodiment, in order to reduce the amount of calculation, the inter-frame matching of the intensity image adopts a direct method to extract pixel blocks on the generated intensity image. First, pixel blocks with no holes of a specified size and pixel gradients greater than a set threshold are extracted. In order to avoid excessive calculation, the maximum number of pixel blocks to be extracted is set to After obtaining a sufficient number of pixel blocks, save the intensity image and the extracted pixel blocks, and call the frame intensity image the feature map. After obtaining the intensity image generated by the next frame of point cloud (called the matching map), match the corresponding pixel blocks, and minimize the photometric error between pixel blocks through the direct method in the visual odometer to update the predicted value.

[0126] After obtaining the point cloud geometric residual and the intensity image photometric error to form a fully constrained observation and performing degradation detection and degradation processing, it is necessary to update the system state according to the error state Kalman filter. The specific steps of the fully constrained state update are as follows:

[0127] Step 501: Generate an intensity image. The specific generation process is the same as that in step 2.

[0128] Step 502, feature extraction: extract pixel blocks of size 7×7 from the generated intensity image. First, extract pixel blocks of a specified size without holes and with pixel gradients greater than a set threshold. To avoid excessive computation, set the maximum number of pixel blocks to be extracted to After obtaining a sufficient number of pixel blocks, the intensity image and the extracted pixel blocks are saved, and the intensity image of this frame is called a feature map. After obtaining the intensity image generated by the next frame of point cloud (called a matching map), the corresponding pixel blocks are matched, and the photometric error between pixel blocks is minimized by the direct method in the visual odometer to update the predicted value.

[0129] Step 503, calculation of residual of the observed part: In this embodiment, the residual of the intensity image matching is the difference between the pixel value of each pixel point in the pixel block selected by the feature map and the pixel value of the corresponding pixel point in the matching map. for:

[0130] ,

[0131] In the formula, is the pixel value of the pixel in the feature map, is the intensity value of the pixel in the matching image.

[0132] Step 504: Calculate the Jacobian matrix of the observed part. In this embodiment, the intensity image matching adopts the direct method in the visual odometer. According to the definition of the direct method, the observed Jacobian matrix can be obtained: for:

[0133] ,

[0134] in is the error state variable of the system, and the state variable of the system for:

[0135] ,

[0136] ,

[0137] In the formula is a special orthogonal group, referring to the rotation of three-dimensional space, is the rotation matrix, which represents the posture of IMU in the world coordinate system. is the displacement of the IMU in the world coordinate system, is the velocity of the IMU in the world coordinate system, and They are the rotation matrices for transforming the laser radar and wheel speed meter coordinate systems to the IMU coordinate system, and They are the displacements from the laser radar and wheel speed meter installation positions to the IMU installation position, is the gyroscope bias, is the accelerometer bias, is the acceleration due to gravity.

[0138] Represents the pixel gradient at the corresponding pixel point. The pixel gradient is divided into The pixel gradient in the direction and along The pixel gradient in the direction. Two pixels that are adjacent in direction and Calculate the pixel gradient in this direction for:

[0139] ,

[0140] Using this pixel and its Two pixels that are adjacent in direction and Calculate the pixel gradient in this direction for:

[0141] ,

[0142] is the Jacobian matrix of the point cloud projection model, and in the matrix , when using solid-state laser radar, according to the solid-state laser radar projection model, the Jacobian matrix is ​​derived as follows:

[0143] ,

[0144] When using a mechanical laser radar, according to the mechanical laser radar projection model, the Jacobian matrix of the projection model is derived as follows:

[0145] ,

[0146] is the derivative of the point cloud three-dimensional coordinates with respect to the system error state, is the coordinate of the point cloud in the laser radar coordinate system, and its coordinate in the world coordinate system is , the conversion relationship between the point cloud coordinates in the world coordinate system and the lidar coordinate system is:

[0147] ,

[0148] In the formula, It is the rotation matrix of the IMU coordinate system at the time of point cloud measurement to the IMU coordinate system at the end of a frame of point cloud. is the corresponding displacement. and It is the position of the system predicted by the IMU and wheel speed meter at the end of a frame of point cloud. After derivation, we get:

[0149] ,

[0150] In the formula, Represents the antisymmetric matrix of a vector.

[0151] Step 505: Fuse the residual and Jacobian matrix of point-surface matching to form the residual and Jacobian matrix of full-constraint observation. The system increment of geometric constraints is obtained according to the error state iterative Kalman filter gain calculation method. and strength-constrained system increments , and obtain the increment of the fully constrained observation system for:

[0152] .

[0153] Step 506: If the fully constrained observation does not detect the degradation direction, directly use Update the system state; if the fully constrained observation degenerates in some directions, introduce constraint equations in the degenerate directions, constrain the system state update in the degenerate directions through the Lagrange multiplier method, obtain the final system state increment and update the system state.

[0154] After the above steps, the final system state update increment is obtained, which ensures that the system is effectively constrained in each optimization direction and realizes a robust SLAM system in all directions.

[0155] Embodiment 2

[0156] This embodiment provides a 3D laser SLAM system based on full constraints, including:

[0157] A data acquisition module, which is used to acquire laser radar point cloud data;

[0158] The geometric constraint module is used to obtain the information matrix of point-to-surface matching based on the lidar point cloud data, perform eigendecomposition on the sub-blocks related only to the rotation variable and the displacement variable in the information matrix of the point-to-surface matching, obtain multiple eigenvectors, take each eigenvector as an optimization direction, and calculate the constraint contribution value of the point-to-surface matching in each optimization direction;

[0159] An intensity constraint module is used to generate an intensity image based on the lidar point cloud data, and obtain the constraint contribution value of the intensity image in each optimization direction by matching the intensity image frames.

[0160] The fully constrained construction module is used to compare the constraint contribution values ​​of point-to-surface matching and intensity image frame-to-frame matching in each optimization direction, select the one with the larger contribution value, and construct a fully constrained observation;

[0161] The state update module is used to perform degradation detection on the fully constrained observation. If degradation occurs, an additional constraint is added in the degradation direction and then a fully constrained state update is performed. Otherwise, a fully constrained state update is performed directly to obtain the fully constrained observation system increment and update the system state.

[0162] Embodiment 3

[0163] This embodiment provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the steps in the fully-constrained 3D laser SLAM method described above are implemented.

[0164] Embodiment 4

[0165] This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps in the fully-constrained 3D laser SLAM method described above are implemented.

[0166] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A 3D laser SLAM method based on full constraints, characterized in that: The steps include: Get LiDAR point cloud data; Based on the lidar point cloud data, the information matrix of point-to-surface matching is obtained. The sub-blocks related only to the rotation variable and the displacement variable in the information matrix of point-to-surface matching are decomposed to obtain multiple eigenvectors. Each eigenvector is used as an optimization direction to calculate the constraint contribution value of point-to-surface matching in each optimization direction. Generate an intensity image based on the lidar point cloud data, and calculate the constraint contribution value of the intensity image in each optimization direction through the intensity image frame matching; Compare the constraint contribution values ​​of point-to-surface matching and intensity image frame-to-frame matching in each optimization direction, select the one with the larger contribution value, and construct a fully constrained observation; Degradation detection is performed on the fully constrained observation. If degradation occurs, additional constraints are added in the degradation direction and the fully constrained state is updated. Otherwise, the fully constrained state is updated directly to obtain the fully constrained observation system increment and update the system state.

2. The fully-constrained 3D laser SLAM method according to claim 1, wherein: Before obtaining the information matrix of point-surface matching based on the lidar point cloud data, each frame of point cloud data is dedistorted using the predicted value of the system state. The predicted value of the system state is calculated as follows: when only IMU data is available, the predicted value is obtained by forward propagation based on the IMU prediction model; when the tachometer data is received, the IMU prediction data and the tachometer data are fused through the extended Kalman filter to obtain the predicted value.

3. The fully-constrained 3D laser SLAM method according to claim 1, wherein: The step of obtaining the constraint contribution value of the intensity image in each optimization direction by matching the intensity image frames includes: After selecting a pixel block on the intensity image, according to the disturbance given to the laser radar posture, the Jacobian matrix of the laser point corresponding to the central pixel of the pixel block projected onto the intensity image is calculated, which is the projected gradient. The projected gradient is projected in each optimization direction to obtain the projected gradient in each optimization direction. Calculate the second-order moment of the pixel block at its central pixel point, and use the eigenvector corresponding to the maximum eigenvalue of the second-order moment to represent the pixel gradient of the pixel block; Calculate the projection of the projected gradient in each optimization direction on the pixel gradient direction of the pixel block, which is the constraint contribution value of the pixel block to the optimization direction; The sum of the constraint contribution values ​​of all pixel blocks is calculated to obtain the constraint contribution value of the intensity image in each optimization direction.

4. The fully-constrained 3D laser SLAM method according to claim 1, wherein: When an intensity image is generated based on the lidar point cloud data, the intensity image is generated by projecting the lidar point cloud data according to a spherical projection model.

5. The fully-constrained 3D laser SLAM method according to claim 1, wherein: The step of adding additional constraints in the degenerate direction and then updating the full constraint state includes: Obtain the part of the calculated system state variable increment related to the system attitude and position; Construct the update constraint in the degradation direction, combine the part of the system state variable increment related to the system posture and position, and obtain the constraint equation; The Lagrange multiplier method is used to calculate the system state increment obtained under the condition of introducing constraint equations.

6. The fully-constrained 3D laser SLAM method according to claim 1, wherein: When constructing a fully constrained observation, it includes calculating the photometric error of the intensity image and the geometric residual of the point cloud data; Among them, a direct method is used to obtain the photometric error between two frames of intensity images, and then the constraint contribution value of the intensity image in each optimization direction is obtained by matching the intensity images between frames; Among them, for the points in the lidar point cloud, the matching surface is searched in the local map, the distance residual between the point and the surface is constructed, and the geometric residual of the point cloud data is obtained.

7. The fully-constrained 3D laser SLAM method according to claim 1, wherein: The full constraint status update includes the following steps: Calculate the residual of point-to-surface matching and the matching residual of intensity image; Update the Jacobian matrix based on the calculated matching residual of the intensity image; The residuals of point-surface matching and the updated Jacobian matrix are fused to obtain the residuals and Jacobian matrix of the fully constrained observation; Based on the residual and Jacobian matrix of the fully constrained observation, the system increment of the geometric constraint and the system increment of the strength constraint are obtained, and the system increment of the fully constrained observation is obtained by summing them up.

8. A fully constrained 3D laser SLAM system, characterized in that: include: A data acquisition module, which is used to acquire laser radar point cloud data; The geometric constraint module is used to obtain the information matrix of point-to-surface matching based on the lidar point cloud data, perform eigendecomposition on the sub-blocks related only to the rotation variable and the displacement variable in the information matrix of the point-to-surface matching, obtain multiple eigenvectors, take each eigenvector as an optimization direction, and calculate the constraint contribution value of the point-to-surface matching in each optimization direction; An intensity constraint module is used to generate an intensity image based on the lidar point cloud data, and obtain the constraint contribution value of the intensity image in each optimization direction by matching the intensity image frames. The fully constrained construction module is used to compare the constraint contribution values ​​of point-to-surface matching and intensity image frame-to-frame matching in each optimization direction, select the one with the larger contribution value, and construct a fully constrained observation; The state update module is used to perform degradation detection on the fully constrained observation. If degradation occurs, an additional constraint is added in the degradation direction and then a fully constrained state update is performed. Otherwise, a fully constrained state update is performed directly to obtain the fully constrained observation system increment and update the system state.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps in the full-constraint-based 3D laser SLAM method as described in any one of claims 1 to 7 are implemented.

10. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the steps in the fully-constrained 3D laser SLAM method according to any one of claims 1 to 7 are implemented.

Images